Skip to content

Commit 6feb14f

Browse files
franckgagafranckgaga
committed
added : time-varying weights over Hp / Hc 
1 parent 13a7bc1 commit 6feb14f

File tree

4 files changed

+77
-52
lines changed

4 files changed

+77
-52
lines changed

‎src/controller/explicitmpc.jl‎

Lines changed: 16 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -31,17 +31,13 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
3131
D̂E::Vector{Float64}
3232
Ŷop::Vector{Float64}
3333
Dop::Vector{Float64}
34-
function ExplicitMPC{SE}(estim::SE, Hp, Hc, Mwt, Nwt, Lwt) where {SE<:StateEstimator}
34+
function ExplicitMPC{SE}(estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp) where {SE<:StateEstimator}
3535
model = estim.model
3636
nu, ny, nd = model.nu, model.ny, model.nd
3737
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
3838
Cwt = Inf # no slack variable ϵ for ExplicitMPC
3939
Ewt = 0 # economic costs not supported for ExplicitMPC
40-
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt)
41-
M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
42-
N_Hc = Diagonal{Float64}(repeat(Nwt, Hc))
43-
L_Hp = Diagonal{Float64}(repeat(Lwt, Hp))
44-
C = Cwt
40+
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
4541
R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
4642
noR̂u = iszero(L_Hp)
4743
S, T = init_ΔUtoU(nu, Hp, Hc)
@@ -96,6 +92,8 @@ state estimator, a [`SteadyKalmanFilter`](@ref) with default arguments.
9692
- `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
9793
- `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
9894
- `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
95+
- `M_Hp` / `N_Hc` / `L_Hp` : diagonal matrices ``\mathbf{M}_{H_p}, \mathbf{N}_{H_c},
96+
\mathbf{L}_{H_p}``, for time-varying weights (generated by `Mwt`, `Nwt`, `Lwt` if omitted).
9997
- additionnal keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor.
10098
10199
# Examples
@@ -121,10 +119,13 @@ function ExplicitMPC(
121119
Mwt = fill(DEFAULT_MWT, model.ny),
122120
Nwt = fill(DEFAULT_NWT, model.nu),
123121
Lwt = fill(DEFAULT_LWT, model.nu),
122+
M_Hp = nothing,
123+
N_Hc = nothing,
124+
L_Hp = nothing,
124125
kwargs...
125126
)
126127
estim = SteadyKalmanFilter(model; kwargs...)
127-
return ExplicitMPC(estim; Hp, Hc, Mwt, Nwt, Lwt)
128+
return ExplicitMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, M_Hp, N_Hc, L_Hp)
128129
end
129130

130131
"""
@@ -155,11 +156,17 @@ function ExplicitMPC(
155156
Hc::Int = DEFAULT_HC,
156157
Mwt = fill(DEFAULT_MWT, estim.model.ny),
157158
Nwt = fill(DEFAULT_NWT, estim.model.nu),
158-
Lwt = fill(DEFAULT_LWT, estim.model.nu)
159+
Lwt = fill(DEFAULT_LWT, estim.model.nu),
160+
M_Hp = nothing,
161+
N_Hc = nothing,
162+
L_Hp = nothing
159163
) where {SE<:StateEstimator}
160164
isa(estim.model, LinModel) || error("estim.model type must be LinModel")
161165
Hp = default_Hp(estim.model, Hp)
162-
return ExplicitMPC{SE}(estim, Hp, Hc, Mwt, Nwt, Lwt)
166+
isnothing(M_Hp) && (M_Hp = Diagonal(repeat(Mwt, Hp)))
167+
isnothing(N_Hc) && (N_Hc = Diagonal(repeat(Nwt, Hc)))
168+
isnothing(L_Hp) && (L_Hp = Diagonal(repeat(Lwt, Hp)))
169+
return ExplicitMPC{SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
163170
end
164171

165172
setconstraint!(::ExplicitMPC,kwargs...) = error("ExplicitMPC does not support constraints.")

‎src/controller/linmpc.jl‎

Lines changed: 30 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -34,21 +34,17 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
3434
D̂E::Vector{Float64}
3535
Ŷop::Vector{Float64}
3636
Dop::Vector{Float64}
37-
function LinMPC{SE}(estim::SE, Hp, Hc, Mwt, Nwt, Lwt, Cwt, optim) where {SE<:StateEstimator}
37+
function LinMPC{SE}(estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim) where {SE<:StateEstimator}
3838
model = estim.model
3939
nu, ny, nd = model.nu, model.ny, model.nd
4040
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
4141
Ewt = 0 # economic costs not supported for LinMPC
42-
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt)
43-
M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
44-
N_Hc = Diagonal{Float64}(repeat(Nwt, Hc))
45-
L_Hp = Diagonal{Float64}(repeat(Lwt, Hp))
46-
C = Cwt
42+
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
4743
R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
4844
noR̂u = iszero(L_Hp)
4945
S, T = init_ΔUtoU(nu, Hp, Hc)
5046
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
51-
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
47+
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
5248
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5349
Ks, Ps = init_stochpred(estim, Hp)
5450
# dummy vals (updated just before optimization):
@@ -113,6 +109,8 @@ arguments.
113109
- `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in
114110
the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
115111
(default to [`OSQP.jl`](https://osqp.org/docs/parsers/jump.html) optimizer).
112+
- `M_Hp` / `N_Hc` / `L_Hp` : diagonal matrices ``\mathbf{M}_{H_p}, \mathbf{N}_{H_c},
113+
\mathbf{L}_{H_p}``, for time-varying weights (generated by `Mwt`, `Nwt`, `Lwt` if omitted).
116114
- additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor.
117115
118116
# Examples
@@ -137,20 +135,20 @@ costs). The default `Lwt` value implies that this feature is disabled by default
137135
138136
The objective function follows this nomenclature:
139137
140-
| VARIABLE | DESCRIPTION | SIZE |
141-
| :--------------- | :------------------------------------------------- | :--------------- |
142-
| ``H_p`` | prediction horizon (integer) | `()` |
143-
| ``H_c`` | control horizon (integer) | `()` |
144-
| ``\mathbf{ΔU}`` | manipulated input increments over ``H_c`` | `(nu*Hc,)` |
145-
| ``\mathbf{Ŷ}`` | predicted outputs over ``H_p`` | `(ny*Hp,)` |
146-
| ``\mathbf{U}`` | manipulated inputs over ``H_p`` | `(nu*Hp,)` |
147-
| ``\mathbf{R̂_y}`` | predicted output setpoints over ``H_p`` | `(ny*Hp,)` |
148-
| ``\mathbf{R̂_u}`` | predicted manipulated input setpoints over ``H_p`` | `(nu*Hp,)` |
149-
| ``\mathbf{M}`` | output setpoint tracking weights | `(ny*Hp, ny*Hp)` |
150-
| ``\mathbf{N}`` | manipulated input increment weights | `(nu*Hc, nu*Hc)` |
151-
| ``\mathbf{L}`` | manipulated input setpoint tracking weights | `(nu*Hp, nu*Hp)` |
152-
| ``C`` | slack variable weight | `()` |
153-
| ``ϵ`` | slack variable for constraint softening | `()` |
138+
| VARIABLE | DESCRIPTION | SIZE |
139+
| :------------------- | :------------------------------------------------------- | :--------------- |
140+
| ``H_p`` | prediction horizon (integer) | `()` |
141+
| ``H_c`` | control horizon (integer) | `()` |
142+
| ``\mathbf{ΔU}`` | manipulated input increments over ``H_c`` | `(nu*Hc,)` |
143+
| ``\mathbf{Ŷ}`` | predicted outputs over ``H_p`` | `(ny*Hp,)` |
144+
| ``\mathbf{U}`` | manipulated inputs over ``H_p`` | `(nu*Hp,)` |
145+
| ``\mathbf{R̂_y}`` | predicted output setpoints over ``H_p`` | `(ny*Hp,)` |
146+
| ``\mathbf{R̂_u}`` | predicted manipulated input setpoints over ``H_p`` | `(nu*Hp,)` |
147+
| ``\mathbf{M_{H_p}}`` | output setpoint tracking weights over ``H_p`` | `(ny*Hp, ny*Hp)` |
148+
| ``\mathbf{N_{H_c}}`` | manipulated input increment weights over ``H_c`` | `(nu*Hc, nu*Hc)` |
149+
| ``\mathbf{L_{H_p}}`` | manipulated input setpoint tracking weights over ``H_p`` | `(nu*Hp, nu*Hp)` |
150+
| ``C`` | slack variable weight | `()` |
151+
| ``ϵ`` | slack variable for constraint softening | `()` |
154152
"""
155153
function LinMPC(
156154
model::LinModel;
@@ -160,11 +158,14 @@ function LinMPC(
160158
Nwt = fill(DEFAULT_NWT, model.nu),
161159
Lwt = fill(DEFAULT_LWT, model.nu),
162160
Cwt = DEFAULT_CWT,
161+
M_Hp = nothing,
162+
N_Hc = nothing,
163+
L_Hp = nothing,
163164
optim::JuMP.Model = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
164165
kwargs...
165166
)
166167
estim = SteadyKalmanFilter(model; kwargs...)
167-
return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, optim)
168+
return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, optim)
168169
end
169170

170171

@@ -198,11 +199,17 @@ function LinMPC(
198199
Nwt = fill(DEFAULT_NWT, estim.model.nu),
199200
Lwt = fill(DEFAULT_LWT, estim.model.nu),
200201
Cwt = DEFAULT_CWT,
202+
M_Hp = nothing,
203+
N_Hc = nothing,
204+
L_Hp = nothing,
201205
optim::JuMP.Model = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
202206
) where {SE<:StateEstimator}
203207
isa(estim.model, LinModel) || error("estim.model type must be LinModel")
204208
Hp = default_Hp(estim.model, Hp)
205-
return LinMPC{SE}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, optim)
209+
isnothing(M_Hp) && (M_Hp = Diagonal(repeat(Mwt, Hp)))
210+
isnothing(N_Hc) && (N_Hc = Diagonal(repeat(Nwt, Hc)))
211+
isnothing(L_Hp) && (L_Hp = Diagonal(repeat(Lwt, Hp)))
212+
return LinMPC{SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim)
206213
end
207214

208215
"""

‎src/controller/nonlinmpc.jl‎

Lines changed: 20 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -38,21 +38,17 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
3838
Ŷop::Vector{Float64}
3939
Dop::Vector{Float64}
4040
function NonLinMPC{SE, JEFunc}(
41-
estim::SE, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE::JEFunc, optim
41+
estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, optim
4242
) where {SE<:StateEstimator, JEFunc<:Function}
4343
model = estim.model
4444
nu, ny, nd = model.nu, model.ny, model.nd
4545
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
46-
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt)
47-
M_Hp = Diagonal(convert(Vector{Float64}, repeat(Mwt, Hp)))
48-
N_Hc = Diagonal(convert(Vector{Float64}, repeat(Nwt, Hc)))
49-
L_Hp = Diagonal(convert(Vector{Float64}, repeat(Lwt, Hp)))
50-
C = Cwt
46+
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
5147
R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
5248
noR̂u = iszero(L_Hp)
5349
S, T = init_ΔUtoU(nu, Hp, Hc)
5450
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
55-
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
51+
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
5652
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5753
Ks, Ps = init_stochpred(estim, Hp)
5854
# dummy vals (updated just before optimization):
@@ -126,6 +122,8 @@ This method uses the default state estimator :
126122
- `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
127123
- `Ewt=0.0` : economic costs weight ``E`` (scalar).
128124
- `JE=(_,_,_)->0.0` : economic function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E)``.
125+
- `M_Hp` / `N_Hc` / `L_Hp` : diagonal matrices ``\mathbf{M}_{H_p}, \mathbf{N}_{H_c},
126+
\mathbf{L}_{H_p}``, for time-varying weights (generated by `Mwt`, `Nwt`, `Lwt` if omitted).
129127
- `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive
130128
controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
131129
(default to [`Ipopt.jl`](https://github.com/jump-dev/Ipopt.jl) optimizer).
@@ -168,11 +166,14 @@ function NonLinMPC(
168166
Cwt = DEFAULT_CWT,
169167
Ewt = DEFAULT_EWT,
170168
JE::Function = (_,_,_) -> 0.0,
169+
M_Hp = nothing,
170+
N_Hc = nothing,
171+
L_Hp = nothing,
171172
optim::JuMP.Model = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
172173
kwargs...
173174
)
174175
estim = UnscentedKalmanFilter(model; kwargs...)
175-
NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
176+
NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, M_Hp, N_Hc, L_Hp, optim)
176177
end
177178

178179
function NonLinMPC(
@@ -185,11 +186,14 @@ function NonLinMPC(
185186
Cwt = DEFAULT_CWT,
186187
Ewt = DEFAULT_EWT,
187188
JE::Function = (_,_,_) -> 0.0,
189+
M_Hp = nothing,
190+
N_Hc = nothing,
191+
L_Hp = nothing,
188192
optim::JuMP.Model = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
189193
kwargs...
190194
)
191195
estim = SteadyKalmanFilter(model; kwargs...)
192-
NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
196+
NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, M_Hp, N_Hc, L_Hp, optim)
193197
end
194198

195199

@@ -225,10 +229,16 @@ function NonLinMPC(
225229
Cwt = DEFAULT_CWT,
226230
Ewt = DEFAULT_EWT,
227231
JE::JEFunc = (_,_,_) -> 0.0,
232+
M_Hp = nothing,
233+
N_Hc = nothing,
234+
L_Hp = nothing,
228235
optim::JuMP.Model = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
229236
) where {SE<:StateEstimator, JEFunc<:Function}
230237
Hp = default_Hp(estim.model, Hp)
231-
return NonLinMPC{SE, JEFunc}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
238+
isnothing(M_Hp) && (M_Hp = Diagonal(repeat(Mwt, Hp)))
239+
isnothing(N_Hc) && (N_Hc = Diagonal(repeat(Nwt, Hc)))
240+
isnothing(L_Hp) && (L_Hp = Diagonal(repeat(Lwt, Hp)))
241+
return NonLinMPC{SE, JEFunc}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, optim)
232242
end
233243

234244
"""

‎src/predictive_control.jl‎

Lines changed: 11 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1300,20 +1300,21 @@ function init_matconstraint(::SimModel,
13001300
end
13011301

13021302
"Validate predictive controller weight and horizon specified values."
1303-
function validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt=nothing)
1303+
function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C, E=nothing)
13041304
nu, ny = model.nu, model.ny
1305+
nM, nN, nL = ny*Hp, nu*Hc, nu*Hp
13051306
Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1"))
13061307
Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1"))
13071308
Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp"))
1308-
size(Mwt) ≠ (ny,) && throw(ArgumentError("Mwt size $(size(Mwt)) ≠ output size ($ny,)"))
1309-
size(Nwt) ≠ (nu,) && throw(ArgumentError("Nwt size $(size(Nwt)) ≠ manipulated input size ($nu,)"))
1310-
size(Lwt) ≠ (nu,) && throw(ArgumentError("Lwt size $(size(Lwt)) ≠ manipulated input size ($nu,)"))
1311-
size(Cwt) ≠ () && throw(ArgumentError("Cwt should be a real scalar"))
1312-
any(Mwt.<0) && throw(ArgumentError("Mwt weights should be ≥ 0"))
1313-
any(Nwt.<0) && throw(ArgumentError("Nwt weights should be ≥ 0"))
1314-
any(Lwt.<0) && throw(ArgumentError("Lwt weights should be ≥ 0"))
1315-
Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0"))
1316-
!isnothing(Ewt) && size(Ewt) ≠ () && throw(ArgumentError("Ewt should be a real scalar"))
1309+
size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)"))
1310+
size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)"))
1311+
size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)"))
1312+
(!isdiag(M_Hp) || any(diag(M_Hp).<0)) && throw(ArgumentError("M_Hp should be a positive semidefinite diagonal matrix"))
1313+
(!isdiag(N_Hc) || any(diag(N_Hc).<0)) && throw(ArgumentError("N_Hc should be a positive semidefinite diagonal matrix"))
1314+
(!isdiag(L_Hp) || any(diag(L_Hp).<0)) && throw(ArgumentError("L_Hp should be a positive semidefinite diagonal matrix"))
1315+
size(C) ≠ () && throw(ArgumentError("Cwt should be a real scalar"))
1316+
C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0"))
1317+
!isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar"))
13171318
end
13181319

13191320
function Base.show(io::IO, mpc::PredictiveController)

0 commit comments

Comments
 (0)