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