From 822db863a64f41171c52500eb1f46a75f660f899 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 11:34:11 -0500 Subject: [PATCH 01/15] added: `verif_obsv=true` kwarg to all `LinModel` state observers --- src/estimator/construct.jl | 7 +++- src/estimator/kalman.jl | 74 +++++++++++++++++++++++----------- src/estimator/luenberger.jl | 13 +++--- src/estimator/manual.jl | 12 ++++-- src/estimator/mhe/construct.jl | 35 +++++++++++----- test/2_test_state_estim.jl | 2 + 6 files changed, 98 insertions(+), 45 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index c6a19aba9..952c89963 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -375,8 +375,11 @@ julia> default_nint(model) |> println [1, 0, 1] ``` """ -function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0) +function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true) validate_ym(model, i_ym) + if !verify_obsv + return iszero(nint_u) ? fill(1, length(i_ym)) : fill(0, length(i_ym)) + end nint_ym = fill(0, length(i_ym)) for i in eachindex(i_ym) nint_ym[i] = 1 @@ -400,7 +403,7 @@ integrator on each measured output. Theres is no verification the augmented model remains observable. The resulting [`StateEstimator`](@ref) object should be assessed separately with e.g.: [`sim!`](@ref). """ -function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) +function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true) validate_ym(model, i_ym) nint_ym = iszero(nint_u) ? fill(1, length(i_ym)) : fill(0, length(i_ym)) return nint_ym diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 5bd5eaa08..864a2c3c1 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -33,14 +33,15 @@ struct SteadyKalmanFilter{ prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function SteadyKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true + model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true, verify_obsv=true ) where {NT<:Real, SM<:LinModel, KC<:KalmanCovariances} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ 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) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] R̂, Q̂ = cov.R̂, cov.Q̂ K̂, P̂ = init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct) @@ -111,6 +112,10 @@ state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocatio disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators). - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to + `false` to disable this check. +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to + `false` to disable this check. # Examples ```jldoctest @@ -166,7 +171,8 @@ function SteadyKalmanFilter( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), direct = true, @@ -178,7 +184,7 @@ function SteadyKalmanFilter( # estimated covariances matrices (variance = σ²) : Q̂ = Diagonal([σQ; σQint_u; σQint_ym].^2) R̂ = Diagonal([σR;].^2) - return SteadyKalmanFilter(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) + return SteadyKalmanFilter(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct, verify_obsv) end @doc raw""" @@ -189,11 +195,11 @@ Construct the estimator from the augmented covariance matrices `Q̂` and `R̂`. This syntax allows nonzero off-diagonal elements in ``\mathbf{Q̂, R̂}``. """ function SteadyKalmanFilter( - model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true + model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true, verify_obsv=true ) where {NT<:Real, SM<:LinModel{NT}} Q̂, R̂ = to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂) - return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct) + return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct, verify_obsv) end """ @@ -340,14 +346,15 @@ struct KalmanFilter{ prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function KalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true + model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true, verify_obsv=true ) where {NT<:Real, SM<:LinModel, KC<:KalmanCovariances} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ 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) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] K̂ = zeros(NT, nx̂, nym) @@ -407,6 +414,8 @@ This estimator is allocation-free. disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to + `false` to disable this check. # Examples ```jldoctest @@ -431,7 +440,8 @@ function KalmanFilter( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -449,7 +459,7 @@ function KalmanFilter( P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) R̂ = Diagonal([σR;].^2) - return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv) end @doc raw""" @@ -460,11 +470,11 @@ Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` an This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``. """ function KalmanFilter( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true + model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true, verify_obsv=true ) where {NT<:Real, SM<:LinModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) - return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct) + return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct, verify_obsv) end @doc raw""" @@ -566,14 +576,16 @@ struct UnscentedKalmanFilter{ prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function UnscentedKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; direct=true + model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; + direct=true, verify_obsv=true ) where {NT<:Real, SM<:SimModelODE{NT}, KC<:KalmanCovariances} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ 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) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -658,6 +670,8 @@ This estimator is allocation-free if `model` simulations do not allocate. - `κ=0` or *`kappa`* : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to + `false` to disable this check. # Examples ```jldoctest @@ -691,7 +705,8 @@ function UnscentedKalmanFilter( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -715,7 +730,9 @@ function UnscentedKalmanFilter( P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) R̂ = Diagonal([σR;].^2) - return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct) + return UnscentedKalmanFilter( + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct, verify_obsv + ) end @doc raw""" @@ -728,11 +745,14 @@ Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` an This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``. """ function UnscentedKalmanFilter( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; direct=true + model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; + direct=true, verify_obsv=true ) where {NT<:Real, SM<:SimModelODE{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) - return UnscentedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct) + return UnscentedKalmanFilter{NT}( + model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct, verify_obsv + ) end @@ -953,7 +973,8 @@ struct ExtendedKalmanFilter{ function ExtendedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC; - jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, direct=true + jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, + direct=true, verify_obsv=true ) where { NT<:Real, SM<:SimModelODE, @@ -967,7 +988,8 @@ struct ExtendedKalmanFilter{ 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) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] K̂ = zeros(NT, nx̂, nym) @@ -1058,7 +1080,8 @@ function ExtendedKalmanFilter( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -1078,7 +1101,8 @@ function ExtendedKalmanFilter( Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) R̂ = Diagonal([σR;].^2) return ExtendedKalmanFilter( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, direct + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; + jacobian, direct, verify_obsv ) end @@ -1092,13 +1116,15 @@ Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` an This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``. """ function ExtendedKalmanFilter( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian=AutoForwardDiff(), direct=true + model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; + jacobian=AutoForwardDiff(), direct=true, verify_obsv=true ) where {NT<:Real, SM<:SimModelODE{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) linfuncF̂!, linfuncĤ! = get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) return ExtendedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, cov; jacobian, linfuncF̂!, linfuncĤ!, direct + model, i_ym, nint_u, nint_ym, cov; + jacobian, linfuncF̂!, linfuncĤ!, direct, verify_obsv ) end diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index bd8163b0d..9e7d77861 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -25,7 +25,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function Luenberger{NT, SM}( - model, i_ym, nint_u, nint_ym, poles; direct=true + model, i_ym, nint_u, nint_ym, poles; direct=true, verify_obsv=true ) where {NT<:Real, SM<:LinModel} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) @@ -33,7 +33,8 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} 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) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] K̂ = try ControlSystemsBase.place(Â, Ĉ, poles, :o; direct)[:, i_ym] @@ -63,7 +64,8 @@ end nint_u = 0, nint_ym = default_nint(model, i_ym), poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5, - direct = true + direct = true, + verify_obsv = true ) Construct a Luenberger observer with the [`LinModel`](@ref) `model`. @@ -98,11 +100,12 @@ function Luenberger( model::SM; i_ym::AbstractVector{Int} = 1:model.ny, nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5, direct = true ) where{NT<:Real, SM<:LinModel{NT}} - return Luenberger{NT, SM}(model, i_ym, nint_u, nint_ym, poles; direct) + return Luenberger{NT, SM}(model, i_ym, nint_u, nint_ym, poles; direct, verify_obsv) end "Validate the quantity and stability of the Luenberger `poles`." diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index b4014eb9a..8a84dbe5b 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -24,14 +24,15 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function ManualEstimator{NT}( - model::SM, i_ym, nint_u, nint_ym + model::SM, i_ym, nint_u, nint_ym; verify_obsv=true ) where {NT<:Real, SM<:SimModel{NT}} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ 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) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] direct = false @@ -71,6 +72,8 @@ examples. the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). - `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). +- `verify_obsv=true`: verify observability of the augmented model when `model` is a + [`LinModel`](@ref). Set it to `false` to disable this check. # Examples ```jldoctest @@ -139,9 +142,10 @@ function ManualEstimator( model::SM; i_ym::AbstractVector{Int} = 1:model.ny, nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), ) where {NT<:Real, SM<:SimModel{NT}} - return ManualEstimator{NT}(model, i_ym, nint_u, nint_ym) + return ManualEstimator{NT}(model, i_ym, nint_u, nint_ym; verify_obsv) end """ diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index ed5321c34..869db92bf 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -158,7 +158,7 @@ struct MovingHorizonEstimator{ gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE; - direct=true + direct=true, verify_obsv=true ) where { NT<:Real, SM<:SimModel{NT}, @@ -182,7 +182,8 @@ struct MovingHorizonEstimator{ nxs = size(As, 1) nx̂ = model.nx + nxs nŵ = nx̂ - Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = + augment_model(model, As, Cs_u, Cs_y; verify_obsv) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -556,7 +557,8 @@ function MovingHorizonEstimator( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + verify_obsv = true, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -589,7 +591,7 @@ function MovingHorizonEstimator( return MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; gc, gc!, nc, p, - transcription, optim, gradient, jacobian, hessian, covestim, direct + transcription, optim, gradient, jacobian, hessian, covestim, direct, verify_obsv ) end @@ -606,6 +608,7 @@ end hessian=false, covestim=nothing, direct=true, + verify_obsv=true, ) Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` and `R̂`. @@ -627,6 +630,7 @@ function MovingHorizonEstimator( hessian::Union{AbstractADType, Bool, Nothing} = false, covestim::Union{StateEstimator, Nothing} = nothing, direct = true, + verify_obsv = true, ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} if isnothing(P̂_0) if isnothing(covestim) @@ -639,7 +643,9 @@ function MovingHorizonEstimator( gc! = get_mutating_gc_mhe(NT, gc) hessian = validate_hessian(hessian, DEFAULT_NONLINMHE_HESSIAN, gradient) if isnothing(covestim) - covestim = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + covestim = default_covestim_mhe( + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv + ) end validate_covestim(cov, covestim) setstate!(covestim, covestim.x̂0 + covestim.x̂op, P̂_0) @@ -648,7 +654,7 @@ function MovingHorizonEstimator( He, i_ym, nint_u, nint_ym, cov, Cwt, gc!, nc, p, transcription, optim, gradient, jacobian, hessian, covestim; - direct + direct, verify_obsv ) end @@ -662,11 +668,18 @@ function default_optim_mhe(model::SimModel, nc) end "Default arrival covariance estimator for MHE, depending on the model type only." -function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) +function default_covestim_mhe( + model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; + direct, verify_obsv=true +) if model isa LinModel - return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + return KalmanFilter( + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv + ) elseif model isa NonLinModel - return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + return UnscentedKalmanFilter( + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv + ) else # SteadyKalmanFilter with arbitrary LinModel (will be ignored): nx, nu, ny = model.nx, model.nu, model.ny @@ -674,7 +687,9 @@ function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂ A = Diagonal(poles) # distinct and stable poles for observability Bu, C = ones(nx, nu), ones(ny, nx) dummy_model = LinModel(A, Bu, C, 0, 0, model.Ts) - return SteadyKalmanFilter(dummy_model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) + return SteadyKalmanFilter( + dummy_model, i_ym, nint_u, nint_ym, Q̂, R̂; direct, verify_obsv + ) end end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 26c5c9129..852a77981 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -56,6 +56,8 @@ @test_throws ErrorException SteadyKalmanFilter(linmodel3, nint_ym=[1, 0, 0]) model_unobs = LinModel([1 0;0 1.5], [1; 0], [1 0], zeros(2,0), zeros(1,0), 1.0) @test_throws ErrorException SteadyKalmanFilter(model_unobs, nint_ym=[1]) + @test_throws ErrorException KalmanFilter(model_unobs, nint_ym=[1], verify_obsv=true) + @test isa(KalmanFilter(model_unobs, nint_ym=[1], verify_obsv=false), KalmanFilter) @test_throws ErrorException SteadyKalmanFilter(LinModel(tf(1,[10, 1]), 1.0), 1:1, 0, 0, [-1], [1]) @test_throws ErrorException SteadyKalmanFilter(LinModel(tf(1, [1,0]), 1), nint_ym=[1]) @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_u=[1,1], nint_ym=[1,1]) From 5c4affea5d432f3a871b899a34f66d9cadbfc598 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 11:42:41 -0500 Subject: [PATCH 02/15] debug: skip `estimate_delays` with a `@warn` --- src/controller/explicitmpc.jl | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 476a5034a..47787fc9c 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -167,11 +167,16 @@ function ExplicitMPC( L_Hp = Diagonal(repeat(Lwt, Hp)), ) where {NT<:Real, SE<:StateEstimator{NT}} isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) - nk = estimate_delays(estim.model) + nk = try + estimate_delays(estim.model) + catch myerr + NaN + end if Hp ≤ nk @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end + isnan(nk) && @warn("could not estimate the model delay, Hp ($Hp) might be too small.") nb = move_blocking(Hp, Hc) Hc = get_Hc(nb) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp) From ec1e33336933da8783abba04369b09f8c4fbf766 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 11:47:41 -0500 Subject: [PATCH 03/15] debug: skip Hessian validation with `@warn` --- src/controller/construct.jl | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 8672bf9c6..d010cfe41 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -854,13 +854,18 @@ end "Check the condition number of `H̃` and display a `@warn` if `cond(H̃) > warn_cond`." function verify_cond(::SingleShooting, H̃, warn_cond) if !isinf(warn_cond) - cond_H̃ = cond(H̃) + cond_H̃ = try + cond(H̃) + catch + NaN + end cond_H̃ > warn_cond && @warn( "The Hessian condition number cond_H̃ > $warn_cond. The optimization " * "problem may be ill-conditioned.\n Consider changing the tunings, using " * "MultipleShooting, or using an optimizer more robust to this like DAQP.", cond_H̃, ) + isnan(cond_H̃) && @warn("could not validate the condition number of the QP Hessian.") end return nothing end From 0b192bc545c97743c7b74abde3a7a06a938e1fd9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 12:12:57 -0500 Subject: [PATCH 04/15] doc: remove duplicate entry --- src/estimator/kalman.jl | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 864a2c3c1..877e46045 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -114,8 +114,6 @@ state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocatio estimator, in opposition to the delayed/predictor form). - `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to `false` to disable this check. -- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to - `false` to disable this check. # Examples ```jldoctest From b554b524c2c7417b60e719cefad0b5873c964937 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 12:19:43 -0500 Subject: [PATCH 05/15] revert: `verify_obsv` kwarg addition in MHE constructor --- src/estimator/mhe/construct.jl | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 869db92bf..60ffdee97 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -158,7 +158,7 @@ struct MovingHorizonEstimator{ gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE; - direct=true, verify_obsv=true + direct=true ) where { NT<:Real, SM<:SimModel{NT}, @@ -182,8 +182,7 @@ struct MovingHorizonEstimator{ nxs = size(As, 1) nx̂ = model.nx + nxs nŵ = nx̂ - Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = - augment_model(model, As, Cs_u, Cs_y; verify_obsv) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -557,8 +556,7 @@ function MovingHorizonEstimator( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - verify_obsv = true, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -591,7 +589,7 @@ function MovingHorizonEstimator( return MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; gc, gc!, nc, p, - transcription, optim, gradient, jacobian, hessian, covestim, direct, verify_obsv + transcription, optim, gradient, jacobian, hessian, covestim, direct ) end @@ -607,8 +605,7 @@ end jacobian=AutoForwardDiff(), hessian=false, covestim=nothing, - direct=true, - verify_obsv=true, + direct=true ) Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` and `R̂`. @@ -629,8 +626,7 @@ function MovingHorizonEstimator( jacobian::AbstractADType = default_jacobian(transcription), hessian::Union{AbstractADType, Bool, Nothing} = false, covestim::Union{StateEstimator, Nothing} = nothing, - direct = true, - verify_obsv = true, + direct = true ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} if isnothing(P̂_0) if isnothing(covestim) @@ -644,7 +640,7 @@ function MovingHorizonEstimator( hessian = validate_hessian(hessian, DEFAULT_NONLINMHE_HESSIAN, gradient) if isnothing(covestim) covestim = default_covestim_mhe( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv=false ) end validate_covestim(cov, covestim) @@ -654,7 +650,7 @@ function MovingHorizonEstimator( He, i_ym, nint_u, nint_ym, cov, Cwt, gc!, nc, p, transcription, optim, gradient, jacobian, hessian, covestim; - direct, verify_obsv + direct ) end From f0d8d36e31cb3ef13f8ab0b24b0b0b0d209a339d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 13:52:28 -0500 Subject: [PATCH 06/15] changed: wip to support `SteadyKalmanFilter` It seems to crash on `ControlSystems.jl` side, but it's clean in this repo. --- src/estimator/construct.jl | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 952c89963..74dddf7d9 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -90,17 +90,12 @@ struct KalmanCovariances{ Q̂::Q̂C, R̂::R̂C, P̂_0, He ) where {NT<:Real, Q̂C<:AbstractMatrix{NT}, R̂C<:AbstractMatrix{NT}} if isnothing(P̂_0) - P̂_0 = zeros(NT, 0, 0) # does not apply to steady-state Kalman filter - P̂ = zeros(NT, size(Q̂)) # will hold the steady-state error covariance + P̂_0 = Hermitian(zeros(NT, 0, 0), :L) # don't apply to SteadyKalmanFilter + P̂ = Hermitian(zeros(NT, size(Q̂)), :L) # will hold the steady-state error covariance + invP̄ = copy(P̂_0) else - P̂ = copy(P̂_0) - end - P̂_0 = Hermitian(P̂_0, :L) - P̂ = Hermitian(P̂, :L) - Q̂ = Hermitian(Q̂, :L) - R̂ = Hermitian(R̂, :L) - # the following variables are only for the moving horizon estimator: - invP̄, invQ̂, invR̂ = copy(P̂_0), copy(Q̂), copy(R̂) + P̂_0 = Hermitian(P̂_0, :L) + P̂, invP̄ = copy(P̂_0), copy(P̂_0) try inv!(invP̄) catch err @@ -110,6 +105,13 @@ struct KalmanCovariances{ rethrow() end end + end + P̂_0 = Hermitian(P̂_0, :L) + P̂ = Hermitian(P̂, :L) + Q̂ = Hermitian(Q̂, :L) + R̂ = Hermitian(R̂, :L) + # the following variables are only for the moving horizon estimator: + invQ̂, invR̂ = copy(Q̂), copy(R̂) try inv!(invQ̂) catch err From ffa0eccd662d84b08a800e2db59ebb6e7751e7c5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 14:18:56 -0500 Subject: [PATCH 07/15] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index bfa439348..d58d884e0 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.16.4" +version = "2.16.5" authors = ["Francis Gagnon"] [deps] From c3c410606faa813f6934e50ed35bce5b4fe49ddd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 16:26:58 -0500 Subject: [PATCH 08/15] doc: avoid `TIME_LIMIT` on `ManualEstimator` example --- src/estimator/manual.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index b4014eb9a..ed26d4baa 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -100,7 +100,7 @@ ManualEstimator estimator with a sample time Ts = 0.5 s: julia> function man_sim() f(x,u,_,_) = 0.5*sin.(x + u) h(x,_,_) = x - model = NonLinModel(f, h, 100.0, 1, 1, 1, solver=nothing) + model = NonLinModel(f, h, 1000.0, 1, 1, 1, solver=nothing) linModel = linearize(model, x=[0], u=[0]) man = ManualEstimator(linModel, nint_u=[1]) mpc = LinMPC(man) From a98b7c9cf879201e8bfd9adbe670fa89b2117038 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 17:13:18 -0500 Subject: [PATCH 09/15] changed: cleanup --- src/estimator/construct.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 74dddf7d9..6dce09f8a 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -379,10 +379,10 @@ julia> default_nint(model) |> println """ function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true) validate_ym(model, i_ym) - if !verify_obsv - return iszero(nint_u) ? fill(1, length(i_ym)) : fill(0, length(i_ym)) - end - nint_ym = fill(0, length(i_ym)) + nym = length(i_ym) + verify_obsv || return iszero(nint_u) ? fill(1, nym) : fill(0, nym) + nint_ym = fill(0, nym) + if verify_obsv for i in eachindex(i_ym) nint_ym[i] = 1 As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) From 523f92144ff4ba01665e8e2e309eb33157d49fb9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 17:15:34 -0500 Subject: [PATCH 10/15] doc: info about `verify_obsv` kwarg of `default_nint` --- src/estimator/construct.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 6dce09f8a..73467c076 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -359,7 +359,7 @@ function augment_model(model::SimModel{NT}, As, args... ; verify_obsv=false) whe end @doc raw""" - default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0) -> nint_ym + default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0, verify_obsv=true) -> nint_ym Get default integrator quantity per measured outputs `nint_ym` for [`LinModel`](@ref). @@ -367,7 +367,7 @@ The arguments `i_ym` and `nint_u` are the measured output indices and the integr quantity on each manipulated input, respectively. By default, one integrator is added on each measured outputs. If ``\mathbf{Â, Ĉ}`` matrices of the augmented model become unobservable, the integrator is removed. This approach works well for stable, integrating -and unstable `model` (see Examples). +and unstable `model` (see Examples). The observability checkup is skipped if `!verify_obsv`. # Examples ```jldoctest From d24ffc22c318991dae16d5b4cedca653b74b01a5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 17:16:41 -0500 Subject: [PATCH 11/15] doc: `;`, it's a keyword argument --- src/estimator/construct.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 73467c076..60a99f401 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -359,7 +359,7 @@ function augment_model(model::SimModel{NT}, As, args... ; verify_obsv=false) whe end @doc raw""" - default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0, verify_obsv=true) -> nint_ym + default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true) -> nint_ym Get default integrator quantity per measured outputs `nint_ym` for [`LinModel`](@ref). From bf8522660beb1ed3f903a05e05eae6f3521e4b85 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 20:35:31 -0500 Subject: [PATCH 12/15] debug: correct syntax error --- src/estimator/construct.jl | 6 +++--- src/estimator/kalman.jl | 9 +++------ 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 60a99f401..e84a5024d 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -382,7 +382,6 @@ function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=tr nym = length(i_ym) verify_obsv || return iszero(nint_u) ? fill(1, nym) : fill(0, nym) nint_ym = fill(0, nym) - if verify_obsv for i in eachindex(i_ym) nint_ym[i] = 1 As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -394,7 +393,7 @@ function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=tr end """ - default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) -> nint_ym + default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0; verify_obsv=false) -> nint_ym One integrator on each measured output by default for other cases e.g. [`NonLinModel`](@ref). @@ -405,7 +404,8 @@ integrator on each measured output. Theres is no verification the augmented model remains observable. The resulting [`StateEstimator`](@ref) object should be assessed separately with e.g.: [`sim!`](@ref). """ -function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true) +function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0; verify_obsv=false) + verify_obsv && error("observability verification of non-LinModel is not supported.") validate_ym(model, i_ym) nint_ym = iszero(nint_u) ? fill(1, length(i_ym)) : fill(0, length(i_ym)) return nint_ym diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 877e46045..3ef0fad9a 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -112,8 +112,7 @@ state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocatio disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators). - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). -- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to - `false` to disable this check. +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). # Examples ```jldoctest @@ -412,8 +411,7 @@ This estimator is allocation-free. disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). -- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to - `false` to disable this check. +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). # Examples ```jldoctest @@ -668,8 +666,7 @@ This estimator is allocation-free if `model` simulations do not allocate. - `κ=0` or *`kappa`* : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). -- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). Set to - `false` to disable this check. +- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). # Examples ```jldoctest From 4f0c2033be24cc45cd0c36796de9aba4f4f71db9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 20:55:02 -0500 Subject: [PATCH 13/15] removed: revert `verify_obsv` kwarg in UKF and EKF --- src/estimator/kalman.jl | 30 ++++++++++-------------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3ef0fad9a..37ca1131b 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -666,7 +666,6 @@ This estimator is allocation-free if `model` simulations do not allocate. - `κ=0` or *`kappa`* : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). -- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref). # Examples ```jldoctest @@ -700,8 +699,7 @@ function UnscentedKalmanFilter( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - verify_obsv = true, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -725,9 +723,7 @@ function UnscentedKalmanFilter( P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) R̂ = Diagonal([σR;].^2) - return UnscentedKalmanFilter( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct, verify_obsv - ) + return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct) end @doc raw""" @@ -741,12 +737,12 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat """ function UnscentedKalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; - direct=true, verify_obsv=true + direct=true ) where {NT<:Real, SM<:SimModelODE{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) return UnscentedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct, verify_obsv + model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct ) end @@ -969,7 +965,7 @@ struct ExtendedKalmanFilter{ model::SM, i_ym, nint_u, nint_ym, cov::KC; jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, - direct=true, verify_obsv=true + direct=true ) where { NT<:Real, SM<:SimModelODE, @@ -983,8 +979,7 @@ struct ExtendedKalmanFilter{ 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; verify_obsv) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] K̂ = zeros(NT, nx̂, nym) @@ -1075,8 +1070,7 @@ function ExtendedKalmanFilter( sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - verify_obsv = true, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), @@ -1095,10 +1089,7 @@ function ExtendedKalmanFilter( P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) R̂ = Diagonal([σR;].^2) - return ExtendedKalmanFilter( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; - jacobian, direct, verify_obsv - ) + return ExtendedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, direct) end @doc raw""" @@ -1112,14 +1103,13 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat """ function ExtendedKalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; - jacobian=AutoForwardDiff(), direct=true, verify_obsv=true + jacobian=AutoForwardDiff(), direct=true ) where {NT<:Real, SM<:SimModelODE{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) linfuncF̂!, linfuncĤ! = get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) return ExtendedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, cov; - jacobian, linfuncF̂!, linfuncĤ!, direct, verify_obsv + model, i_ym, nint_u, nint_ym, cov; jacobian, linfuncF̂!, linfuncĤ!, direct ) end From 5e9bb4af4273b3597c7331809e5a52f596642fb6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 21:05:47 -0500 Subject: [PATCH 14/15] removed: `verify_obsv` kwarg from `default_covestim_mhe` --- src/estimator/mhe/construct.jl | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 60ffdee97..cc85a72c9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -639,9 +639,7 @@ function MovingHorizonEstimator( gc! = get_mutating_gc_mhe(NT, gc) hessian = validate_hessian(hessian, DEFAULT_NONLINMHE_HESSIAN, gradient) if isnothing(covestim) - covestim = default_covestim_mhe( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv=false - ) + covestim = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end validate_covestim(cov, covestim) setstate!(covestim, covestim.x̂0 + covestim.x̂op, P̂_0) @@ -664,18 +662,11 @@ function default_optim_mhe(model::SimModel, nc) end "Default arrival covariance estimator for MHE, depending on the model type only." -function default_covestim_mhe( - model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; - direct, verify_obsv=true -) +function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) if model isa LinModel - return KalmanFilter( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv - ) + return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) elseif model isa NonLinModel - return UnscentedKalmanFilter( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv - ) + return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) else # SteadyKalmanFilter with arbitrary LinModel (will be ignored): nx, nu, ny = model.nx, model.nu, model.ny @@ -684,7 +675,7 @@ function default_covestim_mhe( Bu, C = ones(nx, nu), ones(ny, nx) dummy_model = LinModel(A, Bu, C, 0, 0, model.Ts) return SteadyKalmanFilter( - dummy_model, i_ym, nint_u, nint_ym, Q̂, R̂; direct, verify_obsv + dummy_model, i_ym, nint_u, nint_ym, Q̂, R̂; direct, verify_obsv=false ) end end From 481ad885f6fd19f1d121d95e1b85e6640dbd0f46 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 18 Nov 2026 21:11:26 -0500 Subject: [PATCH 15/15] removed: `verify_obsv` kwarg from `ManualEstimator` It will never check observability for this `StateEstimator`, since the estimation job is delegated externally. --- src/estimator/manual.jl | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index f4d40423d..1be080a08 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -24,15 +24,14 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} prepared::Vector{Bool} buffer::StateEstimatorBuffer{NT} function ManualEstimator{NT}( - model::SM, i_ym, nint_u, nint_ym; verify_obsv=true + model::SM, i_ym, nint_u, nint_ym ) where {NT<:Real, SM<:SimModel{NT}} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ 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; verify_obsv) + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y; verify_obsv=false) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] direct = false @@ -72,8 +71,6 @@ examples. the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). - `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). -- `verify_obsv=true`: verify observability of the augmented model when `model` is a - [`LinModel`](@ref). Set it to `false` to disable this check. # Examples ```jldoctest @@ -142,10 +139,9 @@ function ManualEstimator( model::SM; i_ym::AbstractVector{Int} = 1:model.ny, nint_u ::IntVectorOrInt = 0, - verify_obsv = true, - nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv), + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), ) where {NT<:Real, SM<:SimModel{NT}} - return ManualEstimator{NT}(model, i_ym, nint_u, nint_ym; verify_obsv) + return ManualEstimator{NT}(model, i_ym, nint_u, nint_ym) end """