added: be more efficient with weights and conversion matrices for NonLinMPC

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.6.0"
+version = "1.6.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

src/controller/construct.jl

@@ -33,41 +33,64 @@ function PredictiveControllerBuffer(
 end
 
 "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}}
+struct ControllerWeights{
+    NT<:Real,
+    # parameters to support both dense and Diagonal matrices (with specialization):
+    MW<:AbstractMatrix{NT}, 
+    NW<:AbstractMatrix{NT},  
+    LW<:AbstractMatrix{NT}, 
+}
+    M_Hp::Hermitian{NT, MW}
+    Ñ_Hc::Hermitian{NT, NW}
+    L_Hp::Hermitian{NT, LW}
     E   ::NT
     iszero_M_Hp::Vector{Bool}
     iszero_Ñ_Hc::Vector{Bool}
     iszero_L_Hp::Vector{Bool}
     iszero_E::Bool
+    isinf_C ::Bool
     function ControllerWeights{NT}(
-        model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
-    ) where NT<:Real
+        model, Hp, Hc, M_Hp::MW, N_Hc::NW, L_Hp::LW, Cwt=Inf, Ewt=0
+    ) where {
+        NT<:Real, 
+        MW<:AbstractMatrix{NT}, 
+        NW<:AbstractMatrix{NT}, 
+        LW<:AbstractMatrix{NT}
+    }
         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)  
+        isinf_C = isinf(C)
+        if !isinf_C  
             # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable)
-            Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C], :L)
+            Ñ_Hc = [N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C]
+            isdiag(N_Hc) && (Ñ_Hc = Diagonal(Ñ_Hc)) # NW(Ñ_Hc) does not work on Julia 1.10
         else
             # ΔŨ = ΔU (only hard constraints)
             Ñ_Hc = N_Hc
-        end   
+        end
+        M_Hp = Hermitian(M_Hp, :L)
+        Ñ_Hc = Hermitian(Ñ_Hc, :L)
+        L_Hp = Hermitian(L_Hp, :L)
         E = Ewt
         iszero_M_Hp = [iszero(M_Hp)]
         iszero_Ñ_Hc = [iszero(Ñ_Hc)]
         iszero_L_Hp = [iszero(L_Hp)]
         iszero_E = iszero(E)
-        return new{NT}(M_Hp, Ñ_Hc, L_Hp, E, iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E)
+        return new{NT, MW, NW, LW}(
+            M_Hp, Ñ_Hc, L_Hp, E, 
+            iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E, isinf_C
+        )
     end
 end
 
+"Outer constructor to convert weight matrix number type to `NT` if necessary."
+function ControllerWeights{NT}(
+        model, Hp, Hc, M_Hp::MW, N_Hc::NW, L_Hp::LW, Cwt=Inf, Ewt=0
+    ) where {NT<:Real, MW<:AbstractMatrix, NW<:AbstractMatrix, LW<:AbstractMatrix}
+    return ControllerWeights{NT}(model, Hp, Hc, NT.(M_Hp), NT.(N_Hc), NT.(L_Hp), Cwt, Ewt)
+end
+
 "Include all the data for the constraints of [`PredictiveController`](@ref)"
 struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     # matrices for the terminal constraints:
@@ -478,8 +501,10 @@ end
 
 """
     init_defaultcon_mpc(
-        estim::StateEstimator, transcription::TranscriptionMethod,
-        Hp, Hc, C, 
+        estim::StateEstimator, 
+        weights::ControllerWeights
+        transcription::TranscriptionMethod,
+        Hp, Hc, 
         PΔu, Pu, E, 
         ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
         Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
@@ -491,16 +516,18 @@ Init `ControllerConstraint` struct with default parameters based on estimator `e
 Also return `P̃Δu`, `P̃u`, `Ẽ` and `Ẽŝ` matrices for the the augmented decision vector `Z̃`.
 """
 function init_defaultcon_mpc(
-    estim::StateEstimator{NT}, transcription::TranscriptionMethod,
-    Hp,  Hc, C, 
+    estim::StateEstimator{NT}, 
+    weights::ControllerWeights,
+    transcription::TranscriptionMethod,
+    Hp,  Hc, 
     PΔu, Pu, E, 
     ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
     Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
     gc!::GCfunc = nothing, nc = 0
 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}}
     model = estim.model
     nu, ny, nx̂ = model.nu, model.ny, estim.nx̂
-    nϵ = isinf(C) ? 0 : 1
+    nϵ = weights.isinf_C ? 0 : 1
     u0min,      u0max   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     Δumin,      Δumax   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     y0min,      y0max   = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny)

src/controller/execute.jl

@@ -212,7 +212,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
         mul!(F, mpc.G, mpc.d0, 1, 1)            # F = F + G*d0
         mul!(F, mpc.J, mpc.D̂0, 1, 1)            # F = F + J*D̂0
     end
-    Cy, Cu, M_Hp_Ẽ, L_Hp_P̃U = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.P̃u
+    Cy, Cu, M_Hp_Ẽ, L_Hp_P̃u = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.P̃u
     q̃, r = mpc.q̃, mpc.r
     q̃ .= 0
     r .= 0
@@ -226,8 +226,8 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     # --- input setpoint tracking term ---
     if !mpc.weights.iszero_L_Hp[]
         Cu .= mpc.Tu_lastu0 .+ mpc.Uop .- R̂u 
-        mul!(L_Hp_P̃U, mpc.weights.L_Hp, mpc.P̃u)
-        mul!(q̃, L_Hp_P̃U', Cu, 1, 1)             # q̃ = q̃ + L_Hp*P̃u'*Cu
+        mul!(L_Hp_P̃u, mpc.weights.L_Hp, mpc.P̃u)
+        mul!(q̃, L_Hp_P̃u', Cu, 1, 1)             # q̃ = q̃ + L_Hp*P̃u'*Cu
         r .+= dot(Cu, mpc.weights.L_Hp, Cu)     # r = r + Cu'*L_Hp*Cu
     end
     # --- finalize ---

src/controller/explicitmpc.jl

@@ -1,12 +1,16 @@
-struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
+struct ExplicitMPC{
+    NT<:Real, 
+    SE<:StateEstimator, 
+    CW<:ControllerWeights
+} <: PredictiveController{NT}
     estim::SE
     transcription::SingleShooting
     Z̃::Vector{NT}
     ŷ::Vector{NT}
     Hp::Int
     Hc::Int
     nϵ::Int
-    weights::ControllerWeights{NT}
+    weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
     P̃Δu::Matrix{NT}
@@ -34,17 +38,12 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function ExplicitMPC{NT}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp
-    ) where {NT<:Real, SE<:StateEstimator}
+        estim::SE, Hp, Hc, weights::CW
+    ) where {NT<:Real, SE<:StateEstimator, CW<:ControllerWeights}
         model = estim.model
         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
-        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)
-        L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         transcription = SingleShooting() # explicit MPC only supports SingleShooting
@@ -53,7 +52,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         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̂)
-        P̃Δu, P̃u, Ñ_Hc, Ẽ = PΔu, Pu, N_Hc, E # no slack variable ϵ for ExplicitMPC
+        P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
@@ -65,7 +64,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
-        mpc = new{NT, SE}(
+        mpc = new{NT, SE, CW}(
             estim,
             transcription,
             Z̃, ŷ,
@@ -131,9 +130,9 @@ function ExplicitMPC(
     Mwt = fill(DEFAULT_MWT, model.ny),
     Nwt = fill(DEFAULT_NWT, model.nu),
     Lwt = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     kwargs...
 ) 
     estim = SteadyKalmanFilter(model; kwargs...)
@@ -154,17 +153,18 @@ function ExplicitMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
 ) where {NT<:Real, SE<:StateEstimator{NT}}
     isa(estim.model, LinModel) || error("estim.model type must be a LinModel") 
     nk = estimate_delays(estim.model)
     if Hp ≤ nk
         @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 ExplicitMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
+    weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp)
+    return ExplicitMPC{NT}(estim, Hp, Hc, weights)
 end
 
 setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.")

src/controller/linmpc.jl

@@ -4,6 +4,7 @@ const DEFAULT_LINMPC_TRANSCRIPTION = SingleShooting()
 struct LinMPC{
     NT<:Real, 
     SE<:StateEstimator, 
+    CW<:ControllerWeights,
     TM<:TranscriptionMethod,
     JM<:JuMP.GenericModel
 } <: PredictiveController{NT}
@@ -18,7 +19,7 @@ struct LinMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
-    weights::ControllerWeights{NT}
+    weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
     P̃Δu::Matrix{NT}
@@ -45,13 +46,18 @@ struct LinMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function LinMPC{NT}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, 
+        estim::SE, Hp, Hc, weights::CW, 
         transcription::TM, optim::JM
-    ) where {NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, JM<:JuMP.GenericModel}
+    ) where {
+            NT<:Real, 
+            SE<:StateEstimator, 
+            CW<:ControllerWeights,
+            TM<:TranscriptionMethod, 
+            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, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
@@ -63,8 +69,9 @@ struct LinMPC{
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ = init_defaultcon_mpc(
-            estim, transcription,
-            Hp, Hc, Cwt, PΔu, Pu, E, 
+            estim, weights, transcription,
+            Hp, Hc, 
+            PΔu, Pu, E, 
             ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
         )
@@ -78,7 +85,7 @@ struct LinMPC{
         nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
-        mpc = new{NT, SE, TM, JM}(
+        mpc = new{NT, SE, CW, TM, JM}(
             estim, transcription, optim, con,
             Z̃, ŷ,
             Hp, Hc, nϵ,
@@ -140,9 +147,9 @@ arguments. This controller allocates memory at each time step for the optimizati
 - `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
 - `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
 - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
-- `M_Hp=diagm(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
-- `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}``.
+- `M_Hp=Diagonal(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
+- `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
+- `L_Hp=Diagonal(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()` : a [`TranscriptionMethod`](@ref) for the optimization.
 - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in
@@ -199,9 +206,9 @@ function LinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt = DEFAULT_CWT,
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
@@ -242,9 +249,9 @@ function LinMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
@@ -255,7 +262,8 @@ 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, transcription, optim)
+    weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
+    return LinMPC{NT}(estim, Hp, Hc, weights, transcription, optim)
 end
 
 """

src/controller/nonlinmpc.jl

@@ -9,8 +9,9 @@ const DEFAULT_NONLINMPC_JACSPARSE = AutoSparse(
 )
 
 struct NonLinMPC{
-    NT<:Real, 
+    NT<:Real,
     SE<:StateEstimator,
+    CW<:ControllerWeights,
     TM<:TranscriptionMethod,
     JM<:JuMP.GenericModel,
     GB<:AbstractADType,
@@ -32,7 +33,7 @@ struct NonLinMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
-    weights::ControllerWeights{NT}
+    weights::CW
     JE::JEfunc
     p::PT
     R̂u::Vector{NT}
@@ -61,12 +62,14 @@ struct NonLinMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function NonLinMPC{NT}(
-        estim::SE, 
-        Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::PT, 
-        transcription::TM, optim::JM, gradient::GB, jacobian::JB
+        estim::SE, Hp, Hc, weights::CW,
+        JE::JEfunc, gc!::GCfunc, nc, p::PT, 
+        transcription::TM, optim::JM, 
+        gradient::GB, jacobian::JB
     ) where {
             NT<:Real, 
-            SE<:StateEstimator, 
+            SE<:StateEstimator,
+            CW<:ControllerWeights,
             TM<:TranscriptionMethod,
             JM<:JuMP.GenericModel,
             GB<:AbstractADType,
@@ -78,7 +81,6 @@ struct NonLinMPC{
         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, Ewt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
@@ -90,8 +92,9 @@ struct NonLinMPC{
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ = init_defaultcon_mpc(
-            estim, transcription,
-            Hp, Hc, Cwt, PΔu, Pu, E, 
+            estim, weights, transcription,
+            Hp, Hc, 
+            PΔu, Pu, E, 
             ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
             gc!, nc
@@ -107,7 +110,7 @@ struct NonLinMPC{
         nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
-        mpc = new{NT, SE, TM, JM, GB, JB, PT, JEfunc, GCfunc}(
+        mpc = new{NT, SE, CW, TM, JM, GB, JB, PT, JEfunc, GCfunc}(
             estim, transcription, optim, con,
             gradient, jacobian,
             Z̃, ŷ,
@@ -187,9 +190,9 @@ This controller allocates memory at each time step for the optimization.
 - `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
 - `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
 - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
-- `M_Hp=diagm(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
-- `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}``.
+- `M_Hp=Diagonal(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
+- `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
+- `L_Hp=Diagonal(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.
 - `Ewt=0.0` : economic costs weight ``E`` (scalar). 
 - `JE=(_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e},
@@ -279,40 +282,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
-    Cwt  = DEFAULT_CWT,
-    Ewt  = DEFAULT_EWT,
-    JE ::Function = (_,_,_,_) -> 0.0,
-    gc!::Function = (_,_,_,_,_,_) -> nothing,
-    gc ::Function = gc!,
-    nc::Int = 0,
-    p = model.p,
-    transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION,
-    optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
-    gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT,
-    jacobian::AbstractADType = default_jacobian(transcription),
-    kwargs...
-)
-    estim = UnscentedKalmanFilter(model; kwargs...)
-    return NonLinMPC(
-        estim; 
-        Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, 
-        transcription, optim, gradient, jacobian
-    )
-end
-
-function NonLinMPC(
-    model::LinModel;
-    Hp::Int = default_Hp(model),
-    Hc::Int = DEFAULT_HC,
-    Mwt  = fill(DEFAULT_MWT, model.ny),
-    Nwt  = fill(DEFAULT_NWT, model.nu),
-    Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE ::Function = (_,_,_,_) -> 0.0,
@@ -326,14 +298,16 @@ function NonLinMPC(
     jacobian::AbstractADType = default_jacobian(transcription),
     kwargs...
 )
-    estim = SteadyKalmanFilter(model; kwargs...)
+    estim = default_estimator(model; kwargs...)
     return NonLinMPC(
         estim; 
         Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, 
         transcription, optim, gradient, jacobian
     )
 end
 
+default_estimator(model::SimModel; kwargs...) = UnscentedKalmanFilter(model; kwargs...)
+default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...)
 
 """
     NonLinMPC(estim::StateEstimator; <keyword arguments>)
@@ -365,9 +339,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE ::Function = (_,_,_,_) -> 0.0,
@@ -390,9 +364,9 @@ function NonLinMPC(
     end
     validate_JE(NT, JE)
     gc! = get_mutating_gc(NT, gc)
+    weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
     return NonLinMPC{NT}(
-        estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, 
-        transcription, optim, gradient, jacobian
+        estim, Hp, Hc, weights, JE, gc!, nc, p, transcription, optim, gradient, jacobian
     )
 end
 

src/controller/transcription.jl

@@ -1022,8 +1022,14 @@ function set_warmstart!(mpc::PredictiveController, transcription::MultipleShooti
     return Z̃s
 end
 
-getΔŨ!(ΔŨ, mpc::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃) # since mpc.P̃Δu = I
-getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃) = mul!(ΔŨ, mpc.P̃Δu, Z̃)
+getΔŨ!(ΔŨ, mpc::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃)
+function getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃)
+    # avoid explicit matrix multiplication with mpc.P̃Δu for performance:
+    nΔU = mpc.Hc*mpc.estim.model.nu
+    ΔŨ[1:nΔU] .= @views Z̃[1:nΔU]
+    mpc.nϵ == 1 && (ΔŨ[end] = Z̃[end])
+    return ΔŨ
+end
 getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_lastu0)
 
 @doc raw"""

test/3_test_predictive_control.jl

@@ -11,10 +11,13 @@
     @test mpc3.weights.Ñ_Hc[end] ≈ 1e6
     mpc4 = LinMPC(model, Mwt=[1,2], Hp=15)
     @test mpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15)))
+    @test mpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     mpc5 = LinMPC(model, Nwt=[3,4], Cwt=1e3, Hc=5)
     @test mpc5.weights.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
+    @test mpc5.weights.Ñ_Hc isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     mpc6 = LinMPC(model, Lwt=[0,1], Hp=15)
     @test mpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
+    @test mpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     mpc7 = @test_logs(
         (:warn, "Solving time limit is not supported by the optimizer."), 
         LinMPC(model, optim=JuMP.Model(DAQP.Optimizer))
@@ -26,15 +29,19 @@
     mpc9 = LinMPC(model, nint_u=[1, 1], nint_ym=[0, 0])
     @test mpc9.estim.nint_u  == [1, 1]
     @test mpc9.estim.nint_ym == [0, 0]
-    mpc10 = LinMPC(model, M_Hp=Diagonal(collect(1.01:0.01:1.2)))
-    @test mpc10.weights.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2))
-    mpc11 = LinMPC(model, N_Hc=Diagonal([0.1,0.11,0.12,0.13]), Cwt=Inf)
-    @test mpc11.weights.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13])
-    mcp12 = LinMPC(model, L_Hp=Diagonal(collect(0.001:0.001:0.02)))
-    @test mcp12.weights.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02))
+    mpc10 = LinMPC(model, M_Hp=diagm(collect(1.01:0.01:1.2)))
+    @test mpc10.weights.M_Hp ≈ diagm(collect(1.01:0.01:1.2))
+    @test mpc10.weights.M_Hp isa Hermitian{Float64, Matrix{Float64}}
+    mpc11 = LinMPC(model, N_Hc=diagm([0.1,0.11,0.12,0.13]), Cwt=Inf)
+    @test mpc11.weights.Ñ_Hc ≈ diagm([0.1,0.11,0.12,0.13])
+    @test mpc11.weights.Ñ_Hc isa Hermitian{Float64, Matrix{Float64}}
+    mcp12 = LinMPC(model, L_Hp=diagm(collect(0.001:0.001:0.02)))
+    @test mcp12.weights.L_Hp ≈ diagm(collect(0.001:0.001:0.02))
+    @test mcp12.weights.L_Hp isa Hermitian{Float64, Matrix{Float64}}
     model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mpc13  = LinMPC(model2)
     @test isa(mpc13, LinMPC{Float32})
+    @test isa(mpc13.estim, SteadyKalmanFilter{Float32})
     @test isa(mpc13.optim, JuMP.GenericModel{Float64}) # OSQP does not support Float32
     mpc14  = LinMPC(model2, transcription=MultipleShooting())
     @test mpc14.transcription == MultipleShooting()
@@ -400,27 +407,40 @@ end
     @test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny
     mpc4 = ExplicitMPC(model, Mwt=[1,2], Hp=15)
     @test mpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15)))
+    @test mpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     mpc5 = ExplicitMPC(model, Nwt=[3,4], Hc=5)
     @test mpc5.weights.Ñ_Hc ≈ Diagonal(diagm(repeat(Float64[3, 4], 5)))
+    @test mpc5.weights.Ñ_Hc isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     mpc6 = ExplicitMPC(model, Lwt=[0,1], Hp=15)
     @test mpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
+    @test mpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     kf = KalmanFilter(model)
     mpc8 = ExplicitMPC(kf)
     @test isa(mpc8.estim, KalmanFilter)
     mpc9 = ExplicitMPC(model, nint_u=[1, 1], nint_ym=[0, 0])
     @test mpc9.estim.nint_u  == [1, 1]
     @test mpc9.estim.nint_ym == [0, 0]
-    mpc10 = ExplicitMPC(model, M_Hp=Diagonal(collect(1.01:0.01:1.2)))
-    @test mpc10.weights.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2))
-    mpc11 = ExplicitMPC(model, N_Hc=Diagonal([0.1,0.11,0.12,0.13]))
-    @test mpc11.weights.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13])
-    mcp12 = ExplicitMPC(model, L_Hp=Diagonal(collect(0.001:0.001:0.02)))
-    @test mcp12.weights.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02))
+    mpc10 = ExplicitMPC(model, M_Hp=diagm(collect(1.01:0.01:1.2)))
+    @test mpc10.weights.M_Hp ≈ diagm(collect(1.01:0.01:1.2))
+    @test mpc10.weights.M_Hp isa Hermitian{Float64, Matrix{Float64}}
+    mpc11 = ExplicitMPC(model, N_Hc=diagm([0.1,0.11,0.12,0.13]))
+    @test mpc11.weights.Ñ_Hc ≈ diagm([0.1,0.11,0.12,0.13])
+    @test mpc11.weights.Ñ_Hc isa Hermitian{Float64, Matrix{Float64}}
+    mcp12 = ExplicitMPC(model, L_Hp=diagm(collect(0.001:0.001:0.02)))
+    @test mcp12.weights.L_Hp ≈ diagm(collect(0.001:0.001:0.02))
+    @test mcp12.weights.L_Hp isa Hermitian{Float64, Matrix{Float64}}
     model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mpc13  = ExplicitMPC(model2)
     @test isa(mpc13, ExplicitMPC{Float32})
+    @test isa(mpc13.estim, SteadyKalmanFilter{Float32})
+
+    @test_logs(
+        (:warn, 
+        "prediction horizon Hp (0) ≤ estimated number of delays in model (0), the "*
+        "closed-loop system may be unstable or zero-gain (unresponsive)"), 
+        @test_throws ArgumentError ExplicitMPC(model, Hp=0)
+    )
 
-    @test_throws ArgumentError LinMPC(model, Hp=0)
 end
 
 @testitem "ExplicitMPC moves and getinfo" setup=[SetupMPCtests] begin
@@ -576,10 +596,13 @@ end
     @test nmpc3.weights.Ñ_Hc[end] == 1e6
     nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[1,2])
     @test nmpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15)))
+    @test nmpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     nmpc5 = NonLinMPC(nonlinmodel, Hp=15 ,Nwt=[3,4], Cwt=1e3, Hc=5)
     @test nmpc5.weights.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
+    @test nmpc5.weights.Ñ_Hc isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1])
     @test nmpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
+    @test nmpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10)
     @test nmpc7.weights.E == 1e-3
     @test nmpc7.JE([1,2],[3,4],[4,6],10) == 10*dot([1,2],[3,4])+sum([4,6])
@@ -596,12 +619,15 @@ end
     nmpc11 = NonLinMPC(nonlinmodel, Hp=15, nint_u=[1, 1], nint_ym=[0, 0])
     @test nmpc11.estim.nint_u  == [1, 1]
     @test nmpc11.estim.nint_ym == [0, 0]
-    nmpc12 = NonLinMPC(nonlinmodel, Hp=10, M_Hp=Diagonal(collect(1.01:0.01:1.2)))
-    @test nmpc12.weights.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2))
-    nmpc13 = NonLinMPC(nonlinmodel, Hp=10, N_Hc=Diagonal([0.1,0.11,0.12,0.13]), Cwt=Inf)
-    @test nmpc13.weights.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13])
-    nmcp14 = NonLinMPC(nonlinmodel, Hp=10, L_Hp=Diagonal(collect(0.001:0.001:0.02)))
-    @test nmcp14.weights.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02))
+    nmpc12 = NonLinMPC(nonlinmodel, Hp=10, M_Hp=diagm(collect(1.01:0.01:1.2)))
+    @test nmpc12.weights.M_Hp ≈ diagm(collect(1.01:0.01:1.2))
+    @test nmpc12.weights.M_Hp isa Hermitian{Float64, Matrix{Float64}}
+    nmpc13 = NonLinMPC(nonlinmodel, Hp=10, N_Hc=diagm([0.1,0.11,0.12,0.13]), Cwt=Inf)
+    @test nmpc13.weights.Ñ_Hc ≈ diagm([0.1,0.11,0.12,0.13])
+    @test nmpc13.weights.Ñ_Hc isa Hermitian{Float64, Matrix{Float64}}
+    nmcp14 = NonLinMPC(nonlinmodel, Hp=10, L_Hp=diagm(collect(0.001:0.001:0.02)))
+    @test nmcp14.weights.L_Hp ≈ diagm(collect(0.001:0.001:0.02))
+    @test nmcp14.weights.L_Hp isa Hermitian{Float64, Matrix{Float64}}
     nmpc15 = NonLinMPC(nonlinmodel, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10)
     LHS = zeros(1)
     nmpc15.con.gc!(LHS,[1,2],[3,4],[4,6],10,0.1) 
@@ -626,6 +652,7 @@ end
     nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing)
     nmpc15  = NonLinMPC(nonlinmodel2, Hp=15)
     @test isa(nmpc15, NonLinMPC{Float32})
+    @test isa(nmpc15.estim, UnscentedKalmanFilter{Float32})
     @test isa(nmpc15.optim, JuMP.GenericModel{Float64}) # Ipopt does not support Float32
 
     @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1])