debug: fallback if MovingHorizonEstimator arrival covariance update fails

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/luenberger.jl

@@ -137,7 +137,7 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0)
 end
 
 "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values."
-function setmodel!(estim::Luenberger, model, args...)
+function setmodel_estimator!(estim::Luenberger, model, args...)
     if estim.model !== model
         error("Luenberger does not support setmodel!")
     end

src/estimator/mhe/execute.jl

@@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0)
         estim.D0[1:estim.model.nd] .= d0
     end
     # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0)
-    estim.invP̄      .= inv(estim.P̂_0)
+    invert_cov!(estim, estim.P̂_0)
     estim.P̂arr_old  .= estim.P̂_0
     estim.x̂0arr_old .= 0
     return nothing
@@ -433,9 +433,17 @@ function correct_cov!(estim::MovingHorizonEstimator)
     y0marr, d0arr = @views estim.Y0m[1:nym], estim.D0[1:nd]
     estim.covestim.x̂0 .= estim.x̂0arr_old
     estim.covestim.P̂  .= estim.P̂arr_old
-    correct_estimate!(estim.covestim, y0marr, d0arr)
-    estim.P̂arr_old .= estim.covestim.P̂
-    estim.invP̄     .= inv(estim.P̂arr_old)
+    try
+        correct_estimate!(estim.covestim, y0marr, d0arr)
+        estim.P̂arr_old .= estim.covestim.P̂
+        invert_cov!(estim, estim.P̂arr_old)
+    catch err
+        if err isa PosDefException
+            @warn("Arrival covariance is not positive definite: keeping the old one")
+        else
+            rethrow(err)
+        end
+    end
     return nothing
 end
 
@@ -445,9 +453,31 @@ function update_cov!(estim::MovingHorizonEstimator)
     u0arr, y0marr, d0arr = @views estim.U0[1:nu], estim.Y0m[1:nym], estim.D0[1:nd]
     estim.covestim.x̂0 .= estim.x̂0arr_old
     estim.covestim.P̂  .= estim.P̂arr_old
-    update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
-    estim.P̂arr_old    .= estim.covestim.P̂
-    estim.invP̄        .= inv(estim.P̂arr_old)
+    try
+        update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
+        estim.P̂arr_old .= estim.covestim.P̂
+        invert_cov!(estim, estim.P̂arr_old)
+    catch err
+        if err isa PosDefException
+            @warn("Arrival covariance is not positive definite: keeping the old one")
+        else
+            rethrow(err)
+        end
+    end
+    return nothing
+end
+
+"Invert the covariance estimate at arrival `P̄`."
+function invert_cov!(estim::MovingHorizonEstimator, P̄)
+    try
+        estim.invP̄ .= inv(P̄)
+    catch err
+        if err isa SingularException
+            @warn("Arrival covariance is not invertible: keeping the old one")
+        else
+            rethrow(err)
+        end
+    end
     return nothing
 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)