From b608582a11d1a2e03b4bc251140ff7e35a02befe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 12 Jun 2025 14:46:16 -0400 Subject: [PATCH 1/3] added: `inv!` for MHE covariance matrices allocation-free with `LinearAlgebra` standard types (and numerically robust since it relies on a cholesky factorization) there is also a fallback method with other `AbstractMatrix` with some allocations --- src/estimator/mhe/construct.jl | 14 ++++++++++---- src/estimator/mhe/execute.jl | 14 +++++++++++--- src/general.jl | 33 ++++++++++++++++++++++++--------- 3 files changed, 45 insertions(+), 16 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 4958256f0..fe95277aa 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -159,10 +159,16 @@ struct MovingHorizonEstimator{ buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) P̂_0 = Hermitian(P̂_0, :L) Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - P̂_0 = Hermitian(P̂_0, :L) - invP̄ = inv_cholesky!(buffer.P̂, P̂_0) - invQ̂ = inv_cholesky!(buffer.Q̂, Q̂) - invR̂ = inv_cholesky!(buffer.R̂, R̂) + + invP̄ = Hermitian(buffer.P̂, :L) + invP̄ .= P̂_0 + inv!(invP̄) + invQ̂ = Hermitian(buffer.Q̂, :L) + invQ̂ .= Q̂ + inv!(invQ̂) + invR̂ = Hermitian(buffer.R̂, :L) + invR̂ .= R̂ + inv!(invR̂) invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L) invR̂_He = Hermitian(repeatdiag(invR̂, He), :L) x̂0arr_old = zeros(NT, nx̂) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c0d294434..5a02f9f4a 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -528,8 +528,10 @@ end "Invert the covariance estimate at arrival `P̄`." function invert_cov!(estim::MovingHorizonEstimator, P̄) + invP̄ = Hermitian(estim.buffer.P̂, :L) + invP̄ .= P̄ try - estim.invP̄ .= inv_cholesky!(estim.buffer.P̂, P̄) + inv!(invP̄) catch err if err isa PosDefException @error("Arrival covariance P̄ is not invertible: keeping the old one") @@ -792,11 +794,17 @@ function setmodel_estimator!( # --- covariance matrices --- if !isnothing(Q̂) estim.Q̂ .= to_hermitian(Q̂) - estim.invQ̂_He .= repeatdiag(inv(estim.Q̂), He) + invQ̂ = Hermitian(estim.buffer.Q̂, :L) + invQ̂ .= estim.Q̂ + inv!(invQ̂) + estim.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L) end if !isnothing(R̂) estim.R̂ .= to_hermitian(R̂) - estim.invR̂_He .= repeatdiag(inv(estim.R̂), He) + invR̂ = Hermitian(estim.buffer.R̂, :L) + invR̂ .= estim.R̂ + inv!(invR̂) + estim.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L) end return nothing end diff --git a/src/general.jl b/src/general.jl index 94ed3e63f..6567b0db5 100644 --- a/src/general.jl +++ b/src/general.jl @@ -84,15 +84,30 @@ to_hermitian(A::Hermitian) = A to_hermitian(A) = A """ -Compute the inverse of a the Hermitian positive definite matrix `A` using `cholesky`. +Compute the inverse of a the Hermitian positive definite matrix `A` in-place and return it. -Builtin `inv` function uses LU factorization which is not the best choice for Hermitian -positive definite matrices. The function will mutate `buffer` to reduce memory allocations. +There is 3 methods for this function: +- If `A` is a `Hermitian{ for the source. +- If `A` is a `Hermitian{ Date: Thu, 12 Jun 2025 15:15:12 -0400 Subject: [PATCH 2/3] =?UTF-8?q?debug:=20use=20dummy=20`b`=20vector=20in=20?= =?UTF-8?q?MHE=20`setmodel!`=20The=20vector=20may=20containt=20=C2=B1Inf?= =?UTF-8?q?=20value=20for=20the=20MHE=20and=20it=20is=20not=20supported=20?= =?UTF-8?q?by=20JuMP.=20The=20values=20are=20updated=20before=20optimizati?= =?UTF-8?q?on=20anyway.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/mhe/construct.jl | 3 +-- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index fe95277aa..4af87b140 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -159,7 +159,6 @@ struct MovingHorizonEstimator{ buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) P̂_0 = Hermitian(P̂_0, :L) Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - invP̄ = Hermitian(buffer.P̂, :L) invP̄ .= P̂_0 inv!(invP̄) @@ -690,7 +689,7 @@ function setconstraint!( con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max ) A = con.A[con.i_b, :] - b = con.b[con.i_b] + b = zeros(count(con.i_b)) # dummy value, updated before optimization (avoid ±Inf) Z̃var = optim[:Z̃var] JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 5a02f9f4a..ec79a783b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -761,7 +761,7 @@ function setmodel_estimator!( con.A_V̂max ] A = con.A[con.i_b, :] - b = con.b[con.i_b] + b = zeros(count(con.i_b)) # dummy value, updated before optimization (avoid ±Inf) Z̃var::Vector{JuMP.VariableRef} = estim.optim[:Z̃var] JuMP.delete(estim.optim, estim.optim[:linconstraint]) JuMP.unregister(estim.optim, :linconstraint) From 13387b421588ae592e0920225d6dda1878a58c35 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 12 Jun 2025 15:31:21 -0400 Subject: [PATCH 3/3] test: verify `setmodel!` with `He>1` for MHE --- test/2_test_state_estim.jl | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 995d96781..b85d009e7 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1318,7 +1318,8 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) - mhe = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, direct=false) + He = 5 + mhe = MovingHorizonEstimator(linmodel; He, nint_ym=0, direct=false) setconstraint!(mhe, x̂min=[-1000], x̂max=[1000]) @test mhe. ≈ [0.5] @test evaloutput(mhe) ≈ [50.0] @@ -1331,19 +1332,18 @@ end @test mhe. ≈ [0.2] @test evaloutput(mhe) ≈ [55.0] @test mhe.lastu0 ≈ [2.0 - 3.0] - @test mhe.U0 ≈ [2.0 - 3.0] - @test mhe.Y0m ≈ [50.0 - 55.0] - preparestate!(mhe, [55.0]) - x̂ = updatestate!(mhe, [3.0], [55.0]) + @test mhe.U0 ≈ repeat([2.0 - 3.0], He) + @test mhe.Y0m ≈ repeat([50.0 - 55.0], He) + x̂ = preparestate!(mhe, [55.0]) @test x̂ ≈ [3.0] newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0]) setmodel!(mhe, newlinmodel) @test mhe.x̂0 ≈ [3.0 - 8.0] @test mhe.Z̃[1] ≈ 3.0 - 8.0 - @test mhe.X̂0 ≈ [3.0 - 8.0] + @test mhe.X̂0 ≈ repeat([3.0 - 8.0], He) @test mhe.x̂0arr_old ≈ [3.0 - 8.0] - @test mhe.con.X̂0min ≈ [-1000 - 8.0] - @test mhe.con.X̂0max ≈ [+1000 - 8.0] + @test mhe.con.X̂0min ≈ repeat([-1000 - 8.0], He) + @test mhe.con.X̂0max ≈ repeat([+1000 - 8.0], He) @test mhe.con.x̃0min ≈ [-1000 - 8.0] @test mhe.con.x̃0max ≈ [+1000 - 8.0] setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6]) @@ -1352,7 +1352,7 @@ end f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d h(x,d,model) = model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing) - mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0) + mhe2 = MovingHorizonEstimator(nonlinmodel; He, nint_ym=0) setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6]) @test mhe2.Q̂ ≈ [1e-3] @test mhe2.R̂ ≈ [1e-6]