From c5eff2a30b3a00082debaeded934afe6aec1dac0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 11:37:38 -0500 Subject: [PATCH 1/2] added: smarter `initstate!` for non-`SingleShooting` --- docs/src/internals/predictive_control.md | 6 ++++++ src/controller/execute.jl | 10 +++++---- src/controller/transcription.jl | 26 ++++++++++++++++++++++++ 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index ef79ef649..b06d9c534 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -30,6 +30,12 @@ ModelPredictiveControl.get_nonlinobj_op(::NonLinMPC, ::ModelPredictiveControl.Ge ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveControl.GenericModel) ``` +## Init Decision Vector + +```@docs +ModelPredictiveControl.init_decision! +``` + ## Update Quadratic Optimization ```@docs diff --git a/src/controller/execute.jl b/src/controller/execute.jl index bb059ca4e..6acd4e8e8 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -1,15 +1,17 @@ @doc raw""" initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂ -Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.Z̃` at zero. +Init the states of `mpc.estim` [`StateEstimator`](@ref) and the warm start value `mpc.Z̃`. It also stores `u - mpc.estim.model.uop` at `mpc.lastu0` for converting the input increments -``\mathbf{ΔU}`` to inputs ``\mathbf{U}``. +``\mathbf{ΔU}`` to inputs ``\mathbf{U}``. See [`init_decision!`](@ref) for details on how +`mpc.Z̃` is initialized. The function returns the initial state estimate `x̂`. """ function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) - mpc.Z̃ .= 0 + x̂ = initstate!(mpc.estim, u, ym, d) mpc.lastu0 .= u .- mpc.estim.model.uop - return initstate!(mpc.estim, u, ym, d) + init_decision!(mpc, mpc.transcription, x̂) + return x̂ end @doc raw""" diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d18d4365d..6b488a218 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -766,6 +766,32 @@ function init_matconstraint_mpc( return i_b, i_g, A, Aeq, neq end +@doc raw""" + init_decision!(mpc::PredictiveController, ::SingleShooting) + +Init the decision vector ``\mathbf{Z̃ = ΔŨ}`` with zeros for [`SingleShooting`](@ref). +""" +init_decision!(mpc::PredictiveController, ::SingleShooting, _) = (mpc.Z̃ .= 0; nothing) + +@dow raw""" + init_decision!(mpc::PredictiveController, ::TranscriptionMethod) + +Also init the state part of the decision vector for other [`TranscriptionMethod`](@ref)s. + +The ``\mathbf{X̂_0}`` component is assumed constant over ``H_p`` at the current value stored +in `mpc.estim.x̂0`. +""" +function init_decision!(mpc::PredictiveController, ::TranscriptionMethod) + nΔU, nx̂ = mpc.Hc*mpc.model.nu, mpc.estim.nx̂ + mpc.Z̃ .= 0 + for j=1:mpc.Hp + iRow = nΔU .+ (1:nx̂) .+ nx̂*(j-1) + mpc.Z̃[iRow] .= mpc.estim.x̂0 + end + return nothing +end + + @doc raw""" linconstraint!(mpc::PredictiveController, model::LinModel) From c843b02d45b5d99f0c7d3b7d196e10681daf6a2e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 19 Feb 2026 12:19:46 -0500 Subject: [PATCH 2/2] debug: correct signature --- src/controller/execute.jl | 2 +- src/controller/transcription.jl | 6 +++--- src/plot_sim.jl | 4 ++++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 6acd4e8e8..4fee392ab 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -10,7 +10,7 @@ It also stores `u - mpc.estim.model.uop` at `mpc.lastu0` for converting the inpu function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) x̂ = initstate!(mpc.estim, u, ym, d) mpc.lastu0 .= u .- mpc.estim.model.uop - init_decision!(mpc, mpc.transcription, x̂) + init_decision!(mpc, mpc.transcription) return x̂ end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 6b488a218..50c0e458a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -771,9 +771,9 @@ end Init the decision vector ``\mathbf{Z̃ = ΔŨ}`` with zeros for [`SingleShooting`](@ref). """ -init_decision!(mpc::PredictiveController, ::SingleShooting, _) = (mpc.Z̃ .= 0; nothing) +init_decision!(mpc::PredictiveController, ::SingleShooting) = (mpc.Z̃ .= 0; nothing) -@dow raw""" +@doc raw""" init_decision!(mpc::PredictiveController, ::TranscriptionMethod) Also init the state part of the decision vector for other [`TranscriptionMethod`](@ref)s. @@ -782,7 +782,7 @@ The ``\mathbf{X̂_0}`` component is assumed constant over ``H_p`` at the current in `mpc.estim.x̂0`. """ function init_decision!(mpc::PredictiveController, ::TranscriptionMethod) - nΔU, nx̂ = mpc.Hc*mpc.model.nu, mpc.estim.nx̂ + nΔU, nx̂ = mpc.Hc*mpc.estim.model.nu, mpc.estim.nx̂ mpc.Z̃ .= 0 for j=1:mpc.Hp iRow = nΔU .+ (1:nx̂) .+ nx̂*(j-1) diff --git a/src/plot_sim.jl b/src/plot_sim.jl index 737153dd2..a8c2de6b5 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -286,7 +286,11 @@ function sim_closedloop!( X̂_data = Matrix{NT}(undef, estim.nx̂, N) setstate!(plant, x_0) lastd, lasty = d, evaloutput(plant, d) + # 1st `setstate!`, for correct `mpc.Z̃` initialization in `initstate!`: + isnothing(x̂_0) || setstate!(est_mpc, x̂_0) + # calling `initstate!` no matter what, to initialize `mpc.Z̃` and covariance matrices: initstate!(est_mpc, lastu, lasty[estim.i_ym], lastd) + # 2nd `setstate!`, since `initstate!` overwrite the state for `LinModel`: isnothing(x̂_0) || setstate!(est_mpc, x̂_0) @progressif progress name="$(nameof(typeof(est_mpc))) simulation" for i=1:N d = lastd + d_step + d_noise.*randn(plant.nd)