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..4fee392ab 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) + return x̂ end @doc raw""" diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d18d4365d..50c0e458a 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) + +@doc 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.estim.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) 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)