From 51f590c54e461fa963f4bd1000ac849752f457dc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 22 Jan 2025 11:37:36 -0500 Subject: [PATCH 01/16] debug: correct method for `Luenberger` model modification --- src/estimator/luenberger.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 8924af2aa..6000cf073 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -137,7 +137,7 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0) end "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values." -function setmodel!(estim::Luenberger, model, args...) +function setmodel_estimator!(estim::Luenberger, model, args...) if estim.model !== model error("Luenberger does not support setmodel!") end From 1aee9306a3fe7d52bf60c9ed5375abda4a3131bd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 22 Jan 2025 12:46:58 -0500 Subject: [PATCH 02/16] added : fallbacks for `MovingHorizonEstimator` arrival covariance update --- src/estimator/mhe/execute.jl | 52 +++++++++++++++++++++++++++++++----- 1 file changed, 45 insertions(+), 7 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index bc0c4ef38..6ffb16aa9 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) estim.D0[1:estim.model.nd] .= d0 end # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0) - estim.invP̄ .= inv(estim.P̂_0) + invert_cov!(estim, estim.P̂_0) estim.P̂arr_old .= estim.P̂_0 estim.x̂0arr_old .= 0 return nothing @@ -433,9 +433,21 @@ function correct_cov!(estim::MovingHorizonEstimator) y0marr, d0arr = @views estim.Y0m[1:nym], estim.D0[1:nd] estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.P̂ .= estim.P̂arr_old - correct_estimate!(estim.covestim, y0marr, d0arr) - estim.P̂arr_old .= estim.covestim.P̂ - estim.invP̄ .= inv(estim.P̂arr_old) + try + correct_estimate!(estim.covestim, y0marr, d0arr) + catch err + if err isa PosDefException + @warn("Arrival covariance not positive definite: using nearest symmetric one") + estim.covestim.P̂ .= estim.P̂arr_old .+ estim.P̂arr_old' + lmul!(0.5, estim.covestim.P̂) + correct_estimate!(estim.covestim, y0marr, d0arr) + else + rethrow(err) + end + end + P̄ = estim.covestim.P̂ + invert_cov!(estim, P̄) + estim.P̂arr_old .= P̄ return nothing end @@ -445,9 +457,35 @@ function update_cov!(estim::MovingHorizonEstimator) u0arr, y0marr, d0arr = @views estim.U0[1:nu], estim.Y0m[1:nym], estim.D0[1:nd] estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.P̂ .= estim.P̂arr_old - update_estimate!(estim.covestim, y0marr, d0arr, u0arr) - estim.P̂arr_old .= estim.covestim.P̂ - estim.invP̄ .= inv(estim.P̂arr_old) + try + update_estimate!(estim.covestim, y0marr, d0arr, u0arr) + catch err + if err isa PosDefException + @warn("Arrival covariance not positive definite: using nearest symmetric one") + estim.covestim.P̂ .= estim.P̂arr_old .+ estim.P̂arr_old' + lmul!(0.5, estim.covestim.P̂) + update_estimate!(estim.covestim, y0marr, d0arr, u0arr) + else + rethrow(err) + end + end + P̄ = estim.covestim.P̂ + invert_cov!(estim, P̄) + estim.P̂arr_old .= P̄ + return nothing +end + +"Invert the covariance estimate at arrival `P̄`." +function invert_cov!(estim::MovingHorizonEstimator, P̄) + try + estim.invP̄ .= inv(P̄) + catch err + if err isa SingularException + @warn("Arrival covariance not invertible: using pseudo-inverse") + estim.invP̄ .= pinv(P̄) + else + rethrow(err) + end return nothing end From 8383d0712b5c0847b54564fecbeeea48d705b6c6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 22 Jan 2025 13:25:06 -0500 Subject: [PATCH 03/16] changed: small `eps()` term instead of `pinv` for MHE inversion --- src/estimator/mhe/execute.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 6ffb16aa9..b6778e72d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -481,8 +481,8 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) estim.invP̄ .= inv(P̄) catch err if err isa SingularException - @warn("Arrival covariance not invertible: using pseudo-inverse") - estim.invP̄ .= pinv(P̄) + @warn("Arrival covariance is singular: adding small regularization term") + estim.invP̄ .= inv(P̄ + eps()*I) else rethrow(err) end From 0e60f548ac9bf97e5658ab9e14eb980baf75ab8b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 22 Jan 2025 13:36:45 -0500 Subject: [PATCH 04/16] debug: forgot `end` keyword --- src/estimator/mhe/execute.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index b6778e72d..45fcb072e 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -486,6 +486,7 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) else rethrow(err) end + end return nothing end From 532223f04db41b11bd285b512d80c7caa8503546 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 22 Jan 2025 18:41:01 -0500 Subject: [PATCH 05/16] added: testing the `MovingHorizonEstimator` fallbacks --- Project.toml | 2 +- src/estimator/mhe/execute.jl | 25 ++++++++----------------- test/test_state_estim.jl | 29 +++++++++++++++++++++++++++++ 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/Project.toml b/Project.toml index cd78878f3..d5cd7aba7 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.2.0" +version = "1.2.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 45fcb072e..9473e4caa 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -435,19 +435,15 @@ function correct_cov!(estim::MovingHorizonEstimator) estim.covestim.P̂ .= estim.P̂arr_old try correct_estimate!(estim.covestim, y0marr, d0arr) + estim.P̂arr_old .= estim.covestim.P̂ + invert_cov!(estim, estim.covestim.P̂) catch err if err isa PosDefException - @warn("Arrival covariance not positive definite: using nearest symmetric one") - estim.covestim.P̂ .= estim.P̂arr_old .+ estim.P̂arr_old' - lmul!(0.5, estim.covestim.P̂) - correct_estimate!(estim.covestim, y0marr, d0arr) + @warn("Arrival covariance is not positive definite: keeping the old one") else rethrow(err) end end - P̄ = estim.covestim.P̂ - invert_cov!(estim, P̄) - estim.P̂arr_old .= P̄ return nothing end @@ -458,20 +454,16 @@ function update_cov!(estim::MovingHorizonEstimator) estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.P̂ .= estim.P̂arr_old try - update_estimate!(estim.covestim, y0marr, d0arr, u0arr) + correct_estimate!(estim.covestim, y0marr, d0arr) + estim.P̂arr_old .= estim.covestim.P̂ + invert_cov!(estim, estim.covestim.P̂) catch err if err isa PosDefException - @warn("Arrival covariance not positive definite: using nearest symmetric one") - estim.covestim.P̂ .= estim.P̂arr_old .+ estim.P̂arr_old' - lmul!(0.5, estim.covestim.P̂) - update_estimate!(estim.covestim, y0marr, d0arr, u0arr) + @warn("Arrival covariance is not positive definite: keeping the old one") else rethrow(err) end end - P̄ = estim.covestim.P̂ - invert_cov!(estim, P̄) - estim.P̂arr_old .= P̄ return nothing end @@ -481,8 +473,7 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) estim.invP̄ .= inv(P̄) catch err if err isa SingularException - @warn("Arrival covariance is singular: adding small regularization term") - estim.invP̄ .= inv(P̄ + eps()*I) + @warn("Arrival covariance is not invertible: keeping the old one") else rethrow(err) end diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 9eec37db7..c3b168351 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -975,6 +975,35 @@ end @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 end +@testset "MovingHorizonEstimator fallbacks for arrival covariance estimation" begin + linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) + f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h(x,d,_) = linmodel.C*x + linmodel.Dd*d + nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5]) + mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1) + preparestate!(mhe, [50, 30], [5]) + updatestate!(mhe, [10, 50], [50, 30], [5]) + mhe.P̂arr_old[1, 1] = -1e-3 # negative eigenvalue to trigger fallback + P̂arr_old_copy = deepcopy(mhe.P̂arr_old) + invP̄_copy = deepcopy(mhe.invP̄) + @test_logs( + (:warn, "Arrival covariance is not positive definite: 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( + (:warn, "Arrival covariance is not positive definite: 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 + @test_logs( + (:warn, "Arrival covariance is not invertible: keeping the old one"), + ModelPredictiveControl.invert_cov!(mhe, zeros(mhe.nx̂, mhe.nx̂)) + ) +end + @testset "MovingHorizonEstimator set constraints" begin linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe1 = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0, Cwt=1e3) From ccfafd09b104cfd05921913762167ba1ac8ae440 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 22 Jan 2025 21:10:07 -0500 Subject: [PATCH 06/16] debug: call the good method in `update_cov!` --- src/estimator/mhe/execute.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 9473e4caa..eaf255a5d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -436,7 +436,7 @@ function correct_cov!(estim::MovingHorizonEstimator) try correct_estimate!(estim.covestim, y0marr, d0arr) estim.P̂arr_old .= estim.covestim.P̂ - invert_cov!(estim, estim.covestim.P̂) + invert_cov!(estim, estim.P̂arr_old) catch err if err isa PosDefException @warn("Arrival covariance is not positive definite: keeping the old one") @@ -454,9 +454,9 @@ function update_cov!(estim::MovingHorizonEstimator) estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.P̂ .= estim.P̂arr_old try - correct_estimate!(estim.covestim, y0marr, d0arr) + update_estimate!(estim.covestim, y0marr, d0arr, u0arr) estim.P̂arr_old .= estim.covestim.P̂ - invert_cov!(estim, estim.covestim.P̂) + invert_cov!(estim, estim.P̂arr_old) catch err if err isa PosDefException @warn("Arrival covariance is not positive definite: keeping the old one") From b49190fc5b4f587e30fb8c31c1139b5cfde426b0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 10:18:08 -0500 Subject: [PATCH 07/16] debug: Julia 1.10 and `rethrow` correct syntax --- src/estimator/mhe/execute.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index eaf255a5d..0505a8b56 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -441,7 +441,7 @@ function correct_cov!(estim::MovingHorizonEstimator) if err isa PosDefException @warn("Arrival covariance is not positive definite: keeping the old one") else - rethrow(err) + rethrow() end end return nothing @@ -461,7 +461,7 @@ function update_cov!(estim::MovingHorizonEstimator) if err isa PosDefException @warn("Arrival covariance is not positive definite: keeping the old one") else - rethrow(err) + rethrow() end end return nothing @@ -472,7 +472,7 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) try estim.invP̄ .= inv(P̄) catch err - if err isa SingularException + if err isa SingularException || err isa LAPACKException @warn("Arrival covariance is not invertible: keeping the old one") else rethrow(err) From 98f921449c927185755955e57b873a62a7579b8b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 10:38:34 -0500 Subject: [PATCH 08/16] idem --- src/estimator/mhe/execute.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 0505a8b56..5723da485 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -475,7 +475,7 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) if err isa SingularException || err isa LAPACKException @warn("Arrival covariance is not invertible: keeping the old one") else - rethrow(err) + rethrow() end end return nothing From eeccd9d1d211e088c553ad70c7f3ffd07c60d4c6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 14:45:40 -0500 Subject: [PATCH 09/16] changed: `getinfo` dictionnary value type is now `Any` the `Union` type was overly complex and `getinfo` should not be used in "normal" operation, only for troubleshoothing. The computational performance are not that important (there is already many allocations in this function) --- src/controller/execute.jl | 2 +- src/estimator/mhe/execute.jl | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 29e6c5a53..07119919f 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -114,7 +114,7 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) function getinfo(mpc::PredictiveController{NT}) where NT<:Real model = mpc.estim.model nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu - info = Dict{Symbol, Union{JuMP._SolutionSummary, Vector{NT}, NT}}() + info = Dict{Symbol, Any}() Ŷ0, u0, û0 = similar(mpc.Yop), similar(model.uop), similar(model.uop) Ŷs = similar(mpc.Yop) x̂0, x̂0next = similar(mpc.estim.x̂0), similar(mpc.estim.x̂0) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index bc0c4ef38..24337bcb5 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -108,8 +108,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nu, ny, nd = model.nu, model.ny, model.nd nx̂, nym, nŵ, nϵ = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ nx̃ = nϵ + nx̂ - MyTypes = Union{JuMP._SolutionSummary, Hermitian{NT, Matrix{NT}}, Vector{NT}, NT} - info = Dict{Symbol, MyTypes}() + info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) û0, ŷ0 = similar(model.uop), similar(model.yop) V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, estim.Z̃) From 3721d8cf87d0b882cb91ebedc226cabbdceb87bc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 14:50:16 -0500 Subject: [PATCH 10/16] added: call `getinfo` when debug logging is activated and not solved --- src/controller/execute.jl | 2 +- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 07119919f..20a464e1b 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -524,7 +524,7 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} @warn("MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* "solution anyway", status) end - @debug JuMP.solution_summary(optim, verbose=true) + @debug getinfo(mpc) end if iserror(optim) mpc.ΔŨ .= ΔŨ0 diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 24337bcb5..3f5cab10b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -411,7 +411,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* "solution anyway", status) end - @debug JuMP.solution_summary(optim, verbose=true) + @debug getinfo(estim) end if iserror(optim) estim.Z̃ .= Z̃_0 From d0ae483efccc80a58296c04d2c87bdb47f605825 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 14:59:34 -0500 Subject: [PATCH 11/16] doc: mention the debug log dictionary in the docstrings --- src/controller/execute.jl | 3 ++- src/estimator/mhe/execute.jl | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 20a464e1b..fb22491cc 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -491,7 +491,8 @@ If supported by `mpc.optim`, it warm-starts the solver at: where ``\mathbf{Δu}_{k-1}(k+j)`` is the input increment for time ``k+j`` computed at the last control period ``k-1``. It then calls `JuMP.optimize!(mpc.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the -warm-start value. +warm-start value. A failed optimization also prints [`getinfo`](@ref) results in +the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). """ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} model, optim = mpc.estim.model, mpc.optim diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 3f5cab10b..c37b7f414 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -369,7 +369,8 @@ If supported by `estim.optim`, it warm-starts the solver at: where ``\mathbf{ŵ}_{k-1}(k-j)`` is the input increment for time ``k-j`` computed at the last time step ``k-1``. It then calls `JuMP.optimize!(estim.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the -warm-start value. +warm-start value. A failed optimization also prints [`getinfo`](@ref) results in +the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). """ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real model, optim = estim.model, estim.optim From 07391515ccc98707ff4787b6a54c43d82a544206 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 15:21:10 -0500 Subject: [PATCH 12/16] changed: better formatting for debug log The full output without ellipsis can still be shown in the REPL with the following logger: `debuglogger = ConsoleLogger(stderr, Logging.Debug, show_limited=false)` --- src/controller/execute.jl | 2 +- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index fb22491cc..a6531ddde 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -525,7 +525,7 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} @warn("MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* "solution anyway", status) end - @debug getinfo(mpc) + @debug("The function getinfo returns: ", getinfo(mpc)) end if iserror(optim) mpc.ΔŨ .= ΔŨ0 diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c37b7f414..0d96d5d26 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -412,7 +412,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* "solution anyway", status) end - @debug getinfo(estim) + @debug("The function getinfo returns: ", getinfo(estim)) end if iserror(optim) estim.Z̃ .= Z̃_0 From 22cc35a3926f270c3908699c1c32ef8362d6db77 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 15:46:48 -0500 Subject: [PATCH 13/16] changed: suggesting `show_limited=false` in debug message --- src/controller/execute.jl | 19 ++++++++++++++----- src/estimator/mhe/execute.jl | 19 ++++++++++++++----- 2 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index a6531ddde..3a4ef1d05 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -519,13 +519,22 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} if !issolved(optim) status = JuMP.termination_status(optim) if iserror(optim) - @error("MPC terminated without solution: returning last solution shifted", - status) + @error( + "MPC terminated without solution: estimation in open-loop "* + "(more info in debug log)", + status + ) else - @warn("MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* - "solution anyway", status) + @warn( + "MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "* + "anyway (more info in debug log)", + status + ) end - @debug("The function getinfo returns: ", getinfo(mpc)) + @debug( + "calling getinfo (use logger with show_limited=false if values are truncated)", + getinfo(estim) + ) end if iserror(optim) mpc.ΔŨ .= ΔŨ0 diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 0d96d5d26..d19b962f7 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -406,13 +406,22 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real if !issolved(optim) status = JuMP.termination_status(optim) if iserror(optim) - @error("MHE terminated without solution: estimation in open-loop", - status) + @error( + "MHE terminated without solution: estimation in open-loop "* + "(more info in debug log)", + status + ) else - @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* - "solution anyway", status) + @warn( + "MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "* + "anyway (more info in debug log)", + status + ) end - @debug("The function getinfo returns: ", getinfo(estim)) + @debug( + "calling getinfo (use logger with show_limited=false if values are truncated)", + getinfo(estim) + ) end if iserror(optim) estim.Z̃ .= Z̃_0 From 371a150173c6debfeb814f4355c8b9e29d4625f3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 15:56:46 -0500 Subject: [PATCH 14/16] debug: the debug message (debug-ception XD) --- src/controller/execute.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 3a4ef1d05..51a366029 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -533,7 +533,7 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} end @debug( "calling getinfo (use logger with show_limited=false if values are truncated)", - getinfo(estim) + getinfo(mpc) ) end if iserror(optim) From 51737c42ca2bb160e42ec2baa7fc9b6ca7c859b7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 16:24:04 -0500 Subject: [PATCH 15/16] changed: computing `inv` with `cholesky` in `MovingHorizonEstimator` --- Project.toml | 2 +- src/estimator/mhe/construct.jl | 15 +++++++++------ src/estimator/mhe/execute.jl | 4 ++-- src/general.jl | 16 +++++++++++++++- 4 files changed, 27 insertions(+), 10 deletions(-) diff --git a/Project.toml b/Project.toml index d5cd7aba7..c23223b59 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.2.1" +version = "1.3.0" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index ca86c59ba..68c5bf271 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -123,11 +123,6 @@ struct MovingHorizonEstimator{ validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - P̂_0 = Hermitian(P̂_0, :L) - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - invP̄ = Hermitian(inv(P̂_0), :L) - invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L) - invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) r = direct ? 0 : 1 E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, r @@ -146,10 +141,18 @@ struct MovingHorizonEstimator{ nD0 = direct ? nd*(He+1) : nd*He U0, D0 = zeros(NT, nu*He), zeros(NT, nD0) Ŵ = zeros(NT, nx̂*He) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + 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̂) + invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L) + invR̂_He = Hermitian(repeatdiag(invR̂, He), :L) x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(P̂_0) Nk = [0] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) corrected = [false] estim = new{NT, SM, JM, CE}( model, optim, con, covestim, diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index bca0ba5c9..5dcb89692 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -479,9 +479,9 @@ end "Invert the covariance estimate at arrival `P̄`." function invert_cov!(estim::MovingHorizonEstimator, P̄) try - estim.invP̄ .= inv(P̄) + estim.invP̄ .= inv_cholesky!(estim.buffer.P̂, P̄) catch err - if err isa SingularException || err isa LAPACKException + if err isa PosDefException @warn("Arrival covariance is not invertible: keeping the old one") else rethrow() diff --git a/src/general.jl b/src/general.jl index 40f5e6264..1e512b62d 100644 --- a/src/general.jl +++ b/src/general.jl @@ -59,4 +59,18 @@ end to_hermitian(A::AbstractVector) = Hermitian(reshape(A, 1, 1), :L) to_hermitian(A::AbstractMatrix) = Hermitian(A, :L) to_hermitian(A::Hermitian) = A -to_hermitian(A) = A \ No newline at end of file +to_hermitian(A) = A + +""" +Compute the inverse of a the Hermitian positive definite matrix `A` using `cholesky`. + +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. +""" +function inv_cholesky!(buffer::Matrix, A::Hermitian) + Achol = Hermitian(buffer, :L) + Achol .= A + chol_obj = cholesky!(Achol) + invA = Hermitian(inv(chol_obj), :L) + return invA +end \ No newline at end of file From c2bb87552e8c269aba3d74517ad9ce4ece133b39 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 23 Jan 2025 16:33:47 -0500 Subject: [PATCH 16/16] test: fallback inversion tests with hermitian --- test/test_state_estim.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index c3b168351..6a4ce0166 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -1000,7 +1000,7 @@ end @test mhe.invP̄ ≈ invP̄_copy @test_logs( (:warn, "Arrival covariance is not invertible: keeping the old one"), - ModelPredictiveControl.invert_cov!(mhe, zeros(mhe.nx̂, mhe.nx̂)) + ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L)) ) end