added: testing the MovingHorizonEstimator fallbacks

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.2.0"
+version = "1.2.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

src/estimator/mhe/execute.jl

@@ -435,19 +435,15 @@ function correct_cov!(estim::MovingHorizonEstimator)
     estim.covestim.P̂  .= estim.P̂arr_old
     try
         correct_estimate!(estim.covestim, y0marr, d0arr)
+        estim.P̂arr_old .= estim.covestim.P̂
+        invert_cov!(estim, estim.covestim.P̂)
     catch err
         if err isa PosDefException
-            @warn("Arrival covariance not positive definite: using nearest symmetric one")
-            estim.covestim.P̂ .= estim.P̂arr_old .+ estim.P̂arr_old'
-            lmul!(0.5, estim.covestim.P̂)
-            correct_estimate!(estim.covestim, y0marr, d0arr)
+            @warn("Arrival covariance is not positive definite: keeping the old one")
         else
             rethrow(err)
         end
     end
-    P̄ = estim.covestim.P̂
-    invert_cov!(estim, P̄)
-    estim.P̂arr_old .= P̄
     return nothing
 end
 
@@ -458,20 +454,16 @@ function update_cov!(estim::MovingHorizonEstimator)
     estim.covestim.x̂0 .= estim.x̂0arr_old
     estim.covestim.P̂  .= estim.P̂arr_old
     try
-        update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
+        correct_estimate!(estim.covestim, y0marr, d0arr)
+        estim.P̂arr_old .= estim.covestim.P̂
+        invert_cov!(estim, estim.covestim.P̂)
     catch err
         if err isa PosDefException
-            @warn("Arrival covariance not positive definite: using nearest symmetric one")
-            estim.covestim.P̂ .= estim.P̂arr_old .+ estim.P̂arr_old'
-            lmul!(0.5, estim.covestim.P̂)
-            update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
+            @warn("Arrival covariance is not positive definite: keeping the old one")
         else
             rethrow(err)
         end
     end
-    P̄ = estim.covestim.P̂
-    invert_cov!(estim, P̄)
-    estim.P̂arr_old .= P̄
     return nothing
 end
 
@@ -481,8 +473,7 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄)
         estim.invP̄ .= inv(P̄)
     catch err
         if err isa SingularException
-            @warn("Arrival covariance is singular: adding small regularization term")
-            estim.invP̄ .= inv(P̄ + eps()*I)
+            @warn("Arrival covariance is not invertible: keeping the old one")
         else
             rethrow(err)
         end

test/test_state_estim.jl

@@ -975,6 +975,35 @@ end
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
 end
 
+@testset "MovingHorizonEstimator fallbacks for arrival covariance estimation" begin
+    linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])
+    f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
+    h(x,d,_)   = linmodel.C*x + linmodel.Dd*d
+    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5])
+    mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1)
+    preparestate!(mhe, [50, 30], [5])
+    updatestate!(mhe, [10, 50], [50, 30], [5])
+    mhe.P̂arr_old[1, 1] = -1e-3 # negative eigenvalue to trigger fallback
+    P̂arr_old_copy = deepcopy(mhe.P̂arr_old)
+    invP̄_copy = deepcopy(mhe.invP̄)
+    @test_logs(
+        (:warn, "Arrival covariance is not positive definite: keeping the old one"), 
+        preparestate!(mhe, [50, 30], [5])
+    )
+    @test mhe.P̂arr_old ≈ P̂arr_old_copy
+    @test mhe.invP̄ ≈ invP̄_copy
+    @test_logs(
+        (:warn, "Arrival covariance is not positive definite: keeping the old one"), 
+        updatestate!(mhe, [10, 50], [50, 30], [5])
+    )
+    @test mhe.P̂arr_old ≈ P̂arr_old_copy
+    @test mhe.invP̄ ≈ invP̄_copy
+    @test_logs(
+        (:warn, "Arrival covariance is not invertible: keeping the old one"), 
+        ModelPredictiveControl.invert_cov!(mhe, zeros(mhe.nx̂, mhe.nx̂))
+    )
+end
+
 @testset "MovingHorizonEstimator set constraints" begin
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     mhe1 = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0, Cwt=1e3)