buffer StateEstimatorwith more attributes

Author: franckgaga

src/estimator/internal_model.jl

@@ -26,18 +26,19 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{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}(nx̂, nym)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
             lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, 

src/estimator/kalman.jl

@@ -26,6 +26,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{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)
@@ -34,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
@@ -46,10 +47,10 @@ 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}(nx̂, nym)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
             lastu0, x̂op, f̂op, x̂0, 
@@ -241,19 +242,20 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{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}(nx̂, nym)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
             lastu0, x̂op, f̂op, x̂0, P̂, 
@@ -412,14 +414,15 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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)
@@ -428,7 +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}(nx̂, nym)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model,
             lastu0, x̂op, f̂op, x̂0, P̂, 
@@ -711,21 +714,22 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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̂)
-        buffer = StateEstimatorBuffer{NT}(nx̂, nym)
+        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̂, 

src/estimator/luenberger.jl

@@ -24,6 +24,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{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)
@@ -35,9 +36,9 @@ 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}(nx̂, nym)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
             lastu0, x̂op, f̂op, x̂0,

src/estimator/mhe/construct.jl

@@ -106,7 +106,7 @@ struct MovingHorizonEstimator{
     function MovingHorizonEstimator{NT, SM, JM, CE}(
         model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, optim::JM, covestim::CE
     ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
-        nu, nd = model.nu, model.nd
+        nu, ny, nd = model.nu, model.ny, model.nd
         He < 1  && throw(ArgumentError("Estimation horizon He should be ≥ 1"))
         Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0"))
         nym, nyu = validate_ym(model, i_ym)
@@ -115,7 +115,7 @@ struct MovingHorizonEstimator{
         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̂, R̂ = Hermitian(Q̂, :L),  Hermitian(R̂, :L)
@@ -141,7 +141,7 @@ struct MovingHorizonEstimator{
         x̂0arr_old = zeros(NT, nx̂)
         P̂arr_old = copy(P̂_0)
         Nk = [0]
-        buffer = StateEstimatorBuffer{NT}(nx̂, nym)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         estim = new{NT, SM, JM, CE}(
             model, optim, con, covestim,  
             Z̃, lastu0, x̂op, f̂op, x̂0, 

src/state_estim.jl

@@ -19,8 +19,12 @@ julia> ŷ = kf()
 abstract type StateEstimator{NT<:Real} end
 
 struct StateEstimatorBuffer{NT<:Real}
+    u ::Vector{NT}
     x̂ ::Vector{NT}
     ym::Vector{NT}
+    ŷ ::Vector{NT}
+    d ::Vector{NT}
+    empty::Vector{NT}
 end
 
 @doc raw"""
@@ -30,10 +34,16 @@ Create a buffer for `StateEstimator` objects for estimated states and measured o
 
 The buffer is used to store intermediate results during simulation without allocating.
 """
-function StateEstimatorBuffer{NT}(nx̂::Int, nym::Int) where NT <: Real
+function StateEstimatorBuffer{NT}(
+    nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int
+) where NT <: Real
+    u  = Vector{NT}(undef, nu)
     x̂  = Vector{NT}(undef, nx̂)
     ym = Vector{NT}(undef, nym)
-    return StateEstimatorBuffer{NT}(x̂, ym)
+    ŷ  = Vector{NT}(undef, ny)
+    d  = Vector{NT}(undef, nd)
+    empty = Vector{NT}(undef, 0)
+    return StateEstimatorBuffer{NT}(u, x̂, ym, ŷ, d, empty)
 end
 
 const IntVectorOrInt = Union{Int, Vector{Int}}