Merge pull request #156 from JuliaControl/debug_mhe_fallback

debug: fallback if nonfinite arr. cov. after correction/update

Author: franckgaga

src/estimator/mhe/execute.jl

@@ -441,11 +441,14 @@ function correct_cov!(estim::MovingHorizonEstimator)
     estim.covestim.P̂  .= estim.P̂arr_old
     try
         correct_estimate!(estim.covestim, y0marr, d0arr)
+        all(isfinite, estim.covestim.P̂) || error("Arrival covariance P̄ is not finite")
         estim.P̂arr_old .= estim.covestim.P̂
         invert_cov!(estim, estim.P̂arr_old)
     catch err
         if err isa PosDefException
             @error("Arrival covariance P̄ is not positive definite: keeping the old one")
+        elseif err isa ErrorException
+            @error("Arrival covariance P̄ is not finite: keeping the old one")
         else
             rethrow()
         end
@@ -461,11 +464,14 @@ function update_cov!(estim::MovingHorizonEstimator)
     estim.covestim.P̂  .= estim.P̂arr_old
     try
         update_estimate!(estim.covestim, y0marr, d0arr, u0arr)
+        all(isfinite, estim.covestim.P̂) || error("Arrival covariance P̄ is not finite")
         estim.P̂arr_old .= estim.covestim.P̂
         invert_cov!(estim, estim.P̂arr_old)
     catch err
         if err isa PosDefException
             @error("Arrival covariance P̄ is not positive definite: keeping the old one")
+        elseif err isa ErrorException
+            @error("Arrival covariance P̄ is not finite: keeping the old one")
         else
             rethrow()
         end

test/test_state_estim.jl

@@ -1003,6 +1003,21 @@ end
         (:error, "Arrival covariance P̄ is not invertible: keeping the old one"), 
         ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L))
     )
+    mhe.P̂arr_old[1, 1] = Inf # Inf to trigger fallback
+    P̂arr_old_copy = deepcopy(mhe.P̂arr_old)
+    invP̄_copy = deepcopy(mhe.invP̄)
+    @test_logs(
+        (:error, "Arrival covariance P̄ is not finite: 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(
+        (:error, "Arrival covariance P̄ is not finite: 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
 end
 
 @testset "MovingHorizonEstimator set constraints" begin