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] 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 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) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index c6a19aba9..e84a5024d 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 @@ -357,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). @@ -365,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 @@ -375,9 +377,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) - 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) for i in eachindex(i_ym) nint_ym[i] = 1 As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -389,7 +393,7 @@ function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0) 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). @@ -400,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) +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 5bd5eaa08..37ca1131b 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,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). # Examples ```jldoctest @@ -166,7 +168,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 +181,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 +192,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 +343,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 +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). # Examples ```jldoctest @@ -431,7 +436,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 +455,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 +466,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 +572,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)] @@ -728,11 +736,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 ) 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 + ) end @@ -953,7 +964,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 ) where { NT<:Real, SM<:SimModelODE, @@ -1077,9 +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 - ) + return ExtendedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, direct) end @doc raw""" @@ -1092,7 +1102,8 @@ 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 ) 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) 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..1be080a08 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -31,7 +31,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: 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=false) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] direct = false @@ -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) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index ed5321c34..cc85a72c9 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -605,7 +605,7 @@ end jacobian=AutoForwardDiff(), hessian=false, covestim=nothing, - direct=true, + direct=true ) Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` and `R̂`. @@ -626,7 +626,7 @@ function MovingHorizonEstimator( jacobian::AbstractADType = default_jacobian(transcription), hessian::Union{AbstractADType, Bool, Nothing} = false, covestim::Union{StateEstimator, Nothing} = nothing, - direct = true, + direct = true ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} if isnothing(P̂_0) if isnothing(covestim) @@ -666,7 +666,7 @@ function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂ if model isa LinModel 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) + 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 @@ -674,7 +674,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=false + ) 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])