Skip to content

Commit c80e951

Browse files
franckgagafranckgaga
committed
added: MHE with OSQP for LinModel 
1 parent db9542c commit c80e951

File tree

7 files changed

+227
-106
lines changed

7 files changed

+227
-106
lines changed

‎docs/src/internals/predictive_control.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
1414
```@docs
1515
ModelPredictiveControl.init_predmat
1616
ModelPredictiveControl.init_ΔUtoU
17-
ModelPredictiveControl.init_quadprog
17+
ModelPredictiveControl.init_quadprog_mpc
1818
ModelPredictiveControl.init_stochpred
1919
ModelPredictiveControl.init_matconstraint
2020
```

‎src/ModelPredictiveControl.jl‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,9 @@ const FATAL_STATUSES = [
2727
OTHER_ERROR
2828
]
2929

30+
"Evaluate the quadratic programming objective function `0.5x'*H*x + q'*x` at `x`."
31+
obj_quadprog(x, H, q) = 0.5*dot(x, H, x) + q'*x # dot(x, H, x) is faster than x'*H*x
32+
3033
"Generate a block diagonal matrix repeating `n` times the matrix `A`."
3134
repeatdiag(A, n::Int) = kron(I(n), A)
3235

‎src/controller/explicitmpc.jl‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,10 +20,10 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
2020
J::Matrix{NT}
2121
K::Matrix{NT}
2222
V::Matrix{NT}
23-
P̃::Hermitian{NT, Matrix{NT}}
23+
H̃::Hermitian{NT, Matrix{NT}}
2424
q̃::Vector{NT}
2525
p::Vector{NT}
26-
P̃_chol::Cholesky{NT, Matrix{NT}}
26+
H̃_chol::Cholesky{NT, Matrix{NT}}
2727
Ks::Matrix{NT}
2828
Ps::Matrix{NT}
2929
d0::Vector{NT}
@@ -47,8 +47,8 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
4747
S, T = init_ΔUtoU(model, Hp, Hc)
4848
E, F, G, J, K, V = init_predmat(estim, model, Hp, Hc)
4949
S̃, Ñ_Hc, Ẽ = S, N_Hc, E # no slack variable ϵ for ExplicitMPC
50-
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
51-
P̃_chol = cholesky(P̃)
50+
H̃, q̃, p = init_quadprog_mpc(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
51+
H̃_chol = cholesky(H̃)
5252
Ks, Ps = init_stochpred(estim, Hp)
5353
# dummy vals (updated just before optimization):
5454
d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
@@ -62,8 +62,8 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
6262
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt,
6363
R̂u, R̂y, noR̂u,
6464
S̃, T,
65-
Ẽ, F, G, J, K, V, P̃, q̃, p,
66-
P̃_chol,
65+
Ẽ, F, G, J, K, V, H̃, q̃, p,
66+
H̃_chol,
6767
Ks, Ps,
6868
d0, D̂0, D̂E,
6969
Ŷop, Dop,
@@ -195,10 +195,10 @@ linconstraint!(::ExplicitMPC, ::LinModel) = nothing
195195
@doc raw"""
196196
Analytically solve the optimization problem for [`ExplicitMPC`](@ref).
197197
198-
The solution is ``\mathbf{ΔŨ = - P̃^{-1} q̃}``, see [`init_quadprog`](@ref).
198+
The solution is ``\mathbf{ΔŨ = - H̃^{-1} q̃}``, see [`init_quadprog`](@ref).
199199
"""
200200
function optim_objective!(mpc::ExplicitMPC)
201-
return lmul!(-1, ldiv!(mpc.ΔŨ, mpc.P̃_chol, mpc.q̃))
201+
return lmul!(-1, ldiv!(mpc.ΔŨ, mpc.H̃_chol, mpc.q̃))
202202
end
203203

204204
"Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)."

‎src/controller/linmpc.jl‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ struct LinMPC{
3030
J::Matrix{NT}
3131
K::Matrix{NT}
3232
V::Matrix{NT}
33-
P̃::Hermitian{NT, Matrix{NT}}
33+
H̃::Hermitian{NT, Matrix{NT}}
3434
q̃::Vector{NT}
3535
p::Vector{NT}
3636
Ks::Matrix{NT}
@@ -54,8 +54,8 @@ struct LinMPC{
5454
noR̂u = iszero(L_Hp)
5555
S, T = init_ΔUtoU(model, Hp, Hc)
5656
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
57-
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
58-
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
57+
con, S̃, Ñ_Hc, Ẽ = init_defaultcon_mhe(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
58+
H̃, q̃, p = init_quadprog_mpc(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5959
Ks, Ps = init_stochpred(estim, Hp)
6060
# dummy vals (updated just before optimization):
6161
d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
@@ -69,7 +69,7 @@ struct LinMPC{
6969
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt,
7070
R̂u, R̂y, noR̂u,
7171
S̃, T,
72-
Ẽ, F, G, J, K, V, P̃, q̃, p,
72+
Ẽ, F, G, J, K, V, H̃, q̃, p,
7373
Ks, Ps,
7474
d0, D̂0, D̂E,
7575
Ŷop, Dop,
@@ -121,7 +121,7 @@ arguments.
121121
- `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
122122
- `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in
123123
the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
124-
(default to [`OSQP.jl`](https://osqp.org/docs/parsers/jump.html) optimizer).
124+
(default to [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer).
125125
- `M_Hp` / `N_Hc` / `L_Hp` : diagonal matrices ``\mathbf{M}_{H_p}, \mathbf{N}_{H_c},
126126
\mathbf{L}_{H_p}``, for time-varying weights (generated from `Mwt/Nwt/Lwt` args if omitted).
127127
- additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor.
@@ -241,7 +241,7 @@ function init_optimization!(mpc::LinMPC, optim::JuMP.GenericModel)
241241
b = con.b[con.i_b]
242242
@constraint(optim, linconstraint, A*ΔŨvar .≤ b)
243243
# --- quadratic optimization init ---
244-
@objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.P̃, mpc.q̃))
244+
@objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.H̃, mpc.q̃))
245245
return nothing
246246
end
247247

‎src/controller/nonlinmpc.jl‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ struct NonLinMPC{
3232
J::Matrix{NT}
3333
K::Matrix{NT}
3434
V::Matrix{NT}
35-
P̃::Hermitian{NT, Matrix{NT}}
35+
H̃::Hermitian{NT, Matrix{NT}}
3636
q̃::Vector{NT}
3737
p::Vector{NT}
3838
Ks::Matrix{NT}
@@ -55,8 +55,8 @@ struct NonLinMPC{
5555
noR̂u = iszero(L_Hp)
5656
S, T = init_ΔUtoU(model, Hp, Hc)
5757
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
58-
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
59-
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
58+
con, S̃, Ñ_Hc, Ẽ = init_defaultcon_mhe(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
59+
H̃, q̃, p = init_quadprog_mpc(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
6060
Ks, Ps = init_stochpred(estim, Hp)
6161
# dummy vals (updated just before optimization):
6262
d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
@@ -70,7 +70,7 @@ struct NonLinMPC{
7070
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE,
7171
R̂u, R̂y, noR̂u,
7272
S̃, T,
73-
Ẽ, F, G, J, K, V, P̃, q̃, p,
73+
Ẽ, F, G, J, K, V, H̃, q̃, p,
7474
Ks, Ps,
7575
d0, D̂0, D̂E,
7676
Ŷop, Dop,
@@ -136,7 +136,7 @@ This method uses the default state estimator :
136136
\mathbf{L}_{H_p}``, for time-varying weights (generated from `Mwt/Nwt/Lwt` args if omitted).
137137
- `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive
138138
controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
139-
(default to [`Ipopt.jl`](https://github.com/jump-dev/Ipopt.jl) optimizer).
139+
(default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer).
140140
- additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor
141141
(or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)).
142142

0 commit comments

Comments
 (0)