KalmanFilter and SteadyKalmanFilter parametric to avoid dynamic dispatch

Author: franckgaga

src/estimator/kalman.jl

@@ -1,5 +1,5 @@
-struct SteadyKalmanFilter <: StateEstimator
-    model::LinModel
+struct SteadyKalmanFilter{M<:LinModel} <: StateEstimator
+    model::M
     x̂::Vector{Float64}
     i_ym::Vector{Int}
     nx̂::Int
@@ -19,7 +19,7 @@ struct SteadyKalmanFilter <: StateEstimator
     Q̂::Hermitian{Float64, Matrix{Float64}}
     R̂::Hermitian{Float64, Matrix{Float64}}
     K::Matrix{Float64}
-    function SteadyKalmanFilter(model, i_ym, nint_ym, Asm, Csm, Q̂, R̂)
+    function SteadyKalmanFilter{M}(model::M, i_ym, nint_ym, Asm, Csm, Q̂, R̂) where {M<:LinModel}
         nx, ny = model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
@@ -119,21 +119,21 @@ you can use 0 integrator on `model` integrating outputs, or the alternative time
 [`KalmanFilter`](@ref).
 """
 function SteadyKalmanFilter(
-    model::LinModel;
+    model::M;
     i_ym::IntRangeOrVector = 1:model.ny,
     σQ::Vector{<:Real} = fill(0.1, model.nx),
     σR::Vector{<:Real} = fill(0.1, length(i_ym)),
     nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
     σQ_int::Vector{<:Real} = fill(0.1, max(sum(nint_ym), 0))
-)
+) where {M<:LinModel}
     if nint_ym == 0 # alias for no output integrator at all :
         nint_ym = fill(0, length(i_ym));
     end
     Asm, Csm = init_estimstoch(i_ym, nint_ym)
     # estimated covariances matrices (variance = σ²) :
     Q̂  = Diagonal{Float64}([σQ   ; σQ_int    ].^2);
     R̂  = Diagonal{Float64}(σR.^2);
-    return SteadyKalmanFilter(model, i_ym, nint_ym, Asm, Csm, Q̂ , R̂)
+    return SteadyKalmanFilter{M}(model, i_ym, nint_ym, Asm, Csm, Q̂ , R̂)
 end
 
 @doc raw"""
@@ -166,8 +166,8 @@ function updatestate!(estim::SteadyKalmanFilter, u, ym, d=Float64[])
 end
 
 
-struct KalmanFilter <: StateEstimator
-    model::LinModel
+struct KalmanFilter{M<:LinModel} <: StateEstimator
+    model::M
     x̂::Vector{Float64}
     P̂::Hermitian{Float64, Matrix{Float64}}
     i_ym::Vector{Int}
@@ -188,7 +188,7 @@ struct KalmanFilter <: StateEstimator
     P̂0::Hermitian{Float64, Matrix{Float64}}
     Q̂::Hermitian{Float64, Matrix{Float64}}
     R̂::Hermitian{Float64, Matrix{Float64}}
-    function KalmanFilter(model, i_ym, nint_ym, Asm, Csm, P̂0, Q̂, R̂)
+    function KalmanFilter{M}(model::M, i_ym, nint_ym, Asm, Csm, P̂0, Q̂, R̂) where {M<:LinModel}
         nx, ny = model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
@@ -247,15 +247,15 @@ KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
 ```
 """
 function KalmanFilter(
-    model::LinModel;
+    model::M;
     i_ym::IntRangeOrVector = 1:model.ny,
     σP0::Vector{<:Real} = fill(10, model.nx),
     σQ::Vector{<:Real} = fill(0.1, model.nx),
     σR::Vector{<:Real} = fill(0.1, length(i_ym)),
     nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
     σP0_int::Vector{<:Real} = fill(10, max(sum(nint_ym), 0)),
     σQ_int::Vector{<:Real} = fill(0.1, max(sum(nint_ym), 0))
-)
+) where {M<:LinModel}
     if nint_ym == 0 # alias for no output integrator at all :
         nint_ym = fill(0, length(i_ym));
     end
@@ -264,7 +264,7 @@ function KalmanFilter(
     P̂0 = Diagonal{Float64}([σP0  ; σP0_int   ].^2);
     Q̂  = Diagonal{Float64}([σQ   ; σQ_int    ].^2);
     R̂  = Diagonal{Float64}(σR.^2);
-    return KalmanFilter(model, i_ym, nint_ym, Asm, Csm, P̂0, Q̂ , R̂)
+    return KalmanFilter{M}(model, i_ym, nint_ym, Asm, Csm, P̂0, Q̂ , R̂)
 end
 
 @doc raw"""

src/predictive_control.jl

@@ -659,13 +659,13 @@ julia> u = moveinput!(mpc, [5]); round.(u, digits=3)
 ```
 """
 function moveinput!(
-    mpc::C, 
+    mpc::PredictiveController, 
     ry::Vector{<:Real}, 
     d ::Vector{<:Real} = Float64[];
     R̂y::Vector{<:Real} = repeat(ry, mpc.Hp),
     D̂ ::Vector{<:Real} = repeat(d,  mpc.Hp),
     ym::Union{Vector{<:Real}, Nothing} = nothing
-) where {C<:PredictiveController}
+)
     lastu = mpc.info.u
     x̂d, x̂s = split_state(mpc.estim)
     ŷs, Ŷs = predict_stoch(mpc, mpc.estim, x̂s, d, ym)
@@ -752,8 +752,8 @@ Init linear model prediction matrices `F`, `q̃` and `p`.
 See [`init_deterpred`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
 """
 function init_prediction(
-    mpc::C, model::LinModel, d, D̂, Ŷs, R̂y, x̂d, lastu
-) where {C<:PredictiveController}
+    mpc::PredictiveController, model::LinModel, d, D̂, Ŷs, R̂y, x̂d, lastu
+)
     F = mpc.Kd*x̂d + mpc.Q*(lastu - model.uop) + Ŷs + mpc.Yop
     if model.nd ≠ 0
         F += mpc.G*(d - model.dop) + mpc.J*(D̂ - mpc.Dop)
@@ -774,7 +774,7 @@ end
 
 Calc `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
 """
-function linconstraint(mpc::C, ::LinModel, lastu, F) where {C<:PredictiveController}
+function linconstraint(mpc::PredictiveController, ::LinModel, lastu, F)
     b = [
         -mpc.con.Umin + mpc.T_Hc*lastu
         +mpc.con.Umax - mpc.T_Hc*lastu 
@@ -791,7 +791,7 @@ end
 
 Calc `b` without predicted output ``\mathbf{Ŷ}`` constraints for [`NonLinModel`](@ref). 
 """
-function linconstraint(mpc::C, ::NonLinModel, lastu, _ ) where {C<:PredictiveController}
+function linconstraint(mpc::PredictiveController, ::NonLinModel, lastu, _ )
     b = [
         -mpc.con.Umin + mpc.T_Hc*lastu
         +mpc.con.Umax - mpc.T_Hc*lastu 

src/state_estim.jl

@@ -161,7 +161,7 @@ function returns the next state of the augmented model, defined as:
 \end{aligned}
 ```
 """
-function f̂(estim::E, x̂, u, d) where {E<:StateEstimator}
+function f̂(estim::StateEstimator, x̂, u, d)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
     nx = estim.model.nx
     @views return [estim.model.f(x̂[1:nx], u, d); estim.As*x̂[nx+1:end]]
@@ -172,7 +172,7 @@ end
 
 Output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂`](@ref) for details.
 """
-function ĥ(estim::E, x̂, d) where {E<:StateEstimator}
+function ĥ(estim::StateEstimator, x̂, d)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
     nx = estim.model.nx
     @views return estim.model.h(x̂[1:nx], d) + estim.Cs*x̂[nx+1:end]