Skip to content

Commit c8b662f

Browse files
franckgagafranckgaga
committed
reduce allocations for NonLinMPC 
1 parent c69c6e3 commit c8b662f

File tree

4 files changed

+27
-27
lines changed

4 files changed

+27
-27
lines changed

‎src/controller/explicitmpc.jl‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,8 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
2626
P̃_chol::Cholesky{Float64, Matrix{Float64}}
2727
Ks::Matrix{Float64}
2828
Ps::Matrix{Float64}
29-
d::Vector{Float64}
30-
D̂::Vector{Float64}
29+
d0::Vector{Float64}
30+
D̂0::Vector{Float64}
3131
Ŷop::Vector{Float64}
3232
Dop::Vector{Float64}
3333
function ExplicitMPC{SE}(estim::SE, Hp, Hc, Mwt, Nwt, Lwt) where {SE<:StateEstimator}
@@ -49,7 +49,7 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
4949
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5050
P̃_chol = cholesky(P̃)
5151
Ks, Ps = init_stochpred(estim, Hp)
52-
d, D̂ = zeros(nd), zeros(nd*Hp)
52+
d0, D̂0 = zeros(nd), zeros(nd*Hp)
5353
Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
5454
nvar = size(Ẽ, 2)
5555
ΔŨ = zeros(nvar)
@@ -62,7 +62,7 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
6262
Ẽ, F, G, J, K, Q, P̃, q̃, p,
6363
P̃_chol,
6464
Ks, Ps,
65-
d, D̂,
65+
d0, D̂0,
6666
Ŷop, Dop,
6767
)
6868
return mpc

‎src/controller/linmpc.jl‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,8 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
2727
p::Vector{Float64}
2828
Ks::Matrix{Float64}
2929
Ps::Matrix{Float64}
30-
d::Vector{Float64}
31-
D̂::Vector{Float64}
30+
d0::Vector{Float64}
31+
D̂0::Vector{Float64}
3232
Ŷop::Vector{Float64}
3333
Dop::Vector{Float64}
3434
function LinMPC{SE}(estim::SE, Hp, Hc, Mwt, Nwt, Lwt, Cwt, optim) where {SE<:StateEstimator}
@@ -48,7 +48,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
4848
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S, N_Hc, E)
4949
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5050
Ks, Ps = init_stochpred(estim, Hp)
51-
d, D̂ = zeros(nd), zeros(nd*Hp)
51+
d0, D̂0 = zeros(nd), zeros(nd*Hp)
5252
Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
5353
nvar = size(Ẽ, 2)
5454
ΔŨ = zeros(nvar)
@@ -60,7 +60,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
6060
S̃, T,
6161
Ẽ, F, G, J, K, Q, P̃, q̃, p,
6262
Ks, Ps,
63-
d, D̂,
63+
d0, D̂0,
6464
Ŷop, Dop,
6565
)
6666
init_optimization!(mpc)

‎src/controller/nonlinmpc.jl‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
3030
p::Vector{Float64}
3131
Ks::Matrix{Float64}
3232
Ps::Matrix{Float64}
33-
d::Vector{Float64}
34-
D̂::Vector{Float64}
33+
d0::Vector{Float64}
34+
D̂0::Vector{Float64}
3535
Ŷop::Vector{Float64}
3636
Dop::Vector{Float64}
3737
function NonLinMPC{SE, JEFunc}(
@@ -52,7 +52,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
5252
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S, N_Hc, E)
5353
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5454
Ks, Ps = init_stochpred(estim, Hp)
55-
d, D̂ = zeros(nd), zeros(nd*Hp)
55+
d0, D̂0 = zeros(nd), zeros(nd*Hp)
5656
Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
5757
nvar = size(Ẽ, 2)
5858
ΔŨ = zeros(nvar)
@@ -64,7 +64,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
6464
S̃, T,
6565
Ẽ, F, G, J, K, Q, P̃, q̃, p,
6666
Ks, Ps,
67-
d, D̂,
67+
d0, D̂0,
6868
Ŷop, Dop,
6969
)
7070
init_optimization!(mpc)
@@ -230,10 +230,10 @@ end
230230
For [`NonLinMPC`](@ref), add `:sol` and the optimal economic cost `:JE`.
231231
"""
232232
function addinfo!(info, mpc::NonLinMPC)
233-
U, Ŷ, D̂ = info[:U], info[:Ŷ], info[:D̂]
233+
U, Ŷ, D̂, ŷ, d = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d]
234234
UE = [U; U[(end - mpc.estim.model.nu + 1):end]]
235-
ŶE = [mpc.ŷ; Ŷ]
236-
D̂E = [mpc.d; D̂]
235+
ŶE = [ŷ; Ŷ]
236+
D̂E = [d; D̂]
237237
info[:JE] = mpc.JE(UE, ŶE, D̂E)
238238
info[:sol] = solution_summary(mpc.optim, verbose=true)
239239
return info

‎src/predictive_control.jl‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -380,8 +380,8 @@ function getinfo(mpc::PredictiveController)
380380
info[:J] = obj_nonlinprog(mpc, mpc.estim.model, Ŷ, mpc.ΔŨ) + mpc.p[]
381381
info[:U] = mpc.S̃*mpc.ΔŨ + mpc.T*(mpc.estim.lastu0 + mpc.estim.model.uop)
382382
info[:u] = info[:U][1:mpc.estim.model.nu]
383-
info[:d] = mpc.d
384-
info[:D̂] = mpc.D̂
383+
info[:d] = mpc.d0 + mpc.estim.model.dop
384+
info[:D̂] = mpc.D̂0 + mpc.Dop
385385
info[:ŷ] = mpc.ŷ
386386
info[:Ŷ] = Ŷ
387387
info[:Ŷs] = mpc.Ŷop - repeat(mpc.estim.model.yop, mpc.Hp) # Ŷop = Ŷs + Yop
@@ -448,8 +448,8 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
448448
predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
449449
mpc.F[:] = mpc.K*mpc.estim.x̂ + mpc.Q*mpc.estim.lastu0 + mpc.Ŷop
450450
if model.nd ≠ 0
451-
mpc.d[:], mpc.D̂[:] = d, D̂
452-
mpc.F[:] = mpc.F + mpc.G*(mpc.d - model.dop) + mpc.J*(mpc.D̂ - mpc.Dop)
451+
mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
452+
mpc.F[:] = mpc.F + mpc.G*mpc.d0 + mpc.J*mpc.D̂0
453453
end
454454
mpc.R̂y[:], mpc.R̂u[:] = R̂y, R̂u
455455
Ẑ = mpc.F - R̂y
@@ -458,8 +458,8 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
458458
if ~mpc.noR̂u
459459
lastu = mpc.estim.lastu0 + model.uop
460460
V̂ = mpc.T*lastu - mpc.R̂u
461-
mpc.q̃[:] += 2(mpc.L_Hp*mpc.S̃)'*V̂
462-
mpc.p[] += V̂'*mpc.L_Hp*V̂
461+
mpc.q̃[:] = mpc.q̃ + 2(mpc.L_Hp*mpc.S̃)'*V̂
462+
mpc.p[] = mpc.p[] + V̂'*mpc.L_Hp*V̂
463463
end
464464
return nothing
465465
end
@@ -476,7 +476,7 @@ Init `Ŷop`, `d0` and `D̂0` matrices when model is not a [`LinModel`](@ref).
476476
function initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y, R̂u)
477477
predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
478478
if model.nd ≠ 0
479-
mpc.d[:], mpc.D̂[:] = d, D̂
479+
mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
480480
end
481481
mpc.R̂y[:], mpc.R̂u[:] = R̂y, R̂u
482482
return nothing
@@ -522,13 +522,13 @@ function predict!(Ŷ, mpc::PredictiveController, model::SimModel, ΔŨ::Vector
522522
nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
523523
x̂ ::Vector{T} = copy(mpc.estim.x̂)
524524
u0::Vector{T} = copy(mpc.estim.lastu0)
525-
d0 = mpc.d - model.dop
525+
d0::Vector{T} = copy(mpc.d0)
526526
for j=1:Hp
527527
if j ≤ Hc
528-
u0[:] = @views ΔŨ[(1 + nu*(j-1)):(nu*j)] + u0
528+
u0[:] = u0 + ΔŨ[(1 + nu*(j-1)):(nu*j)]
529529
end
530530
x̂[:] = f̂(mpc.estim, x̂, u0, d0)
531-
d0[:] = @views mpc.D̂[(1 + nd*(j-1)):(nd*j)] - model.dop
531+
d0[:] = mpc.D̂0[(1 + nd*(j-1)):(nd*j)]
532532
Ŷ[(1 + ny*(j-1)):(ny*j)] = ĥ(mpc.estim, x̂, d0)
533533
end
534534
Ŷ[:] = Ŷ + mpc.Ŷop # Ŷop = Ŷs + Yop, and Ŷs=0 if mpc.estim is not an InternalModel
@@ -789,7 +789,7 @@ function obj_nonlinprog(
789789
U = mpc.S̃*ΔŨ + mpc.T*(mpc.estim.lastu0 + model.uop)
790790
UE = [U; U[(end - model.nu + 1):end]]
791791
ŶE = [mpc.ŷ; Ŷ]
792-
D̂E = [mpc.d; mpc.D̂]
792+
D̂E = [mpc.d0 + model.dop; mpc.D̂0 + mpc.Dop]
793793
J += mpc.E*mpc.JE(UE, ŶE, D̂E)
794794
end
795795
return J
@@ -823,7 +823,7 @@ function obj_nonlinprog(
823823
if !iszero(mpc.E)
824824
UE = [U; U[(end - model.nu + 1):end]]
825825
ŶE = [mpc.ŷ; Ŷ]
826-
D̂E = [mpc.d; mpc.D̂]
826+
D̂E = [mpc.d0 + model.dop; mpc.D̂0 + mpc.Dop]
827827
E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
828828
else
829829
E_JE = 0.0

0 commit comments

Comments
 (0)