Merge pull request #248 from JuliaControl/skf_mhe

added: `covestim` in `MovingHorizonEstimator` now supports `SteadyKalmanFilter`

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

@@ -56,10 +56,13 @@ struct KalmanCovariances{
         Q̂::Q̂C, R̂::R̂C, P̂_0, He
     ) where {NT<:Real, Q̂C<:AbstractMatrix{NT}, R̂C<:AbstractMatrix{NT}}
         if isnothing(P̂_0)
-            P̂_0 = zeros(NT, 0, 0)
+            P̂_0 = zeros(NT, 0, 0)    # does not apply to steady-state Kalman filter
+            P̂   = zeros(NT, size(Q̂)) # will hold the steady-state error covariance
+        else
+            P̂   = copy(P̂_0)
         end
         P̂_0 = Hermitian(P̂_0, :L)
-        P̂   = copy(P̂_0)
+        P̂   = Hermitian(P̂, :L)
         Q̂   = Hermitian(Q̂, :L)
         R̂   = Hermitian(R̂, :L)
         # the following variables are only for the moving horizon estimator:

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}
@@ -40,24 +43,8 @@ struct SteadyKalmanFilter{
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         R̂, Q̂ = cov.R̂, cov.Q̂
-        if ny == nym
-            R̂_y = R̂
-        else
-            R̂_y = zeros(NT, ny, ny)
-            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]
-        catch my_error
-            if isa(my_error, ErrorException)
-                error("Cannot compute the optimal Kalman gain K̂ for the "* 
-                      "SteadyKalmanFilter. You may try to remove integrators with "*
-                      "nint_u/nint_ym parameter or use the time-varying KalmanFilter.")
-            else
-                rethrow()
-            end
-        end
+        K̂, P̂ = init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct)
+        cov.P̂ .= P̂
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
         corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
@@ -208,6 +195,36 @@ function SteadyKalmanFilter(
     return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct)
 end
 
+"""
+    init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct=true) -> K̂, P̂
+
+Initialize the steady-state Kalman gain `K̂` and estimation error covariance `P̂`.
+"""
+function init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct=true)
+    ny, nym = size(Ĉ, 1), length(i_ym)
+    if ny != nym
+        R̂_y = zeros(eltype(R̂), ny, ny)
+        R̂_y[i_ym, i_ym] = R̂
+        R̂ = Hermitian(R̂_y, :L)
+    end
+    K̂, P̂ = try 
+        ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂, R̂; direct, extra=Val(true))
+    catch my_error
+        if isa(my_error, ErrorException)
+            error("Cannot compute the optimal Kalman gain K̂ for the "* 
+                  "SteadyKalmanFilter. You may try to remove integrators with "*
+                  "nint_u/nint_ym parameter or use the time-varying KalmanFilter.")
+        else
+            rethrow()
+        end
+    end
+    if ny != nym 
+        K̂ = K̂[:, i_ym]
+    end
+    P̂ = Hermitian(P̂, :L)
+    return K̂, P̂
+end
+
 "Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values."
 function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂, R̂)
     if estim.model !== model || !isnothing(Q̂) || !isnothing(R̂)
@@ -216,12 +233,6 @@ function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂,
     return nothing
 end
 
-"Throw an error if P̂ != nothing."
-function setstate_cov!(::SteadyKalmanFilter, P̂)
-    isnothing(P̂) || error("SteadyKalmanFilter does not compute an estimation covariance matrix P̂.")
-    return nothing
-end
-
 @doc raw"""
     correct_estimate!(estim::SteadyKalmanFilter, y0m, d0)
 
@@ -296,7 +307,7 @@ struct KalmanFilter{
     NT<:Real, 
     SM<:LinModel,
     KC<:KalmanCovariances
-} <: StateEstimator{NT}
+} <: KalmanEstimator{NT}
     model::SM
     cov  ::KC
     x̂op::Vector{NT}
@@ -508,7 +519,7 @@ struct UnscentedKalmanFilter{
     NT<:Real, 
     SM<:SimModel,
     KC<:KalmanCovariances
-} <: StateEstimator{NT}
+} <: KalmanEstimator{NT}
     model::SM
     cov  ::KC
     x̂op ::Vector{NT}
@@ -554,7 +565,7 @@ struct UnscentedKalmanFilter{
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
-        nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ)
+        nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
         x̂0  = [zeros(NT, model.nx); zeros(NT, nxs)]
         K̂ = zeros(NT, nx̂, nym)
         M̂ = Hermitian(zeros(NT, nym, nym), :L)
@@ -715,7 +726,7 @@ end
 
 
 @doc raw"""
-    init_ukf(model, nx̂, α, β, κ) -> nσ, γ, m̂, Ŝ
+    init_ukf(nx̂, α, β, κ) -> nσ, γ, m̂, Ŝ
 
 Compute the [`UnscentedKalmanFilter`](@ref) constants from ``α, β`` and ``κ``.
 
@@ -735,14 +746,15 @@ covariance are respectively:
 ```
 See [`update_estimate!(::UnscentedKalmanFilter)`](@ref) for other details.
 """
-function init_ukf(::SimModel{NT}, nx̂, α, β, κ) where {NT<:Real}
-    nσ = 2nx̂ + 1                                  # number of sigma points
-    γ = α * √(nx̂ + κ)                             # constant factor of standard deviation √P
+function init_ukf(nx̂, α, β, κ)
+    α, β, κ = promote(α, β, κ)
+    nσ =2nx̂ + 1                                 # number of sigma points
+    γ = α * √(nx̂ + κ)                           # constant factor of standard deviation √P
     m̂_0 = 1 - nx̂ / γ^2
     Ŝ_0 = m̂_0 + 1 - α^2 + β
     w = 1 / 2 / γ^2
-    m̂ = NT[m̂_0; fill(w, 2 * nx̂)]                  # weights for the mean
-    Ŝ = Diagonal(NT[Ŝ_0; fill(w, 2 * nx̂)])        # weights for the covariance
+    m̂ = [m̂_0; fill(w, 2 * nx̂)]                  # weights for the mean
+    Ŝ = Diagonal([Ŝ_0; fill(w, 2 * nx̂)])        # weights for the covariance
     return nσ, γ, m̂, Ŝ
 end
 
@@ -885,7 +897,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"))
@@ -280,6 +280,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
 ├ optimizer: Ipopt
 ├ gradient: AutoForwardDiff
 ├ jacobian: AutoForwardDiff
+├ arrival covariance: UnscentedKalmanFilter
 └ dimensions:
   ├ 5 estimation steps He
   ├ 0 slack variable ε (estimation constraints)
@@ -423,8 +424,10 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂_i}, \mathbf{Q
 where ``\mathbf{P̂_i}`` is the initial estimation covariance, provided by `P̂_0` argument. The
 keyword argument `covestim` also allows specifying a custom [`StateEstimator`](@ref) object
 for the estimation of covariance at the arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. The
-supported types are [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref) and 
-[`ExtendedKalmanFilter`](@ref).
+supported types are [`SteadyKalmanFilter`](@ref), [`KalmanFilter`](@ref), 
+[`UnscentedKalmanFilter`](@ref) and [`ExtendedKalmanFilter`](@ref). A constant arrival
+covariance is supported with [`SteadyKalmanFilter`](@ref), and by setting the `P̂` argument 
+of [`setstate!`](@ref) at the desired value.
 """
 function MovingHorizonEstimator(
     model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
@@ -436,6 +439,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 +448,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
 
@@ -498,6 +515,7 @@ julia> estim = setconstraint!(estim, x̂min=[-50, -50], x̂max=[50, 50])
 MovingHorizonEstimator estimator with a sample time Ts = 1.0 s:
 ├ model: LinModel
 ├ optimizer: OSQP
+├ arrival covariance: KalmanFilter
 └ dimensions:
   ├ 3 estimation steps He
   ├ 0 slack variable ε (estimation constraints)

test/2_test_state_estim.jl

@@ -112,7 +112,6 @@ end
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
     @test_throws ArgumentError updatestate!(kalmanfilter1, [10, 50])
-    @test_throws ErrorException setstate!(kalmanfilter1, [1,2,3,4], diagm(.1:.1:.4))
 end 
 
 @testitem "SteadyKalmanFilter set model" setup=[SetupMPCtests] begin
@@ -904,17 +903,26 @@ end
     @test_throws ArgumentError MovingHorizonEstimator(linmodel)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1)
+    @test_throws ErrorException MovingHorizonEstimator(
+        nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Inf; optim, 
+        covestim = InternalModel(nonlinmodel)
+    )
+    @test_throws ArgumentError MovingHorizonEstimator(
+        nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Inf; optim, 
+        covestim = UnscentedKalmanFilter(nonlinmodel, nint_ym=[2,2])
+    )
 end
 
 @testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin
-    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt, ForwardDiff
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff
+    using JuMP, Ipopt, DAQP
     linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3])
     linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
     f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
     h(x,d,model)   = model.C*x + model.Dd*d
     nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel)
     nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5])
-    
+
     mhe1 = MovingHorizonEstimator(nonlinmodel, He=2)
     JuMP.set_attribute(mhe1.optim, "tol", 1e-7)
     preparestate!(mhe1, [50, 30], [5])
@@ -950,7 +958,6 @@ end
     x̂ = updatestate!(mhe1, [10, 50], [50, 30], [5])
     @test x̂ ≈ zeros(6) atol=1e-9
     @test mhe1.x̂0 ≈ zeros(6) atol=1e-9
-    @test evaloutput(mhe1, [5]) ≈ mhe1([5]) ≈ [50, 30]
     info = getinfo(mhe1)
     @test info[:x̂] ≈ x̂ atol=1e-9
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
@@ -976,7 +983,6 @@ end
     @test x̂ ≈ zeros(6) atol=1e-9
     @test mhe2.x̂0 ≈ zeros(6) atol=1e-9
     preparestate!(mhe2, [50, 30], [5])
-    @test evaloutput(mhe2, [5]) ≈ mhe2([5]) ≈ [50, 30]
     info = getinfo(mhe2)
     @test info[:x̂] ≈ x̂ atol=1e-9
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
@@ -998,7 +1004,6 @@ end
     x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
     @test x̂ ≈ zeros(6) atol=1e-9
     @test mhe2.x̂0 ≈ zeros(6) atol=1e-9
-    @test evaloutput(mhe2, [5]) ≈ mhe2([5]) ≈ [50, 30]
     info = getinfo(mhe2)
     @test info[:x̂] ≈ x̂ atol=1e-9
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
@@ -1012,12 +1017,29 @@ end
         updatestate!(mhe2, [10, 50], [51, 32], [5])
     end
     @test mhe2([5]) ≈ [51, 32] atol=1e-2
+
+    Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) 
+    R̂ = diagm([1, 1].^2)
+    optim = Model(DAQP.Optimizer)
+    covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂)
+    P̂_0 = covestim.cov.P̂
+    mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim)
+    preparestate!(mhe3, [50, 30], [5])
+    x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5])
+    @test x̂ ≈ zeros(4) atol=1e-9
+    @test mhe3.x̂0 ≈ zeros(4) atol=1e-9
+    preparestate!(mhe3, [50, 30], [5])
+    info = getinfo(mhe3)
+    @test info[:x̂] ≈ x̂ atol=1e-9
+    @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
+
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mhe3 = MovingHorizonEstimator(linmodel3, He=1)
     preparestate!(mhe3, [0])
     x̂ = updatestate!(mhe3, [0], [0])
     @test x̂ ≈ [0, 0] atol=1e-3
     @test isa(x̂, Vector{Float32})
+    
     mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), v̂max=[50,50])
     g_V̂max_end = mhe4.optim[:g_V̂max_2].func
     # execute update_predictions! branch in `gfunc_i` for coverage:
@@ -1027,7 +1049,7 @@ end
     R̂ = diagm([1, 1].^2)
     optim = Model(Ipopt.Optimizer)
     covestim = ExtendedKalmanFilter(nonlinmodel, 1:2, 0, 0, Q̂, Q̂, R̂)
-    mhe5 = MovingHorizonEstimator(nonlinmodel, 1, 1:2, 0, 0, Q̂, Q̂, R̂, Inf; optim, covestim)
+    mhe5 = MovingHorizonEstimator(nonlinmodel, 1, 1:2, 0, 0, Q̂, Q̂, R̂; optim, covestim)
     preparestate!(mhe5, [50, 30], [5])
     x̂ = updatestate!(mhe5, [10, 50], [50, 30], [5])
     @test x̂ ≈ zeros(4) atol=1e-9