Non-linear Reference Tracking
via Model Predictive Control (MPC)
and Extended Kalman Filter (EKF)

Exam Project
Modelling and Control of Cyber-Physical Systems II
SDIC Master Degree, University of Trieste (UniTS)

Marco Tallone
February 2025

Introduction


Objectives

  • Implement a Model Predictive Controller (MPC) for non-linear systems (in MATLAB)
  • Implement an Extended Kalman Filter (EKF) for state estimation (in MATLAB)
  • Reproduce results by Kunz, Huck and Summers [1] on a miniature helicopter
  • Test and validate the implementation on different models and trajectories


Table of Contents

  1. Non-Linear Models
  2. Trajectory Generation Mehtods
  3. MPC Problem Formulation
  4. Extended Kalman Filter
  5. Validation and Results

Non-Linear Models

Unicycle Model

Non-linear Dynamics

\[ \begin{cases} \dot{x} = \frac{r}{2} (\omega_1 + \omega_2) \cos(\theta)\\ \dot{y} = \frac{r}{2} (\omega_1 + \omega_2) \sin(\theta)\\ \dot{\theta} = \frac{r}{L} (\omega_1 - \omega_2) \end{cases} \]

\[ \begin{aligned} \text{State:} \quad & \mathbf{x}(t) = \begin{bmatrix} x(t) & y(t) & \theta(t) \end{bmatrix}^T \\ \text{Input:} \quad & \mathbf{u}(t) = \begin{bmatrix} \omega_1(t) & \omega_2(t) \end{bmatrix}^T \end{aligned} \]

MATLAB Class Implementation

Helicopter Model

Non-linear Dynamics

\[ \begin{cases} \dot{x}_I = cos(\psi) \dot{x}_B - sin(\psi) \dot{y}_B\\ \dot{y}_I = sin(\psi) \dot{x}_B + cos(\psi) \dot{y}_B\\ \dot{z}_I = \dot{z}_B\\ \ddot{x}_B = b_x u_x + k_x \dot{x}_B + \dot{\psi} \dot{y}_B\\ \ddot{y}_B = b_y u_y + k_y \dot{y}_B - \dot{\psi} \dot{x}_B\\ \dot{z}_B = b_z u_z - g\\ \dot{\psi} = \dot{\psi}\\ \ddot{\psi} = b_{\psi} u_{\psi} + k_{\psi} \dot{\psi} \end{cases} \]

\[ 8 \text{ states}, \quad\quad 4 \text{ inputs} \]

MATLAB Class Implementation

Trajectory Generation


Flat Outputs


Differential Flatness

A non-linear dynamical system $\mathbf{\dot{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u})$ is said to be differentially flat if there exists a function $\Gamma(\cdot)$ such that \[ \mathbf{z} = \Gamma(\mathbf{x}, \mathbf{u}, \dot{\mathbf{u}}, \ldots, \mathbf{u}^{(p)}) \] where $\mathbf{z}$ are flat outputs.

Unicycle Model

\[z_1 = x \quad\text{ and }\quad z_2 = y \] \[ \begin{aligned} \theta &= \arctan\left(\frac{\dot{z_2}}{\dot{z_1}}\right)\\ \omega_1 &= \frac{2 \sqrt{\dot{z_1}^2 + \dot{z_2}^2} + L \dot{\theta}}{2 r}\\ \omega_2 &= \frac{2 \sqrt{\dot{z_1}^2 + \dot{z_2}^2} - L \dot{\theta}}{2 r} \end{aligned} \]

Helicopter Model

\[ z_1 = x_I, \quad z_2 = y_I, \quad z_3 = z_I, \quad z_4 = \psi \] \[ \begin{aligned} \dot{x}_B &= \cos(z_4) \dot{z}_1 + \sin(z_4) \dot{z}_2, \quad\quad \dot{z}_B = \dot{z}_3\\ \dot{y}_B &= -\sin(z_4) \dot{z}_1 + \cos(z_4) \dot{z}_2, \quad\quad \dot{\psi} = \dot{z}_4\\ u_x &= \frac{1}{b_x} \left( cos(z_4) \left[ \ddot{z}_1 - k_x \dot{z}_1 \right] + sin(z_4) \left[ \ddot{z}_2 - k_x \dot{z}_2 \right] \right)\\ u_y &= \frac{1}{b_y} \left( cos(z_4) \left[ \ddot{z}_2 - k_y \dot{z}_2 \right] + sin(z_4) \left[ -\ddot{z}_1 + k_y \dot{z}_1 \right] \right)\\ u_z &= \frac{1}{b_z} \left( \ddot{z}_3 + g \right), \quad u_{\psi} = \frac{1}{b_{\psi}} \left( \ddot{z}_4 - k_{\psi} \dot{z}_4 \right) \end{aligned} \]

Analytical Trajectory Generation


Circle

\[ \begin{cases} z_1(t) = r_0 \cos(t)\\ z_2(t) = r_0 \sin(t)\\ z_3(t) = 0\\ z_4(t) = \text{atan2}\left(\dot{z}_2(t), \dot{z}_1(t)\right) \end{cases} \]

Lemniscate

\[ \begin{cases} z_1(t) = \frac{a \sqrt{2} \cos(t)}{\sin(t)^2 + 1}\\ z_2(t) = \frac{a \sqrt{2} \cos(t) \sin(t)}{\sin(t)^2 + 1}\\ z_3(t) = 0\\ z_4(t) = \text{atan2}\left(\dot{z}_2(t), \dot{z}_1(t)\right) \end{cases} \]

Sample trajectories at time steps $\{t_k\}_{k=1}^{N_{guide}}$ $\rightarrow$ obtain $N_{guide}$ reference points $\{(\mathbf{x}_{\text{ref}, k}, \mathbf{u}_{\text{ref}, k})\}_{k=1}^{N_{guide}}$

Total time to complete the trajectory set to: $\quad T_{end} = N_{guide} \cdot T_s$

⚠ Discrete reference inputs $\mathbf{u}_{\text{ref}, k}$ $\rightarrow$ from averages of continuous-time inputs $\mathbf{u}_{\text{ref}}(t)$:
\[ \mathbf{u}_{\text{ref}, k} = \frac{1}{N_{samples}} \sum_{i=1}^{N_{samples}} \mathbf{u}_{\text{ref}}\left(t_k + \frac{i \cdot T_s}{N_{samples}} \right) \]

Analytical Trajectory Generation


Circle

\[ \begin{cases} z_1(t) = r_0 \cos(t)\\ z_2(t) = r_0 \sin(t)\\ z_3(t) = 0\\ z_4(t) = \text{atan2}\left(\dot{z}_2(t), \dot{z}_1(t)\right) \end{cases} \]

Lemniscate

\[ \begin{cases} z_1(t) = \frac{a \sqrt{2} \cos(t)}{\sin(t)^2 + 1}\\ z_2(t) = \frac{a \sqrt{2} \cos(t) \sin(t)}{\sin(t)^2 + 1}\\ z_3(t) = 0\\ z_4(t) = \text{atan2}\left(\dot{z}_2(t), \dot{z}_1(t)\right) \end{cases} \]

Sample trajectories at time steps $\{t_k\}_{k=1}^{N_{guide}}$ $\rightarrow$ obtain $N_{guide}$ reference points $\{(\mathbf{x}_{\text{ref}, k}, \mathbf{u}_{\text{ref}, k})\}_{k=1}^{N_{guide}}$

Total time to complete the trajectory set to: $\quad T_{end} = N_{guide} \cdot T_s$

⚠ Discrete reference inputs $\mathbf{u}_{\text{ref}, k}$ $\rightarrow$ from averages of continuous-time inputs $\mathbf{u}_{\text{ref}}(t)$:
\[ \mathbf{u}_{\text{ref}, k} = \frac{1}{N_{samples}} \sum_{i=1}^{N_{samples}} \mathbf{u}_{\text{ref}}\left(t_k + \frac{i \cdot T_s}{N_{samples}} \right) \]

Analytical Trajectory Generation


⚠ Discrete reference inputs $\mathbf{u}_{\text{ref}, k}$ $\rightarrow$ from averages of continuous-time inputs $\mathbf{u}_{\text{ref}}(t)$:
\[ \mathbf{u}_{\text{ref}, k} = \frac{1}{N_{samples}} \sum_{i=1}^{N_{samples}} \mathbf{u}_{\text{ref}}\left(t_k + \frac{i \cdot T_s}{N_{samples}} \right) \]

Analytical Trajectory Generation


Simulations from first reference (Helicopter)

Analytical Trajectory Generation


Simulations from first reference (Helicopter)

Murray Method


Non-linear dynamics $\quad \mathbf{\dot{x}}(t) = \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t))$
Flat outputs $\quad \mathbf{z}(t)$

Given the known flat outputs and their derivatives up to order $q$ between $T_0=0$ and $T_f=T$: \[ \bar{\mathbf{z}} = \begin{bmatrix} \mathbf{z}(0) & \dot{\mathbf{z}}(0) & \ldots & \mathbf{z}^{(q)}(0) & \mathbf{z}(T) & \dot{\mathbf{z}}(T) & \ldots & \mathbf{z}^{(q)}(T) \end{bmatrix}^T \]

Parametrize $\mathbf{z}(t)$ as a linear combination of $N_{\text{basis}}$ basis functions $\mathbf{b}_i(t)$: $\quad \mathbf{z}(t) = \sum_{i=1}^{N_{\text{basis}}} \alpha_i \mathbf{b}_i(t) $
As explained in [2], find coefficients $\alpha_i$ solving the linear system:

\[ \begin{bmatrix} \mathbf{b}_1(0) & \mathbf{b}_2(0) & \ldots & \mathbf{b}_{N_{\text{basis}}}(0) \\ \dot{\mathbf{b}}_1(0) & \dot{\mathbf{b}}_2(0) & \ldots & \dot{\mathbf{b}}_{N_{\text{basis}}}(0) \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{b}_1^{(q)}(0) & \mathbf{b}_2^{(q)}(0) & \ldots & \mathbf{b}_{N_{\text{basis}}}^{(q)}(0) \\ \mathbf{b}_1(T) & \mathbf{b}_2(T) & \ldots & \mathbf{b}_{N_{\text{basis}}}(T) \\ \dot{\mathbf{b}}_1(T) & \dot{\mathbf{b}}_2(T) & \ldots & \dot{\mathbf{b}}_{N_{\text{basis}}}(T) \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{b}_1^{(q)}(T) & \mathbf{b}_2^{(q)}(T) & \ldots & \mathbf{b}_{N_{\text{basis}}}^{(q)}(T) \end{bmatrix} \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \vdots \\ \alpha_{N_{\text{basis}}} \end{bmatrix} = \begin{bmatrix} \mathbf{z}(0) \\ \dot{\mathbf{z}}(0) \\ \vdots \\ \mathbf{z}^{(q)}(0) \\ \mathbf{z}(T) \\ \dot{\mathbf{z}}(T) \\ \vdots \\ \mathbf{z}^{(q)}(T) \end{bmatrix} \]

MPC Problem Formulation

LTV Model

Given set of nominal state and inputs at regular times $T_s$: \[ \begin{aligned} \bar{\mathbf{x}}_k &= \bar{\mathbf{x}}(k T_s), \quad k = k_0, \ldots, k_0 + N + 1\\ \bar{\mathbf{u}}_k &= \bar{\mathbf{u}}(k T_s), \quad k = k_0, \ldots, k_0 + N \end{aligned} \]

Linearize the non-linear model around the nominal trajectory: \[ \delta \mathbf{x}(t) = \mathbf{A}_k \delta \mathbf{x}(t) + \mathbf{B}_k \delta \mathbf{u}(t) \] \[ \mathbf{A}_k = \left. \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \right|_{\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k}, \quad \mathbf{B}_k = \left. \frac{\partial \mathbf{f}}{\partial \mathbf{u}} \right|_{\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k} \] \[ \delta \mathbf{x}(t) = \mathbf{x}(t) - \bar{\mathbf{x}}_k, \quad \delta \mathbf{u}(t) = \mathbf{u}(t) - \bar{\mathbf{u}}_k \] with $k T_s \leq t < (k + 1) T_s$ for $k = k_0, \ldots, k_0 + N$

LTV Model

Then discretize using a forward Euler method: \[ \delta \mathbf{x}_{k+1} = \mathbf{A}_{d,k} \delta \mathbf{x}_k + \mathbf{B}_{d,k} \delta \mathbf{u}_k \] \[ \mathbf{A}_{d,k} = \mathbb{I} + T_s \mathbf{A}_k, \quad \mathbf{B}_{d,k} = T_s \mathbf{B}_k \] \[ \delta \mathbf{x}_k = \mathbf{x}_k - \bar{\mathbf{x}}_k, \quad \delta \mathbf{u}_k = \mathbf{u}_k - \bar{\mathbf{u}}_k \]

Which can eventually be rewritten as: \[ \begin{aligned} \mathbf{x}_{k+1} &= \mathbf{A}_{d,k} \mathbf{x}_k + \mathbf{B}_{d,k} \mathbf{u}_k + \mathbf{d}_k\\ \mathbf{d}_k &= \bar{\mathbf{x}}_{k+1} - \mathbf{A}_{d,k} \bar{\mathbf{x}}_k - \mathbf{B}_{d,k} \bar{\mathbf{u}}_k \end{aligned} \]

MPC Problem Formulation

MPC optimization problem with horizon $N$: \[ \begin{aligned} \min_{\mathbf{U}_{t \to t+N | t}} \quad & \mathbf{J} = \sum_{k=t}^{t+N} \left( \Delta \mathbf{x}_k^T \mathbf{Q} \Delta \mathbf{x}_k + \Delta \mathbf{u}_k^T \mathbf{R} \Delta \mathbf{u}_k \right)\\ \text{s.t.} \quad & \mathbf{x}_{k+1 | t} = \mathbf{A}_{d,k | t} \mathbf{x}_{k | t} + \mathbf{B}_{d,k | t} \mathbf{u}_{k | t} + \mathbf{d}_{k | t}\\ & \mathbf{d}_{k | t} = \bar{\mathbf{x}}_{k+1 | t} - \mathbf{A}_{d,k | t} \bar{\mathbf{x}}_{k | t} - \mathbf{B}_{d,k | t} \bar{\mathbf{u}}_{k | t}\\ & \mathbf{x}_{k | t} \in \mathcal{X}, \quad k = t, \ldots, t+N\\ & \mathbf{u}_{k | t} \in \mathcal{U}, \quad k = t, \ldots, t+N+1\\ \end{aligned} \] with:

  • Difference from state reference: $\Delta \mathbf{x}_k = \mathbf{x}_{k | t} - \mathbf{x}_{\text{ref},k | t}$
  • Difference from input reference: $\Delta \mathbf{u}_k = \mathbf{u}_{k | t} - \mathbf{u}_{\text{ref},k | t}$
  • State constraints set $\mathcal{X}$
  • Input constraints set $\mathcal{U}$
  • Weight matrices $\mathbf{Q}$ and $\mathbf{R}$

Dense & Sparse Formulations

Dense Formulation

Introduce vectorized notation: \[ \begin{aligned} \mathbf{X}_k &= \begin{bmatrix} \mathbf{x}_k^T & \mathbf{x}_{k+1}^T & \ldots & \mathbf{x}_{k+N-1}^T \end{bmatrix}^T\\ \mathbf{U}_k &= \begin{bmatrix} \mathbf{u}_k^T & \mathbf{u}_{k+1}^T & \ldots & \mathbf{u}_{k+N-1}^T \end{bmatrix}^T \end{aligned} \] so that \[ \mathbf{X}_k = \mathcal{A} \mathbf{x}_k + \mathcal{B} \mathbf{U}_k + \mathcal{D} \] (see Appendix $1$ for $\mathcal{A}$, $\mathcal{B}$, and $\mathcal{D}$)

Then introduce also: \[ \begin{aligned} \mathcal{Q} &= \text{diag}(\mathbf{Q}, \ldots, \mathbf{Q})\\ \mathcal{R} &= \text{diag}(\mathbf{R}, \ldots, \mathbf{R}) \end{aligned} \]

Sparse Formulation

Introduce vectorized notation: \[ \begin{aligned} \delta \mathbf{X}_k &= \begin{bmatrix} \delta \mathbf{x}_k^T & \delta \mathbf{x}_{k+1}^T & \ldots & \delta \mathbf{x}_{k+N-1}^T \end{bmatrix}^T\\ \delta \mathbf{U}_k &= \begin{bmatrix} \delta \mathbf{u}_k^T & \delta \mathbf{u}_{k+1}^T & \ldots & \delta \mathbf{u}_{k+N-1}^T \end{bmatrix}^T \end{aligned} \] so that \[ \delta \mathbf{X}_k = \mathcal{A} \delta \mathbf{X}_k + \mathcal{B} \delta \mathbf{U}_k + \mathcal{D} \delta \mathbf{x}_k \] (see Appendix $2$ for $\mathcal{A}$, $\mathcal{B}$, and $\mathcal{D}$)

Then introduce also: \[ \begin{aligned} \mathcal{Q} = \text{diag}(\mathbf{Q}, \ldots, \mathbf{Q}), &\quad \mathcal{R} = \text{diag}(\mathbf{R}, \ldots, \mathbf{R})\\ \text{and} \quad \mathbf{V} &= \begin{bmatrix} \mathcal{Q} & \mathbf{0}\\ \mathbf{0} & \mathcal{R} \end{bmatrix} \end{aligned} \]

Dense & Sparse Formulations

Dense Formulation

Rewrite cost function: \[ \begin{aligned} \mathbf{J} &= \Delta \mathbf{X}_k^T \mathcal{Q} \Delta \mathbf{X}_k + \Delta \mathbf{U}_k^T \mathcal{R} \Delta \mathbf{U}_k\\ & = \left( \mathbf{X}_k - \mathbf{X}_{\text{ref},k} \right)^T \mathcal{Q} \left( \mathbf{X}_k - \mathbf{X}_{\text{ref},k} \right) + \ldots\\ & \quad \ldots + \left( \mathbf{U}_k - \mathbf{U}_{\text{ref},k} \right)^T \mathcal{R} \left( \mathbf{U}_k - \mathbf{U}_{\text{ref},k} \right)\\ &= \mathbf{U}_k^T \left( \mathcal{B}^T \mathcal{Q} \mathcal{B} + \mathcal{R} \right) \mathbf{U}_k + \dots\\ & \quad\dots + 2 \big( \mathbf{x}_k^T \mathcal{A}^T \mathcal{Q} \mathcal{B} + \ldots\\ & \quad \ldots + \mathcal{D}^T \mathcal{Q} \mathcal{B} - \mathbf{X}_{\text{ref},k}^T \mathcal{Q} \mathcal{B} - \dots\\ &\quad \dots - \mathbf{U}_{\text{ref},k}^T \mathcal{R} \big) \mathbf{U}_k\\ &= \mathbf{U}_k^T \mathbf{H}_k \mathbf{U}_k + 2 \mathbf{f}_k^T \mathbf{U}_k \end{aligned} \]

Sparse Formulation

Introduce augmented vector variables: \[ \mathbf{Z}_k = \begin{bmatrix} \mathbf{X}_k\\ \mathbf{U}_k \end{bmatrix} \quad \text{and} \quad \mathbf{Z_{\text{ref},k}} = \begin{bmatrix} \mathbf{X}_{\text{ref},k}\\ \mathbf{U}_{\text{ref},k} \end{bmatrix} \] Rewrite cost function: \[ \begin{aligned} \mathbf{J} &= \Delta \mathbf{X}_k^T \mathcal{Q} \Delta \mathbf{X}_k + \Delta \mathbf{U}_k^T \mathcal{R} \Delta \mathbf{U}_k\\ &= \Delta \mathbf{Z}_k^T \mathbf{V} \Delta \mathbf{Z}_k\\ &= \left( \mathbf{Z}_k - \mathbf{Z}_{\text{ref},k} \right)^T \mathbf{V} \left( \mathbf{Z}_k - \mathbf{Z}_{\text{ref},k} \right)\\ &= \mathbf{Z}_k^T \mathbf{V} \mathbf{Z}_k + 2 \left( - \mathbf{Z}_{\text{ref},k}^T \mathbf{V} \right) \mathbf{Z}_k\\ &= \mathbf{Z}_k^T \mathbf{V}_k \mathbf{Z}_k + 2 \tilde{\mathbf{f}}_k^T \mathbf{Z}_k \end{aligned} \]

Dense & Sparse Formulations

Dense Formulation

Constraints with $\boldsymbol{\varepsilon} = \begin{bmatrix} +1 & -1 \end{bmatrix}^T$: \[ \boldsymbol{\varepsilon} \mathbf{x}_k \leq \mathbf{f}_{\mathbf{x}} = \begin{bmatrix} \mathbf{x}_{\max}\\ \mathbf{x}_{\min} \end{bmatrix} \text{ and } \boldsymbol{\varepsilon} \mathbf{u}_k \leq \mathbf{f}_{\mathbf{u}} = \begin{bmatrix} \mathbf{u}_{\max}\\ \mathbf{u}_{\min} \end{bmatrix} \] Hence: \[ \mathcal{E}_{\mathbf{x}} = \mathcal{E}_{\mathbf{u}} = \text{diag}(\boldsymbol{\varepsilon}), \, \mathcal{F}_{\mathbf{x}} = \begin{bmatrix} \mathbf{f}_{\mathbf{x}}\\ \vdots\\ \mathbf{f}_{\mathbf{x}} \end{bmatrix}, \, \mathcal{F}_{\mathbf{u}} = \begin{bmatrix} \mathbf{f}_{\mathbf{u}}\\ \vdots\\ \mathbf{f}_{\mathbf{u}} \end{bmatrix} \] which leads to: \[ \mathcal{E} \mathbf{U}_k \leq \mathcal{F} \] \[ \mathcal{E} = \begin{bmatrix} \mathcal{E}_{\mathbf{u}}\\ \mathcal{E}_{\mathbf{x}} \mathcal{B} \end{bmatrix} \text{ and } \mathcal{F} = \begin{bmatrix} \mathcal{F}_{\mathbf{u}}\\ \mathcal{F}_{\mathbf{x}} - \mathcal{E}_{\mathbf{x}} (\mathcal{A} \mathbf{x}_k + \mathcal{D}) \end{bmatrix} \]

Sparse Formulation

Inequality constraints (same notation): \[ \mathcal{E}_{\mathbf{Z}} \mathbf{Z}_k \leq \mathcal{F}_{\mathbf{Z}} \] where \[ \mathcal{E}_{\mathbf{Z}} = \begin{bmatrix} \mathcal{E}_{\mathbf{x}} & \mathbf{0}\\ \mathbf{0} & \mathcal{E}_{\mathbf{u}} \end{bmatrix} \quad \text{and} \quad \mathcal{F}_{\mathbf{Z}} = \begin{bmatrix} \mathcal{F}_{\mathbf{x}}\\ \mathcal{F}_{\mathbf{u}} \end{bmatrix} \] Equality constraints: \[ \begin{bmatrix} \mathbb{I} - \mathcal{A} & -\mathcal{B} \end{bmatrix} \delta \mathbf{Z}_k = \mathcal{D} \delta \mathbf{x}_k \Rightarrow \mathcal{E}_{\text{eq.}} \mathbf{Z}_k = \mathcal{F}_{\text{eq.}} \] where \[ \begin{aligned} \mathcal{E}_{\text{eq.}} &= \begin{bmatrix} \mathbb{I} - \mathcal{A} & -\mathcal{B} \end{bmatrix}\\ \mathcal{F}_{\text{eq.}} &= \mathcal{D} \delta \mathbf{x}_k + \begin{bmatrix} \mathbb{I} - \mathcal{A} & -\mathcal{B} \end{bmatrix} \bar{\mathbf{Z}}_k \end{aligned} \]

Dense & Sparse Formulations

Dense Formulation

Final QP problem: \[ \begin{aligned} \min_{\mathbf{U}_k} \quad & \mathbf{U}_k^T \mathbf{H}_k \mathbf{U}_k + 2 \mathbf{f}_k^T \mathbf{U}_k\\ \text{s.t.} \quad & \mathcal{E} \mathbf{U}_k \leq \mathcal{F} \end{aligned} \]

  • QP problem
  • decision variables: $\mathbf{U}_k$ control inputs
  • only inequality constraints

Sparse Formulation

Final QP problem: \[ \begin{aligned} \min_{\mathbf{Z}_k} \quad & \mathbf{Z}_k^T \mathbf{V} \mathbf{Z}_k + 2 \tilde{\mathbf{f}}_k^T \mathbf{Z}_k\\ \text{s.t.} \quad & \mathcal{E}_{\mathbf{Z}} \mathbf{Z}_k \leq \mathcal{F}_{\mathbf{Z}}\\ & \mathcal{E}_{\text{eq.}} \mathbf{Z}_k = \mathcal{F}_{\text{eq.}} \end{aligned} \]

  • QP problem
  • more decision variables: $\mathbf{Z}_k = \begin{bmatrix} \mathbf{X}_k & \mathbf{U}_k \end{bmatrix}^T$
  • equality and inequality constraints
  • better for solver (sparsity)

MPC Algorithm Implementation

Store remaining optimization results as next iteration nominal inputs

Update the state vector simulating the system with the optimal input

Extract optimal input ($1^{\text{st}}$ input vector)

Optimization phase: set constraints and solve QP problem

Prediction horizon loop: linearize, discretize, build formulation matrices and fix angles

Set first nominal inputs from reference ($1^{\text{st}}$ iteration)

Set initial conditions (with or without noise)

Set horizon $N$, number of states $n$ and inputs $m$ 


% MPC optimization function
function [x, u] = optimize(obj)

	% Define number of states, inputs and horizon
	n = obj.model.n;
	m = obj.model.m;
	N = obj.N;

	% Initialize the state and input vectors
	x = obj.x0;
	if obj.noise
		% Noise components generation for Kalman filter
		w = mvnrnd(zeros(obj.model.n, 1), obj.model.Q_tilde, obj.Nsteps);
		v = mvnrnd(zeros(obj.model.p, 1), obj.model.R_tilde, obj.Nsteps);
		x_last = obj.x0 + w(1, :)';
	else
		x_last = obj.x0;
	end
	u = [];
	obj.set_reference(1);
	u_pred = obj.U_REF;

	% Solve optimization problem at each time step
	for k = 1:obj.Nsteps

		% Set reference states and inputs with or without preview
		obj.set_reference(k);

		% Define formulation matrices
		[A, B, D] = obj.set_formulation(x_last, u_pred);

		% Fix angle representation in X_REF
		obj.X_REF = obj.model.fix_angles(obj.x_pred, obj.X_REF);

		% Set augmented reference state Z_REF
		obj.Z_REF = [obj.X_REF; obj.U_REF];

		% Set constraints matrices
		[H, f, EPS, F, EPS_eq, F_eq] = obj.set_constraints(A, B, D, x_last);

		% Solve the optimization problem and get optimal input sequence
		UMPC = obj.solve(H, f, EPS, F, EPS_eq, F_eq);

		% Extract the optimal input and update the input vector
		u_optimal = UMPC(1:m); % only pick the first inputs (of size m)
		u = [u; u_optimal];

		% Update the state vector simulating the system with the optimal input
		if obj.noise
			update_last = obj.model.simulate(x_last, u_optimal, obj.Ts) + w(k,:)';
			measured_output = obj.model.output(update_last, u_optimal) + v(k,:)';
			x_last = obj.model.EKF_estimate(x_last, u_optimal, measured_output);
		else
			x_last = obj.model.simulate(x_last, u_optimal, obj.Ts);
		end
		x = [x; x_last];

		% Update the prediction inputs with the remaining optimization results
		u_remaining = UMPC(m+1:end);
		u_pred = [u_remaining; u_remaining(end-m+1:end)]; % duplicate last inputs
	end

Extended Kalman Filter

Extended Kalman Filter

Discrete-time non-linear system:

\[ \begin{aligned} \dot{\mathbf{x}}_k &= \mathbf{f}(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_k \\ \mathbf{y}_k &= \mathbf{g}(\mathbf{x}_k) + \mathbf{v}_k \end{aligned} \]

Process Noise $\quad \mathbf{w}_k \sim \mathcal{N}(0, \tilde{\mathbf{Q}}_k)$
Measurement Noise $\quad \mathbf{v}_k \sim \mathcal{N}(0, \tilde{\mathbf{R}}_k)$

Extended Kalman Filter (EKF) steps:


1. Prediction

\[ \begin{aligned} \hat{\mathbf{x}}_{k | k-1} = \mathbf{f}(\hat{\mathbf{x}}_{k-1 | k-1}, \mathbf{u}_{k-1}) \quad & \quad \text{Predicted state estimate} \\ \mathbf{P}_{k | k-1} = \mathbf{A}_k \mathbf{P}_{k-1 | k-1} \mathbf{A}_k^T + \tilde{\mathbf{Q}}_{k - 1} \quad & \quad \text{Predicted covariance estimate} \end{aligned} \]


2. Update

\[ \begin{aligned} \mathbf{K}_k = \mathbf{P}_{k | k-1} \mathbf{C}_k^T \left( \mathbf{C}_k \mathbf{P}_{k | k-1} \mathbf{C}_k^T + \tilde{\mathbf{R}}_k \right)^{-1} \quad & \quad \text{Kalman gain} \\ \hat{\mathbf{x}}_{k | k} = \hat{\mathbf{x}}_{k | k-1} + \mathbf{K}_k \left( \mathbf{y}_k - \mathbf{g}(\hat{\mathbf{x}}_{k | k-1}) \right) \quad & \quad \text{Updated state estimate} \\ \mathbf{P}_{k | k} = \left( \mathbb{I} - \mathbf{K}_k \mathbf{C}_k \right) \mathbf{P}_{k | k-1} \quad & \quad \text{Updated covariance estimate} \end{aligned} \]

Validation and Results

Parameters & Constraints

Unicycle Model

Model parameters:
\[ \begin{aligned} &r = 0.03 \; \text{m}\\ &L = 0.3 \; \text{m}\\ &T_s = 0.1 \; \text{s}\\ &x \in [-2, 2] \; \text{m}, \quad y \in [-2, 2] \; \text{m}\\ &\omega_1, \; \omega_2 \in [-50, 50] \; \text{rad/s} \end{aligned} \]

MPC Parameters: \[ \begin{aligned} &N = 10\\ &\mathbf{Q} = 10^3 \cdot \mathbb{I}_{3 \times 3}\\ &\mathbf{R} = \mathbb{I}_{2 \times 2} \end{aligned} \]

Helicopter Model

Model parameters [1]: \[ \begin{aligned} &b_x = 2, \quad b_y = 2, \quad b_z = 18, \quad b_{\psi} = 111\\ &k_x = -0.5, \quad k_y = -0.5, \quad k_{\psi} = -5\\ &T_s = 0.1 \; \text{s}\\ &x_I \in [-2, 2] \; \text{m}, \quad y_I \in [-2, 2] \; \text{m}, \quad z_I \in [-2, 2] \; \text{m}\\ &\ddot{x}_B \in [-3, 3] \; \frac{\text{m}}{\text{s}^2}, \quad \ddot{y}_B \in [-3, 3] \; \frac{\text{m}}{\text{s}^2}, \quad \ddot{z}_B \in [-2, 2] \; \frac{\text{m}}{\text{s}^2}\\ &\dot{\psi} \in [-25, 25] \; \text{rad/s}, \quad u_i \in [-2, 2] \; \text{for } i = x, y, z, \psi \end{aligned} \]

MPC Parameters [1]: \[ \begin{aligned} &N = 18\\ &\mathbf{Q} = \text{diag}\left(50, 50, 5, 10, 3, 3, 1, 2\right)\\ &\mathbf{R} = 2 \cdot \mathbb{I}_{4 \times 4} \end{aligned} \]

Experimental Setup

Experiment $1$: Loop around trajectory
  • $N_{\text{guide}} = 100$
  • $\mathbf{x}_0$ randomly initialized s.t. $||\mathbf{x}_0 - \mathbf{x}_{\text{ref}, 1}||_2 \leq 0.05$
  • $100$ repetitions
Experiment $2$: Loop around trajectory with noise
  • $N_{\text{guide}} = 100$
  • $\mathbf{x}_0$ randomly initialized s.t. $||\mathbf{x}_0 - \mathbf{x}_{\text{ref}, 1}||_2 \leq 0.05$
  • $20$ repetitions
  • Process noise: $\tilde{\mathbf{Q}} = 0.75 \cdot 10^{-3} \cdot \mathbb{I}_{n \times n}$
  • Measurement noise: $\tilde{\mathbf{R}} = 10^{-2} \cdot \mathbb{I}_{m \times m}$
  • Initial state covariance: $\mathbf{P}_0 = \mathbb{I}_{n \times n}$
Experiment $3$: Increasing horizon $N$
  • $N = 1, 2, \ldots, 20$
  • Same setup as Experiment $1$
  • $10$ repetitions per $N$ value

Results Experiments $1$ and $2$

Assessment metric: Root Mean Squared Error (RMSE) \[ \text{RMSE}(\mathbf{x}) = \sqrt{\frac{1}{N_{\text{guide}}} \sum_{i=1}^{N_{\text{guide}}} || \mathbf{x}_i - \mathbf{x}_{\text{ref}, i} ||_2^2} \quad \text{and} \quad \text{RMSE}(\mathbf{u}) = \sqrt{\frac{1}{N_{\text{guide}}} \sum_{i=1}^{N_{\text{guide}}} || \mathbf{u}_i - \mathbf{u}_{\text{ref}, i} ||_2^2} \]

Model Trajectory State RMSE Input RMSE
Unicycle Circle 0.020 ± 0.007 0.227 ± 0.068
Lemniscate 0.030 ± 0.002 0.857 ± 0.023
Helicopter Circle 0.034 ± 0.013 0.096 ± 0.011
Lemniscate 0.686 ± 0.001 0.302 ± 0.007
Unicycle
(with noise)		 Circle 0.056 ± 0.015 0.634 ± 0.190
Lemniscate 0.204 ± 0.251 1.037 ± 0.106
Helicopter
(with noise)		 Circle 1.073 ± 0.329 0.215 ± 0.025
Lemniscate 1.169 ± 0.207 0.569 ± 0.071

Results Experiments $1$ and $2$

Simulations without noise (Unicycle)

Results Experiments $1$ and $2$

Simulations with noise (Unicycle)

Results Experiments $1$ and $2$

Simulations without noise (Helicopter)

Results Experiments $1$ and $2$

Simulations with noise (Helicopter)

Results Experiment $3$

$3$D Trajectories with Helicopter

\[ z_I(t) = 0.05 \cdot t \]

\[ z_I(t) = 0.2 \cdot \sin(4 \cdot t) \]

\[\textcolor{gray}{\bullet}: \text{Reference} \quad \textcolor{red}{\bullet}: \text{Target} \quad \textcolor{blue}{\bullet}: \text{Real Trajectory}\]

References

  1. K. Kunz, S. M. Huck, T. H. Summers,
    "Fast Model Predictive Control of miniature helicopters",
    2013 European Control Conference (ECC), 2013, Pages 1377-1382,
    https://ieeexplore.ieee.org/document/6669699
  2. R.M. Murray,
    "Optimization Based Control",
    California Institute of Technology, 2023, Chapter Trajectory Generation and Differential Flatness,
    http://www.cds.caltech.edu/~murray/books/AM08/pdf/obc-complete_12Mar2023.pdf
  3. R. E. Kalman,
    "A New Approach to Linear Filtering and Prediction Problems",
    Journal of Basic Engineering, Volume 82, 1960, Pages 35-45,
    https://asmedigitalcollection.asme.org/fluidsengineering/article-pdf/82/1/35/5518977/35_1.pdf
  4. R. E. Kalman, R. S. Bucy,
    "New Results in Linear Filtering and Prediction Theory",
    Journal of Basic Engineering, Volume 83, 1961, Pages 95-108,
    https://asmedigitalcollection.asme.org/fluidsengineering/article-pdf/83/1/95/5503549/95_1.pdf
  5. The MathWorks Inc.,
    "MATLAB version: 24.2.0 (R2024b)",
    The MathWorks Inc., 2024,
    https://www.mathworks.com

Appendix $1$: Dense Formulation matrices

\[ \begin{aligned} \mathcal{A} &= \begin{bmatrix} \mathbb{I}\\ \mathbf{A}_{d,k}\\ \mathbf{A}_{d,k+1} \mathbf{A}_{d,k}\\ \vdots\\ \mathbf{A}_{d,k+N-2} \ldots \mathbf{A}_{d,k} \end{bmatrix}\\ \mathcal{B} &= \begin{bmatrix} \mathbf{0} & \mathbf{0} & \ldots & \mathbf{0} & \mathbf{0}\\ \mathbf{B}_{d,k} & \mathbf{0} & \ldots & \mathbf{0} & \mathbf{0}\\ \mathbf{A}_{d,k+1} \mathbf{B}_{d,k} & \mathbf{B}_{d,k+1} & \ldots & \mathbf{0} & \mathbf{0}\\ \vdots & \vdots & \ddots & \vdots\\ \mathbf{A}_{d,k+N-2} \ldots \mathbf{A}_{d,k} \mathbf{B}_{d,k} & \ldots & \ldots & \mathbf{B}_{d,k+N-2} & \mathbf{0} \end{bmatrix}\\ \text{and}\quad \mathcal{D} &= \begin{bmatrix} \mathbf{0}\\ \mathbf{d}_k\\ \mathbf{A}_{d,k+1} \mathbf{d}_k + \mathbf{d}_{k+1}\\ \mathbf{A}_{d,k+2} \mathbf{A}_{d,k+1} \mathbf{d}_k + \mathbf{A}_{d,k+2} \mathbf{d}_{k+1} + \mathbf{d}_{k+2}\\ \vdots \end{bmatrix} \end{aligned} \]
Back to Dense & Sparse Formulations

Appendix $2$: Sparse Formulation matrices

\[ \begin{aligned} \mathcal{A} &= \begin{bmatrix} \mathbf{0} & \mathbf{0} & \ldots & \mathbf{0} & \mathbf{0}\\ \mathbf{A}_{d,k} & \mathbf{0} & \ldots & \mathbf{0} & \mathbf{0}\\ \mathbf{0} & \mathbf{A}_{d,k+1} & \ldots & \mathbf{0} & \mathbf{0}\\ \vdots & \vdots & \ddots & \vdots\\ \mathbf{0} & \mathbf{0} & \ldots & \mathbf{A}_{d,k+N-1} & \mathbf{0} \end{bmatrix}\\ \mathcal{B} &= \begin{bmatrix} \mathbf{0} & \mathbf{0} & \ldots & \mathbf{0} & \mathbf{0}\\ \mathbf{B}_{d,k} & \mathbf{0} & \ldots & \mathbf{0} & \mathbf{0}\\ \mathbf{0} & \mathbf{B}_{d,k+1} & \ldots & \mathbf{0} & \mathbf{0}\\ \vdots & \vdots & \ddots & \vdots\\ \mathbf{0} & \mathbf{0} & \ldots & \mathbf{B}_{d,k+N-1} & \mathbf{0} \end{bmatrix}\\ \mathcal{D} &= \begin{bmatrix} \mathbb{I}\\ \mathbf{0}\\ \vdots\\ \mathbf{0}\\ \end{bmatrix} \end{aligned} \]
Back to Dense & Sparse Formulations