added: error throw if direct==true and preparestate! is not called

Author: franckgaga

src/estimator/execute.jl

@@ -227,7 +227,8 @@ function preparestate!(estim::StateEstimator, ym, d=estim.model.dop)
     if estim.direct
         validate_args(estim, ym, d)
         y0m, d0 = remove_op!(estim, ym, d)
-        correct_estimate!(estim, y0m, d0) # compute x̂0corr
+        correct_estimate!(estim, y0m, d0)
+        estim.corrected[] = true
     end
     x̂  = estim.buffer.x̂
     x̂ .= estim.x̂0 .+ estim.x̂op
@@ -258,9 +259,13 @@ julia> x̂ = updatestate!(kf, u, ym) # x̂[2] is the integrator state (nint_ym a
 ```
 """
 function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
+    if estim.direct && !estim.corrected[]
+        error("preparestate! must be called before updatestate! with direct=true option")
+    end
     validate_args(estim, ym, d, u)
     y0m, d0, u0 = remove_op!(estim, ym, d, u)
     update_estimate!(estim, y0m, d0, u0)
+    estim.corrected[] = false
     x̂next  = estim.buffer.x̂
     x̂next .= estim.x̂0 .+ estim.x̂op
     return x̂next

src/estimator/internal_model.jl

@@ -25,6 +25,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     Âs::Matrix{NT}
     B̂s::Matrix{NT}
     direct::Bool
+    corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function InternalModel{NT, SM}(
         model::SM, i_ym, Asm, Bsm, Csm, Dsm
@@ -43,6 +44,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         x̂s = zeros(NT, nxs)
         y0m, d0 = zeros(NT, nym), zeros(NT, model.nd)
         direct = true # InternalModel always uses direct transmission from ym
+        corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
@@ -52,7 +54,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             As, Bs, Cs, Ds, 
             Â, B̂u, Ĉ, B̂d, D̂d,
             Âs, B̂s,
-            direct,
+            direct, corrected,
             buffer
         )
     end

src/estimator/kalman.jl

@@ -23,6 +23,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     R̂::Hermitian{NT, Matrix{NT}}
     K̂::Matrix{NT}
     direct::Bool
+    corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function SteadyKalmanFilter{NT, SM}(
         model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true
@@ -51,6 +52,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         lastu0 = zeros(NT, nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
         Q̂, R̂ = Hermitian(Q̂, :L),  Hermitian(R̂, :L)
+        corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
@@ -60,7 +62,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
             Â, B̂u, Ĉ, B̂d, D̂d,
             Q̂, R̂,
             K̂,
-            direct,
+            direct, corrected,
             buffer
         )
     end
@@ -271,6 +273,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     K̂::Matrix{NT}
     M̂::Matrix{NT}
     direct::Bool
+    corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function KalmanFilter{NT, SM}(
         model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true
@@ -288,6 +291,7 @@ 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)
+        corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
@@ -297,7 +301,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
             Â, B̂u, Ĉ, B̂d, D̂d,
             P̂_0, Q̂, R̂,
             K̂, M̂,
-            direct,
+            direct, corrected,
             buffer
         )
     end
@@ -459,6 +463,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     m̂::Vector{NT}
     Ŝ::Diagonal{NT, Vector{NT}}
     direct::Bool
+    corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function UnscentedKalmanFilter{NT, SM}(
         model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct=true
@@ -480,6 +485,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̂))
+        corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model,
@@ -490,7 +496,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             P̂_0, Q̂, R̂,
             K̂, M̂, X̂0, Ŷ0m, sqrtP̂,
             nσ, γ, m̂, Ŝ,
-            direct,
+            direct, corrected,
             buffer
         )
     end
@@ -777,6 +783,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     F̂_û::Matrix{NT}
     Ĥ  ::Matrix{NT}
     direct::Bool
+    corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function ExtendedKalmanFilter{NT, SM}(
         model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true
@@ -796,6 +803,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̂+nu, nx̂), zeros(NT, ny, nx̂)
+        corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model,
@@ -806,7 +814,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             P̂_0, Q̂, R̂,
             K̂, M̂,
             F̂_û, Ĥ,
-            direct,
+            direct, corrected,
             buffer
         )
     end
@@ -1007,7 +1015,7 @@ end
 """
     update_estimate_kf!(estim::StateEstimator, y0m, d0, u0, Ĉm, Â)
 
-Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
+Update time-varying/extended Kalman Filter estimates with augmented `Ĉm` and `Â` matrices.
 
 Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter`](@ref).
 They update the state `x̂` and covariance `P̂` with the same equations. The extended filter

src/estimator/luenberger.jl

@@ -21,6 +21,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     D̂d  ::Matrix{NT}
     K̂::Matrix{NT}
     direct::Bool
+    corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function Luenberger{NT, SM}(
         model, i_ym, nint_u, nint_ym, poles; direct=true
@@ -39,6 +40,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         end
         lastu0 = zeros(NT, nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
+        corrected = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
         return new{NT, SM}(
             model, 
@@ -47,7 +49,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
             As, Cs_u, Cs_y, nint_u, nint_ym,
             Â, B̂u, Ĉ, B̂d, D̂d,
             K̂,
-            direct,
+            direct, corrected,
             buffer
         )
     end

src/estimator/mhe/construct.jl

@@ -103,6 +103,7 @@ struct MovingHorizonEstimator{
     P̂arr_old ::Hermitian{NT, Matrix{NT}}
     Nk::Vector{Int}
     direct::Bool
+    corrected::Vector{Bool}
     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;
@@ -144,6 +145,7 @@ struct MovingHorizonEstimator{
         P̂arr_old = copy(P̂_0)
         Nk = [0]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
+        corrected = [false]
         estim = new{NT, SM, JM, CE}(
             model, optim, con, covestim,  
             Z̃, lastu0, x̂op, f̂op, x̂0, 
@@ -156,7 +158,7 @@ struct MovingHorizonEstimator{
             P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt,
             X̂op, X̂0, Y0m, U0, D0, Ŵ, 
             x̂0arr_old, P̂arr_old, Nk,
-            direct,
+            direct, corrected,
             buffer
         )
         init_optimization!(estim, model, optim)