Functions: Miscellaneous Internals

init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo

Init the differentiation and continuity matrices for OrthogonalCollocation.

Introducing $τ_i$, the $i$th root of the orthogonal polynomial normalized to the interval $[0, 1]$ with $τ_0=0$, the trajectories for each state are approximated by a distinct polynomial of degree $n_o$. The differentiation matrix $\mathbf{M_o}$, the continuity matrix $\mathbf{C_o}$ and the continuity coefficient $λ_o$ are pre-computed with the identity matrix $\mathbf{I}$ of size (model.nx, model.nx) and:

\[\begin{aligned} \mathbf{P_o} &= \begin{bmatrix} τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ \vdots & \vdots & \ddots & \vdots \\ τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ \mathbf{Ṗ_o} &= \begin{bmatrix} τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ \vdots & \vdots & \ddots & \vdots \\ τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ \mathbf{C_o} &= \begin{bmatrix} L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \\ λ_o &= L_0(1) \end{aligned}\]

where $T_s$ is the sampling time model.Ts, $\mathbf{P_o}$ is a matrix to evaluate the polynomial values w/o the coefficients and Y-intercept, and $\mathbf{Ṗ_o}$, to evaluate its derivatives. The Lagrange polynomial $L_j(τ)$ bases are defined as:

\[L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i}\]

The $\mathbf{M_o}$ matrix is used in the nonlinear collocation constraints. The defects between the deterministic state derivative for the $n_o$ collocation points and the model dynamics at the discrete time $k$ are given by:

\[\begin{aligned} \mathbf{ŝ_k}(k) &= \mathbf{M_o} \begin{bmatrix} \mathbf{k}_1(k) - \mathbf{x̂_d}(k) \\ \mathbf{k}_2(k) - \mathbf{x̂_d}(k) \\ \vdots \\ \mathbf{k}_{n_o}(k) - \mathbf{x̂_d}(k) \end{bmatrix} - \begin{bmatrix} \mathbf{k̇}_1(k) \\ \mathbf{k̇}_2(k) \\ \vdots \\ \mathbf{k̇}_{n_o}(k) \end{bmatrix} \\ &= \mathbf{0} \end{aligned}\]

knowing that the $\mathbf{k}_i(k)$ vectors are directly extracted from the decision variables in Z̃. The $\mathbf{x̂_d}(k)$ vector is the estimated deterministic state at the beginning of the interval $τ_0=0$, and is also extracted from Z̃. The $\mathbf{k̇}_i$ derivatives for the $i$th collocation point are computed from the continuous-time function model.f! and:

\[\mathbf{k̇}_i(k) = \mathbf{f}\Big(\mathbf{k}_i(k), \mathbf{û}_i(k), \mathbf{d}_i(k), \mathbf{p}\Big)\]

Based on the normalized time $τ_i$ and the hold order transcription.h, the inputs and disturbances are either piecewise constant or linear:

\[\begin{aligned} \mathbf{û}_i(k) &= \begin{cases} \mathbf{û_0}(k) & h = 0 \\ (1-τ_i)\mathbf{û_0}(k) + τ_i\mathbf{û_0}(k+1) & h = 1 \end{cases} \\ \mathbf{d}_i(k) &= (1-τ_i)\mathbf{d_0}(k) + τ_i\mathbf{d_0}(k+1) \end{aligned}\]

The disturbed input $\mathbf{û_0}$ is defined in f̂!.

The defects of the deterministic states $\mathbf{x̂_d}$ for the continuity constraints are in fact linear equality constraints:

\[\begin{aligned} \mathbf{ŝ_c}(k+1) &= \mathbf{C_o} \begin{bmatrix} \mathbf{k}_1(k) \\ \mathbf{k}_2(k) \\ \vdots \\ \mathbf{k}_{n_o}(k) \end{bmatrix} + λ_o \mathbf{x̂_d}(k) + \mathbf{ŵ_d}(k) - \mathbf{x̂_d}(k+1) \\ &= \mathbf{0} \end{aligned}\]

This is a purely linear equation since the $\mathbf{k}_i$, $\mathbf{x̂_d}$ and $\mathbf{ŵ_d}$ vectors are all extracted from the decision variables in Z̃. The estimated process noises of the deterministic states $\mathbf{ŵ_d}(k) = \mathbf{0}$ for NonLinMPC objects (only used for MovingHorizonEstimator). Note that handling the estimated process noise in the continuity constraint implicitly assumes that it's a discrete stochastic process (like all the other StateEstimator types in this package).

init_orthocolloc(model::SimModel, transcription::TranscriptionMethod)

Return empty sparse matrices and NaN value for other TranscriptionMethod types.

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_ŵ}$.

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

Does nothing if model is not a LinModel.

initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing

Init linear model prediction matrices F, q̃, r and current estimated output ŷ.

See init_predmat and init_quadprog for the definition of the matrices. They are computed with these equations using in-place operations:

\[\begin{aligned} \mathbf{F} &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k) + \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s} \\ \mathbf{C_y} &= \mathbf{F} + \mathbf{Y_{op}} - \mathbf{R̂_y} \\ \mathbf{C_u} &= \mathbf{T_u}\mathbf{u}(k-1) - \mathbf{R̂_u} \\ \mathbf{q̃} &= 2[ (\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} + (\mathbf{L}_{H_p} \mathbf{P̃_U})' \mathbf{C_u} ] \\ r &= \mathbf{C_y'} \mathbf{M}_{H_p} \mathbf{C_y} + \mathbf{C_u'} \mathbf{L}_{H_p} \mathbf{C_u} \end{aligned}\]

initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing

Init lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u vectors when model is not a LinModel.

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

Set b vector for the linear 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.

Set b excluding sensor noise bounds if for NonLinModel and non-SingleShooting.

Set b excluding state and sensor noise bounds for NonLinModel and SingleShooting.

linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod)

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

Also init $\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}$ vector for the terminal constraints, see init_predmat. The $\mathbf{F_w}$ vector for the custom linear constraints is also updated, see relaxW.

Set b excluding predicted output constraints for NonLinModel and not SingleShooting.

Also exclude terminal constraints for NonLinModel and SingleShooting.

linconstrainteq!(
    estim::MovingHorizonEstimator, model::LinModel, ::MultipleShooting
)

Set Aeq matrix and beq vector for the linear equality constraints of MHE.

They are defined by $\mathbf{A_{eq} Z̃ ≤ b_{eq}}$. The method also inits $\mathbf{F_S = G_S U_0 + J_S D_0 + B_S}$ vector for the state defect constraints, see init_defectmat_mhe.

The number of linear equality constraints grows when $N_k < H_e$. A temporary :linconstrainteq_temp structure is overwritten at each time step during this period. A permanent :linconstrainteq structure is created when $N_k = H_e$ is reached. From this point on, only the the $beq$ vector is updated with JuMP.set_normalized_rhs function for efficiency.

linconstrainteq!(
    estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod
)

By default, only update Aeq when Nk < He for other TranscriptionMethod.

The linear equality constraints include the stochastic defects only, and the beq vector is only zeros for this specific case. See init_defectmat_mhe for the equations.

No linear equality constraints for all cases of SingleShooting.

linconstrainteq!(
    mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting
)

Set beq vector for the linear model equality constraints ($\mathbf{A_{eq} Z̃ = b_{eq}}$).

Also init $\mathbf{F_S} = \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} + \mathbf{K_S x̂_0}(k) + \mathbf{V_S u_0}(k-1) + \mathbf{B_S}$, see init_defectmat.

linconstrainteq!(
    mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod
)

By default, fallback to doing same the but using the shorter equations.

The linear equality constraints include the stochastic defects only, and the continuity constraints of OrthogonalCollocation, if applicable. See init_defectmat for the equation.

linconstrainteq!(
    mpc::PredictiveController, ::NonLinModel, ::InternalModel, ::OrthogonalCollocation
)

Same as above but only for the continuity constraints of OrthogonalCollocation.

There are no stochastic defects, the InternalModel does not augment the states.

No linear equality constraints for other cases of InternalModel.

No linear equality constraints for all cases of SingleShooting (N/A).