added: support SteadyKalmanFilter for covestim argument in MovingHorizonEstimator

Author: franckgaga

Project.toml

@@ -21,7 +21,7 @@ SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5"
 SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35"
 
 [compat]
-ControlSystemsBase = "1.9"
+ControlSystemsBase = "1.18.2"
 DAQP = "0.6, 0.7.1"
 DifferentiationInterface = "0.6.45, 0.7"
 Documenter = "1"

src/estimator/construct.jl

@@ -59,9 +59,9 @@ struct KalmanCovariances{
             P̂_0 = zeros(NT, 0, 0)
         end
         P̂_0 = Hermitian(P̂_0, :L)
-        P̂   = copy(P̂_0)
         Q̂   = Hermitian(Q̂, :L)
         R̂   = Hermitian(R̂, :L)
+        P̂   = copy(Q̂)
         # the following variables are only for the moving horizon estimator:
         invP̄, invQ̂, invR̂ = copy(P̂_0), copy(Q̂), copy(R̂)
         try

src/estimator/kalman.jl

@@ -1,8 +1,11 @@
+"Abstract supertype of all Kalman-type state estimators."
+abstract type KalmanEstimator{NT<:Real} <: StateEstimator{NT} end
+
 struct SteadyKalmanFilter{
     NT<:Real, 
     SM<:LinModel, 
     KC<:KalmanCovariances
-} <: StateEstimator{NT}
+} <: KalmanEstimator{NT}
     model::SM
     cov  ::KC
     x̂op ::Vector{NT}
@@ -47,8 +50,8 @@ struct SteadyKalmanFilter{
             R̂_y[i_ym, i_ym] = R̂
             R̂_y = Hermitian(R̂_y, :L)
         end
-        K̂ = try
-            ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂, R̂_y; direct)[:, i_ym]
+        K̂_y, P̂ = try
+            ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂, R̂_y; direct, extra=Val(true))
         catch my_error
             if isa(my_error, ErrorException)
                 error("Cannot compute the optimal Kalman gain K̂ for the "* 
@@ -58,6 +61,8 @@ struct SteadyKalmanFilter{
                 rethrow()
             end
         end
+        K̂ = (ny == nym) ? K̂_y : K̂_y[:, i_ym]
+        cov.P̂ .= Hermitian(P̂, :L)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
         corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
@@ -296,7 +301,7 @@ struct KalmanFilter{
     NT<:Real, 
     SM<:LinModel,
     KC<:KalmanCovariances
-} <: StateEstimator{NT}
+} <: KalmanEstimator{NT}
     model::SM
     cov  ::KC
     x̂op::Vector{NT}
@@ -508,7 +513,7 @@ struct UnscentedKalmanFilter{
     NT<:Real, 
     SM<:SimModel,
     KC<:KalmanCovariances
-} <: StateEstimator{NT}
+} <: KalmanEstimator{NT}
     model::SM
     cov  ::KC
     x̂op ::Vector{NT}
@@ -885,7 +890,7 @@ struct ExtendedKalmanFilter{
         JB<:AbstractADType, 
         FF<:Function,
         HF<:Function
-} <: StateEstimator{NT}
+} <: KalmanEstimator{NT}
     model::SM
     cov  ::KC
     x̂op ::Vector{NT}

src/estimator/mhe.jl

@@ -5,6 +5,7 @@ include("mhe/execute.jl")
 function print_details(io::IO, estim::MovingHorizonEstimator)
     println(io, "├ optimizer: $(JuMP.solver_name(estim.optim)) ")
     print_backends(io, estim, estim.model)
+    println(io, "├ arrival covariance: $(nameof(typeof(estim.covestim))) ")
 end
 
 "Print the differentiation backends for `SimModel`."

src/estimator/mhe/construct.jl

@@ -51,7 +51,7 @@ struct MovingHorizonEstimator{
     JM<:JuMP.GenericModel,
     GB<:AbstractADType,
     JB<:AbstractADType,
-    CE<:StateEstimator,
+    CE<:KalmanEstimator,
 } <: StateEstimator{NT}
     model::SM
     cov  ::KC
@@ -121,7 +121,7 @@ struct MovingHorizonEstimator{
             JM<:JuMP.GenericModel, 
             GB<:AbstractADType,
             JB<:AbstractADType,
-            CE<:StateEstimator{NT}
+            CE<:KalmanEstimator{NT}
         }
         nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk
         He < 1  && throw(ArgumentError("Estimation horizon He should be ≥ 1"))
@@ -436,6 +436,7 @@ function MovingHorizonEstimator(
 ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
     P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
     cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0, He)
+    validate_covestim(cov, covestim)
     return MovingHorizonEstimator{NT}(
         model, 
         He, i_ym, nint_u, nint_ym, cov, Cwt, 
@@ -444,13 +445,26 @@ function MovingHorizonEstimator(
     )
 end
 
+
 function default_covestim_mhe(model::LinModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
     return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
     return UnscentedKalmanFilter(model,  i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 
+"Validate covestim type and dimensions."
+function validate_covestim(cov::KalmanCovariances, covestim::KalmanEstimator)
+    if size(cov.P̂) != size(covestim.cov.P̂)
+        throw(ArgumentError("estimation covariance covestim.cov.P̂ size does not match the MHE"))
+    end
+    return nothing
+end
+function validate_covestim(::KalmanCovariances, ::StateEstimator)
+    error(  "covestim argument must be a SteadyKalmanFilter, KalmanFilter, "*
+            "ExtendedKalmanFilter or UnscentedKalmanFilter")
+end
+
 @doc raw"""
     setconstraint!(estim::MovingHorizonEstimator; <keyword arguments>) -> estim