POC

src/DataInterpolations.jl

@@ -86,9 +86,10 @@ function Base.showerror(io::IO, e::IntegralNotInvertibleError)
 end
 
 export LinearInterpolation, QuadraticInterpolation, LagrangeInterpolation,
-       AkimaInterpolation, ConstantInterpolation, QuadraticSpline, CubicSpline,
-       BSplineInterpolation, BSplineApprox, CubicHermiteSpline, PCHIPInterpolation,
-       QuinticHermiteSpline, LinearInterpolationIntInv, ConstantInterpolationIntInv
+       AkimaInterpolation, ConstantInterpolation, SmoothedConstantInterpolation,
+       QuadraticSpline, CubicSpline, BSplineInterpolation, BSplineApprox,
+       CubicHermiteSpline, PCHIPInterpolation, QuinticHermiteSpline,
+       LinearInterpolationIntInv, ConstantInterpolationIntInv
 
 # added for RegularizationSmooth, JJS 11/27/21
 ### Regularization data smoothing and interpolation

src/interpolation_caches.jl

@@ -285,6 +285,37 @@ function ConstantInterpolation(
     ConstantInterpolation(u, t, I, dir, extrapolate, cache_parameters, assume_linear_t)
 end
 
+struct SmoothedConstantInterpolation{uType, tType, dmaxType, IType, pType, T, N} <:
+       AbstractInterpolation{T, N}
+    u::uType
+    t::tType
+    I::IType
+    p::SmoothedConstantParameterCache{uType, tType}
+    d_max::dmaxType
+    extrapolate::Bool
+    iguesser::Guesser{tType}
+    cache_parameters::Bool
+    linear_lookup::Bool
+    function SmoothedConstantInterpolation(
+            u, t, I, p, d_max, extrapolate, cache_parameters, assume_linear_t)
+        linear_lookup = seems_linear(assume_linear_t, t)
+        N = get_output_dim(u)
+        new{typeof(u), typeof(t), typeof(d_max), typeof(I), typeof(p.d), eltype(u), N}(
+            u, t, I, p, d_max, extrapolate, Guesser(t), cache_parameters, linear_lookup)
+    end
+end
+
+function SmoothedConstantInterpolation(u, t; d_max = Inf, extrapolate = false,
+        cache_parameters = false, assume_linear_t = 1e-2)
+    u, t = munge_data(u, t)
+    p = SmoothedConstantParameterCache(u, t, cache_parameters, d_max)
+    A = SmoothedConstantInterpolation(
+        u, t, nothing, p, d_max, extrapolate, cache_parameters, assume_linear_t)
+    I = cumulative_integral(A, cache_parameters)
+    SmoothedConstantInterpolation(
+        u, t, I, p, d_max, extrapolate, cache_parameters, assume_linear_t)
+end
+
 """
     QuadraticSpline(u, t; extrapolate = false, cache_parameters = false)
 

src/interpolation_methods.jl

@@ -116,7 +116,7 @@ function _interpolate(A::AkimaInterpolation{<:AbstractVector}, t::Number, iguess
     @evalpoly wj A.u[idx] A.b[idx] A.c[idx] A.d[idx]
 end
 
-# ConstantInterpolation Interpolation
+# Constant Interpolation
 function _interpolate(A::ConstantInterpolation{<:AbstractVector}, t::Number, iguess)
     if A.dir === :left
         # :left means that value to the left is used for interpolation
@@ -139,6 +139,21 @@ function _interpolate(A::ConstantInterpolation{<:AbstractMatrix}, t::Number, igu
     A.u[:, idx]
 end
 
+# Smoothed constant Interpolation
+function _interpolate(A::SmoothedConstantInterpolation{<:AbstractVector}, t::Number, iguess)
+    idx = get_idx(A, t, iguess)
+    d_lower, d_upper, c_lower, c_upper = get_parameters(A, idx)
+    out = A.u[idx]
+
+    if (t - A.t[idx]) < d_lower
+        out -= c_lower * (((t - A.t[idx]) / d_lower - 1))^2
+    elseif (A.t[idx + 1] - t) < d_upper
+        out += c_upper * ((1 - (A.t[idx + 1] - t) / d_upper))^2
+    end
+
+    out
+end
+
 # QuadraticSpline Interpolation
 function _interpolate(A::QuadraticSpline{<:AbstractVector}, t::Number, iguess)
     idx = get_idx(A, t, iguess)

src/interpolation_utils.jl

@@ -207,6 +207,21 @@ function get_parameters(A::LinearInterpolation, idx)
     end
 end
 
+function get_parameters(A::SmoothedConstantInterpolation, idx)
+    if A.cache_parameters
+        d_lower = A.p.d[idx]
+        d_upper = A.p.d[idx + 1]
+        c_lower = A.p.c[idx]
+        c_upper = A.p.c[idx + 1]
+        d_lower, d_upper, c_lower, c_upper
+    else
+        d_lower, c_lower = smoothed_linear_interpolation_parameters(A.u, A.t, A.d_max, idx)
+        d_upper, c_upper = smoothed_linear_interpolation_parameters(
+            A.u, A.t, A.d_max, idx + 1)
+        d_lower, d_upper, c_lower, c_upper
+    end
+end
+
 function get_parameters(A::QuadraticInterpolation, idx)
     if A.cache_parameters
         A.p.α[idx], A.p.β[idx]

src/parameter_caches.jl

@@ -32,6 +32,31 @@ function linear_interpolation_parameters(u::AbstractArray{T, N}, t, idx) where {
     return slope
 end
 
+struct SmoothedConstantParameterCache{uType, tType}
+    d::tType
+    c::uType
+end
+
+function SmoothedConstantParameterCache(u, t, cache_parameters, d_max)
+    if cache_parameters
+        parameters = smoothed_linear_interpolation_parameters.(
+            Ref(u), Ref(t), d_max, eachindex(t))
+        d, c = collect.(eachrow(stack(collect.(parameters))))
+        SmoothedConstantParameterCache(d, c)
+    else
+        SmoothedConstantParameterCache(eltype(t)[], eltype(u)[])
+    end
+end
+
+function smoothed_linear_interpolation_parameters(u, t, d_max, idx)
+    # TODO: Add support for making periodic extrapolation smooth
+    if isone(idx) || (idx == length(t))
+        zero(eltype(t)), zero(eltype(u))
+    else
+        min(t[idx] - t[idx - 1], t[idx + 1] - t[idx], 2d_max) / 2, (u[idx] - u[idx - 1]) / 2
+    end
+end
+
 struct QuadraticParameterCache{pType}
     α::pType
     β::pType