From 071256289302e759683f74dcb0f97f84ba4f0b79 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 11:16:11 -0400 Subject: [PATCH 1/2] 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 c2dd801d45056a4d709a6f7348c83820ef08fc14 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 13 Jun 2026 12:45:38 -0400 Subject: [PATCH 2/2] bench: MHE test with `covestim=SteadyKalmanFilter` --- benchmark/2_bench_state_estim.jl | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/benchmark/2_bench_state_estim.jl b/benchmark/2_bench_state_estim.jl index 982aeeaa2..c53c0e444 100644 --- a/benchmark/2_bench_state_estim.jl +++ b/benchmark/2_bench_state_estim.jl @@ -146,6 +146,7 @@ UNIT_ESTIM["ExtendedKalmanFilter"]["evaloutput"]["NonLinModel"] = mhe_lin_curr = MovingHorizonEstimator(linmodel, He=10, direct=true) mhe_lin_pred = MovingHorizonEstimator(linmodel, He=10, direct=false) +mhe_lin_skf = MovingHorizonEstimator(linmodel, He=10, covestim=SteadyKalmanFilter(linmodel)) mhe_nonlin_curr = MovingHorizonEstimator(nonlinmodel, He=10, direct=true) mhe_nonlin_pred = MovingHorizonEstimator(nonlinmodel, He=10, direct=false) @@ -161,6 +162,12 @@ UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["LinModel"]["Current form"] setup=preparestate!($mhe_lin_curr, $y, $d), samples=samples, evals=evals, seconds=seconds, ) +UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["LinModel"]["Constant arr. cov."] = + @benchmarkable( + updatestate!($mhe_lin_skf, $u, $y, $d), + setup=preparestate!($mhe_lin_skf, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) UNIT_ESTIM["MovingHorizonEstimator"]["preparestate!"]["LinModel"]["Prediction form"] = @benchmarkable( preparestate!($mhe_lin_pred, $y, $d),