Skip to content

Commit ccecf56

Browse files
franckgagafranckgaga
committed
changed: store T_lastu0 instead of T_lastu
to save some computation during `NonLinMPC` optimization
1 parent f2a50ea commit ccecf56

File tree

5 files changed

+17
-23
lines changed

5 files changed

+17
-23
lines changed

‎src/controller/execute.jl

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
129129
ΔŨ, Ŷe, Ue = nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc, Ŷ0, Z̃)
130130
J = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
131131
U, Ŷ = Ū, Ȳ
132-
U .= mul!(U, mpc.P̃u, Z̃) .+ mpc.Tu_lastu
132+
U .= getU0!(U0, mpc, Z̃) .+ mpc.Uop
133133
Ŷ .= Ŷ0 .+ mpc.Yop
134134
predictstoch!(Ŷs, mpc, mpc.estim)
135135
info[:ΔU] = Z̃[1:mpc.Hc*model.nu]
@@ -211,7 +211,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
211211
end
212212
# --- input setpoint tracking term ---
213213
if !mpc.weights.iszero_L_Hp[]
214-
Cu .= mpc.Tu_lastu .- R̂u
214+
Cu .= mpc.Tu_lastu0 .+ mpc.Uop .- R̂u
215215
mul!(L_Hp_P̃U, mpc.weights.L_Hp, mpc.P̃u)
216216
mul!(q̃, L_Hp_P̃U', Cu, 1, 1) # q̃ = q̃ + L_Hp*P̃u'*Cu
217217
r .+= dot(Cu, mpc.weights.L_Hp, Cu) # r = r + Cu'*L_Hp*Cu
@@ -240,9 +240,7 @@ Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs`
240240
is an [`InternalModel`](@ref). The function returns `mpc.F`.
241241
"""
242242
function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
243-
lastu = mpc.buffer.u
244-
lastu .= mpc.estim.lastu0 .+ model.uop
245-
mul!(mpc.Tu_lastu, mpc.Tu, lastu)
243+
mul!(mpc.Tu_lastu0, mpc.Tu, mpc.estim.lastu0)
246244
mpc.ŷ .= evaloutput(mpc.estim, d)
247245
if model.nd ≠ 0
248246
mpc.d0 .= d .- model.dop
@@ -289,9 +287,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel)
289287
mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
290288
end
291289
n = 0
292-
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.Tu_lastu
290+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
293291
n += nU
294-
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.Tu_lastu
292+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
295293
n += nU
296294
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
297295
n += nΔŨ
@@ -315,9 +313,9 @@ end
315313
function linconstraint!(mpc::PredictiveController, ::SimModel)
316314
nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin)
317315
n = 0
318-
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.Tu_lastu
316+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
319317
n += nU
320-
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.Tu_lastu
318+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
321319
n += nU
322320
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
323321
n += nΔŨ

‎src/controller/explicitmpc.jl

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
1212
P̃Δu::Matrix{NT}
1313
P̃u ::Matrix{NT}
1414
Tu ::Matrix{NT}
15-
Tu_lastu::Vector{NT}
15+
Tu_lastu0::Vector{NT}
1616
Ẽ::Matrix{NT}
1717
F::Vector{NT}
1818
G::Matrix{NT}
@@ -46,7 +46,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
4646
N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
4747
L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
4848
# dummy vals (updated just before optimization):
49-
R̂y, R̂u, Tu_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
49+
R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
5050
transcription = SingleShooting() # explicit MPC only supports SingleShooting
5151
PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
5252
Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
@@ -72,7 +72,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
7272
Hp, Hc, nϵ,
7373
weights,
7474
R̂u, R̂y,
75-
P̃Δu, P̃u, Tu, Tu_lastu,
75+
P̃Δu, P̃u, Tu, Tu_lastu0,
7676
Ẽ, F, G, J, K, V, B,
7777
H̃, q̃, r,
7878
H̃_chol,

‎src/controller/linmpc.jl

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ struct LinMPC{
2424
P̃Δu::Matrix{NT}
2525
P̃u ::Matrix{NT}
2626
Tu ::Matrix{NT}
27-
Tu_lastu::Vector{NT}
27+
Tu_lastu0::Vector{NT}
2828
Ẽ::Matrix{NT}
2929
F::Vector{NT}
3030
G::Matrix{NT}
@@ -53,7 +53,7 @@ struct LinMPC{
5353
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
5454
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
5555
# dummy vals (updated just before optimization):
56-
R̂y, R̂u, Tu_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
56+
R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
5757
PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
5858
Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
5959
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
@@ -84,7 +84,7 @@ struct LinMPC{
8484
Hp, Hc, nϵ,
8585
weights,
8686
R̂u, R̂y,
87-
P̃Δu, P̃u, Tu, Tu_lastu,
87+
P̃Δu, P̃u, Tu, Tu_lastu0,
8888
Ẽ, F, G, J, K, V, B,
8989
H̃, q̃, r,
9090
Ks, Ps,

‎src/controller/nonlinmpc.jl

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ struct NonLinMPC{
2929
P̃Δu::Matrix{NT}
3030
P̃u ::Matrix{NT}
3131
Tu ::Matrix{NT}
32-
Tu_lastu::Vector{NT}
32+
Tu_lastu0::Vector{NT}
3333
Ẽ::Matrix{NT}
3434
F::Vector{NT}
3535
G::Matrix{NT}
@@ -67,7 +67,7 @@ struct NonLinMPC{
6767
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
6868
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
6969
# dummy vals (updated just before optimization):
70-
R̂y, R̂u, Tu_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
70+
R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
7171
PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
7272
Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
7373
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
@@ -101,7 +101,7 @@ struct NonLinMPC{
101101
weights,
102102
JE, p,
103103
R̂u, R̂y,
104-
P̃Δu, P̃u, Tu, Tu_lastu,
104+
P̃Δu, P̃u, Tu, Tu_lastu0,
105105
Ẽ, F, G, J, K, V, B,
106106
H̃, q̃, r,
107107
Ks, Ps,

‎src/controller/transcription.jl

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -607,11 +607,7 @@ end
607607

608608
getΔŨ!(ΔŨ, mpc::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃) # since mpc.P̃Δu = I
609609
getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃) = mul!(ΔŨ, mpc.P̃Δu, Z̃)
610-
function getU0!(U0, mpc::PredictiveController, Z̃)
611-
#TODO: modify Tu_lastu to Tu_lastu0 to save some computations
612-
U0 .= mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_lastu .- mpc.Uop
613-
return U0
614-
end
610+
getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_lastu0)
615611

616612
@doc raw"""
617613
predict!(

0 commit comments

Comments
 (0)