From 9de2b15b29c93a5070ff67e7106163014829f729 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 21 May 2025 20:16:37 -0400 Subject: [PATCH 1/7] changed : minor correction in `InternalModel` notation --- src/estimator/internal_model.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 6619e48df..e5ab37ef6 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -168,15 +168,15 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real end @doc raw""" - f̂!(x̂0next, _ , x̂0i, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) + f̂!(x̂0next, _ , k0, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref). -It calls `model.solver_f!(x̂0next, x̂0i, x̂0, u0 ,d0, model.p)` directly since this estimator +It calls `model.solver_f!(x̂0next, k0, x̂0, u0 ,d0, model.p)` directly since this estimator does not augment the states. """ -function f̂!(x̂0next, _ , x̂0i, ::InternalModel, model::NonLinModel, x̂0, u0, d0) - return model.solver_f!(x̂0next, x̂0i, x̂0, u0, d0, model.p) +function f̂!(x̂0next, _ , k0, ::InternalModel, model::NonLinModel, x̂0, u0, d0) + return model.solver_f!(x̂0next, k0, x̂0, u0, d0, model.p) end @doc raw""" From 24a5afce5008eba4d0bc9c7ab4892d422f313551 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 21 May 2025 20:17:00 -0400 Subject: [PATCH 2/7] wip: debug MHE with `nint_u` option --- src/estimator/mhe/construct.jl | 1 + src/estimator/mhe/execute.jl | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 4c6ebb71c..2468012df 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1364,6 +1364,7 @@ function get_optim_functions( end function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + @show J return J[]::T end ∇Jfunc! = function (∇Jarg::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 70b5ca15a..99b146ba5 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -398,6 +398,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real isfinite(J_0) || (Z̃s = [ϵ_0; estim.x̂0arr_old; zeros(NT, nŵ*estim.He)]) JuMP.set_start_value.(Z̃var, Z̃s) # ------- solve optimization problem -------------- + println("optimizing!") try JuMP.optimize!(optim) catch err @@ -434,9 +435,11 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real end # --------- update estimate ----------------------- estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start + println("solved, needs to call predict! one more time.") V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next + @show estim.Ŵ[1:nŵ*Nk] return estim.Z̃ end @@ -597,6 +600,10 @@ function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, mode x̂0 = x̂0next end end + if eltype(X̂0) == Float64 + @show X̂0 + @show V̂ + end return V̂, X̂0 end From 0b11288568e122a982f89f8cfd82f055067f5a8e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 22 May 2025 10:03:59 -0400 Subject: [PATCH 3/7] debug: correct cache handling in `MovingHorizonEstimator` and `NonLinMPC` --- src/controller/nonlinmpc.jl | 22 +++++++++++----------- src/estimator/mhe/construct.jl | 15 +++++++-------- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 94aba9b4e..4f95a8083 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -583,9 +583,9 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ) ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) - function update_objective!(J, ∇J, Z̃, Z̃arg) - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg + function update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + if isdifferent(Z̃arg, Z̃_∇J) + Z̃_∇J .= Z̃arg J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) end end @@ -620,10 +620,10 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇g_prep = prepare_jacobian(gfunc!, g, jac, Z̃_∇g, ∇g_context...; strict) mpc.con.i_g[1:end-nc] .= false ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng) - function update_con!(g, ∇g, Z̃, Z̃arg) - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃, ∇g_context...) + function update_con!(g, ∇g, Z̃_∇g, Z̃arg) + if isdifferent(Z̃arg, Z̃_∇g) + Z̃_∇g .= Z̃arg + value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...) end end gfuncs = Vector{Function}(undef, ng) @@ -662,10 +662,10 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ) ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac, Z̃_∇geq, ∇geq_context...; strict) ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq) - function update_con_eq!(geq, ∇geq, Z̃, Z̃arg) - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃, ∇geq_context...) + function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃arg) + if isdifferent(Z̃arg, Z̃_∇geq) + Z̃_∇geq .= Z̃arg + value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃_∇geq, ∇geq_context...) end end geqfuncs = Vector{Function}(undef, neq) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2468012df..19897c98d 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1356,15 +1356,14 @@ function get_optim_functions( ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict) estim.Nk[] = 0 ∇J = Vector{JNT}(undef, nZ̃) - function update_objective!(J, ∇J, Z̃, Z̃arg) - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg + function update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + if isdifferent(Z̃arg, Z̃_∇J) + Z̃_∇J .= Z̃arg J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) end end function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - @show J return J[]::T end ∇Jfunc! = function (∇Jarg::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} @@ -1388,10 +1387,10 @@ function get_optim_functions( estim.con.i_g .= false estim.Nk[] = 0 ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng) - function update_con!(g, ∇g, Z̃, Z̃arg) - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃, ∇g_context...) + function update_con!(g, ∇g, Z̃_∇g, Z̃arg) + if isdifferent(Z̃arg, Z̃_∇g) + Z̃_∇g .= Z̃arg + value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...) end end gfuncs = Vector{Function}(undef, ng) From 3ad3158ae519d9e4dd8e7652c147e6125509e904 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 22 May 2025 14:50:02 -0400 Subject: [PATCH 4/7] debug: force gradient/jacobian update of `MovingHorizonEstimator` when not filled --- docs/src/internals/state_estim.md | 1 + src/controller/transcription.jl | 4 +- src/estimator/mhe/execute.jl | 96 +++++++++++++++++++------------ 3 files changed, 62 insertions(+), 39 deletions(-) diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 749565a4c..d869a2416 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -39,6 +39,7 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel) ```@docs ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) +ModelPredictiveControl.set_warmstart_mhe! ``` ## Remove Operating Points diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 756cd6f38..9048e22a6 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -954,7 +954,7 @@ linconstrainteq!(::PredictiveController, ::SimModel, ::MultipleShooting) = nothi @doc raw""" set_warmstart!(mpc::PredictiveController, transcription::SingleShooting, Z̃var) -> Z̃s -Set and return the warm start value of `Z̃var` for [`SingleShooting`](@ref) transcription. +Set and return the warm-start value of `Z̃var` for [`SingleShooting`](@ref) transcription. If supported by `mpc.optim`, it warm-starts the solver at: ```math @@ -985,7 +985,7 @@ end @doc raw""" set_warmstart!(mpc::PredictiveController, transcription::MultipleShooting, Z̃var) -> Z̃s -Set and return the warm start value of `Z̃var` for [`MultipleShooting`](@ref) transcription. +Set and return the warm-start value of `Z̃var` for [`MultipleShooting`](@ref) transcription. It warm-starts the solver at: ```math diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 99b146ba5..952e75fc3 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -362,43 +362,20 @@ end Optimize objective of `estim` [`MovingHorizonEstimator`](@ref) and return the solution `Z̃`. -If supported by `estim.optim`, it warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ϵ_{k-1} \\ - \mathbf{x̂}_{k-1}(k-N_k+p) \\ - \mathbf{ŵ}_{k-1}(k-N_k+p+0) \\ - \mathbf{ŵ}_{k-1}(k-N_k+p+1) \\ - \vdots \\ - \mathbf{ŵ}_{k-1}(k-p-2) \\ - \mathbf{0} \\ -\end{bmatrix} -``` -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. A failed optimization also prints [`getinfo`](@ref) results in -the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages). +If first warm-starts the solver with [`set_warmstart_mhe!`](@ref). 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. A failed optimization also prints +[`getinfo`](@ref) results in the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages). """ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real model, optim, buffer = estim.model, estim.optim, estim.buffer - nu, ny, nk = model.nu, model.ny, model.nk - nx̂, nym, nŵ, nϵ, Nk = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ, estim.Nk[] + nym, nx̂, nŵ, nϵ, Nk = estim.nym, estim.nx̂, estim.nx̂, estim.nϵ, estim.Nk[] nx̃ = nϵ + nx̂ Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] - V̂ = Vector{NT}(undef, nym*Nk) - X̂0 = Vector{NT}(undef, nx̂*Nk) - û0, ŷ0, x̄, k0 = buffer.û, buffer.ŷ, buffer.x̂, buffer.k - ϵ_0 = estim.nϵ ≠ 0 ? estim.Z̃[begin] : empty(estim.Z̃) - Z̃s = [ϵ_0; estim.x̂0arr_old; estim.Ŵ] - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) - J_0 = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) - # warm-start Z̃s with Ŵ=0 if objective or constraint function not finite : - isfinite(J_0) || (Z̃s = [ϵ_0; estim.x̂0arr_old; zeros(NT, nŵ*estim.He)]) - JuMP.set_start_value.(Z̃var, Z̃s) + V̂ = Vector{NT}(undef, nym*Nk) # TODO: remove this allocation + X̂0 = Vector{NT}(undef, nx̂*Nk) # TODO: remove this allocation + Z̃s = set_warmstart_mhe!(V̂, X̂0, estim, Z̃var) # ------- solve optimization problem -------------- - println("optimizing!") try JuMP.optimize!(optim) catch err @@ -434,15 +411,64 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real estim.Z̃ .= JuMP.value.(Z̃var) end # --------- update estimate ----------------------- + û0, ŷ0, k0 = buffer.û, buffer.ŷ, buffer.k estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - println("solved, needs to call predict! one more time.") V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next - @show estim.Ŵ[1:nŵ*Nk] return estim.Z̃ end +@doc raw""" + set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator, Z̃var) -> Z̃s + +Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref). + +If supported by `estim.optim`, it warm-starts the solver at: +```math +\mathbf{Z̃_s} = +\begin{bmatrix} + ϵ_{k-1} \\ + \mathbf{x̂}_{k-1}(k-N_k+p) \\ + \mathbf{ŵ}_{k-1}(k-N_k+p+0) \\ + \mathbf{ŵ}_{k-1}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{ŵ}_{k-1}(k-p-2) \\ + \mathbf{0} \\ +\end{bmatrix} +``` +where ``ϵ(k-1)``, ``\mathbf{x̂}_{k-1}(k-N_k+p)`` and ``\mathbf{ŵ}_{k-1}(k-j)`` are +respectively the slack variable, the arrival state estimate and the process noise estimates +computed at the last time step ``k-1``. If the objective function is not finite at this +point, all the process noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. The +method mutates all the arguments. +""" +function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var) where NT<:Real + model, buffer = estim.model, estim.buffer + nϵ, nx̂, nŵ, nZ̃, Nk = estim.nϵ, estim.nx̂, estim.nx̂, length(estim.Z̃), estim.Nk[] + nx̃ = nϵ + nx̂ + Z̃s = Vector{NT}(undef, nZ̃) # TODO: remove this allocation + û0, ŷ0, x̄, k0 = buffer.û, buffer.ŷ, buffer.x̂, buffer.k + # --- slack variable ϵ --- + estim.nϵ == 1 && (Z̃s[begin] = estim.Z̃[begin]) + # --- arrival state estimate x̂0arr --- + Z̃s[nϵ+1:nx̃] = estim.x̂0arr_old + # --- process noise estimates Ŵ --- + Z̃s[nx̃+1:end] = estim.Ŵ + # verify definiteness of objective function: + V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) + Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) + if !isfinite(Js) + Z̃s[nx̃+nx̂+1:end] = 0 + end + # --- unused variable in Z̃ (applied only when Nk ≠ He) --- + # We force the update of the NLP gradient and jacobian by warm-starting the unused + # variable in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s + # will be inevitably different at the following time step. + Z̃s[nx̃+Nk*nŵ+1:end] .= 1 + JuMP.set_start_value.(Z̃var, Z̃s) +end + "Correct the covariance estimate at arrival using `covestim` [`StateEstimator`](@ref)." function correct_cov!(estim::MovingHorizonEstimator) nym, nd = estim.nym, estim.model.nd @@ -600,10 +626,6 @@ function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, mode x̂0 = x̂0next end end - if eltype(X̂0) == Float64 - @show X̂0 - @show V̂ - end return V̂, X̂0 end From 658ab1d939174f48c267b95b04ecea62b09b0a67 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 22 May 2025 14:50:44 -0400 Subject: [PATCH 5/7] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 0ccb80e4c..16958f7a3 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.6.1" +version = "1.6.2" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" From c1ea8bbc4adc5a3df19108635a3a3145ed083985 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 22 May 2025 15:36:39 -0400 Subject: [PATCH 6/7] test: add unfilled window test for MHE --- test/2_test_state_estim.jl | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 082e0c45a..a81d0e62a 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1064,6 +1064,29 @@ end @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6)) end +@testitem "MovingHorizonEstimator estimation with unfilled window" setup=[SetupMPCtests] begin + f(x,u,_,_) = 0.5x + u + h(x,_,_) = x + model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing) + mhe1 = MovingHorizonEstimator(model, nint_u=[1], He=3, direct=true) + for i = 1:40 + y = model() + x̂ = preparestate!(mhe1, y) + updatestate!(mhe1, [0.0], y) + updatestate!(model, [0.1]) + end + @test mhe1() ≈ model() atol = 1e-9 + model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing) + mhe2 = MovingHorizonEstimator(model, nint_u=[1], He=3, direct=false) + for i = 1:40 + y = model() + x̂ = preparestate!(mhe2, y) + updatestate!(mhe2, [0.0], y) + updatestate!(model, [0.1]) + end + @test mhe2() ≈ model() atol = 1e-9 +end + @testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) From f579da4c4fe8d470eabe3d17225cb8809b295418 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 22 May 2025 16:09:23 -0400 Subject: [PATCH 7/7] debug: correct index for MHE warm-start vector --- 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 952e75fc3..71bbaa52e 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -459,7 +459,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) if !isfinite(Js) - Z̃s[nx̃+nx̂+1:end] = 0 + Z̃s[nx̃+1:end] = 0 end # --- unused variable in Z̃ (applied only when Nk ≠ He) --- # We force the update of the NLP gradient and jacobian by warm-starting the unused