Skip to content

Commit 4dbd354

Browse files
committed
changed: removed noR̂u field in all PredictiveController
- it's dangerous with `setmodel!` (if `L_Hp` change, `noR̂u` can be incorrect - the code is simpler (no branching) - computational gains are negligible
1 parent ecb2748 commit 4dbd354

File tree

5 files changed

+16
-29
lines changed

5 files changed

+16
-29
lines changed

src/controller/construct.jl

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -434,9 +434,7 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u)
434434
size(d) (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)"))
435435
size(D̂) (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)"))
436436
size(R̂y) (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)"))
437-
if ~mpc.noR̂u
438-
size(R̂u) (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)"))
439-
end
437+
size(R̂u) (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)"))
440438
end
441439

442440

src/controller/execute.jl

Lines changed: 12 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -204,18 +204,19 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
204204
mul!(F, mpc.G, mpc.d0, 1, 1)
205205
mul!(F, mpc.J, mpc.D̂0, 1, 1)
206206
end
207+
# --- output setpoint tracking term ---
207208
mpc.R̂y .= R̂y
208209
Cy = F .- (R̂y .- mpc.Yop)
209210
M_Hp_Ẽ = mpc.weights.M_Hp*mpc.
210211
mul!(q̃, M_Hp_Ẽ', Cy)
211212
r .= dot(Cy, mpc.weights.M_Hp, Cy)
212-
if ~mpc.noR̂u
213-
mpc.R̂u .= R̂u
214-
Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop)
215-
L_Hp_S̃ = mpc.weights.L_Hp*mpc.
216-
mul!(q̃, L_Hp_S̃', Cu, 1, 1)
217-
r .+= dot(Cu, mpc.weights.L_Hp, Cu)
218-
end
213+
# --- input setpoint tracking term ---
214+
mpc.R̂u .= R̂u
215+
Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop)
216+
L_Hp_S̃ = mpc.weights.L_Hp*mpc.
217+
mul!(q̃, L_Hp_S̃', Cu, 1, 1)
218+
r .+= dot(Cu, mpc.weights.L_Hp, Cu)
219+
# --- finalize ---
219220
lmul!(2, q̃)
220221
return nothing
221222
end
@@ -236,9 +237,7 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂
236237
mpc.D̂e[model.nd+1:end] .=
237238
end
238239
mpc.R̂y .= R̂y
239-
if ~mpc.noR̂u
240-
mpc.R̂u .= R̂u
241-
end
240+
mpc.R̂u .= R̂u
242241
return nothing
243242
end
244243

@@ -418,13 +417,9 @@ function obj_nonlinprog!(
418417
# --- move suppression and slack variable term ---
419418
JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ)
420419
# --- input setpoint tracking term ---
421-
if !mpc.noR̂u
422-
Ū .= @views Ue[1:end-nu]
423-
Ū .= mpc.R̂u .-
424-
JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
425-
else
426-
JR̂u = 0.0
427-
end
420+
Ū .= @views Ue[1:end-nu]
421+
Ū .= mpc.R̂u .-
422+
JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
428423
# --- economic term ---
429424
E_JE = obj_econ(mpc, model, Ue, Ŷe)
430425
return JR̂y + JΔŨ + JR̂u + E_JE

src/controller/explicitmpc.jl

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
88
weights::ControllerWeights{NT}
99
R̂u::Vector{NT}
1010
R̂y::Vector{NT}
11-
noR̂u::Bool
1211
::Matrix{NT}
1312
T::Matrix{NT}
1413
T_lastu0::Vector{NT}
@@ -46,7 +45,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
4645
L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
4746
# dummy vals (updated just before optimization):
4847
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
49-
noR̂u = iszero(L_Hp)
5048
S, T = init_ΔUtoU(model, Hp, Hc)
5149
E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc)
5250
# dummy val (updated just before optimization):
@@ -68,7 +66,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
6866
ΔŨ, ŷ,
6967
Hp, Hc, nϵ,
7068
weights,
71-
R̂u, R̂y, noR̂u,
69+
R̂u, R̂y,
7270
S̃, T, T_lastu0,
7371
Ẽ, F, G, J, K, V, B,
7472
H̃, q̃, r,

src/controller/linmpc.jl

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@ struct LinMPC{
1818
weights::ControllerWeights{NT}
1919
R̂u::Vector{NT}
2020
R̂y::Vector{NT}
21-
noR̂u::Bool
2221
::Matrix{NT}
2322
T::Matrix{NT}
2423
T_lastu0::Vector{NT}
@@ -50,7 +49,6 @@ struct LinMPC{
5049
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
5150
# dummy vals (updated just before optimization):
5251
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
53-
noR̂u = iszero(L_Hp)
5452
S, T = init_ΔUtoU(model, Hp, Hc)
5553
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
5654
# dummy vals (updated just before optimization):
@@ -73,7 +71,7 @@ struct LinMPC{
7371
ΔŨ, ŷ,
7472
Hp, Hc, nϵ,
7573
weights,
76-
R̂u, R̂y, noR̂u,
74+
R̂u, R̂y,
7775
S̃, T, T_lastu0,
7876
Ẽ, F, G, J, K, V, B,
7977
H̃, q̃, r,

src/controller/nonlinmpc.jl

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@ struct NonLinMPC{
2323
p::P
2424
R̂u::Vector{NT}
2525
R̂y::Vector{NT}
26-
noR̂u::Bool
2726
::Matrix{NT}
2827
T::Matrix{NT}
2928
T_lastu0::Vector{NT}
@@ -63,7 +62,6 @@ struct NonLinMPC{
6362
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
6463
# dummy vals (updated just before optimization):
6564
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
66-
noR̂u = iszero(L_Hp)
6765
S, T = init_ΔUtoU(model, Hp, Hc)
6866
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
6967
# dummy vals (updated just before optimization):
@@ -88,7 +86,7 @@ struct NonLinMPC{
8886
Hp, Hc, nϵ,
8987
weights,
9088
JE, p,
91-
R̂u, R̂y, noR̂u,
89+
R̂u, R̂y,
9290
S̃, T, T_lastu0,
9391
Ẽ, F, G, J, K, V, B,
9492
H̃, q̃, r,

0 commit comments

Comments
 (0)