Skip to content

Commit 4074c77

Browse files
franckgagafranckgaga
committed
lastu field in StateEstimator
1 parent 92558cb commit 4074c77

File tree

9 files changed

+116
-74
lines changed

9 files changed

+116
-74
lines changed

‎docs/src/internals/predictive_control.md‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@ ModelPredictiveControl.relaxΔU
1313
ModelPredictiveControl.relaxŶ
1414
ModelPredictiveControl.init_quadprog
1515
ModelPredictiveControl.init_stochpred
16-
ModelPredictiveControl.split_state
17-
ModelPredictiveControl.init_prediction
1816
ModelPredictiveControl.init_linconstraint
19-
ModelPredictiveControl.linconstraint
17+
ModelPredictiveControl.getestimates!
18+
ModelPredictiveControl.initprediction!
19+
ModelPredictiveControl.linconstraint!
2020
```

‎example/juMPC.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ setconstraint!(nmpc, ŷmin=[-Inf,-Inf], ŷmax=[55, 35])
9292
setconstraint!(nmpc, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf])
9393

9494
function test_mpc(model, mpc)
95-
N = 200
95+
N = 200
9696
u_data = zeros(2,N)
9797
y_data = zeros(2,N)
9898
r_data = zeros(2,N)

‎src/controller/linmpc.jl‎

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,9 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
33
optim::JuMP.Model
44
info::OptimInfo
55
con::ControllerConstraint
6+
x̂d::Vector{Float64}
7+
x̂s::Vector{Float64}
8+
ŷ ::Vector{Float64}
69
Hp::Int
710
Hc::Int
811
M_Hp::Diagonal{Float64, Vector{Float64}}
@@ -12,7 +15,6 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
1215
R̂u::Vector{Float64}
1316
S̃_Hp::Matrix{Bool}
1417
T_Hp::Matrix{Bool}
15-
S̃_Hc::Matrix{Bool}
1618
T_Hc::Matrix{Bool}
1719
Ẽ ::Matrix{Float64}
1820
F ::Vector{Float64}
@@ -28,7 +30,8 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
2830
Dop::Vector{Float64}
2931
function LinMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim) where {S<:StateEstimator}
3032
model = estim.model
31-
nu, ny = model.nu, model.ny
33+
nu, nxd, nxs, ny = model.nu, model.nx, estim.nxs, model.ny
34+
x̂d, x̂s, ŷ = zeros(nxd), zeros(nxs), zeros(ny)
3235
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
3336
M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
3437
N_Hc = Diagonal{Float64}(repeat(Nwt, Hc))
@@ -48,7 +51,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
4851
repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ŷmin, c_ŷmax)
4952
S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
5053
E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
51-
A_Umin, A_Umax, S̃_Hp, S̃_Hc = relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
54+
A_Umin, A_Umax, S̃_Hp = relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
5255
A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU(C,c_ΔUmin,c_ΔUmax,ΔUmin,ΔUmax,N_Hc)
5356
A_Ŷmin, A_Ŷmax, Ẽ = relaxŶ(model, C, c_Ŷmin, c_Ŷmax, E)
5457
i_Umin, i_Umax = .!isinf.(Umin), .!isinf.(Umax)
@@ -80,9 +83,10 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
8083
info = OptimInfo(ΔŨ0, ϵ, 0, u, U, ŷ, Ŷ, ŷs, Ŷs)
8184
mpc = new(
8285
estim, optim, info, con,
86+
x̂d, x̂s, ŷ,
8387
Hp, Hc,
8488
M_Hp, Ñ_Hc, L_Hp, Cwt, R̂u,
85-
S̃_Hp, T_Hp, S̃_Hc, T_Hc,
89+
S̃_Hp, T_Hp, T_Hc,
8690
Ẽ, F, G, J, Kd, Q, P̃, q̃,
8791
Ks, Ps,
8892
Yop, Dop,
@@ -225,13 +229,13 @@ end
225229
226230
Write `mpc.info` with the [`LinMPC`](@ref) optimization results.
227231
"""
228-
function write_info!(mpc::LinMPC, ΔŨ, J, ŷs, Ŷs, lastu, ym, d)
232+
function write_info!(mpc::LinMPC, ΔŨ, J, ŷs, Ŷs)
229233
mpc.info.ΔŨ = ΔŨ
230234
mpc.info.ϵ = isinf(mpc.C) ? NaN : ΔŨ[end]
231235
mpc.info.J = J
232-
mpc.info.U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*lastu
236+
mpc.info.U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*mpc.estim.lastu
233237
mpc.info.u = mpc.info.U[1:mpc.estim.model.nu]
234-
mpc.info.ŷ = eval_ŷ(mpc.estim, ym, d)
238+
mpc.info.ŷ = mpc.ŷ
235239
mpc.info.Ŷ = mpc.Ẽ*ΔŨ + mpc.F
236240
mpc.info.ŷs, mpc.info.Ŷs = ŷs, Ŷs
237241
end

‎src/controller/nonlinmpc.jl‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,9 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
33
optim::JuMP.Model
44
info::OptimInfo
55
con::ControllerConstraint
6+
x̂d::Vector{Float64}
7+
x̂s::Vector{Float64}
8+
ŷ ::Vector{Float64}
69
Hp::Int
710
Hc::Int
811
M_Hp::Diagonal{Float64, Vector{Float64}}
@@ -14,7 +17,6 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
1417
R̂u::Vector{Float64}
1518
S̃_Hp::Matrix{Bool}
1619
T_Hp::Matrix{Bool}
17-
S̃_Hc::Matrix{Bool}
1820
T_Hc::Matrix{Bool}
1921
Ẽ ::Matrix{Float64}
2022
F ::Vector{Float64}
@@ -32,7 +34,8 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
3234
estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE::JEFunc, ru, optim
3335
) where {S<:StateEstimator, JEFunc<:Function}
3436
model = estim.model
35-
nu, nx, ny, nd = model.nu, model.nx, model.ny, model.nd
37+
nu, nxd, nxs, ny = model.nu, model.nx, estim.nxs, model.ny
38+
x̂d, x̂s, ŷ = zeros(nxd), zeros(nxs), zeros(ny)
3639
validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, Ewt)
3740
M_Hp = Diagonal(convert(Vector{Float64}, repeat(Mwt, Hp)))
3841
N_Hc = Diagonal(convert(Vector{Float64}, repeat(Nwt, Hc)))
@@ -52,7 +55,7 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
5255
repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ŷmin, c_ŷmax)
5356
S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
5457
E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
55-
A_Umin, A_Umax, S̃_Hp, S̃_Hc = relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
58+
A_Umin, A_Umax, S̃_Hp = relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
5659
A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU(C,c_ΔUmin,c_ΔUmax,ΔUmin,ΔUmax,N_Hc)
5760
A_Ŷmin, A_Ŷmax, Ẽ = relaxŶ(model, C, c_Ŷmin, c_Ŷmax, E)
5861
i_Umin, i_Umax = .!isinf.(Umin), .!isinf.(Umax)
@@ -86,9 +89,10 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
8689
# dummy x̂d, F, d0, D̂0, q̃ values (updated just before optimization)
8790
mpc = new(
8891
estim, optim, info, con,
92+
x̂d, x̂s, ŷ,
8993
Hp, Hc,
9094
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u,
91-
S̃_Hp, T_Hp, S̃_Hc, T_Hc,
95+
S̃_Hp, T_Hp, T_Hc,
9296
Ẽ, F, G, J, Kd, Q, P̃, q̃,
9397
Ks, Ps,
9498
Yop, Dop,
@@ -223,6 +227,6 @@ end
223227

224228

225229

226-
function write_info!(mpc::NonLinMPC, ΔŨ, J, ŷs, Ŷs, lastu, ym, d)
230+
function write_info!(mpc::NonLinMPC, ΔŨ, J, ŷs, Ŷs)
227231
return nothing
228232
end

‎src/estimator/internal_model.jl‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
struct InternalModel{M<:SimModel} <: StateEstimator
22
model::M
3+
lastu::Vector{Float64}
34
x̂::Vector{Float64}
45
x̂d::Vector{Float64}
56
x̂s::Vector{Float64}
@@ -37,9 +38,16 @@ struct InternalModel{M<:SimModel} <: StateEstimator
3738
nxs = size(As,1)
3839
Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
3940
i_ym = collect(i_ym)
41+
lastu = copy(model.uop)
4042
x̂d = x̂ = copy(model.x) # x̂ and x̂d are same object (updating x̂d will update x̂)
4143
x̂s = zeros(nxs)
42-
return new(model, x̂, x̂d, x̂s, i_ym, nx̂, nym, nyu, nxs, As, Bs, Cs, Ds, Âs, B̂s)
44+
return new(
45+
model,
46+
lastu, x̂, x̂d, x̂s,
47+
i_ym, nx̂, nym, nyu, nxs,
48+
As, Bs, Cs, Ds,
49+
Âs, B̂s
50+
)
4351
end
4452
end
4553

@@ -145,11 +153,12 @@ Update `estim.x̂` \ `x̂d` \ `x̂s` with current inputs `u`, measured outputs `
145153
"""
146154
function updatestate!(estim::InternalModel, u, ym, d=Float64[])
147155
model = estim.model
148-
u, d, ym = remove_op(estim, u, d, ym)
156+
# ---- remove operating points ----
157+
u, d, ym = remove_op!(estim, u, d, ym)
149158
x̂d, x̂s = estim.x̂d, estim.x̂s
150159
# -------------- deterministic model ---------------------
151160
ŷd = h(model, x̂d, d)
152-
x̂d[:] = f(model, x̂d, u, d) # this also updates estim.xhat (they are the same object)
161+
x̂d[:] = f(model, x̂d, u, d) # this also updates estim.x̂ (they are the same object)
153162
# --------------- stochastic model -----------------------
154163
ŷs = zeros(model.ny,1)
155164
ŷs[estim.i_ym] = ym - ŷd[estim.i_ym] # ŷs=0 for unmeasured outputs

‎src/estimator/kalman.jl‎

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
struct SteadyKalmanFilter <: StateEstimator
22
model::LinModel
3+
lastu::Vector{Float64}
34
x̂::Vector{Float64}
45
i_ym::Vector{Int}
56
nx̂::Int
@@ -38,12 +39,13 @@ struct SteadyKalmanFilter <: StateEstimator
3839
end
3940
end
4041
i_ym = collect(i_ym)
42+
lastu = copy(model.uop)
43+
x̂ = [copy(model.x); zeros(nxs)]
4144
Q̂ = Hermitian(Q̂, :L)
4245
R̂ = Hermitian(R̂, :L)
43-
x̂ = [copy(model.x); zeros(nxs)]
4446
return new(
4547
model,
46-
x̂,
48+
lastu, x̂,
4749
i_ym, nx̂, nym, nyu, nxs,
4850
As, Cs, nint_ym,
4951
Â, B̂u, B̂d, Ĉ, D̂d,
@@ -158,7 +160,7 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
158160
```
159161
"""
160162
function updatestate!(estim::SteadyKalmanFilter, u, ym, d=Float64[])
161-
u, d, ym = remove_op(estim, u, d, ym)
163+
u, d, ym = remove_op!(estim, u, d, ym)
162164
Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm
163165
x̂, K = estim.x̂, estim.K
164166
x̂[:] = Â*x̂ + B̂u*u + B̂d*d + K*(ym - Ĉm*x̂ - D̂dm*d)
@@ -168,6 +170,7 @@ end
168170

169171
struct KalmanFilter <: StateEstimator
170172
model::LinModel
173+
lastu::Vector{Float64}
171174
x̂::Vector{Float64}
172175
P̂::Hermitian{Float64, Matrix{Float64}}
173176
i_ym::Vector{Int}
@@ -198,14 +201,15 @@ struct KalmanFilter <: StateEstimator
198201
Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
199202
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
200203
i_ym = collect(i_ym)
204+
lastu = copy(model.uop)
201205
x̂ = [copy(model.x); zeros(nxs)]
202206
P̂0 = Hermitian(P̂0, :L)
203207
Q̂ = Hermitian(Q̂, :L)
204208
R̂ = Hermitian(R̂, :L)
205209
P̂ = copy(P̂0)
206210
return new(
207211
model,
208-
x̂, P̂,
212+
lastu, x̂, P̂,
209213
i_ym, nx̂, nym, nyu, nxs,
210214
As, Cs, nint_ym,
211215
Â, B̂u, B̂d, Ĉ, D̂d,
@@ -275,7 +279,7 @@ Update `estim.x̂` \ `P̂` with current inputs `u`, measured outputs `ym` and di
275279
See [`updatestate_kf!`](@ref) for the implementation details.
276280
"""
277281
function updatestate!(estim::KalmanFilter, u, ym, d=Float64[])
278-
u, d, ym = remove_op(estim, u, d, ym)
282+
u, d, ym = remove_op!(estim, u, d, ym)
279283
updatestate_kf!(estim, u, ym, d)
280284
return estim.x̂
281285
end
@@ -318,6 +322,7 @@ end
318322

319323
struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
320324
model::M
325+
lastu::Vector{Float64}
321326
x̂::Vector{Float64}
322327
P̂::Hermitian{Float64, Matrix{Float64}}
323328
i_ym::Vector{Int}
@@ -346,14 +351,15 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
346351
As, _ , Cs, _ = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
347352
nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
348353
i_ym = collect(i_ym)
354+
lastu = copy(model.uop)
349355
x̂ = [copy(model.x); zeros(nxs)]
350356
P̂0 = Hermitian(P̂0, :L)
351357
Q̂ = Hermitian(Q̂, :L)
352358
R̂ = Hermitian(R̂, :L)
353359
P̂ = copy(P̂0)
354360
return new(
355361
model,
356-
x̂, P̂,
362+
lastu, x̂, P̂,
357363
i_ym, nx̂, nym, nyu, nxs,
358364
As, Cs, nint_ym,
359365
P̂0, Q̂, R̂,
@@ -470,7 +476,7 @@ Same than `KalmanFilter` but using the unscented estimator.
470476
See [`updatestate_ukf!`](@ref) for the implementation details.
471477
"""
472478
function updatestate!(estim::UnscentedKalmanFilter, u, ym, d=Float64[])
473-
u, d, ym = remove_op(estim, u, d, ym)
479+
u, d, ym = remove_op!(estim, u, d, ym)
474480
updatestate_ukf!(estim, u, ym, d)
475481
return estim.x̂
476482
end

‎src/estimator/luenberger.jl‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
struct Luenberger <: StateEstimator
22
model::LinModel
3+
lastu::Vector{Float64}
34
x̂::Vector{Float64}
45
i_ym::Vector{Int}
56
nx̂::Int
@@ -28,10 +29,11 @@ struct Luenberger <: StateEstimator
2829
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
2930
K = L
3031
i_ym = collect(i_ym)
32+
lastu = copy(model.uop)
3133
x̂ = [copy(model.x); zeros(nxs)]
3234
return new(
3335
model,
34-
x̂,
36+
lastu, x̂,
3537
i_ym, nx̂, nym, nyu, nxs,
3638
As, Cs, nint_ym,
3739
Â, B̂u, B̂d, Ĉ, D̂d,

0 commit comments

Comments
 (0)