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.