Functions: State Estimators

Abstract supertype of all state estimators.

(estim::StateEstimator)(d=[]) -> ŷ

Functor allowing callable StateEstimator object as an alias for evaloutput.

Examples

julia> kf = KalmanFilter(setop!(LinModel(tf(3, [10, 1]), 2), yop=[20]));

julia> ŷ = kf() 
1-element Vector{Float64}:
 20.0

SteadyKalmanFilter(model::LinModel; <keyword arguments>)

Construct a steady-state Kalman Filter with the LinModel model.

The steady-state (or asymptotic) Kalman filter is based on the process model :

\[\begin{aligned} \mathbf{x}(k+1) &= \mathbf{Â x}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) + \mathbf{w}(k) \\ \mathbf{y^m}(k) &= \mathbf{Ĉ^m x}(k) + \mathbf{D̂_d^m d}(k) + \mathbf{v}(k) \\ \mathbf{y^u}(k) &= \mathbf{Ĉ^u x}(k) + \mathbf{D̂_d^u d}(k) \end{aligned}\]

with sensor $\mathbf{v}(k)$ and process $\mathbf{w}(k)$ noises as uncorrelated zero mean white noise vectors, with a respective covariance of $\mathbf{R̂}$ and $\mathbf{Q̂}$. The arguments are in standard deviations σ, i.e. same units than outputs and states. The matrices $\mathbf{Â, B̂_u, B̂_d, Ĉ, D̂_d}$ are model matrices augmented with the stochastic model, which is specified by the numbers of integrator nint_u and nint_ym (see Extended Help). Likewise, the covariance matrices are augmented with $\mathbf{Q̂ = \text{diag}(Q, Q_{int_u}, Q_{int_{ym}})}$ and $\mathbf{R̂ = R}$. The Extended Help provide some guidelines on the covariance tuning. The matrices $\mathbf{Ĉ^m, D̂_d^m}$ are the rows of $\mathbf{Ĉ, D̂_d}$ that correspond to measured outputs $\mathbf{y^m}$ (and unmeasured ones, for $\mathbf{Ĉ^u, D̂_d^u}$). The Kalman filter will estimate the current state with the newest measurements $\mathbf{x̂}_k(k)$ if direct is true, else it will predict the state of the next time step $\mathbf{x̂}_k(k+1)$.

Arguments

• model::LinModel : (deterministic) model for the estimations.
• i_ym=1:model.ny : model output indices that are measured $\mathbf{y^m}$, the rest are unmeasured $\mathbf{y^u}$.
• σQ=fill(1/model.nx,model.nx) or sigmaQ : main diagonal of the process noise covariance $\mathbf{Q}$ of model, specified as a standard deviation vector.
• σR=fill(1,length(i_ym)) or sigmaR : main diagonal of the sensor noise covariance $\mathbf{R}$ of model measured outputs, specified as a standard deviation vector.
• nint_u=0: integrator quantity for the stochastic model of the unmeasured disturbances at 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).
• σQint_u=fill(1,sum(nint_u)) or sigmaQint_u : same than σQ but for the unmeasured disturbances at manipulated inputs $\mathbf{Q_{int_u}}$ (composed of integrators).
• σQint_ym=fill(1,sum(nint_ym)) or sigmaQint_u : same than σQ for the unmeasured 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).

Examples

julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);

julia> estim = SteadyKalmanFilter(model, i_ym=[2], σR=[1], σQint_ym=[0.01])
SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
 1 manipulated inputs u (0 integrating states)
 3 estimated states x̂
 1 measured outputs ym (1 integrating states)
 1 unmeasured outputs yu
 0 measured disturbances d

Extended Help

SteadyKalmanFilter(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true)

Construct the estimator from the augmented covariance matrices Q̂ and R̂.

This syntax allows nonzero off-diagonal elements in $\mathbf{Q̂, R̂}$.

KalmanFilter(model::LinModel; <keyword arguments>)

Construct a time-varying Kalman Filter with the LinModel model.

The process model is identical to SteadyKalmanFilter. The matrix $\mathbf{P̂}$ is the estimation error covariance of model states augmented with the stochastic ones (specified by nint_u and nint_ym). Three keyword arguments specify its initial value with $\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}$. The initial state estimate $\mathbf{x̂}_{-1}(0)$ can be manually specified with setstate!, or automatically with initstate!.

Arguments

• model::LinModel : (deterministic) model for the estimations.
• i_ym=1:model.ny : model output indices that are measured $\mathbf{y^m}$, the rest are unmeasured $\mathbf{y^u}$.
• σP_0=fill(1/model.nx,model.nx) or sigmaP_0 : main diagonal of the initial estimate covariance $\mathbf{P}(0)$, specified as a standard deviation vector.
• σQ=fill(1/model.nx,model.nx) or sigmaQ : main diagonal of the process noise covariance $\mathbf{Q}$ of model, specified as a standard deviation vector.
• σR=fill(1,length(i_ym)) or sigmaR : main diagonal of the sensor noise covariance $\mathbf{R}$ of model measured outputs, specified as a standard deviation vector.
• nint_u=0: integrator quantity for the stochastic model of the unmeasured disturbances at the manipulated inputs (vector), use nint_u=0 for no integrator.
• 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.
• σQint_u=fill(1,sum(nint_u)) or sigmaQint_u : same than σQ but for the unmeasured disturbances at manipulated inputs $\mathbf{Q_{int_u}}$ (composed of integrators).
• σPint_u_0=fill(1,sum(nint_u)) or sigmaPint_u_0 : same than σP_0 but for the unmeasured disturbances at manipulated inputs $\mathbf{P_{int_u}}(0)$ (composed of integrators).
• σQint_ym=fill(1,sum(nint_ym)) or sigmaQint_u : same than σQ for the unmeasured disturbances at measured outputs $\mathbf{Q_{int_{ym}}}$ (composed of integrators).
• σPint_ym_0=fill(1,sum(nint_ym)) or sigmaPint_ym_0 : same than σP_0 but for the unmeasured 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).

Examples

julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);

julia> estim = KalmanFilter(model, i_ym=[2], σR=[1], σP_0=[100, 100], σQint_ym=[0.01])
KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
 1 manipulated inputs u (0 integrating states)
 3 estimated states x̂
 1 measured outputs ym (1 integrating states)
 1 unmeasured outputs yu
 0 measured disturbances d

KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true)

Construct the estimator from the augmented covariance matrices P̂_0, Q̂ and R̂.

This syntax allows nonzero off-diagonal elements in $\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}$.

Luenberger(
    model::LinModel; 
    i_ym = 1:model.ny, 
    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
)

Construct a Luenberger observer with the LinModel model.

i_ym provides the model output indices that are measured $\mathbf{y^m}$, the rest are unmeasured $\mathbf{y^u}$. model matrices are augmented with the stochastic model, which is specified by the numbers of integrator nint_u and nint_ym (see SteadyKalmanFilter Extended Help). The argument poles is a vector of model.nx + sum(nint_u) + sum(nint_ym) elements specifying the observer poles/eigenvalues (near $z=0.5$ by default). The observer is constructed with a direct transmission from $\mathbf{y^m}$ if direct=true (a.k.a. current observers, in opposition to the delayed/prediction form). The method computes the observer gain K̂ with place.

Examples

julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);

julia> estim = Luenberger(model, nint_ym=[1, 1], poles=[0.61, 0.62, 0.63, 0.64])
Luenberger estimator with a sample time Ts = 0.5 s, LinModel and:
 1 manipulated inputs u (0 integrating states)
 4 estimated states x̂
 2 measured outputs ym (2 integrating states)
 0 unmeasured outputs yu
 0 measured disturbances d

UnscentedKalmanFilter(model::SimModel; <keyword arguments>)

Construct an unscented Kalman Filter with the SimModel model.

Both LinModel and NonLinModel are supported. The unscented Kalman filter is based on the process model :

\[\begin{aligned} \mathbf{x}(k+1) &= \mathbf{f̂}\Big(\mathbf{x}(k), \mathbf{u}(k), \mathbf{d}(k)\Big) + \mathbf{w}(k) \\ \mathbf{y^m}(k) &= \mathbf{ĥ^m}\Big(\mathbf{x}(k), \mathbf{d}(k)\Big) + \mathbf{v}(k) \\ \mathbf{y^u}(k) &= \mathbf{ĥ^u}\Big(\mathbf{x}(k), \mathbf{d}(k)\Big) \\ \end{aligned}\]

See SteadyKalmanFilter for details on $\mathbf{v}(k), \mathbf{w}(k)$ noises and $\mathbf{R̂}, \mathbf{Q̂}$ covariances. The two matrices are constructed from $\mathbf{Q̂ = \text{diag}(Q, Q_{int_u}, Q_{int_{ym}})}$ and $\mathbf{R̂ = R}$. The functions $\mathbf{f̂, ĥ}$ are model state-space functions augmented with the stochastic model of the unmeasured disturbances, which is specified by the numbers of integrator nint_u and nint_ym (see Extended Help). The $\mathbf{ĥ^m}$ function represents the measured outputs of $\mathbf{ĥ}$ function (and unmeasured ones, for $\mathbf{ĥ^u}$). The matrix $\mathbf{P̂}$ is the estimation error covariance of model state augmented with the stochastic ones. Three keyword arguments specify its initial value with $\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}$. The initial state estimate $\mathbf{x̂}_{-1}(0)$ can be manually specified with setstate!.

Arguments

• model::SimModel : (deterministic) model for the estimations.
• i_ym=1:model.ny : model output indices that are measured $\mathbf{y^m}$, the rest are unmeasured $\mathbf{y^u}$.
• σP_0=fill(1/model.nx,model.nx) or sigmaP_0 : main diagonal of the initial estimate covariance $\mathbf{P}(0)$, specified as a standard deviation vector.
• σQ=fill(1/model.nx,model.nx) or sigmaQ : main diagonal of the process noise covariance $\mathbf{Q}$ of model, specified as a standard deviation vector.
• σR=fill(1,length(i_ym)) or sigmaR : main diagonal of the sensor noise covariance $\mathbf{R}$ of model measured outputs, specified as a standard deviation vector.
• nint_u=0: integrator quantity for the stochastic model of the unmeasured disturbances at 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).
• σQint_u=fill(1,sum(nint_u)) or sigmaQint_u : same than σQ but for the unmeasured disturbances at manipulated inputs $\mathbf{Q_{int_u}}$ (composed of integrators).
• σPint_u_0=fill(1,sum(nint_u)) or sigmaPint_u_0 : same than σP_0 but for the unmeasured disturbances at manipulated inputs $\mathbf{P_{int_u}}(0)$ (composed of integrators).
• σQint_ym=fill(1,sum(nint_ym)) or sigmaQint_u : same than σQ for the unmeasured disturbances at measured outputs $\mathbf{Q_{int_{ym}}}$ (composed of integrators).
• σPint_ym_0=fill(1,sum(nint_ym)) or sigmaPint_ym_0 : same than σP_0 but for the unmeasured 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).
• α=1e-3 or alpha : alpha parameter, spread of the state distribution $(0 < α ≤ 1)$.
• β=2 or beta : beta parameter, skewness and kurtosis of the states distribution $(β ≥ 0)$.
• κ=0 or kappa : kappa parameter, another spread parameter $(0 ≤ κ ≤ 3)$.

Examples

julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);

julia> estim = UnscentedKalmanFilter(model, σR=[1], nint_ym=[2], σPint_ym_0=[1, 1])
UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and:
 1 manipulated inputs u (0 integrating states)
 3 estimated states x̂
 1 measured outputs ym (2 integrating states)
 0 unmeasured outputs yu
 0 measured disturbances d

Extended Help

UnscentedKalmanFilter(
    model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; direct=true
)

Construct the estimator from the augmented covariance matrices P̂_0, Q̂ and R̂.

This syntax allows nonzero off-diagonal elements in $\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}$.

ExtendedKalmanFilter(model::SimModel; <keyword arguments>)

Construct an extended Kalman Filter with the SimModel model.

Both LinModel and NonLinModel are supported. The process model and the keyword arguments are identical to UnscentedKalmanFilter, except for α, β and κ which do not apply to the extended Kalman Filter. The Jacobians of the augmented model $\mathbf{f̂, ĥ}$ are computed with ForwardDiff.jl automatic differentiation.

Examples

julia> model = NonLinModel((x,u,_)->0.2x+u, (x,_)->-3x, 5.0, 1, 1, 1, solver=nothing);

julia> estim = ExtendedKalmanFilter(model, σQ=[2], σQint_ym=[2], σP_0=[0.1], σPint_ym_0=[0.1])
ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and:
 1 manipulated inputs u (0 integrating states)
 2 estimated states x̂
 1 measured outputs ym (1 integrating states)
 0 unmeasured outputs yu
 0 measured disturbances d

ExtendedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true)

Construct the estimator from the augmented covariance matrices P̂_0, Q̂ and R̂.

This syntax allows nonzero off-diagonal elements in $\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}$.

MovingHorizonEstimator(model::SimModel; <keyword arguments>)

Construct a moving horizon estimator (MHE) based on model (LinModel or NonLinModel).

It can handle constraints on the estimates, see setconstraint!. Additionally, model is not linearized like the ExtendedKalmanFilter, and the probability distribution is not approximated like the UnscentedKalmanFilter. The computational costs are drastically higher, however, since it minimizes the following objective function at each discrete time $k$:

\[\min_{\mathbf{x̂}_k(k-N_k+p), \mathbf{Ŵ}, ϵ} \mathbf{x̄}' \mathbf{P̄}^{-1} \mathbf{x̄} + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ} + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} + C ϵ^2\]

in which the arrival costs are evaluated from the states estimated at time $k-N_k$:

\[\begin{aligned} \mathbf{x̄} &= \mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂}_k(k-N_k+p) \\ \mathbf{P̄} &= \mathbf{P̂}_{k-N_k}(k-N_k+p) \end{aligned}\]

and the covariances are repeated $N_k$ times:

\[\begin{aligned} \mathbf{Q̂}_{N_k} &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)} \\ \mathbf{R̂}_{N_k} &= \text{diag}\mathbf{(R̂,R̂,...,R̂)} \end{aligned}\]

The estimation horizon $H_e$ limits the window length:

\[N_k = \begin{cases} k + 1 & k < H_e \\ H_e & k ≥ H_e \end{cases}\]

The vectors $\mathbf{Ŵ}$ and $\mathbf{V̂}$ respectively encompass the estimated process noises $\mathbf{ŵ}(k-j+p)$ from $j=N_k$ to $1$ and sensor noises $\mathbf{v̂}(k-j+1)$ from $j=N_k$ to $1$. The Extended Help defines the two vectors, the slack variable $ϵ$, and the estimation of the covariance at arrival $\mathbf{P̂}_{k-N_k}(k-N_k+p)$. If the keyword argument direct=true (default value), the constant $p=0$ in the equations above, and the MHE is in the current form. Else $p=1$, leading to the prediction form.

See UnscentedKalmanFilter for details on the augmented process model and $\mathbf{R̂}, \mathbf{Q̂}$ covariances.

Arguments

• model::SimModel : (deterministic) model for the estimations.
• He=nothing : estimation horizon $H_e$, must be specified.
• i_ym=1:model.ny : model output indices that are measured $\mathbf{y^m}$, the rest are unmeasured $\mathbf{y^u}$.
• σP_0=fill(1/model.nx,model.nx) or sigmaP_0 : main diagonal of the initial estimate covariance $\mathbf{P}(0)$, specified as a standard deviation vector.
• σQ=fill(1/model.nx,model.nx) or sigmaQ : main diagonal of the process noise covariance $\mathbf{Q}$ of model, specified as a standard deviation vector.
• σR=fill(1,length(i_ym)) or sigmaR : main diagonal of the sensor noise covariance $\mathbf{R}$ of model measured outputs, specified as a standard deviation vector.
• nint_u=0: integrator quantity for the stochastic model of the unmeasured disturbances at 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).
• σQint_u=fill(1,sum(nint_u)) or sigmaQint_u : same than σQ but for the unmeasured disturbances at manipulated inputs $\mathbf{Q_{int_u}}$ (composed of integrators).
• σPint_u_0=fill(1,sum(nint_u)) or sigmaPint_u_0 : same than σP_0 but for the unmeasured disturbances at manipulated inputs $\mathbf{P_{int_u}}(0)$ (composed of integrators).
• σQint_ym=fill(1,sum(nint_ym)) or sigmaQint_u : same than σQ for the unmeasured disturbances at measured outputs $\mathbf{Q_{int_{ym}}}$ (composed of integrators).
• σPint_ym_0=fill(1,sum(nint_ym)) or sigmaPint_ym_0 : same than σP_0 but for the unmeasured disturbances at measured outputs $\mathbf{P_{int_{ym}}}(0)$ (composed of integrators).
• Cwt=Inf : slack variable weight $C$, default to Inf meaning hard constraints only.
• optim=default_optim_mhe(model) : a JuMP.Model with a quadratic/nonlinear optimizer for solving (default to Ipopt, or OSQP if model is a LinModel).
• direct=true: construct with a direct transmission from $\mathbf{y^m}$ (a.k.a. current estimator, in opposition to the delayed/predictor form).

Examples

julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);

julia> estim = MovingHorizonEstimator(model, He=5, σR=[1], σP_0=[0.01])
MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer, NonLinModel and:
 5 estimation steps He
 0 slack variable ϵ (estimation constraints)
 1 manipulated inputs u (0 integrating states)
 2 estimated states x̂
 1 measured outputs ym (1 integrating states)
 0 unmeasured outputs yu
 0 measured disturbances d

Extended Help

MovingHorizonEstimator(
    model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
    optim=default_optim_mhe(model), 
    direct=true,
    covestim=default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
)

Construct the estimator from the augmented covariance matrices P̂_0, Q̂ and R̂.

This syntax allows nonzero off-diagonal elements in $\mathbf{P̂_i}, \mathbf{Q̂, R̂}$, where $\mathbf{P̂_i}$ is the initial estimation covariance, provided by P̂_0 argument. The keyword argument covestim also allows specifying a custom StateEstimator object for the estimation of covariance at the arrival $\mathbf{P̂}_{k-N_k}(k-N_k+p)$. The supported types are KalmanFilter, UnscentedKalmanFilter and ExtendedKalmanFilter.

InternalModel(model::SimModel; i_ym=1:model.ny, stoch_ym=ss(I,I,I,I,model.Ts))

Construct an internal model estimator based on model (LinModel or NonLinModel).

i_ym provides the model output indices that are measured $\mathbf{y^m}$, the rest are unmeasured $\mathbf{y^u}$. model evaluates the deterministic predictions $\mathbf{ŷ_d}$, and stoch_ym, the stochastic predictions of the measured outputs $\mathbf{ŷ_s^m}$ (the unmeasured ones being $\mathbf{ŷ_s^u=0}$). The predicted outputs sum both values : $\mathbf{ŷ = ŷ_d + ŷ_s}$. See the Extended Help for more details.

Examples

julia> estim = InternalModel(LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5), i_ym=[2])
InternalModel estimator with a sample time Ts = 0.5 s, LinModel and:
 1 manipulated inputs u
 2 estimated states x̂
 1 measured outputs ym
 1 unmeasured outputs yu
 0 measured disturbances d

Extended Help

default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0) -> nint_ym

Get default integrator quantity per measured outputs nint_ym for LinModel.

The arguments i_ym and nint_u are the measured output indices and the integrator 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).

Examples

julia> model = LinModel(append(tf(3, [10, 1]), tf(2, [1, 0]), tf(4,[-5, 1])), 1.0);

julia> nint_ym = default_nint(model)
3-element Vector{Int64}:
 1
 0
 1

default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)

One integrator on each measured output by default if model is not a LinModel.

Theres is no verification the augmented model remains observable. If the integrator quantity per manipulated input nint_u ≠ 0, the method returns zero integrator on each measured output.