Skip to content

added: save some NonLinMPC computations with T_lastu instead of T_lastu0 for input conversion #133

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Dec 5, 2024
Merged

added: save some NonLinMPC computations with T_lastu instead of T_lastu0 for input conversion #133

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
38dbd78
added: save some `NonLinMPC` computations with `T_lastu` instead of `…
franckgaga Dec 4, 2024
1de069f
doc: minor modification
franckgaga Dec 4, 2024
a3940fb
debug: correct `u` bounds for `NonLinModel`
franckgaga Dec 4, 2024
957a028
test: improve constraint violation tests (to catch the last bug)
franckgaga Dec 4, 2024
c541b82
bump
franckgaga Dec 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Project.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
name = "ModelPredictiveControl"
uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
authors = ["Francis Gagnon"]
version = "1.1.1"
version = "1.1.2"

[deps]
ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
Expand Down
38 changes: 33 additions & 5 deletions src/controller/construct.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -454,7 +454,35 @@ Init manipulated input increments to inputs conversion matrices.
The conversion from the input increments ``\mathbf{ΔU}`` to manipulated inputs over ``H_p``
are calculated by:
```math
\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1) \\
\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1)
```
The ``\mathbf{S}`` and ``\mathbf{T}`` matrices are defined in the Extended Help section.

# Extended Help
!!! details "Extended Help"
The ``\mathbf{U}`` vector and the two conversion matrices are defined as:
```math
\mathbf{U} = \begin{bmatrix}
\mathbf{u}(k + 0) \\
\mathbf{u}(k + 1) \\
\vdots \\
\mathbf{u}(k + H_c - 1) \\
\vdots \\
\mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad
\mathbf{S} = \begin{bmatrix}
\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{0} \\
\vdots & \vdots & \ddots & \vdots \\
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \\
\vdots & \vdots & \ddots & \vdots \\
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \end{bmatrix} , \quad
\mathbf{T} = \begin{bmatrix}
\mathbf{I} \\
\mathbf{I} \\
\vdots \\
\mathbf{I} \\
\vdots \\
\mathbf{I} \end{bmatrix}
```
"""
function init_ΔUtoU(model::SimModel{NT}, Hp, Hc) where {NT<:Real}
Expand Down Expand Up @@ -753,12 +781,12 @@ constraints:
\mathbf{A_{U_{max}}}
\end{bmatrix} \mathbf{ΔŨ} ≤
\begin{bmatrix}
- \mathbf{(U_{min} - U_{op}) + T} \mathbf{u_0}(k-1) \\
+ \mathbf{(U_{max} - U_{op}) - T} \mathbf{u_0}(k-1)
- \mathbf{(U_{min}) + T} \mathbf{u}(k-1) \\
+ \mathbf{(U_{max}) - T} \mathbf{u}(k-1)
\end{bmatrix}
```
in which ``\mathbf{U_{min}, U_{max}}`` and ``\mathbf{U_{op}}`` vectors respectively contains
``\mathbf{u_{min}, u_{max}}`` and ``\mathbf{u_{op}}`` repeated ``H_p`` times.
in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively contains
``\mathbf{u_{min}}`` and ``\mathbf{u_{max}}`` repeated ``H_p`` times.
"""
function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real
if nϵ == 1 # ΔŨ = [ΔU; ϵ]
Expand Down
26 changes: 15 additions & 11 deletions src/controller/execute.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,8 +179,8 @@ They are computed with these equations using in-place operations:
\begin{aligned}
\mathbf{F} &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k)
+ \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s} \\
\mathbf{C_y} &= \mathbf{F} - (\mathbf{R̂_y - Y_{op}}) \\
\mathbf{C_u} &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}}) \\
\mathbf{C_y} &= \mathbf{F} + \mathbf{Y_{op}} - \mathbf{R̂_y} \\
\mathbf{C_u} &= \mathbf{T}\mathbf{u}(k-1) - \mathbf{R̂_u} \\
\mathbf{q̃} &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y}
+ (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\
r &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y}
Expand All @@ -189,7 +189,9 @@ They are computed with these equations using in-place operations:
```
"""
function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u)
mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
lastu = mpc.buffer.u
lastu .= mpc.estim.lastu0 .+ model.uop
mul!(mpc.T_lastu, mpc.T, lastu)
ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃
ŷ .= evaloutput(mpc.estim, d)
Expand All @@ -210,15 +212,15 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
# --- output setpoint tracking term ---
mpc.R̂y .= R̂y
if !mpc.weights.iszero_M_Hp[]
Cy .= F .- (R̂y .- mpc.Yop)
Cy .= F .+ mpc.Yop .- R̂y
mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ)
mul!(q̃, M_Hp_Ẽ', Cy, 1, 1) # q̃ = q̃ + M_Hp*Ẽ'*Cy
r .+= dot(Cy, mpc.weights.M_Hp, Cy) # r = r + Cy'*M_Hp*Cy
end
# --- input setpoint tracking term ---
mpc.R̂u .= R̂u
if !mpc.weights.iszero_L_Hp[]
Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop)
Cu .= mpc.T_lastu .- R̂u
mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃)
mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu
r .+= dot(Cu, mpc.weights.L_Hp, Cu) # r = r + Cu'*L_Hp*Cu
Expand All @@ -234,7 +236,9 @@ end
Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref).
"""
function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
lastu = mpc.buffer.u
lastu .= mpc.estim.lastu0 .+ model.uop
mul!(mpc.T_lastu, mpc.T, lastu)
mpc.ŷ .= evaloutput(mpc.estim, d)
predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel
if model.nd ≠ 0
Expand Down Expand Up @@ -281,9 +285,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel)
mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
end
n = 0
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.T_lastu0
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu
n += nU
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.T_lastu0
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu
n += nU
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
n += nΔŨ
Expand All @@ -307,9 +311,9 @@ end
function linconstraint!(mpc::PredictiveController, ::SimModel)
nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin)
n = 0
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.T_lastu0
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu
n += nU
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.T_lastu0
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu
n += nU
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
n += nΔŨ
Expand Down Expand Up @@ -378,7 +382,7 @@ function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
ny, nu = model.ny, model.nu
# --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
U = Ū
U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop
U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu
Ue[1:end-nu] .= U
# u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
Ue[end-nu+1:end] .= @views U[end-nu+1:end]
Expand Down
6 changes: 3 additions & 3 deletions src/controller/explicitmpc.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
R̂y::Vector{NT}
S̃::Matrix{NT}
T::Matrix{NT}
T_lastu0::Vector{NT}
T_lastu::Vector{NT}
Ẽ::Matrix{NT}
F::Vector{NT}
G::Matrix{NT}
Expand Down Expand Up @@ -44,7 +44,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
# dummy vals (updated just before optimization):
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
S, T = init_ΔUtoU(model, Hp, Hc)
E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc)
# dummy val (updated just before optimization):
Expand All @@ -67,7 +67,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
Hp, Hc, nϵ,
weights,
R̂u, R̂y,
S̃, T, T_lastu0,
S̃, T, T_lastu,
Ẽ, F, G, J, K, V, B,
H̃, q̃, r,
H̃_chol,
Expand Down
6 changes: 3 additions & 3 deletions src/controller/linmpc.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ struct LinMPC{
R̂y::Vector{NT}
S̃::Matrix{NT}
T::Matrix{NT}
T_lastu0::Vector{NT}
T_lastu::Vector{NT}
Ẽ::Matrix{NT}
F::Vector{NT}
G::Matrix{NT}
Expand Down Expand Up @@ -48,7 +48,7 @@ struct LinMPC{
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
# dummy vals (updated just before optimization):
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
S, T = init_ΔUtoU(model, Hp, Hc)
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
# dummy vals (updated just before optimization):
Expand All @@ -72,7 +72,7 @@ struct LinMPC{
Hp, Hc, nϵ,
weights,
R̂u, R̂y,
S̃, T, T_lastu0,
S̃, T, T_lastu,
Ẽ, F, G, J, K, V, B,
H̃, q̃, r,
Ks, Ps,
Expand Down
6 changes: 3 additions & 3 deletions src/controller/nonlinmpc.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ struct NonLinMPC{
R̂y::Vector{NT}
S̃::Matrix{NT}
T::Matrix{NT}
T_lastu0::Vector{NT}
T_lastu::Vector{NT}
Ẽ::Matrix{NT}
F::Vector{NT}
G::Matrix{NT}
Expand Down Expand Up @@ -61,7 +61,7 @@ struct NonLinMPC{
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
# dummy vals (updated just before optimization):
R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
S, T = init_ΔUtoU(model, Hp, Hc)
E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
# dummy vals (updated just before optimization):
Expand All @@ -87,7 +87,7 @@ struct NonLinMPC{
weights,
JE, p,
R̂u, R̂y,
S̃, T, T_lastu0,
S̃, T, T_lastu,
Ẽ, F, G, J, K, V, B,
H̃, q̃, r,
Ks, Ps,
Expand Down
Loading
Loading