doc: equations for defects equation and matrices

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -14,6 +14,7 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 ```@docs
 ModelPredictiveControl.init_ΔUtoU
 ModelPredictiveControl.init_predmat
+ModelPredictiveControl.init_defectmat
 ModelPredictiveControl.relaxU
 ModelPredictiveControl.relaxΔU
 ModelPredictiveControl.relaxŶ

src/controller/construct.jl

@@ -508,7 +508,9 @@ end
 
 
 @doc raw"""
-    init_predmat(estim, model::LinModel, Hp, Hc) -> E, G, J, K, V, ex̂, gx̂, jx̂, kx̂, vx̂
+    init_predmat(
+        estim, model::LinModel, Hp, Hc, transcription
+    ) -> E, G, J, K, V, ex̂, gx̂, jx̂, kx̂, vx̂
 
 Construct the prediction matrices for [`LinModel`](@ref) `model`.
 
@@ -603,7 +605,9 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref).
     \end{aligned}
     ```
 """
-function init_predmat(estim::StateEstimator{NT}, model::LinModel, Hp, Hc) where {NT<:Real}
+function init_predmat(
+    estim::StateEstimator{NT}, model::LinModel, Hp, Hc, transcription
+) where {NT<:Real}
     Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d
     nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd
     # --- pre-compute matrix powers ---
@@ -670,7 +674,9 @@ function init_predmat(estim::StateEstimator{NT}, model::LinModel, Hp, Hc) where
 end
 
 "Return empty matrices if `model` is not a [`LinModel`](@ref)"
-function init_predmat(estim::StateEstimator{NT}, model::SimModel, Hp, Hc) where {NT<:Real}
+function init_predmat(
+    estim::StateEstimator{NT}, model::SimModel, Hp, Hc, transcription
+) where {NT<:Real}
     nu, nx̂, nd = model.nu, estim.nx̂, model.nd
     E  = zeros(NT, 0, nu*Hc)
     G  = zeros(NT, 0, nd)
@@ -682,6 +688,59 @@ function init_predmat(estim::StateEstimator{NT}, model::SimModel, Hp, Hc) where
     return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 end
 
+
+@doc raw"""
+    init_defectmat(estim::StateEstimator, model::LinModel, Hp, Hc, transcription)
+
+Init the matrices for computing the defects over the predicted states. 
+
+The defects are calculated with an equation similar to the prediction matrices (see 
+[`init_predmat`](@ref)):
+```math
+\begin{aligned*}
+    \mathbf{Ŝ} &= \mathbf{E_ŝ Z} + \mathbf{G_ŝ d_0}(k)  + \mathbf{J_ŝ D̂_0} 
+                                 + \mathbf{K_ŝ x̂_0}(k) + \mathbf{V_ŝ u_0}(k-1) 
+                                 + \mathbf{B_ŝ}
+               &= \mathbf{E_ŝ Z} + \mathbf{F_ŝ}
+               
+\end{aligned*}
+```   
+They are forced to be ``\mathbf{Ŝ = 0}`` using the equality constraints of the optimizer.
+The matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in the Extended Help
+section.
+
+# Extended Help
+!!! details "Extended Help"
+    The matrices are computed by:
+    ```math
+    \begin{aligned}
+    \mathbf{E_ŝ} &= \begin{bmatrix}
+        \mathbf{B̂_u} & \mathbf{0}   & \cdots & \mathbf{0} & -\mathbf{I} &  \mathbf{0} & \cdots &  \mathbf{0}    \\
+        \mathbf{0}   & \mathbf{B̂_u} & \cdots & \mathbf{0} &  \mathbf{Â} & -\mathbf{I} & \cdots &  \mathbf{0}    \\
+        \vdots       & \vdots       & \ddots & \vdots     &  \vdots     &  \vdots     & \ddots & \vdots         \\
+        \mathbf{0}   & \mathbf{0}   & \cdots & \mathbf{B̂_u}  \mathbf{0} &  \mathbf{0} & \cdots & -\mathbf{I}    \end{bmatrix} \\
+    \mathbf{G_ŝ} &= \begin{bmatrix}
+        \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0}                                                      \end{bmatrix} \\
+    \mathbf{J_ŝ} &= \begin{bmatrix}
+        \mathbf{0}   & \mathbf{0}   & \cdots & \mathbf{0}   & \mathbf{0}                                        \\
+        \mathbf{B̂_d} & \mathbf{0}   & \cdots & \mathbf{0}   & \mathbf{0}                                        \\
+        \vdots       & \vdots       & \ddots & \vdots       & \vdots                                            \\
+        \mathbf{0}   & \mathbf{0}   & \cdots & \mathbf{B̂_d} & \mathbf{0}                                        \end{bmatrix} \\
+    \mathbf{K_ŝ} &= \begin{bmatrix}
+        \mathbf{Â} \\ \mathbf{0} \\ \vdots \\ \mathbf{0}                                                        \end{bmatrix} \\
+    \mathbf{V_ŝ} &= \begin{bmatrix}
+        \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \mathbf{B̂_u}                                            \end{bmatrix} \\
+    \mathbf{B_ŝ} &= \begin{bmatrix}
+        \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}}              \end{bmatrix}
+    \end{aligned}
+    ```
+"""
+function init_defectmat(
+    estim::StateEstimator{NT}, model::LinModel, Hp, Hc, transcription
+) where {NT<:Real}
+
+end
+
 @doc raw"""
     init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, S) -> H̃
 

src/controller/explicitmpc.jl

@@ -45,8 +45,9 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
-        S, T = init_ΔUtoU(model, Hp, Hc)
-        E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc)
+        transcription = :singleshooting
+        S, T = init_ZtoU(estim, Hp, Hc, transcription)
+        E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc, transcription)
         # 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

src/controller/linmpc.jl

@@ -1,4 +1,5 @@
 const DEFAULT_LINMPC_OPTIMIZER = OSQP.MathOptInterfaceOSQP.Optimizer
+const DEFAULT_LINMPC_TRANSCRIPTION = :singleshooting
 
 struct LinMPC{
     NT<:Real, 
@@ -8,6 +9,7 @@ struct LinMPC{
     estim::SE
     # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
     # different since solvers that support non-Float64 are scarce.
+    transcription::Symbol
     optim::JM
     con::ControllerConstraint{NT, Nothing}
     ΔŨ::Vector{NT}
@@ -41,16 +43,19 @@ struct LinMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function LinMPC{NT}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM
+        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, 
+        transcription, optim::JM
     ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel}
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         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)
-        S, T = init_ΔUtoU(model, Hp, Hc)
-        E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
+        S, T = init_ZtoU(estim, Hp, Hc, transcription)
+        E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
+            estim, model, Hp, Hc, transcription
+        )
         # dummy vals (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
         con, nϵ, S̃, Ẽ = init_defaultcon_mpc(
@@ -67,7 +72,7 @@ struct LinMPC{
         ΔŨ = zeros(NT, nΔŨ)
         buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
         mpc = new{NT, SE, JM}(
-            estim, optim, con,
+            estim, transcription, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             weights,
@@ -130,6 +135,8 @@ arguments. This controller allocates memory at each time step for the optimizati
 - `N_Hc=diagm(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
 - `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``.
 - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
+- `transcription=:singleshooting` : transcription method for the nonlinear optimization 
+    problem, can be `:singleshooting` or `:multipleshoting`.
 - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in
   the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
   (default to [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer).
@@ -187,11 +194,12 @@ function LinMPC(
     N_Hc = diagm(repeat(Nwt, Hc)),
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt = DEFAULT_CWT,
+    transcription = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
     estim = SteadyKalmanFilter(model; kwargs...)
-    return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, optim)
+    return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, transcription, optim)
 end
 
 
@@ -229,6 +237,7 @@ function LinMPC(
     N_Hc = diagm(repeat(Nwt, Hc)),
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
+    transcription = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
 ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel}
     isa(estim.model, LinModel) || error("estim.model type must be a LinModel") 
@@ -237,7 +246,7 @@ function LinMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    return LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim)
+    return LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, transcription, optim)
 end
 
 """

src/controller/nonlinmpc.jl

@@ -1,4 +1,5 @@
 const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes")
+const DEFAULT_NONLINMPC_TRANSCRIPTION = :singleshooting
 
 struct NonLinMPC{
     NT<:Real, 
@@ -65,7 +66,9 @@ struct NonLinMPC{
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         S, T = init_ZtoU(estim, Hp, Hc, transcription)
-        E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
+        E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
+            estim, model, Hp, Hc, transcription
+        )
         # dummy vals (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
         con, nϵ, S̃, Ẽ = init_defaultcon_mpc(
@@ -255,7 +258,7 @@ function NonLinMPC(
     gc ::Function = gc!,
     nc::Int = 0,
     p = model.p,
-    transcription = :singleshooting,
+    transcription = DEFAULT_NONLINMPC_TRANSCRIPTION,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
@@ -284,7 +287,7 @@ function NonLinMPC(
     gc ::Function = gc!,
     nc::Int = 0,
     p = model.p,
-    transcription = :singleshooting,
+    transcription = DEFAULT_NONLINMPC_TRANSCRIPTION,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
@@ -337,7 +340,7 @@ function NonLinMPC(
     gc ::Function = gc!,
     nc = 0,
     p::P = estim.model.p,
-    transcription = :singleshooting,
+    transcription = DEFAULT_NONLINMPC_TRANSCRIPTION,
     optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
 ) where {
     NT<:Real,