From 071256289302e759683f74dcb0f97f84ba4f0b79 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 11:16:11 -0400 Subject: [PATCH 1/7] refactor: update arrival covariance handling in MovingHorizonEstimator --- check_syntax.jl | 2 ++ src/estimator/mhe/execute.jl | 17 +++++++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) create mode 100644 check_syntax.jl diff --git a/check_syntax.jl b/check_syntax.jl new file mode 100644 index 000000000..fcb2c74ce --- /dev/null +++ b/check_syntax.jl @@ -0,0 +1,2 @@ +import JuliaFormatter +include("src/estimator/mhe/execute.jl") diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c5f8a515d..670fdb953 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -709,7 +709,7 @@ function correct_cov!(estim::MovingHorizonEstimator) correct_estimate!(estim.covestim, y0marr, d0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ - invert_cov!(estim, estim.P̂arr_old) + update_arrival_cov!(estim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -736,7 +736,7 @@ function update_cov!(estim::MovingHorizonEstimator) update_estimate!(estim.covestim, y0marr, d0arr, u0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ - invert_cov!(estim, estim.P̂arr_old) + update_arrival_cov!(estim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -764,6 +764,19 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) return nothing end +"Update the arrival covariance matrix at the next time step based on the covariance estimator type." +function update_arrival_cov!(estim::MovingHorizonEstimator) + _update_arrival_cov!(estim, estim.covestim) +end + +function _update_arrival_cov!(estim::MovingHorizonEstimator, ::StateEstimator) + invert_cov!(estim, estim.P̂arr_old) +end + +function _update_arrival_cov!(estim::MovingHorizonEstimator, ::SteadyKalmanFilter) + return nothing +end + """ obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) From da92f763982cfa9be40efee05040f9b8f4d1b6a5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 11:23:20 -0400 Subject: [PATCH 2/7] removed: temp file --- check_syntax.jl | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 check_syntax.jl diff --git a/check_syntax.jl b/check_syntax.jl deleted file mode 100644 index fcb2c74ce..000000000 --- a/check_syntax.jl +++ /dev/null @@ -1,2 +0,0 @@ -import JuliaFormatter -include("src/estimator/mhe/execute.jl") From 27f9fc3f22a96252919963b42cca9499348d6c83 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 11:52:15 -0400 Subject: [PATCH 3/7] change: cleanup --- src/estimator/mhe/execute.jl | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 670fdb953..2ec8ccb84 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -764,18 +764,14 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) return nothing end -"Update the arrival covariance matrix at the next time step based on the covariance estimator type." -function update_arrival_cov!(estim::MovingHorizonEstimator) - _update_arrival_cov!(estim, estim.covestim) -end - -function _update_arrival_cov!(estim::MovingHorizonEstimator, ::StateEstimator) +"Invert the arrival covariance matrix and store the result in `estim.invP̄`." +function invert_arrival_cov!(estim::MovingHorizonEstimator, ::StateEstimator) invert_cov!(estim, estim.P̂arr_old) -end - -function _update_arrival_cov!(estim::MovingHorizonEstimator, ::SteadyKalmanFilter) return nothing end +"Do nothing if `covestim` is a [`SteadyKalmanFilter`](@ref)." +update_arrival_cov!(estim::MovingHorizonEstimator, ::SteadyKalmanFilter) = nothing + """ obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) From 4017d5bb8fa61198189ee01c78cae6886120e6f6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 12:03:04 -0400 Subject: [PATCH 4/7] change: cleanup --- src/estimator/mhe/execute.jl | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 2ec8ccb84..d8625e8c4 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -30,8 +30,8 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) nd > 0 && (estim.D0[1:nd] .= d0) # add d0(-1) to the data window estim.lastu0 .= u0 # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0) - invert_cov!(estim, estim.cov.P̂_0) estim.P̂arr_old .= estim.cov.P̂_0 + invert_cov!(estim, estim.covestim) estim.x̂0arr_old .= 0 return nothing end @@ -709,7 +709,7 @@ function correct_cov!(estim::MovingHorizonEstimator) correct_estimate!(estim.covestim, y0marr, d0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ - update_arrival_cov!(estim) + invert_cov!(estim, estim.covestim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -736,7 +736,7 @@ function update_cov!(estim::MovingHorizonEstimator) update_estimate!(estim.covestim, y0marr, d0arr, u0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ - update_arrival_cov!(estim) + invert_cov!(estim, estim.covestim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -749,8 +749,9 @@ function update_cov!(estim::MovingHorizonEstimator) return nothing end -"Invert the covariance estimate at arrival `P̄`." -function invert_cov!(estim::MovingHorizonEstimator, P̄) +"Invert the covariance estimate at arrival `P̄` and store it in `estim.cov.invP̄`." +function invert_cov!(estim::MovingHorizonEstimator, covestim::StateEstimator) + P̄ = estim.P̂arr_old estim.cov.invP̄ .= P̄ try inv!(estim.cov.invP̄) @@ -763,14 +764,8 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) end return nothing end - -"Invert the arrival covariance matrix and store the result in `estim.invP̄`." -function invert_arrival_cov!(estim::MovingHorizonEstimator, ::StateEstimator) - invert_cov!(estim, estim.P̂arr_old) - return nothing -end -"Do nothing if `covestim` is a [`SteadyKalmanFilter`](@ref)." -update_arrival_cov!(estim::MovingHorizonEstimator, ::SteadyKalmanFilter) = nothing +"Do nothing if `covestim` is a [`SteadyKalmanFilter`]." +invert_cov!(::MovingHorizonEstimator, ::SteadyKalmanFilter) = nothing """ From 34daacea83157fd1be032e9b24ff2608c3d4c2e7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 12:40:04 -0400 Subject: [PATCH 5/7] test: new test with `covestim=SteadyKalmanFilter` --- test/2_test_state_estim.jl | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 597e4ff79..261c8f9bc 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1011,6 +1011,17 @@ end info = getinfo(mhe3) @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + covestim = SteadyKalmanFilter(linmodel) + mhe3b = MovingHorizonEstimator(linmodel; He=5, σP_0=nothing, covestim) + preparestate!(mhe3b, [50, 30], [5]) + x̂ = updatestate!(mhe3b, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 + @test mhe3b.x̂0 ≈ zeros(6) atol=1e-9 + @test mhe3b.cov.invP̄ ≈ inv(covestim.cov.P̂) + preparestate!(mhe3b, [50, 30], [5]) + info = getinfo(mhe3b) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mhe3 = MovingHorizonEstimator(linmodel3, He=1) From 2c68523b1f34e19448f7a95333a529007c4ff1d7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 15:20:31 -0400 Subject: [PATCH 6/7] test: debug MHE arrival covariance fallback test --- test/2_test_state_estim.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 261c8f9bc..de364c01e 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1192,7 +1192,7 @@ end @test mhe.cov.invP̄ ≈ invP̄_copy @test_logs( (:error, "Arrival covariance P̄ is not invertible: keeping the old one"), - ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L)) + ModelPredictiveControl.invert_cov!(mhe, mhe.covestim) ) mhe.P̂arr_old[1, 1] = Inf # Inf to trigger fallback P̂arr_old_copy = deepcopy(mhe.P̂arr_old) From 54b3bd514f160493961342db0d02d94ab0d5390d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 16:27:45 -0400 Subject: [PATCH 7/7] test: ensure that windows are full --- test/2_test_state_estim.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index de364c01e..8b37902db 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1012,7 +1012,7 @@ end @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 covestim = SteadyKalmanFilter(linmodel) - mhe3b = MovingHorizonEstimator(linmodel; He=5, σP_0=nothing, covestim) + mhe3b = MovingHorizonEstimator(linmodel; He=1, σP_0=nothing, covestim) preparestate!(mhe3b, [50, 30], [5]) x̂ = updatestate!(mhe3b, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9