Functions: StateEstimator Internals

Functions: StateEstimator Internals

Augmented Model

# ModelPredictiveControl.f̂ — Function

f̂(estim::StateEstimator, model::SimModel, x̂, u, d)

State function of the augmented model.

By introducing an augmented state vector like in augment_model, the function returns the next state of the augmented model, defined as:

Use the augmented model matrices if model is a LinModel.

f̂(::InternalModel, model::NonLinModel, x̂, u, d)

State function of InternalModel for NonLinModel.

It calls model.f(x̂, u ,d) since this estimator does not augment the states.

# ModelPredictiveControl.ĥ — Function

ĥ(estim::StateEstimator, model::SimModel, x̂, d)

Output function of the augmented model, see f̂ for details.

Use the augmented model matrices if model is a LinModel.

ĥ(::InternalModel, model::NonLinModel, x̂, d)

Output function of InternalModel, it calls model.h.

Constraint Relaxation

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:

where is an unknown zero mean white noise and . The estimations does not use , 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
for the unmeasured disturbances at measured outputs

# ModelPredictiveControl.augment_model — Function

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

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

If are model.x states, and , the states defined at init_estimstoch, we define an augmented state vector ]. The method returns the augmented matrices Â, B̂u, Ĉ, B̂d and D̂d:

An error is thrown if the augmented model is not observable and verify_obsv == true.

Return empty matrices if model is not a LinModel.

# ModelPredictiveControl.init_ukf — Function

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

Compute the UnscentedKalmanFilter constants from and .

With elements in the state vector and sigma points, the scaling factor applied on standard deviation matrices is:

The weight vector for the mean and the weight matrix for the covariance are respectively:

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 :

where is conceptual and unknown zero mean white noise. Its optimal estimation is , the expected value. Thus, the Âs and B̂s matrices that optimally update the stochastic estimate are:

with current stochastic outputs estimation , composed of the measured and unmeasured 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 ] with the decision variables, the estimated sensor noises from time to are computed by:

in which and contains respectively the manipulated inputs, measured disturbances and measured outputs from time to . The method also returns similar matrices but for the estimation error at arrival:

Lastly, the estimated states from time to are given by the equation:

All these equations omit the operating points .

Extended Help

Using the augmented matrices , the prediction matrices for the sensor noises are computed by (notice the minus signs after the equalities):

for the estimation error at arrival:

and, for the estimated states:

All these matrices are truncated when (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 ], it returns the matrix that appears in the estimation error at arrival equation . It also returns the augmented constraints and , and the matrices for the inequality constraints:

# 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 ], it returns the matrix that appears in estimated states equation . It also returns the matrices for the inequality constraints:

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 ], it returns the matrices for the inequality constraints:

# 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 ], it returns the matrix that appears in estimated sensor noise equation . It also returns the matrices for the inequality constraints:

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:

i_b is a BitVector including the indices of that are finite numbers. i_g is a similar vector but for the indices of (empty if model is a LinModel). The method also returns the 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 . It also inits estim.optim objective function, expressed as the quadratic general form:

in which ]. Note that is useless at optimization but required to evaluate the objective minima . The Hessian matrix of the quadratic general form is not constant here because of the time-varying covariance . The computed variables are:

# ModelPredictiveControl.linconstraint! — Method

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

Set b vector for the linear model inequality constraints ( ) of MHE.

Also init vector for the state constraints, see init_predmat_mhe.

Evaluate Estimated Output

# ModelPredictiveControl.evalŷ — Function

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

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

Second argument is ignored, except for InternalModel.

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

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

InternalModel estimator needs current measured outputs to estimate its outputs , since the strategy imposes that is always true.

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.

Update Estimate

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̂ estimate with current inputs u, measured outputs ym and dist. d.

The SteadyKalmanFilter updates it with the precomputed Kalman gain :

update_estimate!(estim::KalmanFilter, u, ym, d)

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

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

based on the process model described in SteadyKalmanFilter. The notation refers to the state for the current time estimated at the last control period . See ^[2], EE363: Linear Dynamical Systems, https://web.stanford.edu/class/ee363/lectures/kf.pdf.] for details.

update_estimate!(estim::UnscentedKalmanFilter, u, ym, d)

Update UnscentedKalmanFilter state estim.x̂ 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 and .

Denoting as the state for the current time estimated at the last period , , a null vector, , the number of sigma points, and , the vector at the th column of , the estimator updates the states with:

by using the lower triangular factor of cholesky to compute and . The matrices are the covariance of the estimation error, process noise and sensor noise, respectively.

update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=empty(estim.x̂))

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

The equations are similar to update_estimate!(::KalmanFilter) but with the substitutions and :

ForwardDiff.jacobian automatically computes the Jacobians:

The matrix is the rows of that are measured outputs.

update_estimate!(estim::Luenberger, u, ym, d=empty(estim.x̂))

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 . Once solved, the next estimate is computed by inserting the optimal values of and in the augmented model from to inclusively. Afterward, if , the arrival covariance for the next time step is estimated with the equations of update_estimate!(::ExtendedKalmanFilter), or KalmanFilter, for LinModel.

update_estimate!(estim::InternalModel, u, ym, d=empty(estim.x̂)) -> x̂d

Update estim.x̂ / 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:

This estimator does not augment the state vector, thus . See init_internalmodel for details.

Init Estimate

Same as above: the arguments should be called u0, ym0 and d0, strickly speaking.

# ModelPredictiveControl.init_estimate! — Function

init_estimate!(estim::StateEstimator, model::LinModel, u, ym, d)

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

Using u, ym and d arguments, the steady-state problem combined to the equality constraint engenders the following system to solve:

in which are the rows of estim.Ĉ, estim.D̂d that correspond to measured outputs .

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

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

init_estimate!(estim::InternalModel, model::LinModel, u, ym, d)

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

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

Based on ym argument and current stochastic outputs estimation , composed of the measured and unmeasured outputs, the stochastic estimates also start at steady-state:

This estimator does not augment the state vector, thus . See init_internalmodel for details.

1. Desbiens, 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.

2. Boyd S., "Lecture 8 : The Kalman Filter" (Winter 2008-09) [course slides

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, https://doi.org/10.1002/0470045345.ch14, ISBN9780470045343.