Functions: StateEstimator Internals

Functions: StateEstimator Internals

Augmented Model

ModelPredictiveControl.f̂! — Function

f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing

Mutating state 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, defined as:

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

where $\mathbf{x̂_0}(k+1)$ is stored in x̂next0 argument. The method mutates x̂next0 and û0 in place, the latter stores the input vector of the augmented model $\mathbf{u_0 + ŷ_{s_u}}$.

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

Use the augmented model matrices if model is a LinModel.

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

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

It calls model.f!(x̂next0, x̂0, u0 ,d0) since this estimator does not augment the states.

ModelPredictiveControl.ĥ! — Function

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

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

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

Use the augmented model matrices if model is a LinModel.

ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0)

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

Estimator Construction

ModelPredictiveControl.init_estimstoch — Function

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_{ym}}}(k) &= \mathbf{C_{s_{ym}} 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_{ym}}})$. The estimations does not use $\mathbf{B_s}$, it is thus ignored. The function init_integrators builds the state-space matrices.

ModelPredictiveControl.init_integrators — Function

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 :

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

ModelPredictiveControl.augment_model — Function

augment_model(model::LinModel, As, Cs; verify_obsv=true) -> Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op

Augment LinModel state-space matrices with the stochastic ones As and Cs.

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

\[\begin{aligned} \mathbf{x̂_0}(k+1) &= \mathbf{Â x̂_0}(k) + \mathbf{B̂_u u_0}(k) + \mathbf{B̂_d d_0}(k) \\ \mathbf{ŷ_0}(k) &= \mathbf{Ĉ 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 x̂op and f̂op are simply $\mathbf{x_{op}}$ and $\mathbf{f_{op}}$ vectors appended with zeros (see setop!).

augment_model(model::SimModel, As, _ , _ ) -> Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op

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

ModelPredictiveControl.init_ukf — Function

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

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{Ŝ} &= \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.

ModelPredictiveControl.init_internalmodel — Function

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

Calc stochastic model update matrices Â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{ê=0}$, the expected value. Thus, the Â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}) ŷ_s}(k) \\ &= \mathbf{Â_s x̂_s}(k) + \mathbf{B̂_s ŷ_s}(k) \end{aligned}\]

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

ModelPredictiveControl.init_predmat_mhe — Function

init_predmat_mhe(
    model::LinModel, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d
) -> E, F, G, J, ex̄, fx̄, Ex̂, Fx̂, Gx̂, Jx̂

Construct the MHE prediction matrices for LinModel model.

Introducing the vector $\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂}_k(k-N_k+1) \\ \mathbf{Ŵ} \end{smallmatrix}]$ with the decision variables, the estimated sensor noises from time $k-N_k+1$ to $k$ are computed by:

\[\begin{aligned} \mathbf{V̂} = \mathbf{Y^m - Ŷ^m} &= \mathbf{E Z + G U + J D + Y^m} \\ &= \mathbf{E Z + F} \end{aligned}\]

in which $\mathbf{U, D}$ and $\mathbf{Y^m}$ contains respectively the manipulated inputs, measured disturbances and measured outputs from time $k-N_k+1$ to $k$. The method also returns similar matrices but for the estimation error at arrival:

\[\mathbf{x̄} = \mathbf{x̂}_{k-N_k}(k-N_k+1) - \mathbf{x̂}_{k}(k-N_k+1) = \mathbf{e_x̄ Z + f_x̄}\]

Lastly, the estimated states from time $k-N_k+2$ to $k+1$ are given by the equation:

\[\begin{aligned} \mathbf{X̂} &= \mathbf{E_x̂ Z + G_x̂ U + J_x̂ D} \\ &= \mathbf{E_x̂ Z + F_x̂} \end{aligned}\]

All these equations omit the operating points $\mathbf{u_{op}, y_{op}, d_{op}}$.

Extended Help

Extended Help

Using the augmented matrices $\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}$, the prediction matrices for the sensor noises are computed by (notice the minus signs after the equalities):

\[\begin{aligned} \mathbf{E} &= - \begin{bmatrix} \mathbf{Ĉ^m}\mathbf{Â}^{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Ĉ^m}\mathbf{Â}^{1} & \mathbf{Ĉ^m}\mathbf{Â}^{0} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{G} &= - \begin{bmatrix} \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_u} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{J} &= - \begin{bmatrix} \mathbf{D̂^m} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂^m} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂^m} \end{bmatrix} \end{aligned}\]

for the estimation error at arrival:

\[\mathbf{e_x̄} = \begin{bmatrix} -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix}\]

and, for the estimated states:

\[\begin{aligned} \mathbf{E_x̂} &= \begin{bmatrix} \mathbf{Â}^{1} & \mathbf{I} & \cdots & \mathbf{0} \\ \mathbf{Â}^{2} & \mathbf{Â}^{1} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Â}^{H_e} & \mathbf{Â}^{H_e-1} & \cdots & \mathbf{Â}^{1} \end{bmatrix} \\ \mathbf{G_x̂} &= \begin{bmatrix} \mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Â}^{1}\mathbf{B̂_u} & \mathbf{Â}^{0}\mathbf{B̂_u} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Â}^{H_e-1}\mathbf{B̂_u} & \mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_u} \end{bmatrix} \\ \mathbf{J_x̂} &= \begin{bmatrix} \mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Â}^{1}\mathbf{B̂_d} & \mathbf{Â}^{0}\mathbf{B̂_d} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Â}^{H_e-1}\mathbf{B̂_d} & \mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d} \end{bmatrix} \end{aligned}\]

All these matrices are truncated when $N_k < H_e$ (at the beginning).

Return empty matrices if model is not a LinModel, except for ex̄.

ModelPredictiveControl.relaxarrival — Function

relaxarrival(
    model::SimModel, C, c_x̂min, c_x̂max, x̂min, x̂max, ex̄
) -> A_x̃min, A_x̃max, x̃min, x̃max, ẽ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{ẽ_x̄}$ matrix that appears in the estimation error at arrival equation $\mathbf{x̄} = \mathbf{ẽ_x̄ Z̃ + f_x̄}$. It also returns the augmented constraints $\mathbf{x̃_{min}}$ and $\mathbf{x̃_{max}}$, and 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} + f_x̄} \\ + \mathbf{x̃_{max} - f_x̄} \end{bmatrix}\]

ModelPredictiveControl.relaxX̂ — Function

relaxX̂(model::SimModel, C, C_x̂min, C_x̂max, Ex̂) -> A_X̂min, A_X̂max, Ẽ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{Ẽ_x̂}$ matrix that appears in estimated states equation $\mathbf{X̂} = \mathbf{Ẽ_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} + F_x̂} \\ + \mathbf{X̂_{max} - F_x̂} \end{bmatrix}\]

Return empty matrices if model is not a LinModel

ModelPredictiveControl.relaxŴ — Function

relaxŴ(model::SimModel, C, C_ŵmin, C_ŵmax, nx̂) -> A_Ŵmin, A_Ŵ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_{Ŵ_{min}}} \\ \mathbf{A_{Ŵ_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{Ŵ_{min}} \\ + \mathbf{Ŵ_{max}} \end{bmatrix}\]

ModelPredictiveControl.relaxV̂ — Function

relaxV̂(model::SimModel, C, C_v̂min, C_v̂max, E) -> A_V̂min, A_V̂max, Ẽ

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{Ẽ}$ matrix that appears in estimated sensor noise equation $\mathbf{V̂} = \mathbf{Ẽ 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}\]

Return empty matrices if model is not a LinModel

ModelPredictiveControl.init_matconstraint_mhe — Function

init_matconstraint_mhe(model::LinModel, 
    i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args...
) -> i_b, i_g, A

Init i_b, i_g and A matrices for the MHE linear inequality constraints.

The linear and nonlinear inequality constraints are respectively defined as:

\[\begin{aligned} \mathbf{A Z̃ } &≤ \mathbf{b} \\ \mathbf{g(Z̃)} &≤ \mathbf{0} \end{aligned}\]

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}$ (empty if model is a LinModel). The method also returns the $\mathbf{A}$ matrix if args is provided. In such a case, args needs to contain all the inequality constraint matrices: A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max.

Init i_b, A without state and sensor noise constraints if model is not a LinModel.

Update Quadratic Optimization

ModelPredictiveControl.initpred! — Method

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

Init quadratic optimization matrices F, fx̄, H̃, q̃, p 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̃} + p \]

in which $\mathbf{Z̃} = [\begin{smallmatrix} ϵ \\ \mathbf{Z} \end{smallmatrix}]$. Note that $p$ 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} + \mathbf{J D} + \mathbf{Y^m} \\ \mathbf{f_x̄} &= \mathbf{x̂}_{k-N_k}(k-N_k+1) \\ \mathbf{F_Z̃} &= [\begin{smallmatrix}\mathbf{f_x̄} \\ \mathbf{F} \end{smallmatrix}] \\ \mathbf{Ẽ_Z̃} &= [\begin{smallmatrix}\mathbf{ẽ_x̄} \\ \mathbf{Ẽ} \end{smallmatrix}] \\ \mathbf{M}_{N_k} &= \mathrm{diag}(\mathbf{P̄}^{-1}, \mathbf{R̂}_{N_k}^{-1}) \\ \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C, \mathbf{0}, \mathbf{Q̂}_{N_k}^{-1}) \\ \mathbf{H̃} &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k}) \\ \mathbf{q̃} &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\ p &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} \end{aligned}\]

ModelPredictiveControl.linconstraint! — Method

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

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

Also init $\mathbf{F_x̂ = G_x̂ U + J_x̂ D}$ vector for the state constraints, see init_predmat_mhe.

Evaluate Estimated Output

ModelPredictiveControl.evalŷ — Function

evalŷ(estim::InternalModel, ym, d) -> ŷ

Get InternalModel output ŷ from current measured outputs ym and dist. d.

InternalModel estimator needs current measured outputs $\mathbf{y^m}(k)$ to estimate its outputs $\mathbf{ŷ}(k)$, since the strategy imposes that $\mathbf{ŷ^m}(k) = \mathbf{y^m}(k)$ is always true.

evalŷ(estim::StateEstimator, _ , d) -> ŷ

Evaluate StateEstimator output ŷ from measured disturbance d and estim.x̂0.

Second argument is ignored, except for InternalModel.

Remove Operating Points

ModelPredictiveControl.remove_op! — Function

remove_op!(estim::StateEstimator, u, ym, d) -> u0, ym0, d0

Remove operating points on inputs u, measured outputs ym and disturbances d.

Also store current inputs without operating points u0 in estim.lastu0. This field is used for PredictiveController computations.

Init Estimate

ModelPredictiveControl.init_estimate! — Function

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

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

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

\[\begin{bmatrix} \mathbf{I} - \mathbf{Â} \\ \mathbf{Ĉ^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{Ĉ^m, D̂_d^m}$ are the rows of estim.Ĉ, 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, u0, ym0, d0)

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 ym0 argument and current stochastic outputs estimation $\mathbf{ŷ_s}$, composed of the measured $\mathbf{ŷ_s^m} = \mathbf{y_0^m} - \mathbf{ŷ_{d0}^m}$ and unmeasured $\mathbf{ŷ_s^u = 0}$ outputs, the stochastic estimates also start at steady-state:

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

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

Update Estimate

Info

All these methods assume that the operating points are already removed in u, ym and d arguments. Strickly speaking, the arguments should be called u0, ym0 and d0, following setop! notation. The 0 is dropped to simplify the notation.

ModelPredictiveControl.update_estimate! — Function

update_estimate!(estim::SteadyKalmanFilter, u, ym, d=[])

Update estim.x̂0 estimate with current inputs u, measured outputs ym and dist. d.

The SteadyKalmanFilter updates it with the precomputed Kalman gain $\mathbf{K̂}$:

\[\mathbf{x̂}_{k}(k+1) = \mathbf{Â x̂}_{k-1}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) + \mathbf{K̂}[\mathbf{y^m}(k) - \mathbf{Ĉ^m x̂}_{k-1}(k) - \mathbf{D̂_d^m d}(k)]\]

update_estimate!(estim::KalmanFilter, u, ym, d=[])

Update KalmanFilter state estim.x̂0 and estimation error covariance estim.P̂.

It implements the time-varying Kalman Filter in its predictor (observer) form :

\[\begin{aligned} \mathbf{M̂}(k) &= \mathbf{P̂}_{k-1}(k)\mathbf{Ĉ^m}' [\mathbf{Ĉ^m P̂}_{k-1}(k)\mathbf{Ĉ^m}' + \mathbf{R̂}]^{-1} \\ \mathbf{K̂}(k) &= \mathbf{Â M̂}(k) \\ \mathbf{ŷ^m}(k) &= \mathbf{Ĉ^m x̂}_{k-1}(k) + \mathbf{D̂_d^m d}(k) \\ \mathbf{x̂}_{k}(k+1) &= \mathbf{Â x̂}_{k-1}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) + \mathbf{K̂}(k)[\mathbf{y^m}(k) - \mathbf{ŷ^m}(k)] \\ \mathbf{P̂}_{k}(k+1) &= \mathbf{Â}[\mathbf{P̂}_{k-1}(k) - \mathbf{M̂}(k)\mathbf{Ĉ^m P̂}_{k-1}(k)]\mathbf{Â}' + \mathbf{Q̂} \end{aligned}\]

based on the process model described in SteadyKalmanFilter. The notation $\mathbf{x̂}_{k-1}(k)$ refers to the state for the current time $k$ estimated at the last control period $k-1$. See ^[2] for details.

update_estimate!(estim::UnscentedKalmanFilter, u, ym, d=[])

Update UnscentedKalmanFilter state estim.x̂0 and covariance estimate estim.P̂.

It implements the unscented Kalman Filter in its predictor (observer) form, based on the generalized unscented transform^[3]. See init_ukf for the definition of the constants $\mathbf{m̂, Ŝ}$ and $γ$.

Denoting $\mathbf{x̂}_{k-1}(k)$ as the state for the current time $k$ estimated at the last period $k-1$, $\mathbf{0}$, a null vector, $n_σ = 2 n_\mathbf{x̂} + 1$, the number of sigma points, and $\mathbf{X̂}_{k-1}^j(k)$, the vector at the $j$th column of $\mathbf{X̂}_{k-1}(k)$, the estimator updates the states with:

\[\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{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^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{Ȳ^m}(k) &= \begin{bmatrix} \mathbf{Ŷ^m}^{1}(k) - \mathbf{ŷ^m}(k) & \mathbf{Ŷ^m}^{2}(k) - \mathbf{ŷ^m}(k) & \cdots & \mathbf{Ŷ^m}^{n_σ}(k) - \mathbf{ŷ^m}(k) \end{bmatrix} \\ \mathbf{M̂}(k) &= \mathbf{Ȳ^m}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) + \mathbf{R̂} \\ \mathbf{K̂}(k) &= \mathbf{X̄}_{k-1}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) \mathbf{M̂^{-1}}(k) \\ \mathbf{x̂}_k(k) &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k) \big[ \mathbf{y^m}(k) - \mathbf{ŷ^m}(k) \big] \\ \mathbf{P̂}_k(k) &= \mathbf{P̂}_{k-1}(k) - \mathbf{K̂}(k) \mathbf{M̂}(k) \mathbf{K̂}'(k) \\ \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{Ŝ} \mathbf{X̄}_k'(k+1) + \mathbf{Q̂} \end{aligned} \]

by using the lower triangular factor of cholesky to compute $\sqrt{\mathbf{P̂}_{k-1}(k)}$ and $\sqrt{\mathbf{P̂}_{k}(k)}$. The matrices $\mathbf{P̂, Q̂, R̂}$ are the covariance of the estimation error, process noise and sensor noise, respectively.

update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=[])

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

The equations are similar to update_estimate!(::KalmanFilter) but with the substitutions $\mathbf{Â = F̂}(k)$ and $\mathbf{Ĉ^m = Ĥ^m}(k)$:

\[\begin{aligned} \mathbf{M̂}(k) &= \mathbf{P̂}_{k-1}(k)\mathbf{Ĥ^m}'(k) [\mathbf{Ĥ^m}(k)\mathbf{P̂}_{k-1}(k)\mathbf{Ĥ^m}'(k) + \mathbf{R̂}]^{-1} \\ \mathbf{K̂}(k) &= \mathbf{F̂}(k) \mathbf{M̂}(k) \\ \mathbf{ŷ^m}(k) &= \mathbf{ĥ^m}\Big( \mathbf{x̂}_{k-1}(k), \mathbf{d}(k) \Big) \\ \mathbf{x̂}_{k}(k+1) &= \mathbf{f̂}\Big( \mathbf{x̂}_{k-1}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) + \mathbf{K̂}(k)[\mathbf{y^m}(k) - \mathbf{ŷ^m}(k)] \\ \mathbf{P̂}_{k}(k+1) &= \mathbf{F̂}(k)[\mathbf{P̂}_{k-1}(k) - \mathbf{M̂}(k)\mathbf{Ĥ^m}(k)\mathbf{P̂}_{k-1}(k)]\mathbf{F̂}'(k) + \mathbf{Q̂} \end{aligned}\]

ForwardDiff.jacobian automatically computes the Jacobians:

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

The matrix $\mathbf{Ĥ^m}$ is the rows of $\mathbf{Ĥ}$ that are measured outputs.

update_estimate!(estim::Luenberger, u, ym, d=[])

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

update_estimate!(estim::MovingHorizonEstimator, u, ym, d=[])

Update MovingHorizonEstimator state estim.x̂.

The optimization problem of MovingHorizonEstimator documentation is solved at each discrete time $k$. Once solved, the next estimate $\mathbf{x̂}_k(k+1)$ is computed by inserting the optimal values of $\mathbf{x̂}_k(k-N_k+1)$ and $\mathbf{Ŵ}$ in the augmented model from $j = N_k-1$ to $0$ inclusively. Afterward, if $k ≥ H_e$, the arrival covariance for the next time step $\mathbf{P̂}_{k-N_k+1}(k-N_k+2)$ is estimated with the equations of update_estimate!(::ExtendedKalmanFilter), or KalmanFilter, for LinModel.

update_estimate!(estim::InternalModel, u, ym, d=[])

Update estim.x̂0/x̂d/x̂s with current inputs u, measured outputs ym and dist. d.

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) \Big) \\ \mathbf{x̂_s}(k+1) &= \mathbf{Â_s x̂_s}(k) + \mathbf{B̂_s ŷ_s}(k) \end{aligned}\]

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

1Desbiens, A., D. Hodouin & É. Plamondon. 2000, "Global predictive control : a unified control structure for decoupling setpoint tracking, feedforward compensation and disturbance rejection dynamics", IEE Proceedings - Control Theory and Applications, vol. 147, no 4, https://doi.org/10.1049/ip-cta:20000443, p. 465–475, ISSN 1350-2379.
2Boyd S., "Lecture 8 : The Kalman Filter" (Winter 2008-09) [course slides], EE363: Linear Dynamical Systems, https://web.stanford.edu/class/ee363/lectures/kf.pdf.
3Simon, D. 2006, "Chapter 14: The unscented Kalman filter" in "Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches", John Wiley & Sons, p. 433–459, https://doi.org/10.1002/0470045345.ch14, ISBN9780470045343.