weird bug: the simulations in my own test script are slightly different, to investigate

Author: franckgaga

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.model.buffer.u, estim.model.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"""
@@ -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
@@ -178,11 +181,11 @@ julia> ŷ = evaloutput(kf)
 """
 function evaloutput(estim::StateEstimator{NT}, d=estim.model.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.model.buffer.y, estim.model.buffer.d
+    d0 .= d .- estim.model.dop
     ĥ!(ŷ0, estim, estim.model, estim.x̂0, d0)
-    ŷ   = ŷ0
-    ŷ .+= estim.model.yop
+    ŷ  = estim.model.buffer.y
+    ŷ .= ŷ0 .+ estim.model.yop
     return ŷ
 end
 
@@ -207,7 +210,7 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
  0.0
 ```
 """
-function updatestate!(estim::StateEstimator, u, ym, d=[])
+function updatestate!(estim::StateEstimator, u, ym, d=estim.model.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)

src/estimator/internal_model.jl

@@ -22,6 +22,7 @@ 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}
@@ -36,13 +37,15 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         # 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)
         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,6 +22,7 @@ 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}
@@ -48,14 +49,16 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         lastu0 = zeros(NT, model.nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
         Q̂, R̂ = Hermitian(Q̂, :L),  Hermitian(R̂, :L)
+        buffer = StateEstimatorBuffer{NT}(nx̂, nym)
         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,6 +237,7 @@ 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}
@@ -249,14 +253,16 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         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)
         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,6 +408,7 @@ 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}}
@@ -421,6 +428,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)
         return new{NT, SM}(
             model,
             lastu0, x̂op, f̂op, x̂0, P̂, 
@@ -429,7 +437,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,6 +707,7 @@ 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}
@@ -715,6 +725,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         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)
         return new{NT, SM}(
             model,
             lastu0, x̂op, f̂op, x̂0, P̂, 
@@ -723,7 +734,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,6 +20,7 @@ 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}
@@ -36,13 +37,15 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         end
         lastu0 = zeros(NT, model.nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
+        buffer = StateEstimatorBuffer{NT}(nx̂, nym)
         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

src/estimator/mhe/construct.jl

@@ -102,6 +102,7 @@ struct MovingHorizonEstimator{
     x̂0arr_old::Vector{NT}
     P̂arr_old ::Hermitian{NT, Matrix{NT}}
     Nk::Vector{Int}
+    buffer::StateEstimatorBuffer{NT}
     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}}
@@ -140,6 +141,7 @@ struct MovingHorizonEstimator{
         x̂0arr_old = zeros(NT, nx̂)
         P̂arr_old = copy(P̂_0)
         Nk = [0]
+        buffer = StateEstimatorBuffer{NT}(nx̂, nym)
         estim = new{NT, SM, JM, CE}(
             model, optim, con, covestim,  
             Z̃, lastu0, x̂op, f̂op, x̂0, 
@@ -151,7 +153,8 @@ struct MovingHorizonEstimator{
             H̃, q̃, p,
             P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt,
             X̂op, X̂0, Y0m, U0, D0, Ŵ, 
-            x̂0arr_old, P̂arr_old, Nk
+            x̂0arr_old, P̂arr_old, Nk,
+            buffer
         )
         init_optimization!(estim, model, optim)
         return estim

src/sim_model.jl

@@ -29,13 +29,13 @@ struct SimModelBuffer{NT<:Real}
 end
 
 @doc raw"""
-    SimModelBuffer(nu, nx, ny, nd) -> SimModelBuffer{NT}
+    SimModelBuffer(nu::Int, nx::Int, ny::Int, nd::Int) -> SimModelBuffer{NT}
 
 Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances.
 
 The buffer is used to store intermediate results during simulation without allocating.
 """
-function SimModelBuffer{NT}(nu, nx, ny, nd) where NT <: Real
+function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int) where NT <: Real
     u = Vector{NT}(undef, nu)
     x = Vector{NT}(undef, nx)
     y = Vector{NT}(undef, ny)

src/state_estim.jl

@@ -18,6 +18,24 @@ julia> ŷ = kf()
 """
 abstract type StateEstimator{NT<:Real} end
 
+struct StateEstimatorBuffer{NT<:Real}
+    x̂ ::Vector{NT}
+    ym::Vector{NT}
+end
+
+@doc raw"""
+    StateEstimatorBuffer(nx̂::Int, nym::Int) -> StateEstimatorBuffer{NT}
+
+Create a buffer for `StateEstimator` objects for estimated states and measured outputs.
+
+The buffer is used to store intermediate results during simulation without allocating.
+"""
+function StateEstimatorBuffer{NT}(nx̂::Int, nym::Int) where NT <: Real
+    x̂  = Vector{NT}(undef, nx̂)
+    ym = Vector{NT}(undef, nym)
+    return StateEstimatorBuffer{NT}(x̂, ym)
+end
+
 const IntVectorOrInt = Union{Int, Vector{Int}}
 
 function Base.show(io::IO, estim::StateEstimator)