Merge pull request #192 from JuliaControl/setstate_cov

added: `setstate!` now allows estimation covariance `P̂` modification (if applicable)

Author: franckgaga

.github/workflows/CI.yml

@@ -43,7 +43,7 @@ jobs:
       - uses: julia-actions/julia-buildpkg@v1
       - uses: julia-actions/julia-runtest@v1
       - uses: julia-actions/julia-processcoverage@v1
-      - uses: codecov/codecov-action@v4
+      - uses: codecov/codecov-action@v5
         with:
           token: ${{ secrets.CODECOV_TOKEN }}
           fail_ci_if_error: false 

src/controller/execute.jl

@@ -488,12 +488,11 @@ Call `periodsleep(mpc.estim.model)`.
 periodsleep(mpc::PredictiveController, busywait=false) = periodsleep(mpc.estim.model, busywait)
 
 """
-    setstate!(mpc::PredictiveController, x̂) -> mpc
+    setstate!(mpc::PredictiveController, x̂, P̂=nothing) -> mpc
 
-Set `mpc.estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`.
+Call [`setstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
-setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)
-
+setstate!(mpc::PredictiveController, x̂, P̂=nothing) = (setstate!(mpc.estim, x̂, P̂); return mpc)
 
 @doc raw"""
     setmodel!(mpc::PredictiveController, model=mpc.estim.model; <keyword arguments>) -> mpc

src/estimator/execute.jl

@@ -330,16 +330,29 @@ function validate_args(estim::StateEstimator, ym, d, u=nothing)
 end
 
 """
-    setstate!(estim::StateEstimator, x̂) -> estim
+    setstate!(estim::StateEstimator, x̂, P̂=nothing) -> estim
 
-Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`. 
+Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. 
+
+The covariance error estimate `P̂` can be set only if `estim` is a [`StateEstimator`](@ref)
+that computes it.
 """
-function setstate!(estim::StateEstimator, x̂)
+function setstate!(estim::StateEstimator, x̂, P̂=nothing)
     size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))")
     estim.x̂0 .= x̂ .- estim.x̂op
+    setstate_cov!(estim, P̂)
     return estim
 end
 
+"Set the covariance error estimate `estim.P̂` to `P̂`."
+function setstate_cov!(estim::StateEstimator, P̂)
+    if !isnothing(P̂)
+        size(P̂) == (estim.nx̂, estim.nx̂) || error("P̂ size must be $((estim.nx̂, estim.nx̂))")
+        estim.P̂ .= to_hermitian(P̂)
+    end
+    return nothing
+end
+
 @doc raw"""
     setmodel!(estim::StateEstimator, model=estim.model; <keyword arguments>) -> estim
 

src/estimator/internal_model.jl

@@ -226,6 +226,12 @@ function init_internalmodel(As, Bs, Cs, Ds)
     return Âs, B̂s
 end
 
+"Throw an error if P̂ != nothing."
+function setstate_cov!(estim::InternalModel, P̂)
+    P̂ == nothing || error("InternalModel does not compute an estimation covariance matrix P̂.")
+    return nothing
+end
+
 "Update similar values for [`InternalModel`](@ref) estimator."
 function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ )
     Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)

src/estimator/luenberger.jl

@@ -136,6 +136,12 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0)
     return predict_estimate_obsv!(estim, y0m, d0, u0)
 end
 
+"Throw an error if P̂ != nothing."
+function setstate_cov!(estim::Luenberger, P̂)
+    P̂ == nothing || error("Luenberger does not compute an estimation covariance matrix P̂.")
+    return nothing
+end
+
 "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values."
 function setmodel_estimator!(estim::Luenberger, model, args...)
     if estim.model !== model

src/estimator/mhe/execute.jl

@@ -650,6 +650,12 @@ end
 "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged."
 con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g
 
+"Throw an error if P̂ != nothing."
+function setstate_cov!(estim::MovingHorizonEstimator, P̂)
+    P̂ == nothing || error("MovingHorizonEstimator does not compute an estimation covariance matrix P̂.")
+    return nothing
+end
+
 "Update the augmented model, prediction matrices, constrains and data windows for MHE."
 function setmodel_estimator!(
     estim::MovingHorizonEstimator, model, uop_old, yop_old, dop_old, Q̂, R̂

test/2_test_state_estim.jl

@@ -115,6 +115,7 @@ end
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
     @test_throws ArgumentError updatestate!(skalmanfilter1, [10, 50])
+    @test_throws ErrorException setstate!(skalmanfilter1, [1,2,3,4], diagm(.1:.1:.4))
 end 
 
 @testitem "SteadyKalmanFilter set model" setup=[SetupMPCtests] begin
@@ -211,8 +212,9 @@ end
     @test evaloutput(kalmanfilter1, d) ≈ kalmanfilter1(d) ≈ [50, 30]
     @test_skip @allocations(evaloutput(kalmanfilter1, d)) == 0
     @test initstate!(kalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
-    setstate!(kalmanfilter1, [1,2,3,4])
+    setstate!(kalmanfilter1, [1,2,3,4], diagm(.1:.1:.4))
     @test kalmanfilter1.x̂0 ≈ [1,2,3,4]
+    @test kalmanfilter1.P̂  ≈ diagm(.1:.1:.4)
     for i in 1:40
         preparestate!(kalmanfilter1, [50, 30])
         updatestate!(kalmanfilter1, [11, 52], [50, 30])
@@ -363,6 +365,7 @@ end
     x̂ = updatestate!(lo3, [0], [0])
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
+    @test_throws ErrorException setstate!(lo1, [1,2,3,4], diagm(.1:.1:.4))
 end
 
 @testitem "Luenberger set model" setup=[SetupMPCtests] begin
@@ -484,6 +487,7 @@ end
     x̂ = updatestate!(internalmodel3, [0], [0])
     @test x̂ ≈ [0]
     @test isa(x̂, Vector{Float32})
+    @test_throws ErrorException setstate!(internalmodel1, [1,2,3,4], diagm(.1:.1:.4))
 end
 
 @testitem "InternalModel set model" setup=[SetupMPCtests] begin
@@ -598,8 +602,9 @@ end
     @test evaloutput(ukf1, d) ≈ ukf1(d) ≈ [50, 30]
     @test_skip @allocations(evaloutput(ukf1, d)) == 0
     @test initstate!(ukf1, [10, 50], [50, 30+1]) ≈ zeros(4) atol=1e-9
-    setstate!(ukf1, [1,2,3,4])
+    setstate!(ukf1, [1,2,3,4], diagm(.1:.1:.4))
     @test ukf1.x̂0 ≈ [1,2,3,4]
+    @test ukf1.P̂ ≈ diagm(.1:.1:.4)
     for i in 1:40
         preparestate!(ukf1, [50, 30])
         updatestate!(ukf1, [11, 52], [50, 30])
@@ -757,8 +762,9 @@ end
     @test evaloutput(ekf1, d) ≈ ekf1(d) ≈ [50, 30]
     @test_skip @allocations(evaloutput(ekf1, d)) == 0
     @test initstate!(ekf1, [10, 50], [50, 30+1]) ≈ zeros(4);
-    setstate!(ekf1, [1,2,3,4])
+    setstate!(ekf1, [1,2,3,4], diagm(.1:.1:.4))
     @test ekf1.x̂0 ≈ [1,2,3,4]
+    @test ekf1.P̂ ≈ diagm(.1:.1:.4)
     for i in 1:40
         preparestate!(ekf1, [50, 30])
         updatestate!(ekf1, [11, 52], [50, 30])
@@ -1055,6 +1061,7 @@ end
     x̂ = preparestate!(mhe6, [50, 30], [5])
     @test x̂ ≈ zeros(6) atol=1e-9
     @test_nowarn ModelPredictiveControl.info2debugstr(info)
+    @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6))
 end
 
 @testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin

test/3_test_predictive_control.jl

@@ -163,11 +163,12 @@ end
 @testitem "LinMPC other methods" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
-    mpc1 = LinMPC(linmodel1)
+    mpc1 = LinMPC(KalmanFilter(linmodel1))
     @test initstate!(mpc1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
-    setstate!(mpc1, [1,2,3,4])
+    setstate!(mpc1, [1,2,3,4], diagm(.1:.1:.4))
     @test mpc1.estim.x̂0 ≈ [1,2,3,4]
-    setstate!(mpc1, [0,0,0,0])
+    @test mpc1.estim.P̂  ≈ diagm(.1:.1:.4)
+    setstate!(mpc1, [0,0,0,0], mpc1.estim.P̂_0)
     preparestate!(mpc1, [50, 30])
     updatestate!(mpc1, mpc1.estim.model.uop, [50, 30])
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]