Skip to content

Commit c4ed28c

Browse files
franckgagafranckgaga
authored
Merge pull request #133 from JuliaControl/nmpc_improve
added: save some `NonLinMPC` computations with `T_lastu` instead of `T_lastu0` for input conversion test: improve constraint violation tests to catch bugs in corner cases
2 parents e4522fc + c541b82 commit c4ed28c

File tree

7 files changed

+161
-123
lines changed

7 files changed

+161
-123
lines changed

‎Project.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
name = "ModelPredictiveControl"
22
uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
33
authors = ["Francis Gagnon"]
4-
version = "1.1.1"
4+
version = "1.1.2"
55

66
[deps]
77
ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

‎src/controller/construct.jl‎

Lines changed: 33 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -454,7 +454,35 @@ Init manipulated input increments to inputs conversion matrices.
454454
The conversion from the input increments ``\mathbf{ΔU}`` to manipulated inputs over ``H_p``
455455
are calculated by:
456456
```math
457-
\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1) \\
457+
\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1)
458+
```
459+
The ``\mathbf{S}`` and ``\mathbf{T}`` matrices are defined in the Extended Help section.
460+
461+
# Extended Help
462+
!!! details "Extended Help"
463+
The ``\mathbf{U}`` vector and the two conversion matrices are defined as:
464+
```math
465+
\mathbf{U} = \begin{bmatrix}
466+
\mathbf{u}(k + 0) \\
467+
\mathbf{u}(k + 1) \\
468+
\vdots \\
469+
\mathbf{u}(k + H_c - 1) \\
470+
\vdots \\
471+
\mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad
472+
\mathbf{S} = \begin{bmatrix}
473+
\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\
474+
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{0} \\
475+
\vdots & \vdots & \ddots & \vdots \\
476+
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \\
477+
\vdots & \vdots & \ddots & \vdots \\
478+
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \end{bmatrix} , \quad
479+
\mathbf{T} = \begin{bmatrix}
480+
\mathbf{I} \\
481+
\mathbf{I} \\
482+
\vdots \\
483+
\mathbf{I} \\
484+
\vdots \\
485+
\mathbf{I} \end{bmatrix}
458486
```
459487
"""
460488
function init_ΔUtoU(model::SimModel{NT}, Hp, Hc) where {NT<:Real}
@@ -753,12 +781,12 @@ constraints:
753781
\mathbf{A_{U_{max}}}
754782
\end{bmatrix} \mathbf{ΔŨ} ≤
755783
\begin{bmatrix}
756-
- \mathbf{(U_{min} - U_{op}) + T} \mathbf{u_0}(k-1) \\
757-
+ \mathbf{(U_{max} - U_{op}) - T} \mathbf{u_0}(k-1)
784+
- \mathbf{(U_{min}) + T} \mathbf{u}(k-1) \\
785+
+ \mathbf{(U_{max}) - T} \mathbf{u}(k-1)
758786
\end{bmatrix}
759787
```
760-
in which ``\mathbf{U_{min}, U_{max}}`` and ``\mathbf{U_{op}}`` vectors respectively contains
761-
``\mathbf{u_{min}, u_{max}}`` and ``\mathbf{u_{op}}`` repeated ``H_p`` times.
788+
in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively contains
789+
``\mathbf{u_{min}}`` and ``\mathbf{u_{max}}`` repeated ``H_p`` times.
762790
"""
763791
function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real
764792
if nϵ == 1 # ΔŨ = [ΔU; ϵ]

‎src/controller/execute.jl‎

Lines changed: 15 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -179,8 +179,8 @@ They are computed with these equations using in-place operations:
179179
\begin{aligned}
180180
\mathbf{F} &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k)
181181
+ \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s} \\
182-
\mathbf{C_y} &= \mathbf{F} - (\mathbf{R̂_y - Y_{op}}) \\
183-
\mathbf{C_u} &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}}) \\
182+
\mathbf{C_y} &= \mathbf{F} + \mathbf{Y_{op}} - \mathbf{R̂_y} \\
183+
\mathbf{C_u} &= \mathbf{T}\mathbf{u}(k-1) - \mathbf{R̂_u} \\
184184
\mathbf{q̃} &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y}
185185
+ (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\
186186
r &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y}
@@ -189,7 +189,9 @@ They are computed with these equations using in-place operations:
189189
```
190190
"""
191191
function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u)
192-
mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
192+
lastu = mpc.buffer.u
193+
lastu .= mpc.estim.lastu0 .+ model.uop
194+
mul!(mpc.T_lastu, mpc.T, lastu)
193195
ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
194196
Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃
195197
ŷ .= evaloutput(mpc.estim, d)
@@ -210,15 +212,15 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
210212
# --- output setpoint tracking term ---
211213
mpc.R̂y .= R̂y
212214
if !mpc.weights.iszero_M_Hp[]
213-
Cy .= F .- (R̂y .- mpc.Yop)
215+
Cy .= F .+ mpc.Yop .- R̂y
214216
mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ)
215217
mul!(q̃, M_Hp_Ẽ', Cy, 1, 1) # q̃ = q̃ + M_Hp*Ẽ'*Cy
216218
r .+= dot(Cy, mpc.weights.M_Hp, Cy) # r = r + Cy'*M_Hp*Cy
217219
end
218220
# --- input setpoint tracking term ---
219221
mpc.R̂u .= R̂u
220222
if !mpc.weights.iszero_L_Hp[]
221-
Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop)
223+
Cu .= mpc.T_lastu .- R̂u
222224
mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃)
223225
mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu
224226
r .+= dot(Cu, mpc.weights.L_Hp, Cu) # r = r + Cu'*L_Hp*Cu
@@ -234,7 +236,9 @@ end
234236
Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref).
235237
"""
236238
function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
237-
mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
239+
lastu = mpc.buffer.u
240+
lastu .= mpc.estim.lastu0 .+ model.uop
241+
mul!(mpc.T_lastu, mpc.T, lastu)
238242
mpc.ŷ .= evaloutput(mpc.estim, d)
239243
predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel
240244
if model.nd ≠ 0
@@ -281,9 +285,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel)
281285
mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
282286
end
283287
n = 0
284-
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.T_lastu0
288+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu
285289
n += nU
286-
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.T_lastu0
290+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu
287291
n += nU
288292
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
289293
n += nΔŨ
@@ -307,9 +311,9 @@ end
307311
function linconstraint!(mpc::PredictiveController, ::SimModel)
308312
nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin)
309313
n = 0
310-
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.T_lastu0
314+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu
311315
n += nU
312-
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.T_lastu0
316+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu
313317
n += nU
314318
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
315319
n += nΔŨ
@@ -378,7 +382,7 @@ function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
378382
ny, nu = model.ny, model.nu
379383
# --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
380384
U = Ū
381-
U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop
385+
U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu
382386
Ue[1:end-nu] .= U
383387
# u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
384388
Ue[end-nu+1:end] .= @views U[end-nu+1:end]

‎src/controller/explicitmpc.jl‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
1010
R̂y::Vector{NT}
1111
S̃::Matrix{NT}
1212
T::Matrix{NT}
13-
T_lastu0::Vector{NT}
13+
T_lastu::Vector{NT}
1414
Ẽ::Matrix{NT}
1515
F::Vector{NT}
1616
G::Matrix{NT}
@@ -44,7 +44,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
4444
N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
4545
L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
4646
# dummy vals (updated just before optimization):
47-
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
47+
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
4848
S, T = init_ΔUtoU(model, Hp, Hc)
4949
E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc)
5050
# dummy val (updated just before optimization):
@@ -67,7 +67,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
6767
Hp, Hc, nϵ,
6868
weights,
6969
R̂u, R̂y,
70-
S̃, T, T_lastu0,
70+
S̃, T, T_lastu,
7171
Ẽ, F, G, J, K, V, B,
7272
H̃, q̃, r,
7373
H̃_chol,

‎src/controller/linmpc.jl‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ struct LinMPC{
2020
R̂y::Vector{NT}
2121
S̃::Matrix{NT}
2222
T::Matrix{NT}
23-
T_lastu0::Vector{NT}
23+
T_lastu::Vector{NT}
2424
Ẽ::Matrix{NT}
2525
F::Vector{NT}
2626
G::Matrix{NT}
@@ -48,7 +48,7 @@ struct LinMPC{
4848
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
4949
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
5050
# dummy vals (updated just before optimization):
51-
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
51+
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
5252
S, T = init_ΔUtoU(model, Hp, Hc)
5353
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
5454
# dummy vals (updated just before optimization):
@@ -72,7 +72,7 @@ struct LinMPC{
7272
Hp, Hc, nϵ,
7373
weights,
7474
R̂u, R̂y,
75-
S̃, T, T_lastu0,
75+
S̃, T, T_lastu,
7676
Ẽ, F, G, J, K, V, B,
7777
H̃, q̃, r,
7878
Ks, Ps,

‎src/controller/nonlinmpc.jl‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ struct NonLinMPC{
2525
R̂y::Vector{NT}
2626
S̃::Matrix{NT}
2727
T::Matrix{NT}
28-
T_lastu0::Vector{NT}
28+
T_lastu::Vector{NT}
2929
Ẽ::Matrix{NT}
3030
F::Vector{NT}
3131
G::Matrix{NT}
@@ -61,7 +61,7 @@ struct NonLinMPC{
6161
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
6262
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
6363
# dummy vals (updated just before optimization):
64-
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
64+
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
6565
S, T = init_ΔUtoU(model, Hp, Hc)
6666
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
6767
# dummy vals (updated just before optimization):
@@ -87,7 +87,7 @@ struct NonLinMPC{
8787
weights,
8888
JE, p,
8989
R̂u, R̂y,
90-
S̃, T, T_lastu0,
90+
S̃, T, T_lastu,
9191
Ẽ, F, G, J, K, V, B,
9292
H̃, q̃, r,
9393
Ks, Ps,

0 commit comments

Comments
 (0)