added: smarter initstate! for non-SingleShooting

Author: franckgaga

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, x̂)
+    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)
+
+@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)