From 9294918701a3a861c89f2b20887a296566a248eb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 15:53:50 -0400 Subject: [PATCH 01/13] debug: remove useless type annotation --- src/estimator/kalman.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3a7c1ec20..0e6152d49 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -273,7 +273,7 @@ function update_estimate!(estim::SteadyKalmanFilter, y0m, d0, u0) if !estim.direct correct_estimate_obsv!(estim, y0m, d0, estim.K̂) end - return predict_estimate_obsv!(estim::StateEstimator, y0m, d0, u0) + return predict_estimate_obsv!(estim, y0m, d0, u0) end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." From c1f73149879691697eeb517566648b3bcaaf00b6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 15:59:39 -0400 Subject: [PATCH 02/13] doc: details for `InternalModel` --- src/estimator/internal_model.jl | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 1ff3c4245..ed0b5f71d 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -253,10 +253,13 @@ function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ ) return nothing end -""" +@doc raw""" correct_estimate!(estim::InternalModel, y0m, d0) Compute the current stochastic output estimation `ŷs` for [`InternalModel`](@ref). + +It evaluates ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` and +``\mathbf{ŷ_s^u = 0}`` for the measured and unmeasured outputs, respectively. """ function correct_estimate!(estim::InternalModel, y0m, d0) ŷ0d = estim.buffer.ŷ From 0781f9340527a9d06057fe04bd9817d8861c48a1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 16:49:22 -0400 Subject: [PATCH 03/13] changed: renamed `corrected` field to `prepared` field This is more precise, it means that `preparestate!` was called. --- src/controller/execute.jl | 2 +- src/estimator/execute.jl | 8 ++++---- src/estimator/internal_model.jl | 8 ++++---- src/estimator/kalman.jl | 24 ++++++++++++------------ src/estimator/luenberger.jl | 6 +++--- src/estimator/manual.jl | 6 +++--- src/estimator/mhe/construct.jl | 6 +++--- 7 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 6732fe975..d25d799aa 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -68,7 +68,7 @@ function moveinput!( R̂y = Rhaty, R̂u = Rhatu ) - if mpc.estim.direct && !mpc.estim.corrected[] + if mpc.estim.direct && !mpc.estim.prepared[] @warn "preparestate! should be called before moveinput! with current estimators" end validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 3ad4bb5c1..a8bf3f77f 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -252,7 +252,7 @@ julia> ŷ = evaloutput(kf) ``` """ function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real - if estim.direct && !estim.corrected[] + if estim.direct && !estim.prepared[] @warn "preparestate! should be called before evaloutput with current estimators" end validate_args(estim.model, d) @@ -301,7 +301,7 @@ function preparestate!(estim::StateEstimator, ym, d=estim.buffer.empty) validate_args(estim, ym, d) y0m, d0 = remove_op!(estim, ym, d) correct_estimate!(estim, y0m, d0) - estim.corrected[] = true + estim.prepared[] = true end x̂ = estim.buffer.x̂ x̂ .= estim.x̂0 .+ estim.x̂op @@ -334,13 +334,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[] + if estim.direct && !estim.prepared[] 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 + estim.prepared[] = false x̂next = estim.buffer.x̂ x̂next .= estim.x̂0 .+ estim.x̂op return x̂next diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index ed0b5f71d..84c6f65cd 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -26,7 +26,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Âs::Matrix{NT} B̂s::Matrix{NT} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function InternalModel{NT}( model::SM, i_ym, Asm, Bsm, Csm, Dsm @@ -45,7 +45,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} x̂s, x̂snext = zeros(NT, nxs), zeros(NT, nxs) ŷs = zeros(NT, ny) direct = true # InternalModel always uses direct transmission from ym - corrected = [false] + prepared = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, @@ -54,7 +54,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} As, Bs, Cs, Ds, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Âs, B̂s, - direct, corrected, + direct, prepared, buffer ) end @@ -348,7 +348,7 @@ end # Compute estimated output with current stochastic estimate `estim.ŷs` for `InternalModel` function evaloutput(estim::InternalModel, d) - if !estim.corrected[] + if !estim.prepared[] @warn "preparestate! should be called before evaloutput with InternalModel" end validate_args(estim.model, d) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 0e6152d49..20c819323 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -30,7 +30,7 @@ struct SteadyKalmanFilter{ D̂dm ::Matrix{NT} K̂::Matrix{NT} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function SteadyKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true @@ -46,7 +46,7 @@ struct SteadyKalmanFilter{ K̂, P̂ = init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct) cov.P̂ .= P̂ x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - corrected = [false] + prepared = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM, KC}( model, @@ -56,7 +56,7 @@ struct SteadyKalmanFilter{ As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, K̂, - direct, corrected, + direct, prepared, buffer ) end @@ -333,7 +333,7 @@ struct KalmanFilter{ D̂dm ::Matrix{NT} K̂::Matrix{NT} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function KalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true @@ -347,7 +347,7 @@ struct KalmanFilter{ Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] K̂ = zeros(NT, nx̂, nym) - corrected = [false] + prepared = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM, KC}( model, @@ -357,7 +357,7 @@ struct KalmanFilter{ As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, K̂, - direct, corrected, + direct, prepared, buffer ) end @@ -555,7 +555,7 @@ struct UnscentedKalmanFilter{ m̂::Vector{NT} Ŝ::Diagonal{NT, Vector{NT}} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function UnscentedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; direct=true @@ -573,7 +573,7 @@ struct UnscentedKalmanFilter{ M̂ = Hermitian(zeros(NT, nym, nym), :L) X̂0, X̄0 = zeros(NT, nx̂, nσ), zeros(NT, nx̂, nσ) Ŷ0m, Ȳ0m = zeros(NT, nym, nσ), zeros(NT, nym, nσ) - corrected = [false] + prepared = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM, KC}( model, @@ -585,7 +585,7 @@ struct UnscentedKalmanFilter{ K̂, M̂, X̂0, X̄0, Ŷ0m, Ȳ0m, nσ, γ, m̂, Ŝ, - direct, corrected, + direct, prepared, buffer ) end @@ -932,7 +932,7 @@ struct ExtendedKalmanFilter{ linfuncF̂!::FF linfuncĤ!::HF direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function ExtendedKalmanFilter{NT}( model::SM, @@ -957,7 +957,7 @@ struct ExtendedKalmanFilter{ K̂ = zeros(NT, nx̂, nym) F̂_û, F̂ = zeros(NT, nx̂+nu, nx̂), zeros(NT, nx̂, nx̂) Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) - corrected = [false] + prepared = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM, KC, JB, FF, HF}( model, @@ -969,7 +969,7 @@ struct ExtendedKalmanFilter{ K̂, F̂_û, F̂, Ĥ, Ĥm, jacobian, linfuncF̂!, linfuncĤ!, - direct, corrected, + direct, prepared, buffer ) end diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 5e660de8a..1d6d104a6 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -22,7 +22,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} D̂dm ::Matrix{NT} K̂::Matrix{NT} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function Luenberger{NT, SM}( model, i_ym, nint_u, nint_ym, poles; direct=true @@ -41,7 +41,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} error("Cannot compute the Luenberger gain K̂ with specified poles.") end x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - corrected = [false] + prepared = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, @@ -50,7 +50,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, K̂, - direct, corrected, + direct, prepared, buffer ) end diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index 55ff87254..b604c3d7d 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -21,7 +21,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Ĉm ::Matrix{NT} D̂dm ::Matrix{NT} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function ManualEstimator{NT}( model::SM, i_ym, nint_u, nint_ym @@ -35,7 +35,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] direct = false - corrected = [true] + prepared = [true] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, @@ -43,7 +43,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, - direct, corrected, + direct, prepared, buffer ) end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 686e89c18..2b45e4489 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -129,7 +129,7 @@ struct MovingHorizonEstimator{ P̂arr_old ::Hermitian{NT, Matrix{NT}} Nk::Vector{Int} direct::Bool - corrected::Vector{Bool} + prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function MovingHorizonEstimator{NT}( model::SM, @@ -182,7 +182,7 @@ struct MovingHorizonEstimator{ x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(cov.P̂_0) Nk = [0] - corrected = [false] + prepared = [false] test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) estim = new{NT, SM, KC, JM, GB, JB, HB, PT, GCfunc, CE}( @@ -203,7 +203,7 @@ struct MovingHorizonEstimator{ X̂op, Y0m, Yem, U0, Ue, D0, De, Ŵ, X̂0_old, x̂0arr_old, P̂arr_old, Nk, - direct, corrected, + direct, prepared, buffer ) init_optimization!(estim, model, optim) From 2f155b135424d647bb7c782d3f7e5b98177b7551 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 17:29:29 -0400 Subject: [PATCH 04/13] added: support for `NaN` in `ym` for all `StateEstimator`s --- src/estimator/execute.jl | 4 +++- src/estimator/internal_model.jl | 13 +++++++------ src/estimator/kalman.jl | 22 +++++++++++++--------- src/estimator/luenberger.jl | 7 ++++--- src/estimator/manual.jl | 2 +- 5 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index a8bf3f77f..1260a25a1 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -297,12 +297,13 @@ julia> x̂ = preparestate!(estim1, [1]) ``` """ function preparestate!(estim::StateEstimator, ym, d=estim.buffer.empty) + isnothing(ym) && (ym = estim.buffer.ym; ym .= NaN) # nothing: skip correction step if estim.direct validate_args(estim, ym, d) y0m, d0 = remove_op!(estim, ym, d) correct_estimate!(estim, y0m, d0) - estim.prepared[] = true end + estim.prepared[] = true x̂ = estim.buffer.x̂ x̂ .= estim.x̂0 .+ estim.x̂op return x̂ @@ -334,6 +335,7 @@ 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) + isnothing(ym) && (ym = estim.buffer.ym; ym .= NaN) # nothing: skip correction step if estim.direct && !estim.prepared[] error("preparestate! must be called before updatestate! with direct=true option") end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 84c6f65cd..3e3ba125f 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -259,18 +259,19 @@ end Compute the current stochastic output estimation `ŷs` for [`InternalModel`](@ref). It evaluates ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` and -``\mathbf{ŷ_s^u = 0}`` for the measured and unmeasured outputs, respectively. +``\mathbf{ŷ_s^u = 0}`` for the measured and unmeasured outputs, respectively. If there +is a `NaN` in `y0m`, its associated stochastic output will be `0`. """ function correct_estimate!(estim::InternalModel, y0m, d0) ŷ0d = estim.buffer.ŷ ĥ!(ŷ0d, estim, estim.model, estim.x̂d, d0) ŷs = estim.ŷs - for j in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop - if j in estim.i_ym - i = estim.i_ym[j] - ŷs[j] = y0m[i] - ŷ0d[j] + for i in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop + if i in estim.i_ym + y0m_i = y0m[estim.i_ym[i]] + ŷs[i] = isfinite(y0m_i) ? y0m_i - ŷ0d[i] : 0 else - ŷs[j] = 0 + ŷs[i] = 0 end end return nothing diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 20c819323..09746d5e2 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -243,6 +243,7 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)``. See the docstr [`update_estimate!(::SteadyKalmanFilter, ::Any, ::Any)`](@ref) for the equations. """ function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) + any(isnan, y0m) && return nothing # skip correction step return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) end @@ -270,15 +271,15 @@ provided below. ``` """ function update_estimate!(estim::SteadyKalmanFilter, y0m, d0, u0) - if !estim.direct - correct_estimate_obsv!(estim, y0m, d0, estim.K̂) + if !estim.direct && all(isfinite, y0m) + correct_estimate_obsv!(estim, y0m, d0) end - return predict_estimate_obsv!(estim, y0m, d0, u0) + return predict_estimate_obsv!(estim, u0, d0) end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function correct_estimate_obsv!(estim::StateEstimator, y0m, d0, K̂) - Ĉm, D̂dm = estim.Ĉm, estim.D̂dm +function correct_estimate_obsv!(estim::StateEstimator, y0m, d0) + Ĉm, D̂dm, K̂ = estim.Ĉm, estim.D̂dm, estim.K̂ ŷ0m = @views estim.buffer.ŷ[estim.i_ym] # in-place operations to reduce allocations: mul!(ŷ0m, Ĉm, estim.x̂0) @@ -291,7 +292,7 @@ function correct_estimate_obsv!(estim::StateEstimator, y0m, d0, K̂) end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function predict_estimate_obsv!(estim::StateEstimator, _ , d0, u0) +function predict_estimate_obsv!(estim::StateEstimator, u0, d0) x̂0corr = estim.x̂0 Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d x̂0next = estim.buffer.x̂ @@ -472,6 +473,7 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation cova ``\mathbf{P̂}_{k}(k)``. """ function correct_estimate!(estim::KalmanFilter, y0m, d0) + any(isnan, y0m) && return nothing # skip correction step return correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end @@ -510,7 +512,7 @@ provided below, see [^2] for details. , Accessed 2024-08-08. """ function update_estimate!(estim::KalmanFilter, y0m, d0, u0) - if !estim.direct + if !estim.direct && all(isfinite, y0m) correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end return predict_estimate_kf!(estim, u0, d0, estim.Â) @@ -767,6 +769,7 @@ end Do the same but for the [`UnscentedKalmanFilter`](@ref). """ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) + any(isnan, y0m) && return nothing # skip correction step x̂0, P̂, R̂, K̂ = estim.x̂0, estim.cov.P̂, estim.cov.R̂, estim.K̂ nx̂ = estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ @@ -859,7 +862,7 @@ step is skipped if `estim.direct == true` since it's already done by the user. ISBN9780470045343. """ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) - if !estim.direct + if !estim.direct && all(isfinite, y0m) correct_estimate!(estim, y0m, d0) end x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ @@ -1136,6 +1139,7 @@ end Do the same but for the [`ExtendedKalmanFilter`](@ref). """ function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) + any(isnan, y0m) && return nothing # skip correction step x̂0 = estim.x̂0 cst_d0 = Constant(d0) ŷ0, Ĥ, Ĥm = estim.buffer.ŷ, estim.Ĥ, estim.Ĥm @@ -1184,7 +1188,7 @@ and prediction step equations are provided below. The correction step is skipped ``` """ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT<:Real - if !estim.direct + if !estim.direct && all(isfinite, y0m) correct_estimate!(estim, y0m, d0) end cst_u0, cst_d0 = Constant(u0), Constant(d0) diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 1d6d104a6..b1b6d46b0 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -121,6 +121,7 @@ end Identical to [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ function correct_estimate!(estim::Luenberger, y0m, d0) + any(isnan, y0m) && return nothing # skip correction step return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) end @@ -131,10 +132,10 @@ end Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ function update_estimate!(estim::Luenberger, y0m, d0, u0) - if !estim.direct - correct_estimate_obsv!(estim, y0m, d0, estim.K̂) + if !estim.direct && all(isfinite, y0m) + correct_estimate_obsv!(estim, y0m, d0) end - return predict_estimate_obsv!(estim, y0m, d0, u0) + return predict_estimate_obsv!(estim, u0, d0) end "Throw an error if P̂ != nothing." diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index b604c3d7d..0028a7d18 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -149,7 +149,7 @@ end Do nothing for [`ManualEstimator`](@ref). """ -update_estimate!(::ManualEstimator, y0m, d0, u0) = nothing +update_estimate!(::ManualEstimator,_,_,_) = nothing "Throw an error if P̂ != nothing." function setstate_cov!(::ManualEstimator, P̂) From ddef23eca6c2728a439a8a0b3cb824ba2c17ea0a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 18:06:09 -0400 Subject: [PATCH 05/13] debug: correct signature --- src/estimator/kalman.jl | 2 +- src/estimator/luenberger.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 09746d5e2..75f172227 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -244,7 +244,7 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)``. See the docstr """ function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) any(isnan, y0m) && return nothing # skip correction step - return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) + return correct_estimate_obsv!(estim, y0m, d0) end @doc raw""" diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index b1b6d46b0..61f364d7c 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -122,7 +122,7 @@ Identical to [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenb """ function correct_estimate!(estim::Luenberger, y0m, d0) any(isnan, y0m) && return nothing # skip correction step - return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) + return correct_estimate_obsv!(estim, y0m, d0) end From 7c454fe7b3d995f28a726eeb63d3a8cbe176f885 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 21:41:37 -0400 Subject: [PATCH 06/13] doc: `isnothing(ym)` in `prepare`/`updatestate!` docstring --- Project.toml | 2 +- src/estimator/execute.jl | 12 ++++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/Project.toml b/Project.toml index 0818f1aa9..9b0e0adbf 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.7.0" +version = "2.8.0" authors = ["Francis Gagnon"] [deps] diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 1260a25a1..206dd6c7a 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -278,7 +278,9 @@ delayed/predictor (2.) formulation: 1. If `estim.direct` is `true`, it removes the operating points with [`remove_op!`](@ref), calls [`correct_estimate!`](@ref), and returns the corrected state estimate - ``\mathbf{x̂}_k(k)``. + ``\mathbf{x̂}_k(k)``. The correction step is skipped if `isnothing(ym)`, in case the + measured outputs are temporarily unavailable. The [`MovingHorizonEstimator`](@ref) and + [`InternalModel`](@ref) also support partial correction with `NaN` values in `ym`. 2. Else, it does nothing and returns the current best estimate ``\mathbf{x̂}_{k-1}(k)``. # Examples @@ -318,9 +320,11 @@ This function should be called at the end of each discrete time step. It removes operating points with [`remove_op!`](@ref), calls [`update_estimate!`](@ref) and returns the state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. The method [`preparestate!`](@ref) should be called prior to this one to correct the estimate when applicable (if -`estim.direct == true`). Note that the [`MovingHorizonEstimator`](@ref) with the default -`direct=true` option is not able to estimate ``\mathbf{x̂}_k(k+1)``, the returned value -is therefore the current corrected state ``\mathbf{x̂}_k(k)``. +`estim.direct == true`). If `isnothing(ym)`, only the prediction step is performed in +[`update_estimate!`](@ref), in case the measured outputs are temporarily unavailable. Note +that the [`MovingHorizonEstimator`](@ref) with the default `direct=true` option is not able +to estimate ``\mathbf{x̂}_k(k+1)``, the returned value is therefore the current corrected +state ``\mathbf{x̂}_k(k)``. # Examples ```jldoctest From 4f2b0ff69d87a3f8aade35c27c74e3a099b272a4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 21:51:44 -0400 Subject: [PATCH 07/13] doc: swaping `initstate!` and `preparestate!` order It is more natural to see `initstate!` first. --- docs/src/public/generic_func.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/src/public/generic_func.md b/docs/src/public/generic_func.md index bd07d608a..f29abaa86 100644 --- a/docs/src/public/generic_func.md +++ b/docs/src/public/generic_func.md @@ -21,22 +21,22 @@ evaloutput ## Change State x -### Prepare State x +### Init State x ```@docs -preparestate! +initstate! ``` -### Update State x +### Prepare State x ```@docs -updatestate! +preparestate! ``` -### Init State x +### Update State x ```@docs -initstate! +updatestate! ``` ### Set State x From 17106e7026dea4f8f87a6d6572a92d1b8ac6a0ed Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 21:52:57 -0400 Subject: [PATCH 08/13] doc: minor change --- src/estimator/execute.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 206dd6c7a..33616266c 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -321,7 +321,7 @@ operating points with [`remove_op!`](@ref), calls [`update_estimate!`](@ref) and state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. The method [`preparestate!`](@ref) should be called prior to this one to correct the estimate when applicable (if `estim.direct == true`). If `isnothing(ym)`, only the prediction step is performed in -[`update_estimate!`](@ref), in case the measured outputs are temporarily unavailable. Note +[`update_estimate!`](@ref), for when the measured outputs are temporarily unavailable. Note that the [`MovingHorizonEstimator`](@ref) with the default `direct=true` option is not able to estimate ``\mathbf{x̂}_k(k+1)``, the returned value is therefore the current corrected state ``\mathbf{x̂}_k(k)``. From 96ae20a3f66e3d94187b6f4c7ab7d31b6ad5dc9f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 21:56:01 -0400 Subject: [PATCH 09/13] doc: `NaN` values in `InternalModel` docstring --- src/estimator/internal_model.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 3e3ba125f..53b0a8c68 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -98,8 +98,9 @@ InternalModel estimator with a sample time Ts = 0.5 s: supposes 1 integrator per measured outputs by default, assuming that the current stochastic estimate ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` is constant in the future. This is the dynamic matrix control (DMC) strategy, which is simple but sometimes - too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. The following - block diagram summarizes the internal model structure. + too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. If there + is a `NaN` in ``\mathbf{y^m}(k)``, its associated stochastic output will be `0`. The + following block diagram summarizes the internal model structure. ![block diagram of the internal model structure](../assets/imc.svg) """ From 3f61b28898b23c0e8a7ce24e8532817abe776201 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 17 Jul 2026 22:02:55 -0400 Subject: [PATCH 10/13] changed: order of `update_estimate!` arguments --- src/estimator/execute.jl | 2 +- src/estimator/internal_model.jl | 4 ++-- src/estimator/kalman.jl | 18 +++++++++--------- src/estimator/luenberger.jl | 4 ++-- src/estimator/manual.jl | 2 +- src/estimator/mhe/execute.jl | 6 +++--- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 33616266c..96365cff0 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -345,7 +345,7 @@ function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) end validate_args(estim, ym, d, u) y0m, d0, u0 = remove_op!(estim, ym, d, u) - update_estimate!(estim, y0m, d0, u0) + update_estimate!(estim, u0, y0m, d0) estim.prepared[] = false x̂next = estim.buffer.x̂ x̂next .= estim.x̂0 .+ estim.x̂op diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 53b0a8c68..e13f60864 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -279,7 +279,7 @@ function correct_estimate!(estim::InternalModel, y0m, d0) end @doc raw""" - update_estimate!(estim::InternalModel, _ , d0, u0) + update_estimate!(estim::InternalModel, u0, _ , d0) Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`. @@ -293,7 +293,7 @@ The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``. See [`init_internalmodel`](@ref) for details. """ -function update_estimate!(estim::InternalModel, _ , d0, u0) +function update_estimate!(estim::InternalModel, u0, _ , d0) model = estim.model x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 75f172227..633c7435b 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -240,7 +240,7 @@ end Correct `estim.x̂0` with measured outputs `y0m` and disturbances `d0` for current time step. It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)``. See the docstring of -[`update_estimate!(::SteadyKalmanFilter, ::Any, ::Any)`](@ref) for the equations. +[`update_estimate!(::SteadyKalmanFilter)`](@ref) for the equations. """ function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) any(isnan, y0m) && return nothing # skip correction step @@ -248,7 +248,7 @@ function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) end @doc raw""" - update_estimate!(estim::SteadyKalmanFilter, y0m, d0, u0) + update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) Update `estim.x̂0` estimate with current inputs `u0`, measured outputs `y0m` and dist. `d0`. @@ -270,7 +270,7 @@ provided below. \mathbf{x̂}_{k}(k+1) = \mathbf{Â x̂}_{k}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) ``` """ -function update_estimate!(estim::SteadyKalmanFilter, y0m, d0, u0) +function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) if !estim.direct && all(isfinite, y0m) correct_estimate_obsv!(estim, y0m, d0) end @@ -479,7 +479,7 @@ end @doc raw""" - update_estimate!(estim::KalmanFilter, y0m, d0, u0) + update_estimate!(estim::KalmanFilter, u0, y0m, d0) Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.cov.P̂`. @@ -511,7 +511,7 @@ provided below, see [^2] for details. [^2]: "Kalman Filter", *Wikipedia: The Free Encyclopedia*, , Accessed 2024-08-08. """ -function update_estimate!(estim::KalmanFilter, y0m, d0, u0) +function update_estimate!(estim::KalmanFilter, u0, y0m, d0) if !estim.direct && all(isfinite, y0m) correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end @@ -818,7 +818,7 @@ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) end @doc raw""" - update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) + update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.cov.P̂`. @@ -861,7 +861,7 @@ step is skipped if `estim.direct == true` since it's already done by the user. Kalman, H∞, and Nonlinear Approaches", John Wiley & Sons, p. 433–459, , ISBN9780470045343. """ -function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) +function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) if !estim.direct && all(isfinite, y0m) correct_estimate!(estim, y0m, d0) end @@ -1150,7 +1150,7 @@ end @doc raw""" - update_estimate!(estim::ExtendedKalmanFilter, y0m, d0, u0) + update_estimate!(estim::ExtendedKalmanFilter, u0, y0m, d0) Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.cov.P̂`. @@ -1187,7 +1187,7 @@ and prediction step equations are provided below. The correction step is skipped \end{aligned} ``` """ -function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT<:Real +function update_estimate!(estim::ExtendedKalmanFilter{NT}, u0, y0m, d0) where NT<:Real if !estim.direct && all(isfinite, y0m) correct_estimate!(estim, y0m, d0) end diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 61f364d7c..56554ac3e 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -127,11 +127,11 @@ end """ - update_estimate!(estim::Luenberger, y0m, d0, u0) + update_estimate!(estim::Luenberger, u0, y0m, d0) Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ -function update_estimate!(estim::Luenberger, y0m, d0, u0) +function update_estimate!(estim::Luenberger, u0, y0m, d0) if !estim.direct && all(isfinite, y0m) correct_estimate_obsv!(estim, y0m, d0) end diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index 0028a7d18..e5b0e2d79 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -145,7 +145,7 @@ function ManualEstimator( end """ - update_estimate!(estim::ManualEstimator, y0m, d0, u0) + update_estimate!(estim::ManualEstimator, u0, y0m, d0) Do nothing for [`ManualEstimator`](@ref). """ diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c40ad8328..7d2fbcb80 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -53,7 +53,7 @@ function correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) end @doc raw""" - update_estimate!(estim::MovingHorizonEstimator, y0m, d0, u0) + update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0) Update [`MovingHorizonEstimator`](@ref) state `estim.x̂0`. @@ -67,7 +67,7 @@ step ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated using `estim.covestim` objec also stores `u0` at `estim.lastu0`, so it can be added to the data window at the next time step in [`correct_estimate!`](@ref). """ -function update_estimate!(estim::MovingHorizonEstimator, y0m, d0, u0) +function update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0) if !estim.direct add_data_windows!(estim, y0m, d0, u0) initpred!(estim, estim.model) @@ -747,7 +747,7 @@ function update_cov!(estim::MovingHorizonEstimator) estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.cov.P̂ .= estim.P̂arr_old try - update_estimate!(estim.covestim, y0marr, d0arr, u0arr) + update_estimate!(estim.covestim, u0arr, y0marr, d0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ invert_cov!(estim, estim.covestim) From 5ba0f36dfcbb4bb21ea38b54f76dc00d83f1ad45 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 18 Jul 2026 10:58:02 -0400 Subject: [PATCH 11/13] added: `@warn` with `NaN` in `ym` for all estimators Most state estimator skip the correction step altogether, except `InternalModel` and `MovingHorizonEstimator`, supporting partial correction out-of-the-box. I choose to customize the warning message for each estimator types for this reason. --- src/estimator/internal_model.jl | 3 +++ src/estimator/kalman.jl | 24 ++++++++++++++++++------ src/estimator/luenberger.jl | 5 ++++- 3 files changed, 25 insertions(+), 7 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index e13f60864..d448efcc7 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -264,6 +264,9 @@ It evaluates ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` and is a `NaN` in `y0m`, its associated stochastic output will be `0`. """ function correct_estimate!(estim::InternalModel, y0m, d0) + if !all(isfinite, y0m) + @warn "NaN values in the internal model measurements ym: assigning them ŷs=0" + end ŷ0d = estim.buffer.ŷ ĥ!(ŷ0d, estim, estim.model, estim.x̂d, d0) ŷs = estim.ŷs diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 633c7435b..cd6d3595a 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -243,7 +243,10 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)``. See the docstr [`update_estimate!(::SteadyKalmanFilter)`](@ref) for the equations. """ function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) - any(isnan, y0m) && return nothing # skip correction step + if any(isnan, y0m) + @warn "NaN values in the Kalman filter measurements ym: skipping correction step" + return nothing + end return correct_estimate_obsv!(estim, y0m, d0) end @@ -278,7 +281,7 @@ function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function correct_estimate_obsv!(estim::StateEstimator, y0m, d0) +function correct_estimate_obsv!(estim::Union{SteadyKalmanFilter, Luenberger}, y0m, d0) Ĉm, D̂dm, K̂ = estim.Ĉm, estim.D̂dm, estim.K̂ ŷ0m = @views estim.buffer.ŷ[estim.i_ym] # in-place operations to reduce allocations: @@ -292,7 +295,7 @@ function correct_estimate_obsv!(estim::StateEstimator, y0m, d0) end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function predict_estimate_obsv!(estim::StateEstimator, u0, d0) +function predict_estimate_obsv!(estim::Union{SteadyKalmanFilter, Luenberger}, u0, d0) x̂0corr = estim.x̂0 Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d x̂0next = estim.buffer.x̂ @@ -473,7 +476,10 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation cova ``\mathbf{P̂}_{k}(k)``. """ function correct_estimate!(estim::KalmanFilter, y0m, d0) - any(isnan, y0m) && return nothing # skip correction step + if any(isnan, y0m) + @warn "NaN values in the Kalman filter measurements ym: skipping correction step" + return nothing + end return correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end @@ -769,7 +775,10 @@ end Do the same but for the [`UnscentedKalmanFilter`](@ref). """ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) - any(isnan, y0m) && return nothing # skip correction step + if any(isnan, y0m) + @warn "NaN values in the Kalman filter measurements ym: skipping correction step" + return nothing + end x̂0, P̂, R̂, K̂ = estim.x̂0, estim.cov.P̂, estim.cov.R̂, estim.K̂ nx̂ = estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ @@ -1139,7 +1148,10 @@ end Do the same but for the [`ExtendedKalmanFilter`](@ref). """ function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) - any(isnan, y0m) && return nothing # skip correction step + if any(isnan, y0m) + @warn "NaN values in the Kalman filter measurements ym: skipping correction step" + return nothing + end x̂0 = estim.x̂0 cst_d0 = Constant(d0) ŷ0, Ĥ, Ĥm = estim.buffer.ŷ, estim.Ĥ, estim.Ĥm diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 56554ac3e..2eaa9d3a8 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -121,7 +121,10 @@ end Identical to [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ function correct_estimate!(estim::Luenberger, y0m, d0) - any(isnan, y0m) && return nothing # skip correction step + if any(isnan, y0m) + @warn "NaN values in the Luenberger measurements ym: skipping correction step" + return nothing + end return correct_estimate_obsv!(estim, y0m, d0) end From 5667bd04448d2d215fc60b3a6455b53922d8c21b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 18 Jul 2026 11:30:52 -0400 Subject: [PATCH 12/13] test: `ym` with `NaN` tests for all estimators --- test/2_test_state_estim.jl | 42 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index c9a41e7e4..ceb8565e2 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -112,6 +112,13 @@ end @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) @test_throws ArgumentError updatestate!(kalmanfilter1, [10, 50]) + kalmanfilter4 = SteadyKalmanFilter(linmodel, nint_ym=[1, 1], direct=true) + kalmanfilter4.x̂0 .= 7 + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + preparestate!(kalmanfilter4, [55, NaN]) + ) + @test all(kalmanfilter4.x̂0 .≈ 7) end @testitem "SKF set model" setup=[SetupMPCtests] begin @@ -238,6 +245,13 @@ end @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) @test_throws ArgumentError updatestate!(kalmanfilter1, [10, 50]) + kalmanfilter4 = KalmanFilter(linmodel, direct=true) + kalmanfilter4.x̂0 .= 7 + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + preparestate!(kalmanfilter4, [55, NaN]) + ) + @test all(kalmanfilter4.x̂0 .≈ 7) end @testitem "KF set model" setup=[SetupMPCtests] begin @@ -355,6 +369,13 @@ end @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) @test_throws ErrorException setstate!(lo1, [1,2,3,4], diagm(.1:.1:.4)) + lo4 = Luenberger(linmodel, nint_ym=[1, 1], direct=true) + lo4.x̂0 .= 7 + @test_logs( + (:warn, "NaN values in the Luenberger measurements ym: skipping correction step"), + preparestate!(lo4, [55, NaN]) + ) + @test all(lo4.x̂0 .≈ 7) end @testitem "Luenb. set model" setup=[SetupMPCtests] begin @@ -474,6 +495,13 @@ end @test x̂ ≈ [0] @test isa(x̂, Vector{Float32}) @test_throws ErrorException setstate!(internalmodel1, [1,2,3,4], diagm(.1:.1:.4)) + internalmodel4 = InternalModel(linmodel) + internalmodel4.ŷs .= 7 + @test_logs( + (:warn, "NaN values in the internal model measurements ym: assigning them ŷs=0"), + preparestate!(internalmodel4, [50+7, NaN]) + ) + @test internalmodel4.ŷs ≈ [7, 0] end @testitem "IM set model" setup=[SetupMPCtests] begin @@ -616,6 +644,13 @@ end x̂ = updatestate!(ukf3, [0], [0]) @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) + ukf4 = UnscentedKalmanFilter(linmodel, direct=true) + ukf4.x̂0 .= 7 + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + preparestate!(ukf4, [55, NaN]) + ) + @test all(ukf4.x̂0 .≈ 7) end @testitem "UKF set model" setup=[SetupMPCtests] begin @@ -777,6 +812,13 @@ end @test updatestate!(ekf4, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9 preparestate!(ekf4, [50, 30]) @test evaloutput(ekf4) ≈ ekf4() ≈ [50, 30] + ekf5 = ExtendedKalmanFilter(linmodel, direct=true) + ekf5.x̂0 .= 7 + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + preparestate!(ekf5, [55, NaN]) + ) + @test all(ekf5.x̂0 .≈ 7) end @testitem "EKF set model" setup=[SetupMPCtests] begin From 690296f0581f9d4b0918efd3629e871acdd1d1a1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 18 Jul 2026 11:46:25 -0400 Subject: [PATCH 13/13] debug: `Luenberger` is not defined at this point in the files --- src/estimator/kalman.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index cd6d3595a..3a80dcf96 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -281,7 +281,7 @@ function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function correct_estimate_obsv!(estim::Union{SteadyKalmanFilter, Luenberger}, y0m, d0) +function correct_estimate_obsv!(estim::StateEstimator, y0m, d0) Ĉm, D̂dm, K̂ = estim.Ĉm, estim.D̂dm, estim.K̂ ŷ0m = @views estim.buffer.ŷ[estim.i_ym] # in-place operations to reduce allocations: @@ -295,7 +295,7 @@ function correct_estimate_obsv!(estim::Union{SteadyKalmanFilter, Luenberger}, y0 end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function predict_estimate_obsv!(estim::Union{SteadyKalmanFilter, Luenberger}, u0, d0) +function predict_estimate_obsv!(estim::StateEstimator, u0, d0) x̂0corr = estim.x̂0 Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d x̂0next = estim.buffer.x̂