From c693b0fdaacb1ee0a81c75947da567ec1f0525fc Mon Sep 17 00:00:00 2001
From: franckgaga <franckgaga2@gmail.com>
Date: Thu, 6 Feb 2025 15:52:31 -0500
Subject: [PATCH] debug: fallback if nonfinite arr. cov. after
 correction/update

The fallback is still relevant if new `NaN` or `Inf` values appear in the covariance arrival.

A different message is print tho to help troubleshooting.

As mentioned by @baggepinnen, it may be caused by a different issue, like a sudden NaN values in the measurements `ym`.
---
 src/estimator/mhe/execute.jl |  6 ++++++
 test/test_state_estim.jl     | 15 +++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
index 84368b09..0511eba8 100644
--- a/src/estimator/mhe/execute.jl
+++ b/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
diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl
index c7c3a2e9..d130e450 100644
--- a/test/test_state_estim.jl
+++ b/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