From 18ac0ef0d0fca617b1ac686efa298b59eedd1788 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 24 Sep 2026 10:01:46 -0400 Subject: [PATCH 1/2] debug: correctly warm-start MHE with `OrthogonalCollocation` --- src/estimator/mhe/transcription.jl | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 3416f0fbc..91966b4f1 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1083,9 +1083,12 @@ function reset_warmstart!(estim::MovingHorizonEstimator, ::TrapezoidalCollocatio end return nothing end -function reset_warmstart!(estim::MovingHorizonEstimator, ::OrthogonalCollocation) +function reset_warmstart!(estim::MovingHorizonEstimator, transcription::OrthogonalCollocation) model = estim.model - nx, nx̂, nx̃, nε, He, na = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He, get_na(model) + no = transcription.no + nx, nx̂, nx̃, nε, He = model.nx, estim.nx̂, estim.nx̃, estim.nε, estim.He + na = get_na(model) + nā, nk̄ = get_nā(model, transcription), get_nk̄(model, transcription) as_0 = get_as_0(model) x0s = model.buffer.x x0s .= model.xs_0 .- model.xop @@ -1096,9 +1099,11 @@ function reset_warmstart!(estim::MovingHorizonEstimator, ::OrthogonalCollocation for j in 1:He estim.Z̃[(nx̃+(j-1)*nx̂+1):(nx̃+(j-1)*nx̂+nx)] .= x0s estim.Z̃[(nx̃+nx̂*He+na+(j-1)*na+1):(nx̃+nx̂*He+na+j*na)] .= a0s - estim.Z̃[(nx̃+nx̂*He+na+na*He+(j-1)*nx+1):(nx̃+nx̂*He+na+na*He+j*nx)] .= x0s - estim.Z̃[(nx̃+nx̂*He+na+na*He+nx*He+(j-1)*na+1):(nx̃+nx̂*He+na+na*He+nx*He+j*na)] .= a0s end + K̄ = @views estim.Z̃[(nx̃+nx̂*He+na+na*He+1):(nx̃+nx̂*He+na+na*He+nk̄*He)] + repeat!(K̄, x0s, no*He) + Ā = @views estim.Z̃[(nx̃+nx̂*He+na+na*He+nk̄*He+1):(nx̃+nx̂*He+na+na*He+nk̄*He+nā*He)] + repeat!(Ā, a0s, no*He) return nothing end function reset_warmstart!(estim::MovingHorizonEstimator, ::SingleShooting) From 2af314995fcfa5d014f86608fcc6b087da133c4f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 24 Sep 2026 10:32:43 -0400 Subject: [PATCH 2/2] test: new test related to the last bugfix --- test/2_test_state_estim.jl | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 03363df08..26d288268 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1524,8 +1524,10 @@ end @test mhe2.Z̃[2:2] ≈ mhe2.Z̃[4:4] ≈ mhe2.Z̃[6:6] ≈ xs_0 # xd in X̂0 @test mhe2.Z̃[3:3] ≈ mhe2.Z̃[5:5] ≈ mhe2.Z̃[7:7] ≈ [0.0] # xs in X̂0 @test mhe2.Z̃[8:8] ≈ mhe2.Z̃[9:9] ≈ mhe2.Z̃[10:10] ≈ as_0 # â0 in Â0 - @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ xs_0 # xd in K̄ - @test mhe2.Z̃[13:13] ≈ mhe2.Z̃[14:14] ≈ as_0 # a in Ā + @test mhe2.Z̃[11:11] ≈ mhe2.Z̃[12:12] ≈ xs_0 # xd in 1st k̄ + @test mhe2.Z̃[13:13] ≈ mhe2.Z̃[14:14] ≈ xs_0 # xd in 2nd k̄ + @test mhe2.Z̃[15:15] ≈ mhe2.Z̃[16:16] ≈ as_0 # a in 1st ā + @test mhe2.Z̃[17:17] ≈ mhe2.Z̃[18:18] ≈ as_0 # a in 2nd ā end @testitem "MHE estim. with unfilled window" setup=[SetupMPCtests] begin