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/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 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..96365cff0 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) @@ -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 @@ -297,12 +299,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.corrected[] = true end + estim.prepared[] = true x̂ = estim.buffer.x̂ x̂ .= estim.x̂0 .+ estim.x̂op return x̂ @@ -317,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), 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)``. # Examples ```jldoctest @@ -334,13 +339,14 @@ 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[] + 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 validate_args(estim, ym, d, u) y0m, d0, u0 = remove_op!(estim, ym, d, u) - update_estimate!(estim, y0m, d0, u0) - estim.corrected[] = false + update_estimate!(estim, u0, y0m, d0) + 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 1ff3c4245..d448efcc7 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 @@ -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) """ @@ -253,28 +254,35 @@ 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. If there +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 - 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 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`. @@ -288,7 +296,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 --------------------- @@ -345,7 +353,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 3a7c1ec20..3a80dcf96 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 @@ -240,14 +240,18 @@ 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) - return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) + 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 @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`. @@ -269,16 +273,16 @@ 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) - if !estim.direct - correct_estimate_obsv!(estim, y0m, d0, estim.K̂) +function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) + if !estim.direct && all(isfinite, y0m) + correct_estimate_obsv!(estim, y0m, d0) end - return predict_estimate_obsv!(estim::StateEstimator, 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 +295,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̂ @@ -333,7 +337,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 +351,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 +361,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 @@ -472,12 +476,16 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation cova ``\mathbf{P̂}_{k}(k)``. """ function correct_estimate!(estim::KalmanFilter, y0m, d0) + 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 @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̂`. @@ -509,8 +517,8 @@ 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) - if !estim.direct +function update_estimate!(estim::KalmanFilter, u0, y0m, d0) + if !estim.direct && all(isfinite, y0m) correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end return predict_estimate_kf!(estim, u0, d0, estim.Â) @@ -555,7 +563,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 +581,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 +593,7 @@ struct UnscentedKalmanFilter{ K̂, M̂, X̂0, X̄0, Ŷ0m, Ȳ0m, nσ, γ, m̂, Ŝ, - direct, corrected, + direct, prepared, buffer ) end @@ -767,6 +775,10 @@ end Do the same but for the [`UnscentedKalmanFilter`](@ref). """ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) + 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.Ŝ @@ -815,7 +827,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̂`. @@ -858,8 +870,8 @@ 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) - if !estim.direct +function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) + 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̂ @@ -932,7 +944,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 +969,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 +981,7 @@ struct ExtendedKalmanFilter{ K̂, F̂_û, F̂, Ĥ, Ĥm, jacobian, linfuncF̂!, linfuncĤ!, - direct, corrected, + direct, prepared, buffer ) end @@ -1136,6 +1148,10 @@ end Do the same but for the [`ExtendedKalmanFilter`](@ref). """ function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) + 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 @@ -1146,7 +1162,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̂`. @@ -1183,8 +1199,8 @@ 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 - if !estim.direct +function update_estimate!(estim::ExtendedKalmanFilter{NT}, u0, y0m, d0) where NT<:Real + 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 5e660de8a..2eaa9d3a8 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 @@ -121,20 +121,24 @@ end Identical to [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ function correct_estimate!(estim::Luenberger, y0m, d0) - return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) + 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 """ - 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) - if !estim.direct - correct_estimate_obsv!(estim, y0m, d0, estim.K̂) +function update_estimate!(estim::Luenberger, u0, y0m, d0) + 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 55ff87254..e5b0e2d79 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 @@ -145,11 +145,11 @@ function ManualEstimator( end """ - update_estimate!(estim::ManualEstimator, y0m, d0, u0) + update_estimate!(estim::ManualEstimator, u0, y0m, d0) 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̂) 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) 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) 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