Merge remote-tracking branch 'origin/main' into fix_type_instability

Author: franckgaga

src/controller/construct.jl

@@ -1,3 +1,31 @@
+"Include all the objective function weights of [`PredictiveController`](@ref)"
+struct ControllerWeights{NT<:Real}
+    M_Hp::Hermitian{NT, Matrix{NT}}
+    Ñ_Hc::Hermitian{NT, Matrix{NT}}
+    L_Hp::Hermitian{NT, Matrix{NT}}
+    E   ::NT
+    function ControllerWeights{NT}(
+        model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
+    ) where NT<:Real
+        validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
+        # convert `Diagonal` to normal `Matrix` if required:
+        M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
+        N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
+        L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
+        nΔU = size(N_Hc, 1)
+        C = Cwt
+        if !isinf(Cwt)  
+            # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable)
+            Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C], :L)
+        else
+            # ΔŨ = ΔU (only hard constraints)
+            Ñ_Hc = N_Hc
+        end   
+        E = Ewt         
+        return new{NT}(M_Hp, Ñ_Hc, L_Hp, E)
+    end
+end
+
 "Include all the data for the constraints of [`PredictiveController`](@ref)"
 struct ControllerConstraint{NT<:Real, GCfunc<:Function}
     ẽx̂      ::Matrix{NT}
@@ -609,7 +637,7 @@ function init_predmat(estim::StateEstimator{NT}, model::SimModel, Hp, Hc) where
 end
 
 @doc raw"""
-    init_quadprog(model::LinModel, Ẽ, S, M_Hp, N_Hc, L_Hp) -> H̃
+    init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, S) -> H̃
 
 Init the quadratic programming Hessian `H̃` for MPC.
 
@@ -626,29 +654,30 @@ The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control per
 [`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at
 optimization but required to evaluate the minimal ``J`` value.
 """
-function init_quadprog(::LinModel, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, S̃)
+    M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp
     H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + Ñ_Hc + S̃'*L_Hp*S̃), :L)
     return H̃
 end
-"Return empty matrices if `model` is not a [`LinModel`](@ref)."
-function init_quadprog(::SimModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real}
+"Return empty matrix if `model` is not a [`LinModel`](@ref)."
+function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _) where {NT<:Real}
     H̃ = Hermitian(zeros(NT, 0, 0), :L)
     return H̃
 end
 
 """
     init_defaultcon_mpc(
-        estim, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, 
+        estim, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, 
         gc!=(_,_,_,_,_,_)->nothing, nc=0
-    ) -> con, S̃, Ñ_Hc, Ẽ
+    ) -> con, S̃, Ẽ
 
 Init `ControllerConstraint` struct with default parameters based on estimator `estim`.
 
-Also return `S̃`, `Ñ_Hc` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`.
+Also return `S̃` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`.
 """
 function init_defaultcon_mpc(
     estim::StateEstimator{NT}, 
-    Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
+    Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
     gc!::GCfunc=(_,_,_,_,_,_)->nothing, nc=0
 ) where {NT<:Real, GCfunc<:Function}
     model = estim.model
@@ -667,9 +696,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, Ñ_Hc = relaxΔU(
-        model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc
-    )
+    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax = relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax)
     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̂)
     i_Umin,  i_Umax  = .!isinf.(U0min),  .!isinf.(U0max)
@@ -689,7 +716,7 @@ function init_defaultcon_mpc(
         A       , b     , i_b    , C_ymin   , C_ymax , c_x̂min , c_x̂max , i_g,
         gc!     , nc
     )
-    return con, nϵ, S̃, Ñ_Hc, Ẽ
+    return con, nϵ, S̃, Ẽ
 end
 
 "Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons."
@@ -742,17 +769,14 @@ function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real
 end
 
 @doc raw"""
-    relaxΔU(
-        model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc
-    ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc
+    relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax
 
 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 input increment weights ``\mathbf{Ñ}_{H_c}`` (that incorporate ``C``). It also  
-returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{max}}`` and the 
-``\mathbf{A}`` matrices for the inequality constraints:
+augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{max}}`` and the ``\mathbf{A}``
+matrices for the inequality constraints:
 ```math
 \begin{bmatrix} 
     \mathbf{A_{ΔŨ_{min}}} \\ 
@@ -764,21 +788,19 @@ returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{
 \end{bmatrix}
 ```
 """
-function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) where NT<:Real
-    nΔU = size(N_Hc, 1)
+function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) where NT<:Real
+    nΔU = length(ΔUmin)
     if nϵ == 1 # ΔŨ = [ΔU; ϵ]
         # 0 ≤ ϵ ≤ ∞  
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]]
-        A_ϵ = [zeros(NT, 1, length(ΔUmin)) NT[1.0]]
+        A_ϵ = [zeros(NT, 1, nΔU) NT[1.0]]
         A_ΔŨmin, A_ΔŨmax = -[I  C_Δumin; A_ϵ], [I -C_Δumax; A_ϵ]
-        Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1);zeros(NT, 1, nΔU) C], :L)
     else # ΔŨ = ΔU (only hard constraints)
         ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
         I_Hc = Matrix{NT}(I, nΔU, nΔU)
         A_ΔŨmin, A_ΔŨmax = -I_Hc,  I_Hc
-        Ñ_Hc = N_Hc
     end
-    return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc
+    return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax
 end
 
 @doc raw"""

src/controller/execute.jl

@@ -206,15 +206,15 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     end
     mpc.R̂y .= R̂y
     Cy = F .- (R̂y .- mpc.Yop)
-    M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ
+    M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ
     mul!(q̃, M_Hp_Ẽ', Cy)
-    r .= dot(Cy, mpc.M_Hp, Cy)
+    r .= dot(Cy, mpc.weights.M_Hp, Cy)
     if ~mpc.noR̂u
         mpc.R̂u .= R̂u
         Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) 
-        L_Hp_S̃ = mpc.L_Hp*mpc.S̃
+        L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃
         mul!(q̃, L_Hp_S̃', Cu, 1, 1)
-        r .+= dot(Cu, mpc.L_Hp, Cu)
+        r .+= dot(Cu, mpc.weights.L_Hp, Cu)
     end
     lmul!(2, q̃)
     return nothing
@@ -413,14 +413,14 @@ function obj_nonlinprog!(
     # --- output setpoint tracking term ---
     Ȳ  .= @views Ŷe[ny+1:end]
     Ȳ  .= mpc.R̂y .- Ȳ
-    JR̂y = dot(Ȳ, mpc.M_Hp, Ȳ)
+    JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ)
     # --- move suppression and slack variable term ---
-    JΔŨ = dot(ΔŨ, mpc.Ñ_Hc, ΔŨ)
+    JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ)
     # --- input setpoint tracking term ---
     if !mpc.noR̂u
         Ū  .= @views Ue[1:end-nu]
         Ū  .= mpc.R̂u .- Ū
-        JR̂u = dot(Ū, mpc.L_Hp, Ū)
+        JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
     else
         JR̂u = 0.0
     end
@@ -588,12 +588,12 @@ prediction horizon ``H_p``.
 ```jldoctest
 julia> mpc = LinMPC(KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σR=[√25]), Hp=1, Hc=1);
 
-julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1]
+julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1]
 (0.1, 25.0, 1.0, 0.1)
 
 julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[10], Nwt=[0.666]);
 
-julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1]
+julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1]
 (0.42, 9.0, 10.0, 0.666)
 ```
 """
@@ -616,44 +616,44 @@ function setmodel!(
         size(Mwt) == (ny,) || throw(ArgumentError("Mwt should be a vector of length $ny"))
         any(x -> x < 0, Mwt) && throw(ArgumentError("Mwt values should be nonnegative"))
         for i=1:ny*Hp
-            mpc.M_Hp[i, i] = Mwt[(i-1) % ny + 1]
+            mpc.weights.M_Hp[i, i] = Mwt[(i-1) % ny + 1]
         end
     elseif !isnothing(M_Hp)
         M_Hp = to_hermitian(M_Hp)
         nŶ = ny*Hp
         size(M_Hp) == (nŶ, nŶ) || throw(ArgumentError("M_Hp size should be ($nŶ, $nŶ)"))
-        mpc.M_Hp .= M_Hp
+        mpc.weights.M_Hp .= M_Hp
     end
     if isnothing(Ñ_Hc) && !isnothing(Nwt)
         size(Nwt) == (nu,) || throw(ArgumentError("Nwt should be a vector of length $nu"))
         any(x -> x < 0, Nwt) && throw(ArgumentError("Nwt values should be nonnegative"))
         for i=1:nu*Hc
-            mpc.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1]
+            mpc.weights.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1]
         end
     elseif !isnothing(Ñ_Hc)
         Ñ_Hc = to_hermitian(Ñ_Hc)
         nΔŨ = nu*Hc+nϵ
         size(Ñ_Hc) == (nΔŨ, nΔŨ) || throw(ArgumentError("Ñ_Hc size should be ($nΔŨ, $nΔŨ)"))
-        mpc.Ñ_Hc .= Ñ_Hc
+        mpc.weights.Ñ_Hc .= Ñ_Hc
     end
     if isnothing(L_Hp) && !isnothing(Lwt)
         size(Lwt) == (nu,) || throw(ArgumentError("Lwt should be a vector of length $nu"))
         any(x -> x < 0, Lwt) && throw(ArgumentError("Lwt values should be nonnegative"))
         for i=1:nu*Hp
-            mpc.L_Hp[i, i] = Lwt[(i-1) % nu + 1]
+            mpc.weights.L_Hp[i, i] = Lwt[(i-1) % nu + 1]
         end
     elseif !isnothing(L_Hp)
         L_Hp = to_hermitian(L_Hp)
         nU = nu*Hp
         size(L_Hp) == (nU, nU) || throw(ArgumentError("L_Hp size should be ($nU, $nU)"))
-        mpc.L_Hp .= L_Hp
+        mpc.weights.L_Hp .= L_Hp
     end
-    setmodel_controller!(mpc, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
+    setmodel_controller!(mpc, x̂op_old)
     return mpc
 end
 
 "Update the prediction matrices, linear constraints and JuMP optimization."
-function setmodel_controller!(mpc::PredictiveController, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
+function setmodel_controller!(mpc::PredictiveController, x̂op_old)
     estim, model = mpc.estim, mpc.estim.model
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     optim, con = mpc.optim, mpc.con
@@ -714,7 +714,7 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old, M_Hp, Ñ_Hc
     JuMP.unregister(optim, :linconstraint)
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
     # --- quadratic programming Hessian matrix ---
-    H̃ = init_quadprog(model, mpc.Ẽ, mpc.S̃, mpc.M_Hp, mpc.Ñ_Hc, mpc.L_Hp)
+    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃)
     mpc.H̃ .= H̃
     set_objective_hessian!(mpc, ΔŨvar)
     return nothing

src/controller/explicitmpc.jl

@@ -5,10 +5,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Hp::Int
     Hc::Int
     nϵ::Int
-    M_Hp::Hermitian{NT, Matrix{NT}}
-    Ñ_Hc::Hermitian{NT, Matrix{NT}}
-    L_Hp::Hermitian{NT, Matrix{NT}}
-    E::NT
+    weights::ControllerWeights{NT}
     R̂u::Vector{NT}
     R̂y::Vector{NT}
     noR̂u::Bool
@@ -42,8 +39,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         nϵ = 0    # no slack variable ϵ for ExplicitMPC
-        Ewt = 0   # economic costs not supported for ExplicitMPC
-        validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp)
+        weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp)
         # convert `Diagonal` to normal `Matrix` if required:
         M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
         N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
@@ -56,7 +52,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         # 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
-        H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        H̃ = init_quadprog(model, weights ,Ẽ, S̃)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         H̃_chol = cholesky(H̃)
@@ -71,7 +67,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             estim,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
-            M_Hp, Ñ_Hc, L_Hp, Ewt,  
+            weights,
             R̂u, R̂y, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
@@ -211,7 +207,7 @@ addinfo!(info, mpc::ExplicitMPC) = info
 
 
 "Update the prediction matrices and Cholesky factorization."
-function setmodel_controller!(mpc::ExplicitMPC, _ , M_Hp, Ñ_Hc, L_Hp)
+function setmodel_controller!(mpc::ExplicitMPC, _ )
     estim, model = mpc.estim, mpc.estim.model
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     # --- predictions matrices ---
@@ -224,7 +220,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ , M_Hp, Ñ_Hc, L_Hp)
     mpc.V .= V
     mpc.B .= B
     # --- quadratic programming Hessian matrix ---
-    H̃ = init_quadprog(model, mpc.Ẽ, mpc.S̃, mpc.M_Hp, mpc.Ñ_Hc, mpc.L_Hp)
+    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃)
     mpc.H̃ .= H̃
     set_objective_hessian!(mpc)
     # --- operating points ---

src/controller/linmpc.jl

@@ -15,10 +15,7 @@ struct LinMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
-    M_Hp::Hermitian{NT, Matrix{NT}}
-    Ñ_Hc::Hermitian{NT, Matrix{NT}}
-    L_Hp::Hermitian{NT, Matrix{NT}}
-    E::NT
+    weights::ControllerWeights{NT}
     R̂u::Vector{NT}
     R̂y::Vector{NT}
     noR̂u::Bool
@@ -50,23 +47,18 @@ struct LinMPC{
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
-        Ewt = 0   # economic costs not supported for LinMPC
-        validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
-        # convert `Diagonal` to normal `Matrix` if required:
-        M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
-        N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
-        L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
+        weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_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)
         # dummy vals (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
-        con, nϵ, S̃, Ñ_Hc, Ẽ = init_defaultcon_mpc(
-            estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
+        con, nϵ, S̃, Ẽ = init_defaultcon_mpc(
+            estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
         )
-        H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        H̃ = init_quadprog(model, weights, Ẽ, S̃)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)
@@ -80,7 +72,7 @@ struct LinMPC{
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
-            M_Hp, Ñ_Hc, L_Hp, Ewt, 
+            weights,
             R̂u, R̂y, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,