Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 8 commits into from
May 12, 2025
2 changes: 1 addition & 1 deletion Project.toml
Original file line number Diff line number Diff line change
@@ -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"
65 changes: 46 additions & 19 deletions src/controller/construct.jl
Original file line number Diff line number Diff line change
@@ -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̂
= isinf(C) ? 0 : 1
= 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)
6 changes: 3 additions & 3 deletions src/controller/execute.jl
Original file line number Diff line number Diff line change
@@ -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 ---
36 changes: 18 additions & 18 deletions src/controller/explicitmpc.jl
Original file line number Diff line number Diff line change
@@ -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
::Vector{NT}
::Vector{NT}
Hp::Int
Hc::Int
::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)
= 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
= 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) +
= 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.")
42 changes: 25 additions & 17 deletions src/controller/linmpc.jl
Original file line number Diff line number Diff line change
@@ -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
::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) +
= 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

"""
80 changes: 27 additions & 53 deletions src/controller/nonlinmpc.jl
Original file line number Diff line number Diff line change
@@ -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
::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) +
= 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

10 changes: 8 additions & 2 deletions src/controller/transcription.jl
Original file line number Diff line number Diff line change
@@ -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.== 1 && (ΔŨ[end] = Z̃[end])
return ΔŨ
end
getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_lastu0)

@doc raw"""
65 changes: 46 additions & 19 deletions test/3_test_predictive_control.jl
Original file line number Diff line number Diff line change
@@ -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])