diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 6b92cc2d7..5f6c2c326 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -57,13 +57,7 @@ ModelPredictiveControl.remove_op! ModelPredictiveControl.init_estimate! ``` -## Correct Estimate - -```@docs -ModelPredictiveControl.correct_estimate! -``` - -## Update Estimate +## Correct and Predict Estimate !!! info All these methods assume that the `u0`, `y0m` and `d0` arguments are deviation vectors @@ -74,5 +68,6 @@ ModelPredictiveControl.correct_estimate! ``\mathbf{x̂_0}``, respectively. ```@docs -ModelPredictiveControl.update_estimate! +ModelPredictiveControl.correct_estimate! +ModelPredictiveControl.predict_estimate! ``` diff --git a/docs/src/public/generic_func.md b/docs/src/public/generic_func.md index bd07d608a..dd69804f4 100644 --- a/docs/src/public/generic_func.md +++ b/docs/src/public/generic_func.md @@ -21,22 +21,34 @@ evaloutput ## Change State x +### Init State x + +```@docs +initstate! +``` + ### Prepare State x ```@docs preparestate! ``` +### Correct State x + +```@docs +correctstate! +``` + ### Update State x ```@docs updatestate! ``` -### Init State x +### Predict State x ```@docs -initstate! +predictstate! ``` ### Set State x diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 0129a16da..7c829f22b 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -44,7 +44,10 @@ import FastGaussQuadrature export SimModel, LinModel, NonLinModel export DiffSolver, RungeKutta, ForwardEuler export setop!, setname! -export setstate!, setmodel!, preparestate!, updatestate!, evaloutput, linearize, linearize! +export linearize, linearize! +export evaloutput +export setstate!, preparestate!, updatestate!, correctstate!, predictstate! +export setmodel! export savetime!, periodsleep export StateEstimator, InternalModel, Luenberger export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 6732fe975..54362a06b 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -553,6 +553,15 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty) return preparestate!(mpc.estim, ym, d) end +""" + correctstate!(mpc::PredictiveController, ym, d=[]) -> x̂ + +Call [`correctstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). +""" +function correctstate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty) + return correctstate!(mpc.estim, ym, d) +end + @doc raw""" getinput!(mpc::PredictiveController, Z̃) -> u @@ -584,6 +593,16 @@ function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty end updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym")) + +""" + predictstate!(mpc::PredictiveController, u, d=[]) -> x̂next + +Call [`predictstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). +""" +function predictstate!(mpc::PredictiveController, u, d=mpc.estim.buffer.empty) + return predictstate!(mpc.estim, u, d) +end + """ savetime!(mpc::PredictiveController) -> t diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 3ad4bb5c1..69641a9d3 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -1,15 +1,13 @@ """ - remove_op!(estim::StateEstimator, ym, d, u=nothing) -> y0m, d0, u0 + remove_op!(estim::StateEstimator, ym=nothing, d=nothing, u=nothing) -> y0m, d0, u0 Remove operating pts on measured outputs `ym`, disturbances `d` and inputs `u` (if provided). """ -function remove_op!(estim::StateEstimator, ym, d, u=nothing) +function remove_op!(estim::StateEstimator, ym=nothing, d=nothing, u=nothing) y0m, u0, d0 = estim.buffer.ym, estim.buffer.u, estim.buffer.d - y0m .= @views ym .- estim.model.yop[estim.i_ym] - d0 .= d .- estim.model.dop - if !isnothing(u) - u0 .= u .- estim.model.uop - end + !isnothing(ym) && (y0m .= @views ym .- estim.model.yop[estim.i_ym]) + !isnothing(d) && (d0 .= d .- estim.model.dop) + !isnothing(u) && (u0 .= u .- estim.model.uop) return y0m, d0, u0 end @@ -301,25 +299,47 @@ 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.corrected[] = true end x̂ = estim.buffer.x̂ x̂ .= estim.x̂0 .+ estim.x̂op return x̂ end +@doc raw""" + correctstate!(estim::StateEstimator, ym, d=[]) -> x̂ + +Perform the correction step of `estim` with measured outputs `ym` and disturbances `d`. + +It removes the operating points with [`remove_op!`](@ref), calls [`correct_estimate!`](@ref), +and returns the corrected state estimate ``\mathbf{x̂}_k(k)``. This is a lower-level function +than [`preparestate!`](@ref). Be sure to call it before [`moveinput!`](@ref) if +`estim.direct == true`, or after otherwise. This method is not supported by +[`MovingHorizonEstimator`](@ref), use [`preparestate!`](@ref) instead. +""" +function correctstate!(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 + x̂ = estim.buffer.x̂ + x̂ .= estim.x̂0 .+ estim.x̂op + return x̂ +end + @doc raw""" updatestate!(estim::StateEstimator, u, ym, d=[]) -> x̂next Update `estim.x̂0` estimate with current inputs `u`, measured outputs `ym` and dist. `d`. This function should be called at the end of each discrete time step. It removes the -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)``. +operating points with [`remove_op!`](@ref), calls [`correct_estimate!`](@ref) if +`estim.direct == false`, calls [`predict_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 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)``. # Examples ```jldoctest @@ -339,7 +359,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.corrected[] = false x̂next = estim.buffer.x̂ x̂next .= estim.x̂0 .+ estim.x̂op @@ -347,6 +367,32 @@ function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) end updatestate!(::StateEstimator, _ ) = throw(ArgumentError("missing measured outputs ym")) +@doc raw""" + predictstate!(estim::StateEstimator, u, d=[]) -> x̂next + +Preform the prediction step of `estim` with current inputs `u` and disturbances `d`. + +It removes the operating points with [`remove_op!`](@ref), calls [`predict_estimate!`](@ref), +and returns the state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. This is a +lower-level function than [`updatestate!`](@ref). Be sure to call [`correctstate!`](@ref) +before this function if a new measurement is available, but the correction step can be +skipped otherwise. This method is not supported by [`MovingHorizonEstimator`](@ref), +use [`updatestate!`](@ref). +""" +function predictstate!(estim::StateEstimator, u, d=estim.buffer.empty) + validate_args(estim, estim.buffer.empty, d, u) + _ , d0, u0 = remove_op!(estim, nothing, d, u) + predict_estimate!(estim, u0, d0) + x̂next = estim.buffer.x̂ + x̂next .= estim.x̂0 .+ estim.x̂op + return x̂next +end + +"Call [`correct_estimate!`](@ref) if `!direct` and [`predict_estimate!`](@ref) methods." +function update_estimate!(estim::StateEstimator, u0, y0m, d0) + estim.direct || correct_estimate!(estim, y0m, d0) + return predict_estimate!(estim, u0, d0) +end """ savetime!(estim::StateEstimator) -> t diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 1ff3c4245..b0796cbea 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.ŷ @@ -274,9 +277,9 @@ function correct_estimate!(estim::InternalModel, y0m, d0) end @doc raw""" - update_estimate!(estim::InternalModel, _ , d0, u0) + predict_estimate!(estim::InternalModel, u0, d0) -Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`. +Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0` and measured disturbances `d0`. The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂s` estimates with: ```math @@ -288,7 +291,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 predict_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 3a7c1ec20..cfdbb010e 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -234,76 +234,6 @@ function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂, return nothing end -@doc raw""" - correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) - -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. -""" -function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) - return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) -end - -@doc raw""" - update_estimate!(estim::SteadyKalmanFilter, y0m, d0, u0) - -Update `estim.x̂0` estimate with current inputs `u0`, measured outputs `y0m` and dist. `d0`. - -If `estim.direct == false`, the [`SteadyKalmanFilter`](@ref) first corrects the state -estimate with the precomputed Kalman gain ``\mathbf{K̂}``. Afterward, it predicts the next -state with the augmented process model. The correction step is skipped if `direct == true` -since it is already done by the user through the [`preparestate!`](@ref) function (that -calls [`correct_estimate!`](@ref)). The correction and prediction step equations are -provided below. - -# Correction Step -```math -\mathbf{x̂}_k(k) = \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}[\mathbf{y^m}(k) - \mathbf{Ĉ^m x̂}_{k-1}(k) - - \mathbf{D̂_d^m d}(k) ] -``` - -# Prediction Step -```math -\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̂) - end - return predict_estimate_obsv!(estim::StateEstimator, y0m, d0, u0) -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 - ŷ0m = @views estim.buffer.ŷ[estim.i_ym] - # in-place operations to reduce allocations: - mul!(ŷ0m, Ĉm, estim.x̂0) - mul!(ŷ0m, D̂dm, d0, 1, 1) - v̂ = ŷ0m - v̂ .= y0m .- ŷ0m - x̂0corr = estim.x̂0 - mul!(x̂0corr, K̂, v̂, 1, 1) - return nothing -end - -"Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." -function predict_estimate_obsv!(estim::StateEstimator, _ , d0, u0) - x̂0corr = estim.x̂0 - Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d - x̂0next = estim.buffer.x̂ - # in-place operations to reduce allocations: - mul!(x̂0next, Â, x̂0corr) - mul!(x̂0next, B̂u, u0, 1, 1) - mul!(x̂0next, B̂d, d0, 1, 1) - x̂0next .+= estim.f̂op .- estim.x̂op - estim.x̂0 .= x̂0next - return nothing -end - struct KalmanFilter{ NT<:Real, SM<:LinModel, @@ -463,29 +393,23 @@ function KalmanFilter( return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct) end -@doc raw""" +#=@doc raw""" correct_estimate!(estim::KalmanFilter, y0m, d0) Correct `estim.x̂0` and `estim.cov.P̂` using the time-varying [`KalmanFilter`](@ref). It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation covariance ``\mathbf{P̂}_{k}(k)``. -""" -function correct_estimate!(estim::KalmanFilter, y0m, d0) - return correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) -end - +"""=# @doc raw""" - update_estimate!(estim::KalmanFilter, y0m, d0, u0) + correct_estimate!(estim::KalmanFilter, y0m, d0) -Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.cov.P̂`. +Correct [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.cov.P̂`. It implements the classical time-varying Kalman Filter based on the process model described -in [`SteadyKalmanFilter`](@ref). If `estim.direct == false`, it first corrects the estimate -before predicting the next state. The correction step is skipped if `estim.direct == true` -since it's already done by the user. The correction and prediction step equations are -provided below, see [^2] for details. +in [`SteadyKalmanFilter`](@ref). The correction and prediction step equations are provided +below, see [^2] for details. # Correction Step ```math @@ -509,13 +433,14 @@ 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 - correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) - end - return predict_estimate_kf!(estim, u0, d0, estim.Â) -end +correct_estimate!(estim::KalmanFilter, y0m, d0) = correct_kf!(estim, y0m, d0, estim.Ĉm) + +""" + predict_estimate!(estim::KalmanFilter, u0, d0) +Prediction step of [`KalmanFilter`](@ref), see [`correct_estimate!`](@ref) for equations. +""" +predict_estimate!(estim::KalmanFilter, u0, d0) = predict_kf!(estim, u0, d0, estim.Â) struct UnscentedKalmanFilter{ NT<:Real, @@ -747,7 +672,7 @@ covariance are respectively: \mathbf{Ŝ} &= \mathrm{diag}\big( 2 - α^2 + β - \tfrac{n_\mathbf{x̂}}{γ^2} \:,\; \tfrac{1}{2γ^2} \:,\; \tfrac{1}{2γ^2} \:,\; \cdots \:,\; \tfrac{1}{2γ^2} \big) \end{aligned} ``` -See [`update_estimate!(::UnscentedKalmanFilter)`](@ref) for other details. +See [`correct_estimate!`](@ref) for other details. """ function init_ukf(nx̂, α, β, κ) α, β, κ = promote(α, β, κ) @@ -761,10 +686,48 @@ function init_ukf(nx̂, α, β, κ) return nσ, γ, m̂, Ŝ end -""" +@doc raw""" correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) + +Correct [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.cov.P̂`. + +It implements the unscented Kalman Filter based on the generalized unscented transform[^3]. +See [`init_ukf`](@ref) for the definition of the constants ``\mathbf{m̂, Ŝ}`` and ``γ``. The +superscript in e.g. ``\mathbf{X̂}_{k-1}^j(k)`` refers the vector at the ``j``th column of +``\mathbf{X̂}_{k-1}(k)``. The symbol ``\mathbf{0}`` is a vector with zeros. The number of +sigma points is ``n_σ = 2 n_\mathbf{x̂} + 1``. The matrices ``\sqrt{\mathbf{P̂}_{k-1}(k)}`` +and ``\sqrt{\mathbf{P̂}_{k}(k)}`` are the the lower triangular factors of [`cholesky`](@extref Julia LinearAlgebra.cholesky) +results. The correction and prediction step equations are provided below. + +# Correction Step +```math +\begin{aligned} + \mathbf{X̂}_{k-1}(k) &= \bigg[\begin{matrix} \mathbf{x̂}_{k-1}(k) & \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{x̂}_{k-1}(k) \end{matrix}\bigg] + \bigg[\begin{matrix} \mathbf{0} & γ \sqrt{\mathbf{P̂}_{k-1}(k)} & -γ \sqrt{\mathbf{P̂}_{k-1}(k)} \end{matrix}\bigg] \\ + \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ + \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} \\ + \mathbf{X̄}_{k-1}(k) &= \begin{bmatrix} \mathbf{X̂}_{k-1}^{1}(k) - \mathbf{x̂}_{k-1}(k) & \mathbf{X̂}_{k-1}^{2}(k) - \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{X̂}_{k-1}^{n_σ}(k) - \mathbf{x̂}_{k-1}(k) \end{bmatrix} \\ + \mathbf{Ȳ^m}(k) &= \begin{bmatrix} \mathbf{Ŷ^m}^{1}(k) - \mathbf{ŷ^m}(k) & \mathbf{Ŷ^m}^{2}(k) - \mathbf{ŷ^m}(k) & \cdots & \mathbf{Ŷ^m}^{n_σ}(k) - \mathbf{ŷ^m}(k) \end{bmatrix} \\ + \mathbf{M̂}(k) &= \mathbf{Ȳ^m}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) + \mathbf{R̂} \\ + \mathbf{K̂}(k) &= \mathbf{X̄}_{k-1}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) \mathbf{M̂^{-1}}(k) \\ + \mathbf{x̂}_k(k) &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k) \big[ \mathbf{y^m}(k) - \mathbf{ŷ^m}(k) \big] \\ + \mathbf{P̂}_k(k) &= \mathbf{P̂}_{k-1}(k) - \mathbf{K̂}(k) \mathbf{M̂}(k) \mathbf{K̂}'(k) \\ +\end{aligned} +``` + +# Prediction Step +```math +\begin{aligned} + \mathbf{X̂}_k(k) &= \bigg[\begin{matrix} \mathbf{x̂}_{k}(k) & \mathbf{x̂}_{k}(k) & \cdots & \mathbf{x̂}_{k}(k) \end{matrix}\bigg] + \bigg[\begin{matrix} \mathbf{0} & \gamma \sqrt{\mathbf{P̂}_{k}(k)} & - \gamma \sqrt{\mathbf{P̂}_{k}(k)} \end{matrix}\bigg] \\ + \mathbf{X̂}_{k}(k+1) &= \bigg[\begin{matrix} \mathbf{f̂}\Big( \mathbf{X̂}_{k}^{1}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) & \mathbf{f̂}\Big( \mathbf{X̂}_{k}^{2}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) & \cdots & \mathbf{f̂}\Big( \mathbf{X̂}_{k}^{n_σ}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \end{matrix}\bigg] \\ + \mathbf{x̂}_{k}(k+1) &= \mathbf{X̂}_{k}(k+1)\mathbf{m̂} \\ + \mathbf{X̄}_k(k+1) &= \begin{bmatrix} \mathbf{X̂}_{k}^{1}(k+1) - \mathbf{x̂}_{k}(k+1) & \mathbf{X̂}_{k}^{2}(k+1) - \mathbf{x̂}_{k}(k+1) & \cdots &\, \mathbf{X̂}_{k}^{n_σ}(k+1) - \mathbf{x̂}_{k}(k+1) \end{bmatrix} \\ + \mathbf{P̂}_k(k+1) &= \mathbf{X̄}_k(k+1) \mathbf{Ŝ} \mathbf{X̄}_k'(k+1) + \mathbf{Q̂} +\end{aligned} +``` -Do the same but for the [`UnscentedKalmanFilter`](@ref). +[^3]: Simon, D. 2006, "Chapter 14: The unscented Kalman filter" in "Optimal State Estimation: + Kalman, H∞, and Nonlinear Approaches", John Wiley & Sons, p. 433–459, , + ISBN9780470045343. """ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) x̂0, P̂, R̂, K̂ = estim.x̂0, estim.cov.P̂, estim.cov.R̂, estim.K̂ @@ -814,54 +777,12 @@ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) return nothing end -@doc raw""" - update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) - -Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.cov.P̂`. - -It implements the unscented Kalman Filter based on the generalized unscented transform[^3]. -See [`init_ukf`](@ref) for the definition of the constants ``\mathbf{m̂, Ŝ}`` and ``γ``. The -superscript in e.g. ``\mathbf{X̂}_{k-1}^j(k)`` refers the vector at the ``j``th column of -``\mathbf{X̂}_{k-1}(k)``. The symbol ``\mathbf{0}`` is a vector with zeros. The number of -sigma points is ``n_σ = 2 n_\mathbf{x̂} + 1``. The matrices ``\sqrt{\mathbf{P̂}_{k-1}(k)}`` -and ``\sqrt{\mathbf{P̂}_{k}(k)}`` are the the lower triangular factors of [`cholesky`](@extref Julia LinearAlgebra.cholesky) -results. The correction and prediction step equations are provided below. The correction -step is skipped if `estim.direct == true` since it's already done by the user. - -# Correction Step -```math -\begin{aligned} - \mathbf{X̂}_{k-1}(k) &= \bigg[\begin{matrix} \mathbf{x̂}_{k-1}(k) & \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{x̂}_{k-1}(k) \end{matrix}\bigg] + \bigg[\begin{matrix} \mathbf{0} & γ \sqrt{\mathbf{P̂}_{k-1}(k)} & -γ \sqrt{\mathbf{P̂}_{k-1}(k)} \end{matrix}\bigg] \\ - \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ - \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} \\ - \mathbf{X̄}_{k-1}(k) &= \begin{bmatrix} \mathbf{X̂}_{k-1}^{1}(k) - \mathbf{x̂}_{k-1}(k) & \mathbf{X̂}_{k-1}^{2}(k) - \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{X̂}_{k-1}^{n_σ}(k) - \mathbf{x̂}_{k-1}(k) \end{bmatrix} \\ - \mathbf{Ȳ^m}(k) &= \begin{bmatrix} \mathbf{Ŷ^m}^{1}(k) - \mathbf{ŷ^m}(k) & \mathbf{Ŷ^m}^{2}(k) - \mathbf{ŷ^m}(k) & \cdots & \mathbf{Ŷ^m}^{n_σ}(k) - \mathbf{ŷ^m}(k) \end{bmatrix} \\ - \mathbf{M̂}(k) &= \mathbf{Ȳ^m}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) + \mathbf{R̂} \\ - \mathbf{K̂}(k) &= \mathbf{X̄}_{k-1}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) \mathbf{M̂^{-1}}(k) \\ - \mathbf{x̂}_k(k) &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k) \big[ \mathbf{y^m}(k) - \mathbf{ŷ^m}(k) \big] \\ - \mathbf{P̂}_k(k) &= \mathbf{P̂}_{k-1}(k) - \mathbf{K̂}(k) \mathbf{M̂}(k) \mathbf{K̂}'(k) \\ -\end{aligned} -``` - -# Prediction Step -```math -\begin{aligned} - \mathbf{X̂}_k(k) &= \bigg[\begin{matrix} \mathbf{x̂}_{k}(k) & \mathbf{x̂}_{k}(k) & \cdots & \mathbf{x̂}_{k}(k) \end{matrix}\bigg] + \bigg[\begin{matrix} \mathbf{0} & \gamma \sqrt{\mathbf{P̂}_{k}(k)} & - \gamma \sqrt{\mathbf{P̂}_{k}(k)} \end{matrix}\bigg] \\ - \mathbf{X̂}_{k}(k+1) &= \bigg[\begin{matrix} \mathbf{f̂}\Big( \mathbf{X̂}_{k}^{1}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) & \mathbf{f̂}\Big( \mathbf{X̂}_{k}^{2}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) & \cdots & \mathbf{f̂}\Big( \mathbf{X̂}_{k}^{n_σ}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \end{matrix}\bigg] \\ - \mathbf{x̂}_{k}(k+1) &= \mathbf{X̂}_{k}(k+1)\mathbf{m̂} \\ - \mathbf{X̄}_k(k+1) &= \begin{bmatrix} \mathbf{X̂}_{k}^{1}(k+1) - \mathbf{x̂}_{k}(k+1) & \mathbf{X̂}_{k}^{2}(k+1) - \mathbf{x̂}_{k}(k+1) & \cdots &\, \mathbf{X̂}_{k}^{n_σ}(k+1) - \mathbf{x̂}_{k}(k+1) \end{bmatrix} \\ - \mathbf{P̂}_k(k+1) &= \mathbf{X̄}_k(k+1) \mathbf{Ŝ} \mathbf{X̄}_k'(k+1) + \mathbf{Q̂} -\end{aligned} -``` +""" + predict_estimate!(estim::UnscentedKalmanFilter, u0, d0) -[^3]: Simon, D. 2006, "Chapter 14: The unscented Kalman filter" in "Optimal State Estimation: - Kalman, H∞, and Nonlinear Approaches", John Wiley & Sons, p. 433–459, , - ISBN9780470045343. +Prediction step of [`UnscentedKalmanFilter`](@ref), see [`correct_estimate!`](@ref). """ -function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) - if !estim.direct - correct_estimate!(estim, y0m, d0) - end +function predict_estimate!(estim::UnscentedKalmanFilter, u0, d0) x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ Q̂, nx̂ = estim.cov.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ @@ -1130,29 +1051,14 @@ function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) return linfuncF̂!, linfuncĤ! end -""" - correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) - -Do the same but for the [`ExtendedKalmanFilter`](@ref). -""" -function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) - x̂0 = estim.x̂0 - cst_d0 = Constant(d0) - ŷ0, Ĥ, Ĥm = estim.buffer.ŷ, estim.Ĥ, estim.Ĥm - estim.linfuncĤ!(Ĥ, ŷ0, estim.jacobian, x̂0, cst_d0) - Ĥm .= @views Ĥ[estim.i_ym, :] - return correct_estimate_kf!(estim, y0m, d0, Ĥm) -end - - @doc raw""" - update_estimate!(estim::ExtendedKalmanFilter, y0m, d0, u0) + correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) -Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.cov.P̂`. +Correct [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.cov.P̂`. -The equations are similar to [`update_estimate!(::KalmanFilter)`](@ref) but with the -substitutions ``\mathbf{Ĉ^m = Ĥ^m}(k)`` and ``\mathbf{Â = F̂}(k)``, the Jacobians of the -augmented process model: +The equations are similar to [`KalmanFilter`](@ref) but with the substitutions +``\mathbf{Ĉ^m = Ĥ^m}(k)`` and ``\mathbf{Â = F̂}(k)``, the Jacobians of the augmented process +model: ```math \begin{aligned} \mathbf{Ĥ}(k) &= \left. \frac{∂\mathbf{ĥ}(\mathbf{x̂}, \mathbf{d})}{∂\mathbf{x̂}} \right|_{\mathbf{x̂ = x̂}_{k-1}(k),\, \mathbf{d = d}(k)} \\ @@ -1161,8 +1067,7 @@ augmented process model: ``` The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs. The Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff) by default. The correction -and prediction step equations are provided below. The correction step is skipped if -`estim.direct == true` since it's already done by the user. +and prediction step equations are provided below. # Correction Step ```math @@ -1183,15 +1088,26 @@ 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 - correct_estimate!(estim, y0m, d0) - end +function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) + x̂0 = estim.x̂0 + cst_d0 = Constant(d0) + ŷ0, Ĥ, Ĥm = estim.buffer.ŷ, estim.Ĥ, estim.Ĥm + estim.linfuncĤ!(Ĥ, ŷ0, estim.jacobian, x̂0, cst_d0) + Ĥm .= @views Ĥ[estim.i_ym, :] + return correct_kf!(estim, y0m, d0, Ĥm) +end + +""" + predict_estimate!(estim::ExtendedKalmanFilter, u0, d0) + +Prediction step of [`ExtendedKalmanFilter`](@ref), see [`correct_estimate!`](@ref). +""" +function predict_estimate!(estim::ExtendedKalmanFilter, u0, d0) cst_u0, cst_d0 = Constant(u0), Constant(d0) x̂0corr = estim.x̂0 x̂0next, F̂ = estim.buffer.x̂, estim.F̂ estim.linfuncF̂!(F̂, x̂0next, estim.jacobian, x̂0corr, cst_u0, cst_d0) - return predict_estimate_kf!(estim, u0, d0, F̂) + return predict_kf!(estim, u0, d0, F̂) end "Print the `jacobian` backend and `direct` flag for [`ExtendedKalmanFilter`](@ref)." @@ -1209,14 +1125,14 @@ function init_estimate_cov!( end """ - correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm) + correct_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm) Correct time-varying/extended Kalman Filter estimates with augmented `Ĉm` matrices. Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter`](@ref). See [`update_estimate_kf!`](@ref) for more information. """ -function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm) +function correct_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm) R̂, K̂ = estim.cov.R̂, estim.K̂ x̂0, P̂ = estim.x̂0, estim.cov.P̂ # in-place operations to reduce allocations: @@ -1248,7 +1164,7 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, end """ - predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) + predict_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) Predict time-varying/extended Kalman Filter estimates with augmented `Ĉm` and `Â` matrices. @@ -1256,7 +1172,7 @@ Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter They predict the state `x̂` and covariance `P̂` with the same equations. See [`update_estimate`](@ref) methods for the equations. """ -function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) +function predict_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂ Q̂ = estim.cov.Q̂ x̂0next, û0, k = estim.buffer.x̂, estim.buffer.û, estim.buffer.k diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 5e660de8a..bd82b7ae8 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -114,27 +114,55 @@ function validate_luenberger(model, nint_u, nint_ym, poles) any(abs.(poles) .≥ 1) && error("Observer poles should be inside the unit circles.") end +@doc raw""" + correct_estimate!(estim::Union{SteadyKalmanFilter, Luenberger}, y0m, d0) -""" - correct_estimate!(estim::Luenberger, y0m, d0, _ ) +Correct `estim.x̂0` estimate with current measured outputs `y0m` and disturbances `d0`. + +The computations are identical for both [`SteadyKalmanFilter`](@ref) and [`Luenberger`](@ref) +state estimators. It will corrects the state estimate with the precomputed Kalman/observer +gain ``\mathbf{K̂}``. The correction and prediction step equations are provided below. + +# Correction Step +```math +\mathbf{x̂}_k(k) = \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}[\mathbf{y^m}(k) - \mathbf{Ĉ^m x̂}_{k-1}(k) + - \mathbf{D̂_d^m d}(k) ] +``` -Identical to [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). +# Prediction Step +```math +\mathbf{x̂}_{k}(k+1) = \mathbf{Â x̂}_{k}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) +``` """ -function correct_estimate!(estim::Luenberger, y0m, d0) - return correct_estimate_obsv!(estim, y0m, d0, estim.K̂) +function correct_estimate!(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: + mul!(ŷ0m, Ĉm, estim.x̂0) + mul!(ŷ0m, D̂dm, d0, 1, 1) + v̂ = ŷ0m + v̂ .= y0m .- ŷ0m + x̂0corr = estim.x̂0 + mul!(x̂0corr, K̂, v̂, 1, 1) + return nothing end - """ - update_estimate!(estim::Luenberger, y0m, d0, u0) + predict_estimate!(estim::Union{SteadyKalmanFilter, Luenberger}, u0, d0) -Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). +Prediction step of [`SteadyKalmanFilter`](@ref) and [`Luenberger`](@ref), see [`correct_estimate!`](@ref). """ -function update_estimate!(estim::Luenberger, y0m, d0, u0) - if !estim.direct - correct_estimate_obsv!(estim, y0m, d0, estim.K̂) - end - return predict_estimate_obsv!(estim, y0m, d0, u0) +function predict_estimate!(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̂ + # in-place operations to reduce allocations: + mul!(x̂0next, Â, x̂0corr) + mul!(x̂0next, B̂u, u0, 1, 1) + mul!(x̂0next, B̂d, d0, 1, 1) + x̂0next .+= estim.f̂op .- estim.x̂op + estim.x̂0 .= x̂0next + return nothing end "Throw an error if P̂ != nothing." diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index 55ff87254..a2ed89ed6 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -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/execute.jl b/src/estimator/mhe/execute.jl index c40ad8328..0851eac4e 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -36,10 +36,17 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) return nothing end +function correctstate!(::MovingHorizonEstimator,_,_) + error( + "MovingHorizonEstimator does not support correctstate!, call preparestate! instead.\n", + "The ym argument supports NaN values, to skip unavailable measurements." + ) +end + """ correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) -Do the same but for [`MovingHorizonEstimator`](@ref) objects. +Correct [`MovingHorizonEstimator`](@ref) state `estim.x̂0` if `estim.direct == true`. """ function correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) if estim.direct @@ -52,8 +59,16 @@ function correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) return nothing end + +function predictstate!(::MovingHorizonEstimator,_,_) + error( + "MovingHorizonEstimator does not support predictstate!, call updatestate! instead.\n", + "The ym argument supports NaN values, to skip unavailable measurements." + ) +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 +82,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 +762,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/src/sim_model.jl b/src/sim_model.jl index 15ed1595e..13addb717 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -220,6 +220,13 @@ function preparestate!(model::SimModel) return x end +@doc raw""" + correctstate!(model::SimModel) -> x + +Do nothing for [`SimModel`](@ref) and return the current model state ``\mathbf{x}(k)``. +""" +correctstate!(model::SimModel) = preparestate!(model::SimModel) + @doc raw""" updatestate!(model::SimModel, u, d=[]) -> xnext @@ -249,6 +256,13 @@ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: return xnext end +""" + predictstate!(model::SimModel, u, d=[]) -> xnext + +Same as [`updatestate!`](@ref) for [`SimModel`](@ref). +""" +predictstate!(model::SimModel, u, d=model.buffer.empty) = updatestate!(model, u, d) + @doc raw""" evaloutput(model::SimModel, d=[]) -> y