Skip to content

Commit a44bc93

Browse files
franckgagafranckgaga
committed
debug : StateEstimators with nint_u option now works in PredictiveControllers
1 parent 9f3f3aa commit a44bc93

File tree

8 files changed

+141
-127
lines changed

8 files changed

+141
-127
lines changed

‎src/controller/explicitmpc.jl‎

Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,6 @@
11
struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
22
estim::S
33
ΔŨ::Vector{Float64}
4-
x̂d::Vector{Float64}
5-
x̂s::Vector{Float64}
64
ŷ ::Vector{Float64}
75
Ŷs::Vector{Float64}
86
Hp::Int
@@ -17,15 +15,15 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
1715
S̃_Hp::Matrix{Bool}
1816
T_Hp::Matrix{Bool}
1917
T_Hc::Matrix{Bool}
20-
Ẽ ::Matrix{Float64}
21-
F ::Vector{Float64}
22-
G ::Matrix{Float64}
23-
J ::Matrix{Float64}
24-
Kd::Matrix{Float64}
25-
Q ::Matrix{Float64}
26-
P̃ ::Hermitian{Float64, Matrix{Float64}}
27-
q̃ ::Vector{Float64}
28-
p ::Vector{Float64}
18+
Ẽ::Matrix{Float64}
19+
F::Vector{Float64}
20+
G::Matrix{Float64}
21+
J::Matrix{Float64}
22+
K::Matrix{Float64}
23+
Q::Matrix{Float64}
24+
P̃::Hermitian{Float64, Matrix{Float64}}
25+
q̃::Vector{Float64}
26+
p::Vector{Float64}
2927
Ks::Matrix{Float64}
3028
Ps::Matrix{Float64}
3129
d::Vector{Float64}
@@ -34,8 +32,8 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
3432
Dop::Vector{Float64}
3533
function ExplicitMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, ru) where {S<:StateEstimator}
3634
model = estim.model
37-
nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
38-
x̂d, x̂s, ŷ, Ŷs = zeros(nxd), zeros(nxs), zeros(ny), zeros(ny*Hp)
35+
nu, ny, nd = model.nu, model.ny, model.nd
36+
ŷ, Ŷs = zeros(ny), zeros(ny*Hp)
3937
Cwt = Inf # no slack variable ϵ for ExplicitMPC
4038
Ewt = 0 # economic costs not supported for ExplicitMPC
4139
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
@@ -47,7 +45,7 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
4745
R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[]
4846
R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
4947
S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
50-
E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
48+
E, F, G, J, K, Q = init_deterpred(estim, model, Hp, Hc)
5149
_ , S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
5250
P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
5351
Ks, Ps = init_stochpred(estim, Hp)
@@ -57,11 +55,11 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
5755
ΔŨ = zeros(nvar)
5856
mpc = new(
5957
estim,
60-
ΔŨ, x̂d, x̂s, ŷ, Ŷs,
58+
ΔŨ, ŷ, Ŷs,
6159
Hp, Hc,
6260
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
6361
S̃_Hp, T_Hp, T_Hc,
64-
Ẽ, F, G, J, Kd, Q, P̃, q̃, p,
62+
Ẽ, F, G, J, K, Q, P̃, q̃, p,
6563
Ks, Ps,
6664
d, D̂,
6765
Yop, Dop,

‎src/controller/linmpc.jl‎

Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,6 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
33
optim::JuMP.Model
44
con::ControllerConstraint
55
ΔŨ::Vector{Float64}
6-
x̂d::Vector{Float64}
7-
x̂s::Vector{Float64}
86
ŷ ::Vector{Float64}
97
Ŷs::Vector{Float64}
108
Hp::Int
@@ -19,15 +17,15 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
1917
S̃_Hp::Matrix{Bool}
2018
T_Hp::Matrix{Bool}
2119
T_Hc::Matrix{Bool}
22-
Ẽ ::Matrix{Float64}
23-
F ::Vector{Float64}
24-
G ::Matrix{Float64}
25-
J ::Matrix{Float64}
26-
Kd::Matrix{Float64}
27-
Q ::Matrix{Float64}
28-
P̃ ::Hermitian{Float64, Matrix{Float64}}
29-
q̃ ::Vector{Float64}
30-
p ::Vector{Float64}
20+
Ẽ::Matrix{Float64}
21+
F::Vector{Float64}
22+
G::Matrix{Float64}
23+
J::Matrix{Float64}
24+
K::Matrix{Float64}
25+
Q::Matrix{Float64}
26+
P̃::Hermitian{Float64, Matrix{Float64}}
27+
q̃::Vector{Float64}
28+
p::Vector{Float64}
3129
Ks::Matrix{Float64}
3230
Ps::Matrix{Float64}
3331
d::Vector{Float64}
@@ -36,8 +34,8 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
3634
Dop::Vector{Float64}
3735
function LinMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim) where {S<:StateEstimator}
3836
model = estim.model
39-
nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
40-
x̂d, x̂s, ŷ, Ŷs = zeros(nxd), zeros(nxs), zeros(ny), zeros(ny*Hp)
37+
nu, ny, nd = model.nu, model.ny, model.nd
38+
ŷ, Ŷs = zeros(ny), zeros(ny*Hp)
4139
Ewt = 0 # economic costs not supported for LinMPC
4240
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
4341
M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
@@ -48,7 +46,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
4846
R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[]
4947
R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
5048
S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
51-
E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
49+
E, F, G, J, K, Q = init_deterpred(estim, model, Hp, Hc)
5250
con, S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
5351
P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
5452
Ks, Ps = init_stochpred(estim, Hp)
@@ -58,11 +56,11 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
5856
ΔŨ = zeros(nvar)
5957
mpc = new(
6058
estim, optim, con,
61-
ΔŨ, x̂d, x̂s, ŷ, Ŷs,
59+
ΔŨ, ŷ, Ŷs,
6260
Hp, Hc,
6361
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
6462
S̃_Hp, T_Hp, T_Hc,
65-
Ẽ, F, G, J, Kd, Q, P̃, q̃, p,
63+
Ẽ, F, G, J, K, Q, P̃, q̃, p,
6664
Ks, Ps,
6765
d, D̂,
6866
Yop, Dop,

‎src/controller/nonlinmpc.jl‎

Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,6 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
55
optim::JuMP.Model
66
con::ControllerConstraint
77
ΔŨ::Vector{Float64}
8-
x̂d::Vector{Float64}
9-
x̂s::Vector{Float64}
108
ŷ ::Vector{Float64}
119
Ŷs::Vector{Float64}
1210
Hp::Int
@@ -22,15 +20,15 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
2220
S̃_Hp::Matrix{Bool}
2321
T_Hp::Matrix{Bool}
2422
T_Hc::Matrix{Bool}
25-
Ẽ ::Matrix{Float64}
26-
F ::Vector{Float64}
27-
G ::Matrix{Float64}
28-
J ::Matrix{Float64}
29-
Kd::Matrix{Float64}
30-
Q ::Matrix{Float64}
31-
P̃ ::Hermitian{Float64, Matrix{Float64}}
32-
q̃ ::Vector{Float64}
33-
p ::Vector{Float64}
23+
Ẽ::Matrix{Float64}
24+
F::Vector{Float64}
25+
G::Matrix{Float64}
26+
J::Matrix{Float64}
27+
K::Matrix{Float64}
28+
Q::Matrix{Float64}
29+
P̃::Hermitian{Float64, Matrix{Float64}}
30+
q̃::Vector{Float64}
31+
p::Vector{Float64}
3432
Ks::Matrix{Float64}
3533
Ps::Matrix{Float64}
3634
d::Vector{Float64}
@@ -41,8 +39,8 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
4139
estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE::JEFunc, ru, optim
4240
) where {S<:StateEstimator, JEFunc<:Function}
4341
model = estim.model
44-
nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
45-
x̂d, x̂s, ŷ, Ŷs = zeros(nxd), zeros(nxs), zeros(ny), zeros(ny*Hp)
42+
nu, ny, nd = model.nu, model.ny, model.nd
43+
ŷ, Ŷs = zeros(ny), zeros(ny*Hp)
4644
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, Ewt)
4745
M_Hp = Diagonal(convert(Vector{Float64}, repeat(Mwt, Hp)))
4846
N_Hc = Diagonal(convert(Vector{Float64}, repeat(Nwt, Hc)))
@@ -52,7 +50,7 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
5250
R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[]
5351
R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
5452
S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
55-
E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
53+
E, F, G, J, K, Q = init_deterpred(estim, model, Hp, Hc)
5654
con, S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
5755
P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
5856
Ks, Ps = init_stochpred(estim, Hp)
@@ -62,11 +60,11 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
6260
ΔŨ = zeros(nvar)
6361
mpc = new(
6462
estim, optim, con,
65-
ΔŨ, x̂d, x̂s, ŷ, Ŷs,
63+
ΔŨ, ŷ, Ŷs,
6664
Hp, Hc,
6765
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y,
6866
S̃_Hp, T_Hp, T_Hc,
69-
Ẽ, F, G, J, Kd, Q, P̃, q̃, p,
67+
Ẽ, F, G, J, K, Q, P̃, q̃, p,
7068
Ks, Ps,
7169
d, D̂,
7270
Yop, Dop,

‎src/estimator/internal_model.jl‎

Lines changed: 26 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,11 @@ struct InternalModel{M<:SimModel} <: StateEstimator
1313
Bs::Matrix{Float64}
1414
Cs::Matrix{Float64}
1515
Ds::Matrix{Float64}
16+
 ::Matrix{Float64}
17+
B̂u::Matrix{Float64}
18+
Ĉ ::Matrix{Float64}
19+
B̂d::Matrix{Float64}
20+
D̂d::Matrix{Float64}
1621
Âs::Matrix{Float64}
1722
B̂s::Matrix{Float64}
1823
function InternalModel{M}(model::M, i_ym, Asm, Bsm, Csm, Dsm) where {M<:SimModel}
@@ -30,7 +35,7 @@ struct InternalModel{M<:SimModel} <: StateEstimator
3035
As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm)
3136
nxs = size(As,1)
3237
nx̂ = model.nx
33-
nxs = size(As,1)
38+
Â, B̂u, Ĉ, B̂d, D̂d = matrices_intenalmodel(model)
3439
Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
3540
lastu0 = zeros(nu)
3641
x̂d = x̂ = zeros(model.nx) # x̂ and x̂d are same object (updating x̂d will update x̂)
@@ -40,6 +45,7 @@ struct InternalModel{M<:SimModel} <: StateEstimator
4045
lastu0, x̂, x̂d, x̂s,
4146
i_ym, nx̂, nym, nyu, nxs,
4247
As, Bs, Cs, Ds,
48+
Â, B̂u, Ĉ, B̂d, D̂d,
4349
Âs, B̂s
4450
)
4551
end
@@ -108,6 +114,25 @@ function validate_internalmodel(model::LinModel)
108114
end
109115
validate_internalmodel(::SimModel) = nothing
110116

117+
118+
@doc raw"""
119+
matrices_internalmodel(model::LinModel)
120+
121+
Get state-space matrices of the [`LinModel`](@ref) `model` for [`InternalModel`](@ref).
122+
123+
The [`InternalModel`](@ref) does not augment the state vector, thus:
124+
```math
125+
\mathbf{Â = A, B̂_u = B_u, Ĉ = C, B̂_d = B_d, D̂_d = D_d }
126+
```
127+
"""
128+
function matrices_internalmodel(model::LinModel)
129+
Â, B̂u, Ĉ, B̂d, D̂d = model.A, model.Bu, model.C, model.Bd, model.Dd
130+
return Â, B̂u, Ĉ, B̂d, D̂d
131+
end
132+
"Return empty matrices if `model` is not a [`LinModel`](@ref)."
133+
matrices_intenalmodel(::SimModel) = tuple(fill(Float64[;;],5)...)
134+
135+
111136
@doc raw"""
112137
init_internalmodel(As, Bs, Cs, Ds) -> Âs, B̂s
113138

‎src/estimator/kalman.jl‎

Lines changed: 18 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@ struct SteadyKalmanFilter <: StateEstimator
1414
nint_ym::Vector{Int}
1515
 ::Matrix{Float64}
1616
B̂u ::Matrix{Float64}
17-
B̂d ::Matrix{Float64}
1817
Ĉ ::Matrix{Float64}
18+
B̂d ::Matrix{Float64}
1919
D̂d ::Matrix{Float64}
2020
Ĉm ::Matrix{Float64}
2121
D̂dm ::Matrix{Float64}
@@ -51,7 +51,7 @@ struct SteadyKalmanFilter <: StateEstimator
5151
lastu0, x̂,
5252
i_ym, nx̂, nym, nyu, nxs,
5353
As, Cs_u, Cs_y, nint_u, nint_ym,
54-
Â, B̂u, B̂d, Ĉ, D̂d,
54+
Â, B̂u, Ĉ, B̂d, D̂d,
5555
Ĉm, D̂dm,
5656
Q̂, R̂,
5757
K
@@ -195,8 +195,8 @@ struct KalmanFilter <: StateEstimator
195195
nint_ym::Vector{Int}
196196
 ::Matrix{Float64}
197197
B̂u ::Matrix{Float64}
198-
B̂d ::Matrix{Float64}
199198
Ĉ ::Matrix{Float64}
199+
B̂d ::Matrix{Float64}
200200
D̂d ::Matrix{Float64}
201201
Ĉm ::Matrix{Float64}
202202
D̂dm ::Matrix{Float64}
@@ -220,7 +220,7 @@ struct KalmanFilter <: StateEstimator
220220
lastu0, x̂, P̂,
221221
i_ym, nx̂, nym, nyu, nxs,
222222
As, Cs_u, Cs_y, nint_u, nint_ym,
223-
Â, B̂u, B̂d, Ĉ, D̂d,
223+
Â, B̂u, Ĉ, B̂d, D̂d,
224224
Ĉm, D̂dm,
225225
P̂0, Q̂, R̂
226226
)
@@ -334,6 +334,11 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
334334
Cs_y::Matrix{Float64}
335335
nint_u ::Vector{Int}
336336
nint_ym::Vector{Int}
337+
 ::Matrix{Float64}
338+
B̂u::Matrix{Float64}
339+
Ĉ ::Matrix{Float64}
340+
B̂d::Matrix{Float64}
341+
D̂d::Matrix{Float64}
337342
P̂0::Hermitian{Float64, Matrix{Float64}}
338343
Q̂::Hermitian{Float64, Matrix{Float64}}
339344
R̂::Hermitian{Float64, Matrix{Float64}}
@@ -347,7 +352,7 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
347352
nym, nyu = validate_ym(model, i_ym)
348353
As, Cs_u, Cs_y, nxs, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
349354
nx̂ = model.nx + nxs
350-
augment_model(model, As, Cs_u, Cs_y) # verify observability for LinModels
355+
Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y)
351356
validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
352357
nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
353358
lastu0 = zeros(model.nu)
@@ -360,6 +365,7 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
360365
lastu0, x̂, P̂,
361366
i_ym, nx̂, nym, nyu, nxs,
362367
As, Cs_u, Cs_y, nint_u, nint_ym,
368+
Â, B̂u, Ĉ, B̂d, D̂d,
363369
P̂0, Q̂, R̂,
364370
nσ, γ, m̂, Ŝ
365371
)
@@ -566,6 +572,11 @@ struct ExtendedKalmanFilter{M<:SimModel} <: StateEstimator
566572
Cs_y::Matrix{Float64}
567573
nint_u ::Vector{Int}
568574
nint_ym::Vector{Int}
575+
 ::Matrix{Float64}
576+
B̂u::Matrix{Float64}
577+
Ĉ ::Matrix{Float64}
578+
B̂d::Matrix{Float64}
579+
D̂d::Matrix{Float64}
569580
P̂0::Hermitian{Float64, Matrix{Float64}}
570581
Q̂::Hermitian{Float64, Matrix{Float64}}
571582
R̂::Hermitian{Float64, Matrix{Float64}}
@@ -575,7 +586,7 @@ struct ExtendedKalmanFilter{M<:SimModel} <: StateEstimator
575586
nym, nyu = validate_ym(model, i_ym)
576587
As, Cs_u, Cs_y, nxs, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
577588
nx̂ = model.nx + nxs
578-
augment_model(model, As, Cs_u, Cs_y) # verify observability for LinModels
589+
Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y)
579590
validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
580591
lastu0 = zeros(model.nu)
581592
x̂ = [zeros(model.nx); zeros(nxs)]
@@ -588,6 +599,7 @@ struct ExtendedKalmanFilter{M<:SimModel} <: StateEstimator
588599
lastu0, x̂, P̂,
589600
i_ym, nx̂, nym, nyu, nxs,
590601
As, Cs_u, Cs_y, nint_u, nint_ym,
602+
Â, B̂u, Ĉ, B̂d, D̂d,
591603
P̂0, Q̂, R̂
592604
)
593605
end

‎src/estimator/luenberger.jl‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@ struct Luenberger <: StateEstimator
1414
nint_ym::Vector{Int}
1515
 ::Matrix{Float64}
1616
B̂u ::Matrix{Float64}
17-
B̂d ::Matrix{Float64}
1817
Ĉ ::Matrix{Float64}
18+
B̂d ::Matrix{Float64}
1919
D̂d ::Matrix{Float64}
2020
Ĉm ::Matrix{Float64}
2121
D̂dm ::Matrix{Float64}
@@ -38,7 +38,7 @@ struct Luenberger <: StateEstimator
3838
lastu0, x̂,
3939
i_ym, nx̂, nym, nyu, nxs,
4040
As, Cs_u, Cs_y, nint_u, nint_ym,
41-
Â, B̂u, B̂d, Ĉ, D̂d,
41+
Â, B̂u, Ĉ, B̂d, D̂d,
4242
Ĉm, D̂dm,
4343
K
4444
)

0 commit comments

Comments
 (0)