added: smarter initstate! for non-SingleShooting

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

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"""

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)
 

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)