wip: multiple shooting

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -12,6 +12,7 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 ## Controller Construction
 
 ```@docs
+ModelPredictiveControl.init_ZtoΔU   
 ModelPredictiveControl.init_ZtoU
 ModelPredictiveControl.init_predmat
 ModelPredictiveControl.init_defectmat

src/controller/construct.jl

@@ -499,20 +499,20 @@ end
     init_defaultcon_mpc(
         estim::StateEstimator,
         Hp, Hc, C, 
-        S, E, 
+        P, S, E, 
         ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
         Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
         gc!=nothing, nc=0
     ) -> con, S̃, Ẽ, Ẽŝ
 
 Init `ControllerConstraint` struct with default parameters based on estimator `estim`.
 
-Also return `S̃`, `Ẽ` and `Ẽŝ` matrices for the the augmented decision vector `ΔŨ`.
+Also return `P̃`, `S̃`, `Ẽ` and `Ẽŝ` matrices for the the augmented decision vector `Z̃`.
 """
 function init_defaultcon_mpc(
     estim::StateEstimator{NT}, 
     Hp, Hc, C, 
-    S, E, 
+    P, S, E, 
     ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
     Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
     gc!::GCfunc = nothing, nc = 0
@@ -533,7 +533,7 @@ function init_defaultcon_mpc(
     C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = 
         repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax)
     A_Umin,  A_Umax, S̃  = relaxU(model, nϵ, C_umin, C_umax, S)
-    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax = relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax)
+    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃ = relaxΔU(model, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P)
     A_Ymin,  A_Ymax, Ẽ  = relaxŶ(model, nϵ, C_ymin, C_ymax, E)
     A_x̂min,  A_x̂max, ẽx̂ = relaxterminal(model, nϵ, c_x̂min, c_x̂max, ex̂)
     A_ŝ, Ẽŝ = augmentdefect(model, nϵ, Eŝ)
@@ -558,7 +558,7 @@ function init_defaultcon_mpc(
         C_ymin  , C_ymax , c_x̂min , c_x̂max , i_g,
         gc!     , nc
     )
-    return con, nϵ, S̃, Ẽ, Ẽŝ
+    return con, nϵ, P̃, S̃, Ẽ, Ẽŝ
 end
 
 "Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons."
@@ -579,70 +579,76 @@ end
 
 Augment manipulated inputs constraints with slack variable ϵ for softening.
 
-Denoting the input increments augmented with the slack variable
-``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the
+Denoting the decision variables augmented with the slack variable
+``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the
 augmented conversion matrix ``\mathbf{S̃}``, similar to the one described at
 [`init_ZtoU`](@ref). It also returns the ``\mathbf{A}`` matrices for the inequality
 constraints:
 ```math
 \begin{bmatrix} 
     \mathbf{A_{U_{min}}} \\ 
     \mathbf{A_{U_{max}}} 
-\end{bmatrix} \mathbf{ΔŨ} ≤
+\end{bmatrix} \mathbf{Z̃} ≤
 \begin{bmatrix}
-    - \mathbf{U_{min} + T} \mathbf{u}(k-1) \\
-    + \mathbf{U_{max} - T} \mathbf{u}(k-1)
+    - \mathbf{U_{min} + T u}(k-1) \\
+    + \mathbf{U_{max} - T u}(k-1)
 \end{bmatrix}
 ```
 in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively contains
 ``\mathbf{u_{min}}`` and ``\mathbf{u_{max}}`` repeated ``H_p`` times.
 """
 function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real
-    if nϵ == 1 # ΔŨ = [ΔU; ϵ]
-        # ϵ impacts ΔU → U conversion for constraint calculations:
+    if nϵ == 1 # Z̃ = [Z; ϵ]
+        # ϵ impacts Z → U conversion for constraint calculations:
         A_Umin, A_Umax = -[S  C_umin], [S -C_umax] 
-        # ϵ has no impact on ΔU → U conversion for prediction calculations:
+        # ϵ has no impact on Z → U conversion for prediction calculations:
         S̃ = [S zeros(NT, size(S, 1))]
-    else # ΔŨ = ΔU (only hard constraints)
+    else # Z̃ = Z (only hard constraints)
         A_Umin, A_Umax = -S,  S
         S̃ = S
     end
     return A_Umin, A_Umax, S̃
 end
 
 @doc raw"""
-    relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax
+    relaxΔU(
+        model, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P
+    ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃
 
 Augment input increments constraints with slack variable ϵ for softening.
 
-Denoting the input increments augmented with the slack variable 
-``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the
-augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{max}}`` and the ``\mathbf{A}``
-matrices for the inequality constraints:
+Denoting the decision variables augmented with the slack variable 
+``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the
+augmented conversion matrix ``\mathbf{P̃}``, similar to the one described at
+[`init_ZtoΔU`](@ref). It also returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and
+``\mathbf{ΔŨ_{max}}`` and the ``\mathbf{A}`` matrices for the inequality constraints:
 ```math
 \begin{bmatrix} 
     \mathbf{A_{ΔŨ_{min}}} \\ 
     \mathbf{A_{ΔŨ_{max}}}
-\end{bmatrix} \mathbf{ΔŨ} ≤
+\end{bmatrix} \mathbf{Z̃} ≤
 \begin{bmatrix}
     - \mathbf{ΔŨ_{min}} \\
     + \mathbf{ΔŨ_{max}}
 \end{bmatrix}
 ```
 """
-function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) where NT<:Real
+function relaxΔU(::SimModel{NT}, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P) where NT<:Real
     nΔU = length(ΔUmin)
-    if nϵ == 1 # ΔŨ = [ΔU; ϵ]
-        # 0 ≤ ϵ ≤ ∞  
+    nZ = size(P, 2)
+    if nϵ == 1 # Z̃ = [Z; ϵ]
+        # 0 ≤ ϵ ≤ ∞
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]]
         A_ϵ = [zeros(NT, 1, nΔU) NT[1.0]]
         A_ΔŨmin, A_ΔŨmax = -[I  C_Δumin; A_ϵ], [I -C_Δumax; A_ϵ]
-    else # ΔŨ = ΔU (only hard constraints)
+        P̃ = [P zeros(NT, size(P, 1), 1)]
+    else # Z̃ = Z (only hard constraints)
         ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
         I_Hc = Matrix{NT}(I, nΔU, nΔU)
         A_ΔŨmin, A_ΔŨmax = -I_Hc,  I_Hc
+        P̃ = P
     end
-    return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax
+    return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃
 end
 
 @doc raw"""

src/controller/explicitmpc.jl

@@ -47,11 +47,12 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         transcription = SingleShooting() # explicit MPC only supports SingleShooting
+        P = init_ZtoΔU(estim, transcription, Hp, Hc)
         S, T = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
         # dummy val (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
-        S̃, Ñ_Hc, Ẽ  = S, N_Hc, E # no slack variable ϵ for ExplicitMPC
+        P̃, S̃, Ñ_Hc, Ẽ = P, S, N_Hc, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, weights ,Ẽ, S̃)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
@@ -70,7 +71,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             Hp, Hc, nϵ,
             weights,
             R̂u, R̂y,
-            S̃, T, T_lastu,
+            P̃, S̃, T, T_lastu,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
             H̃_chol,

src/controller/linmpc.jl

@@ -53,14 +53,15 @@ struct LinMPC{
         weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        P = init_ZtoΔU(estim, transcription, Hp, Hc)
         S, T = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
             model, estim, transcription, Hp, Hc
         )
         # dummy vals (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
-        con, nϵ, S̃, Ẽ = init_defaultcon_mpc(
-            estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
+        con, nϵ, P̃, S̃, Ẽ, Ẽŝ = init_defaultcon_mpc(
+            estim, Hp, Hc, Cwt, P, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
         )
         H̃ = init_quadprog(model, weights, Ẽ, S̃)
         # dummy vals (updated just before optimization):
@@ -78,7 +79,7 @@ struct LinMPC{
             Hp, Hc, nϵ,
             weights,
             R̂u, R̂y,
-            S̃, T, T_lastu,
+            P̃, S̃, T, T_lastu,
             Ẽ, F, G, J, K, V, B, 
             H̃, q̃, r,
             Ks, Ps,

src/controller/nonlinmpc.jl

@@ -67,14 +67,15 @@ struct NonLinMPC{
         weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        P = init_ZtoΔU(estim, transcription, Hp, Hc)
         S, T = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
             model, estim, transcription, Hp, Hc
         )
         # dummy vals (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
-        con, nϵ, S̃, Ẽ = init_defaultcon_mpc(
-            estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gc!, nc
+        con, nϵ, P̃, S̃, Ẽ, Ẽŝ = init_defaultcon_mpc(
+            estim, Hp, Hc, Cwt, P, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
         )
         H̃ = init_quadprog(model, weights, Ẽ, S̃)
         # dummy vals (updated just before optimization):
@@ -94,7 +95,7 @@ struct NonLinMPC{
             weights,
             JE, p,
             R̂u, R̂y,
-            S̃, T, T_lastu,
+            P̃, S̃, T, T_lastu,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
             Ks, Ps,

src/controller/transcription.jl

@@ -18,9 +18,8 @@ The decision variable in the optimization problem is (excluding the slack ``ϵ``
     \vdots                          \\ 
     \mathbf{Δu}(k+H_c-1)            \end{bmatrix}
 ```
-
 This method generally more efficient for small control horizon ``H_c``, stable or mildly
-nonlinear plant model.
+nonlinear plant model/constraints.
 """
 struct SingleShooting <: TranscriptionMethod end
 
@@ -42,12 +41,47 @@ operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)):
     \vdots                                      \\ 
     \mathbf{x̂}(k+H_p)   - \mathbf{x̂_{op}}       \end{bmatrix}
 ```
-
 This method is generally more efficient for large control horizon ``H_c``, unstable or
-highly nonlinear plant models.
+highly nonlinear plant models/constraints.
 """
 struct MultipleShooting <: TranscriptionMethod end
 
+
+@doc raw"""
+    init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> P
+
+Init decision variables to input increments over ``H_c`` conversion matrix `P`.
+
+The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{ΔU}``, the input
+increments over ``H_c``, is computed by:
+```math
+\mathbf{ΔU} = \mathbf{P} \mathbf{Z}
+```
+in which ``\mathbf{P} is defined in the Extended Help section.
+
+# Extended Help
+!!! details "Extended Help"
+    Following the decision variable definition of the [`TranscriptionMethod`](@ref), the
+    conversion matrix ``\mathbf{P}``, we have:
+    - ``\mathbf{P} = \mathbf{I}`` if `transcription isa SingleShooting`
+    - ``\mathbf{P} = [\begin{smallmatrix}\mathbf{I} \mathbf{0} \end{smallmatrix}]`` if 
+      `transcription isa MultipleShooting`
+"""
+function init_ZtoU end
+
+function init_ZtoΔU(
+    estim::StateEstimator{NT}, transcription::SingleShooting, _ , Hc
+) where {NT<:Real}
+    return Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
+end
+
+function init_ZtoΔU(
+    estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc
+) where {NT<:Real}
+    I_nu_Hc = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
+    return [I_nu_Hc zeros(NT, estim.model.nu*Hc, estim.nx̂*Hp)]
+end
+
 @doc raw"""
     init_ZtoU(estim, transcription, Hp, Hc) -> S, T