Merge pull request #92 from JuliaControl/buffer_struct

added: `SimModelBuffer` and `StateEsimatorBuffer` objects to reduce the allocations

Author: franckgaga

src/controller/execute.jl

@@ -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.
 
@@ -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
@@ -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,
@@ -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"))

src/estimator/execute.jl

@@ -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"""
@@ -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`.
 
@@ -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 ----
@@ -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
@@ -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`.
 
@@ -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`. 
 
@@ -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

src/estimator/internal_model.jl

@@ -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

src/estimator/kalman.jl

@@ -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)
@@ -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
@@ -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̂
+            K̂,
+            buffer
         )
     end
 end
@@ -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
@@ -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)
@@ -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̂, 
@@ -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
@@ -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̂, 
@@ -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

src/estimator/luenberger.jl

@@ -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)
@@ -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̂
+            K̂,
+            buffer
         )
     end
 end