Skip to content

added: SimModelBuffer and StateEsimatorBuffer objects to reduce the allocations #92

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.

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 6 commits into from
Aug 5, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
17 changes: 7 additions & 10 deletions src/controller/execute.jl
Original file line number Diff line number Diff line change
@@ -1,18 +1,15 @@
@doc raw"""
initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop) -> x̂
initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂

Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero.
"""
function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop)
function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty)
mpc.ΔŨ .= 0
return initstate!(mpc.estim, u, ym, d)
end

@doc raw"""
moveinput!(
mpc::PredictiveController, ry=mpc.estim.model.yop, d=mpc.estim.model.dop;
<keyword arguments>
) -> u
moveinput!(mpc::PredictiveController, ry=mpc.estim.model.yop, d=[]; <keyword args>) -> u

Compute the optimal manipulated input value `u` for the current control period.

Expand All @@ -32,7 +29,7 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref).

- `mpc::PredictiveController` : solve optimization problem of `mpc`.
- `ry=mpc.estim.model.yop` : current output setpoints ``\mathbf{r_y}(k)``.
- `d=mpc.estim.model.dop` : current measured disturbances ``\mathbf{d}(k)``.
- `d=[]` : current measured disturbances ``\mathbf{d}(k)``.
- `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}``, constant
in the future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``.
- `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}``, constant
Expand All @@ -54,7 +51,7 @@ julia> ry = [5]; u = moveinput!(mpc, ry); round.(u, digits=3)
function moveinput!(
mpc::PredictiveController,
ry::Vector = mpc.estim.model.yop,
d ::Vector = mpc.estim.model.dop;
d ::Vector = mpc.estim.buffer.empty;
Dhat ::Vector = repeat(d, mpc.Hp),
Rhaty::Vector = repeat(ry, mpc.Hp),
Rhatu::Vector = mpc.Uop,
Expand Down Expand Up @@ -508,11 +505,11 @@ end
set_objective_linear_coef!(::PredictiveController, _ ) = nothing

"""
updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop) -> x̂
updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂

Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
"""
function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop)
function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty)
return updatestate!(mpc.estim, u, ym, d)
end
updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym"))
Expand Down
33 changes: 18 additions & 15 deletions src/estimator/execute.jl
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,13 @@ Also store current inputs without operating points `u0` in `estim.lastu0`. This
used for [`PredictiveController`](@ref) computations.
"""
function remove_op!(estim::StateEstimator, u, ym, d)
u0 = u - estim.model.uop
ym0 = ym - estim.model.yop[estim.i_ym]
d0 = d - estim.model.dop
estim.lastu0[:] = u0
return u0, ym0, d0
u0, d0 = estim.buffer.u, estim.buffer.d
y0m = estim.buffer.ym
u0 .= u .- estim.model.uop
y0m .= @views ym .- estim.model.yop[estim.i_ym]
d0 .= d .- estim.model.dop
estim.lastu0 .= u0
return u0, y0m, d0
end

@doc raw"""
Expand Down Expand Up @@ -79,7 +81,7 @@ end


@doc raw"""
initstate!(estim::StateEstimator, u, ym, d=estim.model.dop) -> x̂
initstate!(estim::StateEstimator, u, ym, d=[]) -> x̂

Init `estim.x̂0` states from current inputs `u`, measured outputs `ym` and disturbances `d`.

Expand Down Expand Up @@ -111,7 +113,7 @@ julia> evaloutput(estim) ≈ y
true
```
"""
function initstate!(estim::StateEstimator, u, ym, d=estim.model.dop)
function initstate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
# --- validate arguments ---
validate_args(estim, u, ym, d)
# --- init state estimate ----
Expand Down Expand Up @@ -150,6 +152,7 @@ measured outputs ``\mathbf{y^m}``.
function init_estimate!(estim::StateEstimator, ::LinModel, u0, ym0, d0)
Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d
Ĉm, D̂dm = @views Ĉ[estim.i_ym, :], D̂d[estim.i_ym, :] # measured outputs ym only
# TODO: use estim.buffer.x̂ to reduce allocations
estim.x̂0 .= [I - Â; Ĉm]\[B̂u*u0 + B̂d*d0 + estim.f̂op - estim.x̂op; ym0 - D̂dm*d0]
return nothing
end
Expand All @@ -161,7 +164,7 @@ Left `estim.x̂0` estimate unchanged if `model` is not a [`LinModel`](@ref).
init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ ) = nothing

@doc raw"""
evaloutput(estim::StateEstimator, d=estim.model.dop) -> ŷ
evaloutput(estim::StateEstimator, d=[]) -> ŷ

Evaluate `StateEstimator` outputs `ŷ` from `estim.x̂0` states and disturbances `d`.

Expand All @@ -176,21 +179,21 @@ julia> ŷ = evaloutput(kf)
20.0
```
"""
function evaloutput(estim::StateEstimator{NT}, d=estim.model.dop) where NT <: Real
function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real
validate_args(estim.model, d)
ŷ0 = Vector{NT}(undef, estim.model.ny)
d0 = d - estim.model.dop
ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d
d0 .= d .- estim.model.dop
ĥ!(ŷ0, estim, estim.model, estim.x̂0, d0)
ŷ = ŷ0
ŷ .+= estim.model.yop
return ŷ
end

"Functor allowing callable `StateEstimator` object as an alias for `evaloutput`."
(estim::StateEstimator)(d=estim.model.dop) = evaloutput(estim, d)
(estim::StateEstimator)(d=estim.buffer.empty) = evaloutput(estim, d)

@doc raw"""
updatestate!(estim::StateEstimator, u, ym, d=estim.model.dop) -> x̂
updatestate!(estim::StateEstimator, u, ym, d=[]) -> x̂

Update `estim.x̂0` estimate with current inputs `u`, measured outputs `ym` and dist. `d`.

Expand All @@ -207,10 +210,10 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
0.0
```
"""
function updatestate!(estim::StateEstimator, u, ym, d=estim.model.dop)
function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
validate_args(estim, u, ym, d)
u0, ym0, d0 = remove_op!(estim, u, ym, d)
x̂0next = update_estimate!(estim, u0, ym0, d0)
x̂0next = update_estimate!(estim, u0, ym0, d0)
x̂next = x̂0next
x̂next .+= estim.x̂op
return x̂next
Expand Down
8 changes: 6 additions & 2 deletions src/estimator/internal_model.jl
Original file line number Diff line number Diff line change
Expand Up @@ -22,27 +22,31 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
D̂d::Matrix{NT}
Âs::Matrix{NT}
B̂s::Matrix{NT}
buffer::StateEstimatorBuffer{NT}
function InternalModel{NT, SM}(
model::SM, i_ym, Asm, Bsm, Csm, Dsm
) where {NT<:Real, SM<:SimModel}
nu, ny, nd = model.nu, model.ny, model.nd
nym, nyu = validate_ym(model, i_ym)
validate_internalmodel(model, nym, Csm, Dsm)
As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm)
nxs = size(As,1)
nx̂ = model.nx
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)
Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
lastu0 = zeros(NT, model.nu)
lastu0 = zeros(NT, nu)
# x̂0 and x̂d are same object (updating x̂d will update x̂0):
x̂d = x̂0 = zeros(NT, model.nx)
x̂s = zeros(NT, nxs)
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
return new{NT, SM}(
model,
lastu0, x̂op, f̂op, x̂0, x̂d, x̂s,
i_ym, nx̂, nym, nyu, nxs,
As, Bs, Cs, Ds,
Â, B̂u, Ĉ, B̂d, D̂d,
Âs, B̂s
Âs, B̂s,
buffer
)
end
end
Expand Down
36 changes: 26 additions & 10 deletions src/estimator/kalman.jl
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,11 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
Q̂::Hermitian{NT, Matrix{NT}}
R̂::Hermitian{NT, Matrix{NT}}
K̂::Matrix{NT}
buffer::StateEstimatorBuffer{NT}
function SteadyKalmanFilter{NT, SM}(
model::SM, i_ym, nint_u, nint_ym, Q̂, R̂
) where {NT<:Real, SM<:LinModel}
nu, ny, nd = model.nu, model.ny, model.nd
nym, nyu = validate_ym(model, i_ym)
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
Expand All @@ -33,7 +35,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
validate_kfcov(nym, nx̂, Q̂, R̂)
K̂ = try
Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6
R̂_kalman = zeros(NT, model.ny, model.ny)
R̂_kalman = zeros(NT, ny, ny)
R̂_kalman[i_ym, i_ym] = R̂
ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂_kalman, R̂_kalman)[:, i_ym]
catch my_error
Expand All @@ -45,17 +47,19 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
rethrow()
end
end
lastu0 = zeros(NT, model.nu)
lastu0 = zeros(NT, nu)
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L)
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
return new{NT, SM}(
model,
lastu0, x̂op, f̂op, x̂0,
i_ym, nx̂, nym, nyu, nxs,
As, Cs_u, Cs_y, nint_u, nint_ym,
Â, B̂u, Ĉ, B̂d, D̂d,
Q̂, R̂,
K̂,
buffer
)
end
end
Expand Down Expand Up @@ -234,29 +238,33 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
R̂::Hermitian{NT, Matrix{NT}}
K̂::Matrix{NT}
M̂::Matrix{NT}
buffer::StateEstimatorBuffer{NT}
function KalmanFilter{NT, SM}(
model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂
) where {NT<:Real, SM<:LinModel}
nu, ny, nd = model.nu, model.ny, model.nd
nym, nyu = validate_ym(model, i_ym)
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
lastu0 = zeros(NT, model.nu)
lastu0 = zeros(NT, nu)
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L)
P̂_0 = Hermitian(P̂_0, :L)
P̂ = copy(P̂_0)
K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym)
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
return new{NT, SM}(
model,
lastu0, x̂op, f̂op, x̂0, P̂,
i_ym, nx̂, nym, nyu, nxs,
As, Cs_u, Cs_y, nint_u, nint_ym,
Â, B̂u, Ĉ, B̂d, D̂d,
P̂_0, Q̂, R̂,
K̂, M̂
K̂, M̂,
buffer
)
end
end
Expand Down Expand Up @@ -402,17 +410,19 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
γ::NT
m̂::Vector{NT}
Ŝ::Diagonal{NT, Vector{NT}}
buffer::StateEstimatorBuffer{NT}
function UnscentedKalmanFilter{NT, SM}(
model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ
) where {NT<:Real, SM<:SimModel{NT}}
nu, ny, nd = model.nu, model.ny, model.nd
nym, nyu = validate_ym(model, i_ym)
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ)
lastu0 = zeros(NT, model.nu)
lastu0 = zeros(NT, nu)
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L)
P̂_0 = Hermitian(P̂_0, :L)
Expand All @@ -421,6 +431,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
M̂ = Hermitian(zeros(NT, nym, nym), :L)
X̂0, Ŷ0m = zeros(NT, nx̂, nσ), zeros(NT, nym, nσ)
sqrtP̂ = LowerTriangular(zeros(NT, nx̂, nx̂))
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
return new{NT, SM}(
model,
lastu0, x̂op, f̂op, x̂0, P̂,
Expand All @@ -429,7 +440,8 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
Â, B̂u, Ĉ, B̂d, D̂d,
P̂_0, Q̂, R̂,
K̂, M̂, X̂0, Ŷ0m, sqrtP̂,
nσ, γ, m̂, Ŝ
nσ, γ, m̂, Ŝ,
buffer
)
end
end
Expand Down Expand Up @@ -698,23 +710,26 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
M̂::Matrix{NT}
F̂_û::Matrix{NT}
Ĥ ::Matrix{NT}
buffer::StateEstimatorBuffer{NT}
function ExtendedKalmanFilter{NT, SM}(
model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂
) where {NT<:Real, SM<:SimModel}
nu, ny, nd = model.nu, model.ny, model.nd
nym, nyu = validate_ym(model, i_ym)
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
lastu0 = zeros(NT, model.nu)
lastu0 = zeros(NT, nu)
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
P̂_0 = Hermitian(P̂_0, :L)
Q̂ = Hermitian(Q̂, :L)
R̂ = Hermitian(R̂, :L)
P̂ = copy(P̂_0)
K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym)
F̂_û, Ĥ = zeros(NT, nx̂+model.nu, nx̂), zeros(NT, model.ny, nx̂)
F̂_û, Ĥ = zeros(NT, nx̂+nu, nx̂), zeros(NT, ny, nx̂)
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
return new{NT, SM}(
model,
lastu0, x̂op, f̂op, x̂0, P̂,
Expand All @@ -723,7 +738,8 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
Â, B̂u, Ĉ, B̂d, D̂d,
P̂_0, Q̂, R̂,
K̂, M̂,
F̂_û, Ĥ
F̂_û, Ĥ,
buffer
)
end
end
Expand Down
8 changes: 6 additions & 2 deletions src/estimator/luenberger.jl
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,11 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
B̂d ::Matrix{NT}
D̂d ::Matrix{NT}
K̂::Matrix{NT}
buffer::StateEstimatorBuffer{NT}
function Luenberger{NT, SM}(
model, i_ym, nint_u, nint_ym, poles
) where {NT<:Real, SM<:LinModel}
nu, ny, nd = model.nu, model.ny, model.nd
nym, nyu = validate_ym(model, i_ym)
validate_luenberger(model, nint_u, nint_ym, poles)
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
Expand All @@ -34,15 +36,17 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
catch
error("Cannot compute the Luenberger gain K̂ with specified poles.")
end
lastu0 = zeros(NT, model.nu)
lastu0 = zeros(NT, nu)
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
return new{NT, SM}(
model,
lastu0, x̂op, f̂op, x̂0,
i_ym, nx̂, nym, nyu, nxs,
As, Cs_u, Cs_y, nint_u, nint_ym,
Â, B̂u, Ĉ, B̂d, D̂d,
K̂,
buffer
)
end
end
Expand Down
Loading
Loading