Functions: StateEstimator Internals

init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym

Init stochastic model matrices from integrator specifications for state estimation.

The arguments nint_u and nint_ym specify how many integrators are added to each manipulated input and measured outputs. The function returns the state-space matrices As, Cs_u and Cs_y of the stochastic model:

\[\begin{aligned} \mathbf{x_{s}}(k+1) &= \mathbf{A_s x_s}(k) + \mathbf{B_s e}(k) \\ \mathbf{y_{s_{u}}}(k) &= \mathbf{C_{s_{u}} x_s}(k) \\ \mathbf{y_{s_{y}}}(k) &= \mathbf{C_{s_{y}} x_s}(k) \end{aligned}\]

where $\mathbf{e}(k)$ is an unknown zero mean white noise and $\mathbf{A_s} = \mathrm{diag}(\mathbf{A_{s_{u}}, A_{s_{y}}})$. The estimations does not use $\mathbf{B_s}$, it is thus ignored. The function init_integrators builds the state-space matrices.

init_integrators(nint, ny, varname::String) -> A, C, nint

Calc A, C state-space matrices from integrator specifications nint.

This function is used to initialize the stochastic part of the augmented model for the design of state estimators. The vector nint provides how many integrators (in series) should be incorporated for each output. The argument should have ny element, except for nint=0 which is an alias for no integrator at all. The specific case of one integrator per output results in A = I and C = I. The estimation does not use the B matrix, it is thus ignored. This function is called twice :

1. for the unmeasured disturbances at manipulated inputs $\mathbf{u}$
2. for the unmeasured disturbances at measured outputs $\mathbf{y^m}$

augment_model(
    model::LinModel, As, Cs_u, Cs_y; verify_obsv=true
) -> Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op

Augment LinModel state-space matrices with stochastic ones As, Cs_u, Cs_y.

If $\mathbf{x_0}$ is model.x0 state, and $\mathbf{x_s}$, the states defined at init_estimstoch, we define an augmented state vector $\mathbf{x̂_0} = [ \begin{smallmatrix} \mathbf{x_0} \\ \mathbf{x_s} \end{smallmatrix} ]$. The method returns the augmented matrices Â, B̂u, Ĉ, B̂d and D̂d:

\[\begin{aligned} \mathbf{x̂_0}(k+1) &= \mathbf{Â x̂_0}(k) + \mathbf{B̂_u u_0}(k) + \mathbf{B̂_d d_0}(k) \\ \mathbf{ŷ_0}(k) &= \mathbf{Ĉ x̂_0}(k) + \mathbf{D̂_d d_0}(k) \end{aligned}\]

An error is thrown if the augmented model is not observable and verify_obsv == true. The augmented operating points $\mathbf{x̂_{op}}$ and $\mathbf{f̂_{op}}$ are simply $\mathbf{x_{op}}$ and $\mathbf{f_{op}}$ vectors appended with zeros (see setop!). See Extended Help for a detailed definition of the augmented matrices and vectors.

Extended Help

augment_model(
    model::SimModel, As, Cs_u, Cs_y; verify_obsv=false
) -> Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op

Return empty matrices, and x̂op & f̂op vectors, if model is not a LinModel.

init_ukf(nx̂, α, β, κ) -> nσ, γ, m̂, Ŝ

Compute the UnscentedKalmanFilter constants from $α, β$ and $κ$.

With $n_\mathbf{x̂}$ elements in the state vector $\mathbf{x̂}$ and $n_σ = 2 n_\mathbf{x̂} + 1$ sigma points, the scaling factor applied on standard deviation matrices $\sqrt{\mathbf{P̂}}$ is:

\[ γ = α \sqrt{ n_\mathbf{x̂} + κ }\]

The weight vector $(n_σ × 1)$ for the mean and the weight matrix $(n_σ × n_σ)$ for the covariance are respectively:

\[\begin{aligned} \mathbf{m̂} &= \begin{bmatrix} 1 - \tfrac{n_\mathbf{x̂}}{γ^2} & \tfrac{1}{2γ^2} & \tfrac{1}{2γ^2} & \cdots & \tfrac{1}{2γ^2} \end{bmatrix}' \\ \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) for other details.

init_internalmodel(As, Bs, Cs, Ds) -> Âs, B̂s

Calc stochastic model update matrices Âs and B̂s for InternalModel estimator.

As, Bs, Cs and Ds are the stochastic model matrices :

\[\begin{aligned} \mathbf{x_s}(k+1) &= \mathbf{A_s x_s}(k) + \mathbf{B_s e}(k) \\ \mathbf{y_s}(k) &= \mathbf{C_s x_s}(k) + \mathbf{D_s e}(k) \end{aligned}\]

where $\mathbf{e}(k)$ is conceptual and unknown zero mean white noise. Its optimal estimation is $\mathbf{ê=0}$, the expected value. Thus, the Âs and B̂s matrices that optimally update the stochastic estimate $\mathbf{x̂_s}$ are:

\[\begin{aligned} \mathbf{x̂_s}(k+1) &= \mathbf{(A_s - B_s D_s^{-1} C_s) x̂_s}(k) + \mathbf{(B_s D_s^{-1}) ŷ_s}(k) \\ &= \mathbf{Â_s x̂_s}(k) + \mathbf{B̂_s ŷ_s}(k) \end{aligned}\]

with current stochastic outputs estimation $\mathbf{ŷ_s}(k)$, composed of the measured $\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)$ and unmeasured $\mathbf{ŷ_s^u = 0}$ outputs. See ^[1].

init_ZtoŴ(model::SimModel, transcription::TranscriptionMethod, He, nx̂, nŵ) -> Tŵ

Init decision variables to estimated process noise over $H_e$ conversion matrix Tŵ.

This is for the MovingHorizonEstimator only. The conversion from the decision variables $\mathbf{Z}$ to $\mathbf{Ŵ}$, the estimated process noise over $H_e$, is computed by:

\[\mathbf{Ŵ} = \mathbf{T_ŵ Z}\]

in which $\mathbf{T_{ŵ}} = [\begin{smallmatrix} \mathbf{0} & \mathbf{I} \end{smallmatrix}]$ and $\mathbf{0}$ is properly sized for the transcription instance.

init_predmat_mhe(
    model::LinModel, transcription::SingleShooting,
    He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂

Construct the MHE prediction matrices for LinModel and SingleShooting.

We first introduce the deviation vector of the estimated state at arrival $\mathbf{x̂_0}(k-N_k+p) = \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}}$ (see setop!), and the vector $\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂_0}(k-N_k+p) \\ \mathbf{Ŵ} \end{smallmatrix}]$ with the decision variables. Setting the constant $p=0$ produces an estimator in the current form, while the prediction form is obtained with $p=1$. The estimated sensor noises from time $k-N_k+1$ to $k$ are computed by:

\[\begin{aligned} \mathbf{V̂} = \mathbf{Y_0^m - Ŷ_0^m} &= \mathbf{E Z + G U_0 + J D_0 + Y_0^m + B} \\ &= \mathbf{E Z + F} \end{aligned}\]

in which $\mathbf{U_0}$ and $\mathbf{Y_0^m}$ respectively include the deviation values of the manipulated inputs $\mathbf{u_0}(k-j+p)$ from $j=N_k$ to $1$ and measured outputs $\mathbf{y_0^m}(k-j+1)$ from $j=N_k$ to $1$. The vector $\mathbf{D_0}$ includes the the measured disturbance deviation values $\mathbf{d_0}(k-j)$ from from $j=N_k$ to $0$, thus one additional data point. The constant $\mathbf{B}$ is the contribution for non-zero state $\mathbf{x̂_{op}}$ and state update $\mathbf{f̂_{op}}$ operating points (for linearization, see augment_model and linearize). The method also returns the matrices for the estimation error at arrival:

\[ \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄}\]

in which $\mathbf{e_x̄} = [\begin{smallmatrix} -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \end{smallmatrix}]$, and $\mathbf{f_x̄} = \mathbf{x̂_0^†}(k-N_k+p)$. The latter is the deviation vector of the state at arrival, estimated at time $k-N_k$, i.e. $\mathbf{x̂_0^†}(k-N_k+p) = \mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂_{op}}$. Lastly, the estimates $\mathbf{x̂_0}(k-j+p)$ from $j=N_k-1$ to $0$, also in deviation form, are computed with:

\[\begin{aligned} \mathbf{X̂_0} &= \mathbf{E_X̂ Z + G_X̂ U_0 + J_X̂ D_0 + B_X̂} \\ &= \mathbf{E_X̂ Z + F_X̂} \end{aligned}\]

The matrices $\mathbf{E, G, J, B, E_X̂, G_X̂, J_X̂, B_X̂}$ are defined in the Extended Help section. The vectors $\mathbf{F, F_X̂, f_x̄}$ are recalculated at each discrete time step, see initpred!(::MovingHorizonEstimator, ::LinModel) and linconstraint!(::MovingHorizonEstimator, ::LinModel).

Extended Help

init_predmat_mhe(
    model::LinModel, transcription::MultipleShooting, 
    He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂

Construct them for LinModel and MultipleShooting.

The $\mathbf{e_x̄}$ is identical to the SingleShooting transcription. The other matrices are defined in the Extended Help section.

Extended Help

init_predmat_mhe(
    model::SimModel, ::SingleShooting, 
    He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂

Return empty matrices for SingleShooting and non-LinModel, except for ex̄.

init_predmat_mhe(
    model::SimModel, ::TranscriptionMethod, 
    He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂

Return ex̄, EX̂, GX̂, JX̂, BX̂ and empty matrices non-LinModel and other TranscriptionMethod.

init_defectmat_mhe(
    model::LinModel, transcription::MultipleShooting, 
    He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
) -> ES, GS, JS, BS

Init the matrices for computing the defects over the predicted states.

With a MultipleShooting transcription, the decision vector $\mathbf{Z}$ contains the arrival state estimate $\mathbf{x̂_0}(k-N_k+p)$, the stage states $\mathbf{X̂_0}$ (both defined as deviation vector from $\mathbf{x̂_{op}}$) and the estimated process noises $\mathbf{Ŵ}$. Knowing this, an equation similar to the prediction matrices (see init_predmat_mhe) computes the defects of the estimated states over $H_e$:

\[\begin{aligned} \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S U_0} + \mathbf{J_S D_0} + \mathbf{B_S} \\ &= \mathbf{E_S Z} + \mathbf{F_S} \end{aligned}\]

They are forced to be $\mathbf{Ŝ = 0}$ using the optimization equality constraints. The matrices $\mathbf{E_S, G_S, J_S, B_S}$ are defined in the Extended Help section.

Extended Help

Return empty matrices for SimModel (will change in the future).

Return empty matrices for SingleShooting transcription on any SimModel (N/A).

relaxarrival(ex̄, c_x̂min, c_x̂maxm, nε) -> A_x̂min, A_x̂max, ẽx̄

Augment arrival state constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable $\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]$, it returns the $\mathbf{ẽ_x̄}$ matrix that appears in the estimation error at arrival equation $\mathbf{x̄} = \mathbf{ẽ_x̄ Z̃ + f_x̄}$. It also returns the augmented constraints the $\mathbf{A}$ matrices for the inequality constraints:

\[\begin{bmatrix} \mathbf{A_{x̂_{min}}} \\ \mathbf{A_{x̂_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{(x̂_{min} - x̂_{op})} \\ + \mathbf{(x̂_{max} - x̂_{op})} \end{bmatrix}\]

relaxX̂(EX̂, C_x̂min, C_x̂max, nε) -> A_X̂min, A_X̂max, ẼX̂

Augment estimated state constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable $\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]$, it returns the $\mathbf{Ẽ_X̂}$ matrix that appears in estimated states equation $\mathbf{X̂_0} = \mathbf{Ẽ_X̂ Z̃ + F_X̂}$. It also returns the $\mathbf{A}$ matrices for the inequality constraints:

\[\begin{bmatrix} \mathbf{A_{X̂_{min}}} \\ \mathbf{A_{X̂_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{(X̂_{min} - X̂_{op}) + F_X̂} \\ + \mathbf{(X̂_{max} - X̂_{op}) - F_X̂} \end{bmatrix}\]

in which $\mathbf{X̂_{min}, X̂_{max}}$ and $\mathbf{X̂_{op}}$ vectors respectively contains $\mathbf{x̂_{min}, x̂_{max}}$ and $\mathbf{x̂_{op}}$ repeated $H_e$ times.

relaxŴ(Tŵ, C_ŵmin, C_ŵmax, nε) -> A_Ŵmin, A_Ŵmax

Augment estimated process noise constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable $\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]$, it returns the $\mathbf{A}$ matrices for the inequality constraints:

\[\begin{bmatrix} \mathbf{A_{Ŵ_{min}}} \\ \mathbf{A_{Ŵ_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{Ŵ_{min}} \\ + \mathbf{Ŵ_{max}} \end{bmatrix}\]

relaxV̂(E, C_v̂min, C_v̂max, nε) -> A_V̂min, A_V̂max, Ẽ

Augment estimated sensor noise constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable $\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]$, it returns the $\mathbf{Ẽ}$ matrix that appears in estimated sensor noise equation $\mathbf{V̂} = \mathbf{Ẽ Z̃ + F}$. It also returns the $\mathbf{A}$ matrices for the inequality constraints:

\[\begin{bmatrix} \mathbf{A_{V̂_{min}}} \\ \mathbf{A_{V̂_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{V̂_{min} + F} \\ + \mathbf{V̂_{max} - F} \end{bmatrix}\]

init_matconstraint_mhe(
    model::LinModel, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nε,
    x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args...
) -> i_b, i_g, A, Aeq, neq

Init i_b, i_g, neq, and A and Aeq matrices for all the MHE constraints.

The linear and nonlinear inequality constraints are respectively defined as:

\[\begin{aligned} \mathbf{A Z̃ } &≤ \mathbf{b} \\ \mathbf{A_{eq} Z̃} &= \mathbf{b_{eq}} \\ \mathbf{g(Z̃)} &≤ \mathbf{0} \\ \mathbf{g_{eq}(Z̃)} &= \mathbf{0} \end{aligned}\]

The argument nc is the number of custom nonlinear inequality constraints in $\mathbf{g_c}$. i_b is a BitVector including the indices of $\mathbf{b}$ that are finite numbers. i_g is a similar vector but for the indices of $\mathbf{g}$. The method also returns the \mathbf{A, A_{eq}}matrices andneqifargsis provided. In such a case,argsneeds to contain all the inequality and equality constraint matrices:Ax̂min, Ax̂max, AX̂min, AX̂max, AŴmin, AŴmax, AV̂min, AV̂max, Aeq. The integerneqis the number of nonlinear equality constraints in\mathbf{g_{eq}}`.

Init i_b, A without sensor noise and state constraints if NonLinModel and SingleShooting.

Init i_b, A without sensor noise constraints if NonLinModel and other TranscriptionMethod.

get_nonlinobj_op(estim::MovingHorizonEstimator, optim) -> J_op

Return the nonlinear operator for the objective of estim MovingHorizonEstimator.

It is based on the splatting syntax. This method is really intricate and that's because of:

• These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. All the function outputs and derivatives are cached and updated in-place if required to use the efficient value_and_gradient!.
• The splatting syntax for objective functions implies the use of Vararg{T,N} (see the performance tip) and memoization to avoid redundant computations. This is already complex, but it's even worse knowing that the automatic differentiation tools do not support splatting.

get_nonlincon_oracle(estim::MovingHorizonEstimator, optim) -> g_oracle, geq_oracle

Return the nonlinear constraint oracles for MovingHorizonEstimator estim.

Return g_oracle and geq_oracle, the inequality and equality VectorNonlinearOracle for the two respective constraints. Note that g_oracle only includes the non-Inf inequality constraints, thus it must be re-constructed if they change. This method is really intricate because the oracles are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. All the function outputs and derivatives are ached and updated in-place if required to use the efficient value_and_jacobian!.

f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing

Mutating state update function $\mathbf{f̂}$ of the augmented model.

By introducing an augmented state vector $\mathbf{x̂_0}$ like in augment_model, the function returns the next state of the augmented model, as deviation vectors:

\[\begin{aligned} \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\ \mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) \end{aligned}\]

where $\mathbf{x̂_0}(k+1)$ is stored in x̂0next argument. The method mutates x̂0next, û0 and k in place. The argument û0 stores the disturbed input of the augmented model $\mathbf{û_0}$, and k, the intermediate stage values of model.solver, when applicable. The model parameter model.p is not included in the function signature for conciseness. The operating points are handled inside $\mathbf{f̂}$. See Extended Help for details on $\mathbf{û_0, f̂}$ and $\mathbf{ĥ}$ implementations.

Extended Help

f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing

Use the augmented model matrices and operating points if model is a LinModel.

Extended Help

f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0)

Same than f̂! for SimModel but without the estim argument.

f̂!(x̂0next, _ , k, estim::InternalModel, model::NonLinModel, x̂0, u0, d0)

State update function $\mathbf{f̂}$ of InternalModel for NonLinModel.

It calls f! directly since this estimator does not augment the states.

ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing

Mutating output function $\mathbf{ĥ}$ of the augmented model, see f̂!.

ĥ!(ŷ0, estim::StateEstimator, model::LinModel, x̂0, d0) -> nothing

Use the augmented model matrices if model is a LinModel.

ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0)

Same than ĥ! for SimModel but without the estim argument.

ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0)

Output function $\mathbf{ĥ}$ of InternalModel, it calls h!.

initpred!(estim::MovingHorizonEstimator, model::LinModel) -> nothing

Init quadratic optimization matrices F, fx̄, H̃, q̃, r for MovingHorizonEstimator.

See init_predmat_mhe for the definition of the vectors $\mathbf{F, f_x̄}$. It also inits estim.optim objective function, expressed as the quadratic general form:

\[ J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r \]

in which $\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]$. Note that $r$ is useless at optimization but required to evaluate the objective minima $J$. The Hessian $\mathbf{H̃}$ matrix of the quadratic general form is not constant here because of the time-varying $\mathbf{P̄}$ covariance . The computed variables are:

\[\begin{aligned} \mathbf{F} &= \mathbf{G U_0} + \mathbf{J D_0} + \mathbf{Y_0^m} + \mathbf{B} \\ \mathbf{f_x̄} &= \mathbf{x̂_0^†}(k-N_k+1) \\ \mathbf{F_Z̃} &= [\begin{smallmatrix}\mathbf{f_x̄} \\ \mathbf{F} \end{smallmatrix}] \\ \mathbf{Ẽ_Z̃} &= [\begin{smallmatrix}\mathbf{ẽ_x̄} \\ \mathbf{Ẽ} \end{smallmatrix}] \\ \mathbf{M}_{N_k} &= \mathrm{diag}(\mathbf{P̄}^{-1}, \mathbf{R̂}_{N_k}^{-1}) \\ \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C, \mathbf{T_ŵ}'\mathbf{Q̂}_{N_k}^{-1}\mathbf{T_ŵ}) \\ \mathbf{H̃} &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k}) \\ \mathbf{q̃} &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\ r &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} \end{aligned}\]

See init_ZtoŴ for the definition of the conversion matrix $\mathbf{T_ŵ}$.

Does nothing if model is not a LinModel.

linconstraint!(
    estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod
)

Set b vector for the linear model inequality constraints ($\mathbf{A Z̃ ≤ b}$) of MHE.

Also init $\mathbf{F_X̂ = G_X̂ U_0 + J_X̂ D_0 + B_X̂}$ vector for the state constraints, see init_predmat_mhe.

optim_objective!(estim::MovingHorizonEstimator) -> Z̃

Optimize objective of estim MovingHorizonEstimator and return the solution Z̃.

If first warm-starts the solver with set_warmstart_mhe!. It then calls JuMP.optimize!(estim.optim) and extract the solution. A failed optimization prints an @error log in the REPL and returns the warm-start value. A failed optimization also prints getinfo results in the debug log if activated.

set_warmstart_mhe!(
    estim::MovingHorizonEstimator, transcription::SingleShooting, Z̃var
) -> Z̃s

Set and return the warm-start value of Z̃var for MovingHorizonEstimator.

If supported by estim.optim and based a SingleShooting transcription, it warm-starts the solver at:

\[\mathbf{Z̃_s} = \begin{bmatrix} ε_{k-1} \\ \mathbf{x̂_0^†}(k-N_k+p) \\ \mathbf{ŵ}(k-N_k+p+0|k-1) \\ \mathbf{ŵ}(k-N_k+p+1|k-1) \\ \vdots \\ \mathbf{ŵ}(k+p-3|k-1) \\ \mathbf{ŵ}(k+p-2|k-1) \\ \mathbf{0} \\ \mathbf{0_ŵ} \end{bmatrix}\]

where $ε_{k-1}$ and $\mathbf{ŵ}(k-j|k-1)$ are respectively the slack variable and the process noise estimates computed at the last time step $k-1$. The vector $\mathbf{x̂_0^†}(k-N_k+p)$ is the deviation vector of the state at the arrival estimated at time $k-N_k$. If the objective function is not finite at this point, all the process noises $\mathbf{ŵ}_{k-1}(k-j)$ are warm-started at zeros. See the Extended Help of SingleShooting for the defintion of vector $\mathbf{0_ŵ}$. The method mutates all the arguments.

set_warmstart_mhe!(
    estim::MovingHorizonEstimator, transcription::MultipleShooting, Z̃var
) -> Z̃s

Do the same but based on a MultipleShooting transcription.

If supported by estim.optim, it warm-starts the solver at:

\[\mathbf{Z̃_s} = \begin{bmatrix} ε_{k-1} \\ \mathbf{x̂_0^†}(k-N_k+p) \\ \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ \vdots \\ \mathbf{x̂_0}(k+p-2|k-1) \\ \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{x̂_0}(k+p-1|k-1) \\ \mathbf{0_x̂} \\ \mathbf{ŵ}(k-N_k+p+0|k-1) \\ \mathbf{ŵ}(k-N_k+p+1|k-1) \\ \vdots \\ \mathbf{ŵ}(k+p-3|k-1) \\ \mathbf{ŵ}(k+p-2|k-1) \\ \mathbf{0} \\ \mathbf{0_ŵ} \end{bmatrix}\]

where $\mathbf{x̂_0}(k-j|k-1)$ is the predicted state for time $k-j$ computed at the last control period $k-1$, expressed as a deviation from the operating point $\mathbf{x̂_{op}}$. See the Extended Help of MultipleShooting for the defintion of vectors $\mathbf{0_x̂}$ and $\mathbf{0_ŵ}$.

predict_mhe!(
    V̂, X̂0, _ , _ , _ , 
    estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, 
    _ , _ , Z̃
) -> V̂, X̂0

Compute the V̂ vector and X̂0 vectors for the MovingHorizonEstimator and LinModel.

The function mutates V̂ and X̂0 vector arguments. The vector V̂ is the estimated sensor noises from $k-N_k+1$ to $k$. The X̂0 vector is estimated states from $k-N_k+2$ to $k+1$. The computations are (by truncating the matrices when N_k < H_e):

\[\begin{aligned} \mathbf{V̂} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ \mathbf{X̂_0} &= \mathbf{Ẽ_X̂ Z̃} + \mathbf{F_X̂} \end{aligned}\]

predict_mhe!(
    V̂, X̂0, Û0, K, Ŷ0, 
    estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, 
    x̂0arr, Ŵ, _ 
) -> V̂, X̂0

Compute the vectors when model is a NonLinModel with SingleShooting.

The function mutates V̂, X̂0, Û0, K and Ŷ0 vector arguments. The augmented model of f̂! and ĥ! is called recursively in a for loop from $j=1$ to $N_k$, and by adding the estimated process noise $\mathbf{ŵ}$.

predict_mhe!(
    V̂, X̂0, _ , _ , Ŷ0, 
    estim::MovingHorizonEstimator, model::NonLinModel, ::TranscriptionMethod, 
    x̂0arr , _ , Z̃ 
) -> V̂, X̂0

Compute the vectors when model is a NonLinModel and other TranscriptionMethod.

The function mutates V̂, X̂0, and Ŷ0 vector arguments. The augmented output function ĥ! is called multiple times in a for loop from $j=1$ to $N_k$.

con_nonlinprog_mhe!(
    g, estim::MovingHorizonEstimator, ::NonLinModel, ::TranscriptionMethod, _ , V̂, gc, ε
) -> g

Nonlinear MHE constraint when model is NonLinModel with non-SingleShooting.

The method mutates the g vectors in argument and returns it. The estimated sensor noises and custom constraints are included in the g vector.

con_nonlinprog_mhe!(
    g, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, X̂0, V̂, gc, ε
) -> g

Nonlinear MHE constraint when model is NonLinModel with SingleShooting.

The method mutates the g vectors in argument and returns it. The estimated states, estimated sensor noises and custom constraints are included in the g vector.

con_nonlinprog_mhe!(
    g, ::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod, _ , _ , gc, _ 
) -> g

Compute the same but for LinModel.

The nonlinear custom inequality constraints in gc are the only nonlinear constraints for this case.

con_nonlinprogeq_mhe!(
    geq, X̂0, Û0, K,
    estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, 
    x̂0arr, Ŵ, Z̃
) -> geq

Nonlinear MHE equality constrains for NonLinModel and MultipleShooting.

The method mutates the geq, X̂0, Û0 and K vectors in argument. The defects of the estimated states are computed with:

\[\mathbf{ŝ}(k+j+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k+j), \mathbf{u_0}(k+j), \mathbf{d_0}(k+j)\Big) - \mathbf{x̂_0}(k+j+1)\]

for $j = 0, 1, ... , H_p-1$ and in which the augmented state vectors $\mathbf{x̂_0}$ are extracted from the decision variable Z̃. The function $\mathbf{f̂}$ is defined at f̂!.

No nonlinear eq. const. for other cases e.g. SingleShooting, returns geq unchanged.

getstate!(estim::MovingHorizonEstimator, Z̃)

Get current or next state estimate from the solution Z̃ and store it at estim.x̂0.

It extracts and stores at estim.x̂0 the current corrected state if estim.direct == true, otherwise the state is for the next time step.

remove_op!(estim::StateEstimator, ym, d, u=nothing) -> y0m, d0, u0

Remove operating pts on measured outputs ym, disturbances d and inputs u (if provided).

init_estimate!(estim::StateEstimator, model::LinModel, y0m, d0, u0)

Init estim.x̂0 estimate with the steady-state solution if model is a LinModel.

Using u0, y0m and d0 arguments (deviation values, see setop!), the steadystate problem combined to the equality constraint $\mathbf{ŷ_0^m} = \mathbf{y_0^m}$ engenders the following system to solve:

\[\begin{bmatrix} \mathbf{I} - \mathbf{Â} \\ \mathbf{Ĉ^m} \end{bmatrix} \mathbf{x̂_0} = \begin{bmatrix} \mathbf{B̂_u u_0 + B̂_d d_0 + f̂_{op} - x̂_{op}} \\ \mathbf{y_0^m - D̂_d^m d_0} \end{bmatrix}\]

in which $\mathbf{Ĉ^m, D̂_d^m}$ are the rows of estim.Ĉ, estim.D̂d that correspond to measured outputs $\mathbf{y^m}$.

init_estimate!(estim::StateEstimator, model::SimModel, _ , _ , _ )

Left estim.x̂0 estimate unchanged if model is not a LinModel.

init_estimate!(estim::InternalModel, model::LinModel, y0m, d0, u0)

Init estim.x̂0/x̂d/x̂s estimate at steady-state for InternalModel.

The deterministic estimates estim.x̂d start at steady-state using u0 and d0 arguments:

\[ \mathbf{x̂_d} = \mathbf{(I - A)^{-1} (B_u u_0 + B_d d_0 + f_{op} - x_{op})}\]

Based on y0m argument and current stochastic outputs estimation $\mathbf{ŷ_s}$, composed of the measured $\mathbf{ŷ_s^m} = \mathbf{y_0^m} - \mathbf{ŷ_{d0}^m}$ and unmeasured $\mathbf{ŷ_s^u = 0}$ outputs, the stochastic estimates also start at steady-state:

\[ \mathbf{x̂_s} = \mathbf{(I - Â_s)^{-1} B̂_s ŷ_s}\]

This estimator does not augment the state vector, thus $\mathbf{x̂ = x̂_d}$. See init_internalmodel for details.

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) for the equations.

correct_estimate!(estim::KalmanFilter, y0m, d0)

Correct estim.x̂0 and estim.cov.P̂ using the time-varying KalmanFilter.

It computes the corrected state estimate $\mathbf{x̂}_{k}(k)$ estimation covariance $\mathbf{P̂}_{k}(k)$.

correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0)

Do the same but for the UnscentedKalmanFilter.

correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0)

Do the same but for the ExtendedKalmanFilter.

correct_estimate!(estim::Luenberger, y0m, d0, _ )

Identical to correct_estimate!(::SteadyKalmanFilter) but using Luenberger.

correct_estimate!(estim::MovingHorizonEstimator, y0m, d0)

Do the same but for MovingHorizonEstimator objects.

correct_estimate!(estim::InternalModel, y0m, d0)

Compute the current stochastic output estimation ŷs for InternalModel.

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.

update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0)

Update estim.x̂0 estimate with current inputs u0, measured outputs y0m and dist. d0.

If estim.direct == false, the SteadyKalmanFilter 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! function (that calls correct_estimate!). The correction and prediction step equations are provided below.

Correction Step

\[\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

\[\mathbf{x̂}_{k}(k+1) = \mathbf{Â x̂}_{k}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) \]

update_estimate!(estim::KalmanFilter, u0, y0m, d0)

Update KalmanFilter 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. 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.

Correction Step

\[\begin{aligned} \mathbf{M̂}(k) &= \mathbf{Ĉ^m P̂}_{k-1}(k)\mathbf{Ĉ^m}' + \mathbf{R̂} \\ \mathbf{K̂}(k) &= \mathbf{P̂}_{k-1}(k)\mathbf{Ĉ^m}'\mathbf{M̂^{-1}}(k) \\ \mathbf{ŷ^m}(k) &= \mathbf{Ĉ^m x̂}_{k-1}(k) + \mathbf{D̂_d^m d}(k) \\ \mathbf{x̂}_{k}(k) &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k)[\mathbf{y^m}(k) - \mathbf{ŷ^m}(k)] \\ \mathbf{P̂}_{k}(k) &= [\mathbf{I - K̂}(k)\mathbf{Ĉ^m}]\mathbf{P̂}_{k-1}(k) \end{aligned}\]

Prediction Step

\[\begin{aligned} \mathbf{x̂}_{k}(k+1) &= \mathbf{Â x̂}_{k}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) \\ \mathbf{P̂}_{k}(k+1) &= \mathbf{Â P̂}_{k}(k)\mathbf{Â}' + \mathbf{Q̂} \end{aligned}\]

update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0)

Update UnscentedKalmanFilter 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 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 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

\[\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), \mathbf{d}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k), \mathbf{d}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k), \mathbf{d}(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

\[\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}\]

update_estimate!(estim::ExtendedKalmanFilter, u0, y0m, d0)

Update ExtendedKalmanFilter state estim.x̂0 and covariance estim.cov.P̂.

The equations are similar to update_estimate!(::KalmanFilter) but with the substitutions $\mathbf{Ĉ^m = Ĥ^m}(k)$ and $\mathbf{Â = F̂}(k)$, the Jacobians of the augmented process model:

\[\begin{aligned} \mathbf{Ĥ}(k) &= \left. \frac{∂\mathbf{ĥ}(\mathbf{x̂}, \mathbf{d})}{∂\mathbf{x̂}} \right|_{\mathbf{x̂ = x̂}_{k-1}(k),\, \mathbf{d = d}(k)} \\ \mathbf{F̂}(k) &= \left. \frac{∂\mathbf{f̂}(\mathbf{x̂}, \mathbf{u}, \mathbf{d})}{∂\mathbf{x̂}} \right|_{\mathbf{x̂ = x̂}_{k}(k), \, \mathbf{u = u}(k),\, \mathbf{d = d}(k)} \end{aligned}\]

The matrix $\mathbf{Ĥ^m}$ is the rows of $\mathbf{Ĥ}$ that are measured outputs. The Jacobians are computed with 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.

Correction Step

\[\begin{aligned} \mathbf{Ŝ}(k) &= \mathbf{Ĥ^m}(k)\mathbf{P̂}_{k-1}(k)\mathbf{Ĥ^m}'(k) + \mathbf{R̂} \\ \mathbf{K̂}(k) &= \mathbf{P̂}_{k-1}(k)\mathbf{Ĥ^m}'(k)\mathbf{Ŝ^{-1}}(k) \\ \mathbf{ŷ^m}(k) &= \mathbf{ĥ^m}\Big( \mathbf{x̂}_{k-1}(k), \mathbf{d}(k) \Big) \\ \mathbf{x̂}_{k}(k) &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k)[\mathbf{y^m}(k) - \mathbf{ŷ^m}(k)] \\ \mathbf{P̂}_{k}(k) &= [\mathbf{I - K̂}(k)\mathbf{Ĥ^m}(k)]\mathbf{P̂}_{k-1}(k) \end{aligned}\]

Prediction Step

\[\begin{aligned} \mathbf{x̂}_{k}(k+1) &= \mathbf{f̂}\Big( \mathbf{x̂}_{k}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \\ \mathbf{P̂}_{k}(k+1) &= \mathbf{F̂}(k)\mathbf{P̂}_{k}(k)\mathbf{F̂}'(k) + \mathbf{Q̂} \end{aligned}\]

update_estimate!(estim::Luenberger, u0, y0m, d0)

Same than update_estimate!(::SteadyKalmanFilter) but using Luenberger.

update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0)

Update MovingHorizonEstimator state estim.x̂0.

The optimization problem of MovingHorizonEstimator documentation is solved if estim.direct is false (otherwise solved in correct_estimate!). The prediction matrices are provided at init_predmat_mhe documentation. Once solved, the optimal estimate $\mathbf{x̂}_k(k+p)$ is computed by inserting the optimal values of $\mathbf{x̂}_k(k-N_k+p)$ and $\mathbf{Ŵ}$ in the augmented model from $j = N_k-1$ to $0$ inclusively. Afterward, if $N_k = H_e$, the arrival covariance for the next time step $\mathbf{P̂}_{k-N_k}(k-N_k+1)$ is estimated using estim.covestim object. It also stores u0 at estim.lastu0, so it can be added to the data window at the next time step in correct_estimate!.

update_estimate!(estim::InternalModel,  u0, _ , d0)

Update estim.x̂0/x̂d/x̂s with current inputs u0, measured outputs y0m and dist. d0.

The InternalModel updates the deterministic x̂d and stochastic x̂s estimates with:

\[\begin{aligned} \mathbf{x̂_d}(k+1) &= \mathbf{f}\Big( \mathbf{x̂_d}(k), \mathbf{u}(k), \mathbf{d}(k), \mathbf{p} \Big) \\ \mathbf{x̂_s}(k+1) &= \mathbf{Â_s x̂_s}(k) + \mathbf{B̂_s ŷ_s}(k) \end{aligned}\]

This estimator does not augment the state vector, thus $\mathbf{x̂ = x̂_d}$. See init_internalmodel for details.

update_estimate!(estim::ManualEstimator, u0, y0m, d0)

Do nothing for ManualEstimator.