# Online Learning and Control for Data-Augmented Quadrotor Model

Matěj Šmíd, Jindřich Duník M. Šmíd is a Masters student at the Faculty of
Applied Scienced, University of West BohemiaJ. Duník is with the Faculty of
Applied Scienced, University of West BohemiaThanks to Zdeněk Bouček for his
technical expertise with quadrotor control.

###### Abstract

The ability to adapt to changing conditions is a key feature of a successful
autonomous system. In this work, we use the Recursive Gaussian Processes (RGP)
for identification of the quadrotor air drag model online, without the need of
training data. The identified drag model then augments a physics-based model
of the quadrotor dynamics, which allows more accurate quadrotor state
prediction with increased ability to adapt to changing conditions. This data-
augmented physics-based model is utilized for precise quadrotor trajectory
tracking using the suitably modified Model Predictive Control (MPC) algorithm.
The proposed modelling and control approach is evaluated using the Gazebo
simulator and it is shown that the proposed approach tracks a desired
trajectory with a higher accuracy compared to the MPC with the non-augmented
(purely physics-based) model.

inline]uncomment for final version

Index Terms — Data-augmented physics-based model, Adaptive control, Gaussian
process, Predictive control, Quadrotor, Gazebo

Source code: https://github.com/smidmatej/mpc_quad_ros

##  I INTRODUCTION

Given a model of a dynamical system, we seek to find discrepancies between the
model and the actual system. This is a common problem in control theory, where
the dynamics model is often a simplification of the actual system. The ability
to adapt to changing conditions is a key feature of a successful autonomous
system. Machine Learning (ML) models become a popular choice for data-
augmenting a physics-based dynamics model [1, 2, 3, 4].

We present an approach to control a quadrotor using a first principles model
augmented with an Recursive Gaussian Process (RGP) [5, 6] model that serves to
describe the quadrotor’s air drag characteristics. The augmented model is used
in a Model Predictive Control (MPC) framework to allow for precise trajectory
tracking accounting for the quadrotor’s air drag without assuming a priori
knowledge of the air drag form or coefficients. The ML augmented models
typically require a training dataset to be collected in a separate run which
is then used to fit the ML model [4, 7]. In contrast, we fit the RGP model
online at each time step in a recursive fashion. This allows us to adapt to
changing environmental conditions without the need to collect a new training
dataset. inline]Add more references to RGP inline]Add more references for GP
augmentation

inline, color=black!40]Model precitive control (MPC) is a control strategy
that uses a model of the system to predict the future states of the system to
minimize a cost function along a control input trajectory. MPC is
computationally expensive compared to classical control schemes like PID and
requires a precise model of the system to make accurate predictions. inline,
color=black!40]MPC was historically used primarily in the process/chemical
industry where the system dynamics are slow enough to make the MPC
computations feasible in real time. However, with the advent of faster
computers and automatic differential tools like CasADi [16], MPC has become
more popular in the field of robotics.

The rest of the paper is organized as follows. Section II describes the first
principles quadrotor model and assumptions about its drag characteristics. In
Section III we give a high level overview of the proposed approach, seen
through the lens of data-augmented-physics-based modeling. Section IV
describes in detail the proposed method for observing the drag, using those
observations to train the RGP regression, augmenting the first principles
model with the RGP, and using this augmented model in the MPC framework.
Section V describes the simulation environment used for evaluation, the
specificities of the conjunction of the MPC with the RGP, and the results.
Finally, Section VI summarises both the limitations and the possible future
research paths of the proposed approach.

Figure 1: Quadrotor schematic. Thrust $T_{i}$ is generated by individual
rotors given inputs $u_{i}$ The body frame
$\left\\{x_{B},y_{B},z_{B}\right\\}$ is attached to the center of mass of the
quadrotor. The world frame $\left\\{x_{W},y_{W},z_{W}\right\\}$ is attached to
the inertial frame.

##  II PHYSICS-BASED QUADROTOR MODELLING

###  II-A Notation

To describe the quadrotor, we need to consider 2 reference frames. The world
reference frame $W$ is defined by the orthonormal basis
$\left\\{\bm{x}_{B},\bm{y}_{B},\bm{z}_{B}\right\\}$ and the body reference
frame $\left\\{B\right\\}$ by the basis vectors
$\left\\{\bm{x}_{W},\bm{y}_{W},\bm{z}_{W}\right\\}$. The origin of
$\left\\{B\right\\}$ is located in the center of mass of the quadrotor. We
denote the vector from coordinate system $\left\\{W\right\\}$ to coordinate
system $\left\\{B\right\\}$, as seen from the perspective of reference frame
$\\{C\\}$, as $\bm{v}^{C}_{WB}$. In this paper, we only work with the world
frame $\left\\{W\right\\}$ and the body frame $\left\\{B\right\\}$, so we omit
the subscript ”WB” and write $\bm{v}^{B}$ as a shorthand for $\bm{v}^{B}_{WB}$
and $\bm{v}^{W}$ as a shorthand for $\bm{v}^{W}_{WB}$. The quaternion–vector
product is denoted by $\odot$, representing the rotation of $\bm{v}$ by
$\bm{q}$ is defined as $\bm{q}\odot\bm{v}=\bm{q}\bm{v}\bm{q}^{*}$, where
$\bm{q}^{*}$ is the quaternion conjugate of $\bm{q}$. The cross–product of
vectors $\mathbf{a},\mathbf{b}$ is denoted as $\mathbf{a}\times\mathbf{b}$.

###  II-B Quadrotor Model

inline]Fix the drone tikz in fig 1 inline]Change the referential frame labels
in Figure 1 to the current ref frame notation

We assume a quadrotor can be described as a rigid body with 6-DoF in free
space. The state of the quadrotor $\bm{x}$ is chosen as the quadrotor current
position $\bm{p}^{W}=\left[x,y,z\right]$, rotation quaternion111To describe
rotation we use a quaternion describing the rotation of frame
$\left\\{B\right\\}$ relative to frame $\left\\{W\right\\}$. Rotation of a
object in $\mathbb{R}^{3}$ is described fully by 3 angles (such as using Euler
angle representation), but the 4-dimensional quaternion with length 1 allows
us to avoid the gimbal lock problem present in other representations.
$\bm{q}^{W}=\left(q_{w},q_{x},q_{y},q_{z}\right)$, velocity
$\bm{v}^{W}=\left[v_{x},v_{y},v_{z}\right]$ and angular rate
$\bm{\omega}^{W}=\left[\dot{\varphi},\dot{\theta},\dot{\psi}\right]$.

The quadrotor’s rotors are collocated on the $xy$-plane of the $\\{B\\}$ frame
going through the center of mass of the quadrotor. The rotors are numbered $0$
to $3$ in a clockwise direction when seen from the positive $\bm{z}_{B}$
direction. Furthermore, the $\bm{x}_{B}$ axis is pointing forward, in between
the zeroth and the third rotor as seen in Fig. 1. inline]Describe the rotation
between the $x_{B}$ and the rotors better

The model is controlled by an input vector
$\bm{u}=\left[u_{1},u_{2},u_{3},u_{4}\right]$ where
$u_{i}\in\left[0,1\right]\text{ for }i=0,1,2,3$. The input $\bm{u}$
corresponds to the activation of each individual rotor. Each rotor provides a
thrust $T_{i}=T_{\text{max}}\cdot u_{i}$. The collective thrust vector
$\bm{T}^{B}$ and the torque vector $\bm{\tau}^{B}$, both expressed in the body
frame $\left\\{B\right\\}$ and constituted by the individual thrusts $T_{i}$,
are given by

$\displaystyle\bm{T}^{B}=\begin{bmatrix}0\\\ 0\\\
\sum_{i}T_{i}\end{bmatrix}\;\text{and}\;\bm{\tau}^{B}=\begin{bmatrix}d_{y}(-T_{0}-T_{1}+T_{2}+T_{3})\\\
d_{x}(-T_{0}+T_{1}+T_{2}-T_{3})\\\ c_{\tau}(-T_{0}+T_{1}-T_{2}+T_{3})\\\
\end{bmatrix}.$ (1)

We model the nominal quadrotor dynamics using the following ordinary
differential equations (ODEs) [8]

$\displaystyle\begin{split}\dot{\bm{p}}^{W}&=\bm{v}^{W}\\\
\dot{\bm{q}}^{W}&=\bm{q}^{W}\cdot\begin{bmatrix}0\\\
\bm{\omega}_{B}/2\end{bmatrix}\\\
\dot{\bm{v}}^{W}&=\bm{q}^{W}\odot\left(\frac{1}{m}\bm{T}_{B}\right)+\bm{g}^{W}\\\
\dot{\bm{\omega}}^{W}&=\bm{J}^{-1}(\bm{\tau}_{B}-\bm{\omega}_{B}\times\bm{J}\bm{\omega}_{B}).\end{split}$
(2)

inline]Is $\dot{\bm{\omega}}^{W}$ really relative to $\left\\{W\right\\}$? I
dont really understand the
$\dot{\bm{\omega}}^{W}=\bm{J}^{-1}(\bm{\tau}_{B}-\bm{\omega}_{B}\times\bm{J}\bm{\omega}_{B})$
equation. The parameters of the model are the mass $m$, moment of inertia
$\bm{J}\in\mathrm{R}^{3\times 3}$, the rotor displacements $d_{x},d_{y}$, the
rotor drag constant $c_{\tau}$ and the gravitational acceleration vector
$\bm{g}_{W}=[0,0,-9.81]^{T}ms^{-2}$.

Together, these ODEs describe the physical model of the quadrotor dynamics
using a 13-dimensional state vector $\bm{x}$ as

$\displaystyle\dot{\bm{x}}=\bm{f}_{\text{PHYS}}=\left[\begin{smallmatrix}\bm{v}_{WB}\\\
\bm{q}_{WB}\cdot\left[\begin{smallmatrix}0\\\
\bm{\omega}_{B}/2\end{smallmatrix}\right]\\\
\bm{q}_{WB}\odot\left(\frac{1}{m}\bm{T}_{B}\right)+\bm{g}_{W}\\\
\bm{J}^{-1}(\bm{\tau}_{B}-\bm{\omega}_{B}\times\bm{J}\bm{\omega}_{B})\end{smallmatrix}\right].$
(3)

###  II-C Drag Modelling

Drag is a force acting on a body counter to its motion in a fluid. One source
of drag is solid’s body drag, which is proportional to the velocity of the
body and the fluid density. The body drag force acting on the quadrotor body
is described as [9]

$\displaystyle\bm{F}^{B}_{bd}=-\frac{1}{2}\rho|\bm{v}^{B}|^{2}C_{D}A\cdot\bm{v}^{B}$
(4)

where $\rho$ is the fluid density, $C_{D}$ is the drag coefficient and $A$ is
the cross-sectional area of the body. The drag force is applied to the body in
the opposite direction of the velocity vector $\bm{v}^{B}$.

Another source of drag affecting the quadrotor is the rotor drag affecting
each rotor which can be modelled using

$\displaystyle\bm{F}^{B}_{rd}=-\omega
C_{rD}\cdot{\left(\bm{v}^{B}\right)}^{\perp}$ (5)

where $\omega$ is the angular velocity of the rotor, $C_{rD}$ is the drag
coefficient of the rotor and $\left(\bm{v}^{B}\right)^{\perp}$ is the
orthogonal projection of the quadrotor $\left\\{B\right\\}$ velocity onto the
$xy$-plane.

Since we do not have direct measurement of the drag force, we will use the
measured acceleration as a surrogate and work in the acceleration domain for
the rest of the paper.

##  III Motivation, Related Work, Goal of the Paper

The drag model parameters, such as the coefficients, fluid density, or the
quadrotor-related areas, and their spatial and time variability, are,
unfortunately, known with a very limited accuracy. To improve the accuracy,
the model of the total drag acceleration or its parameters should be
identified from measured data, while the physical model (3) is respected.
This, recently developed concept, is referred to as the data-augmented
physics-based (DAPB) modelling [10].

In [4, 7], the total acceleration error made up by both drag forces222Since
the relationship between acceleration and force is given by Newton’s second
law, it is not possible to directly disentangle the effects of the body and
rotor drag from each other. (4), (5), was modelled using an offline trained
Gaussian process (GP) which was then used as an extension to the physical
model for a trajectory control. The approach leads to significant improvement
of the resulting position accuracy assuming no time or spatial variability of
the drag-related acceleration, which might not be always fulfilled.

The goal of this paper is to further extend and simplify the applicability of
the DAPB quadrotor modelling by online identification of the drag-related
acceleration. In particular, we

  * •

Use recursive GP for online drag modelling,

  * •

Design a MPC algorithm capable of working with inherently time-varying DAPB
models.

We illustrate the viability of the proposed method using a realistic scenarios
generated Gazebo simulator [11] and publicly available source code333Source
code available at: https://github.com/smidmatej/mpc_quad_ros.

inline, color=green!40]I believe we would be able to estimate the local wind
speed using the GP model if we had access to both ground speed measurement
(GPS/IMU) and airspeed measurement (pitot tube).

inline, color=green!40]True aerodynamics effects acting on the quad are body
drag, rotor drag and turbulence. RGP allows for a far more complicated mapping
between the input and output than a simple polynomial (not sure if that added
complexity is worth it though).

##  IV DATA-AUGMENTED QUADROTOR MODEL AND PREDICTIVE CONTROL

###  IV-A Drag Estimation

To model the total drag force given by (4), (5), we estimate the drag
acceleration $\tilde{\bm{a}}^{B}$ acting on the quadrotor in the body frame
$\left\\{B\right\\}$ at time $t_{k}$ as

$\displaystyle\tilde{\bm{a}}^{B}_{k}=\frac{\bm{v}^{B}_{k+1}-\hat{\bm{v}}^{B}_{k+1}}{\Delta
t_{k}}$ (6)

where $\hat{\bm{v}}^{B}_{k+1}$ is the velocity predicted using the nominal
model $\bm{f}_{\text{PHYS}}$, $\bm{v}^{B}_{k}$ is the measured velocity,
$\Delta t_{k}=t_{k}-t_{k-1}$, and using the notational shorthand
$\bm{v}^{B}_{k}=\bm{v}^{B}(t_{k})$. The estimated drag acceleration
$\tilde{\bm{a}}^{B}_{k}$, together with the current velocity $\bm{v}^{B}_{k}$
then form an observation pair

$\displaystyle\bm{o}_{k}=\left\\{\bm{v}^{B}_{k},\tilde{\bm{a}}^{B}_{k}\right\\},$
(7)

that is used in the regression procedure described below. inline]Refer to the
RGP in a more elegant way

###  IV-B Drag Modelling by Recursive Gaussian Process

The GP [12] is a non-parametric method that generalizes Gaussian probability
distributions. Instead of describing the probability over a scalar (or vector)
space, stochastic processes (of which GPs are a part of) describe probability
over a function space. GPs do not draw a function as-is, instead we can only
evaluate the function at a finite number of points. Every finite collection of
these function values has a multivariate normal distribution. The GP is the
joint distribution of all of those function values, and is thus a distribution
over functions with continuous domain.

The standard ”offline” GP require access to a training dataset of
$n_{\text{obs}}$ observations
$\bm{o}_{k}=\left\\{\bm{v}^{B}_{k},\tilde{\bm{a}}^{B}_{k}\right\\}$ forming
the dataset
$\mathcal{D}=\left\\{\bm{o}_{k}\;|\;k=0,\ldots,n_{\text{obs}}\right\\}$
gathered from the quadrotor during flight. To reduce the complexity of the GP
we split the observations into its $d\in\\{x,y,z\\}$ components, use a
separate GP for each component and then aggregate their outputs into an
ensemble. The observations $\bm{o}_{k}$ forming the dataset $\mathcal{D}$ is
generated from the noisy process

$\displaystyle\tilde{a}^{B}_{d,k}=g_{d}(v^{B}_{d,k})+\epsilon_{d,k},$ (8)

where $g_{d}$ is the true, unmodelled drag acceleration for dimension $d$ and
$\epsilon_{d,k}\sim\mathcal{N}(0,\sigma_{d}^{2})$ is the noise in the
measurement. The GP is used to infer the latent function $g_{d}$ from
$\mathcal{D}$.

The recursive Gaussian process regression [5] is a method for online learning
of Gaussian processes. Standard GP regression requires the entire training
dataset $\mathcal{D}$ to be available before the model can be learned using
hyperparameter optimization. In contrast to this, RGP allows for updating the
GP model as new observations $\bm{o}_{k}$ become available.

The RGP for each dimension $d\in\\{x,y,z\\}$ is initialized with a set of
basis vectors located at
$\textbf{V}^{+}_{d}\triangleq[v^{+}_{d,1},v^{+}_{d,2},\ldots,v^{+}_{d,m}]$
with the random variable
$\tilde{\textbf{A}}^{+}_{d}\triangleq\left[\tilde{a}^{+}_{d,1},\tilde{a}^{+}_{d,2},\ldots,\tilde{a}^{+}_{d,m}\right]$,
corresponding to output of $g_{d}$ given a velocity $v^{+}_{d,k}$. These basis
vectors can be thought of as an active set allowing a sparse GP
representation. This alleviates the computational requirements immensely,
since $m\ll n_{\text{obs}}$. The initial distribution of the RGP is given by

$\displaystyle
p_{0}(\tilde{\textbf{A}}^{+}_{d})=\mathcal{N}(\tilde{\textbf{A}}^{+}_{d};\bm{\mu}_{d}^{+,0},\bm{C}_{d}^{+,0}),$
(9)

where $\bm{\mu}_{d}^{+,0}\triangleq\bm{0}$ is the mean at $\textbf{V}^{+}_{d}$
and $\bm{C}_{d}^{+,0}\triangleq
K(\textbf{V}^{+}_{d},\textbf{V}^{+}_{d})$444$K(x,x^{\prime})=\sigma_{f}^{2}\exp\left(-\frac{1}{2}\frac{(x-x^{\prime})^{2}}{l}\right)+\sigma^{2}_{n}$
is the squared exponential kernel. When applied to vector arguments, the
kernel $K$ is applied element-wise to each pair of elements in the arguments,
yielding a matrix. The hyperparameters
$\eta=\left(l,\sigma_{f},\sigma_{n}\right)$ are the only hyperparameters of
the GP/RGP regression. is the covariance at $\textbf{V}^{+}_{d}$.

The goal is then to use the new observations $\bm{o}_{k}$ as they become
available to calculate the posterior distribution
$p(\tilde{\textbf{A}}^{+}_{d}|\bm{o}_{1:k})$, where
$\bm{o}_{1:k}\triangleq\\{\bm{o}_{1},\bm{o}_{2},\ldots,\bm{o}_{k}\\}$, from
the prior distribution

$\displaystyle
p(\tilde{\textbf{A}}^{+}_{d}|\bm{o}_{1:k-1})\sim\mathcal{N}(\bm{\mu}_{d}^{+,k-1},\bm{C}_{d}^{+,k-1}).$
(10)

At any point in time, we can run inference to find the distribution of
$\tilde{a}^{\bullet}_{d}=g_{d}(v^{\bullet}_{d})$, i.e., of the function
$g_{d}$ at an arbitrary point $v^{\bullet}_{d}$, in our case corresponding to
the drag acceleration given an instantaneous velocity, by evaluating the joint
prior $p(\tilde{\textbf{A}}^{+}_{d},\tilde{a}^{\bullet}_{d}|\bm{o}_{1:k})$ and
marginalizing out the basis vectors $\tilde{\textbf{A}}^{+}_{d}$ such as

$\displaystyle\begin{aligned}
p(\tilde{\textbf{A}}^{+}_{d},\tilde{a}^{\bullet}_{d}|\bm{o}_{1:k})&=p(\tilde{a}^{\bullet}_{d}|\tilde{\textbf{A}}^{+}_{d})\cdot
p(\tilde{\textbf{A}}^{+}_{d}|\bm{o}_{1:k})\\\
&=\mathcal{N}(\tilde{a}^{\bullet}_{d};\mu^{\bullet,k}_{d},\bm{C}^{\bullet,k}_{d})\cdot\mathcal{N}(\tilde{\textbf{A}}^{+}_{d};\mu^{+,k}_{d},\bm{C}^{+,k}_{d}),\end{aligned}$
(11)

where

$\displaystyle\mu^{\bullet,k}_{d}\triangleq\bm{H}\cdot\bm{\mu}_{d}^{+,k},$
(12) $\displaystyle\bm{C}^{\bullet,k}_{d}\triangleq
K(v^{\bullet}_{d},v^{\bullet}_{d})-\bm{H}\cdot
K(v^{\bullet}_{d},\textbf{V}^{+}_{d}),$ (13) $\displaystyle\bm{H}\triangleq
K(v^{\bullet}_{d},\textbf{V}^{+}_{d})\cdot
K(\textbf{V}^{+}_{d},\textbf{V}^{+}_{d})^{-1}.$ (14)

###  IV-C Model Learning, Utilisation, and Properties

We use the RGP model to compensate the discrepancy between our nominal model
prediction and the measured state of the quadrotor characterised by the drag-
related acceleration (6).

For each RGP in the ensemble, the basis vectors $\textbf{V}^{+}_{d}$ are
initialized by equidistantly sampling the velocity space on
$\left[-v_{max},v_{max}\right]$ using $m$ points, setting their corresponding
estimates of $g_{d}$ to $0$ and their covariance to
$\bm{C}^{+,0}_{d}=K(\textbf{V}^{+}_{d},\textbf{V}^{+}_{d})$.

At each time step $k$, we use the current quadrotor state $\bm{x}_{k}$ with
the RGP model $\bm{f}_{\text{DATA}}$ to infer the acceleration
$\tilde{\bm{a}}$ at the current velocity $\bm{v}_{k}$ using the mean of the
marginalized joint distribution in (11) as

$\displaystyle\bm{f}_{\text{DATA}}(\bm{x}_{k};\bm{\mu}^{\bullet,k})=\begin{bmatrix}\bm{0}^{7\times
1}\\\ \tilde{a}_{x}\\\ \tilde{a}_{y}\\\ \tilde{a}_{z}\\\ \bm{0}^{3\times
1}.\end{bmatrix}=\begin{bmatrix}\bm{0}^{7\times 1}\\\
\mu^{\bullet,k}_{x}\left(v_{x,k};\;\bm{\mu}_{d}^{+,k}\right)\\\
\mu^{\bullet,k}_{y}\left(v_{y,k};\;\bm{\mu}_{y}^{+,k}\right)\\\
\mu^{\bullet,k}_{z}\left(v_{z,k};\;\bm{\mu}_{z}^{+,k}\right)\\\
\bm{0}^{3\times 1}.\end{bmatrix},$ (15)

where
$\bm{\mu}^{\bullet,k}\triangleq\left(\mu^{\bullet,k}_{x},\mu^{\bullet,k}_{y},\mu^{\bullet,k}_{z}\right)$.
To perform the online update of the RGP distribution
$p(\tilde{\textbf{A}}^{+}_{d}|\bm{o}_{1:k})$ we keep the covariance matrices
$\bm{C}_{d}^{+,k-1}$ and the mean $\bm{\mu}_{d}^{+,k-1}$ of the prior
distribution $p(\tilde{\textbf{A}}^{+}_{d}|\bm{o}_{1:k-1})$. However, to run
inference using the RGP, only the means $\bm{\mu}_{d}^{+,k}$ are needed, while
$\bm{C}_{d}^{+,k-1}$ is kept in memory only to perform updates.

###  IV-D Data-Augmented Physics-Based Model

To better model quadrotor dynamics, the physics-based model
$\bm{f}_{\text{PHYS}}$ (3) and the online trained RGP model
$\bm{f}_{\text{DATA}}$(15) can be combined into the single augmented model

$\displaystyle\bm{f}_{\text{DAPB}}\left(\bm{x}_{k},\bm{u}_{k}\right)=\bm{f}_{\text{PHYS}}\left(\bm{x}_{k},\bm{u}_{k}\right)+\bm{f}_{\text{DATA}}\left(\bm{x}_{k};\bm{\mu}^{+,k}\right),$
(16)

which provides more accurate description of the underlying reality.

###  IV-E Model Predictive Control for Augmented Model

The model predictive control [13] is a control scheme used to stabilize a
system subject to dynamic equations
$\dot{\bm{x}}=\bm{f}_{\text{DAPB}}(\bm{x},\bm{u})$ given a reference
trajectory $\bm{x}_{\text{ref}}(t),\bm{u}_{\text{ref}}(t)$ by minimizing a
cost function
$\mathcal{L}(\bm{x},\bm{x}_{\text{ref}},\bm{u},\bm{u}_{\text{ref}})$. MPC
solves the optimal control problem (OCP) defined as

$\displaystyle\begin{split}&\underset{\bm{u}}{\text{min}}\int\mathcal{L}(\bm{x},\bm{x}_{\text{ref}},\bm{u},\bm{u}_{\text{ref}})\\\
\text{subject to
}&\dot{\bm{x}}=\bm{f}_{\text{DAPB}}(\bm{x},\bm{u}),\quad\bm{x}(t_{0})=\bm{x}_{0},\\\
&\bm{r}(\bm{x},\bm{u})=0,\quad\quad\quad\bm{h}(\bm{x},\bm{u})\leq
0,\end{split}$ (17)

where $\bm{f}_{\text{DAPB}}$ denotes the known model of the system,
$\bm{x}_{\text{ref}}$ is the tracked trajectory, $\bm{x}_{0}$ is the initial
state, $\bm{r}$ describes the equality constraints and $\bm{h}$ describes the
inequality constraints. The inclusion of $\bm{r}$ and $\bm{h}$ is one of the
main advantages of MPC, allowing for solutions satisfying specific
constraints. For example, input $\bm{u}$ saturation or staying withing a
specific region of state space using $\bm{h}$.

Because of the changing nature of the $\bm{f}_{\text{DATA}}$ model due to
online learning, it would be computationally inefficient to reconstruct the
OCP at each control time step. Therefore, we propose the following
simplification.

We treat the vector $\bm{\mu}^{+,k}$ as a parameter of the
$\bm{f}_{\text{DATA}}$ model. In this way, we change the parameters of the
nonlinear optimization problem at each control time step $k$ as the RGP
ensemble is updated. The process of computing the control using MPC and
updating its RGP model is shown in Algorithm 1. We refer to this approach as
RGP$-$MPC and as GP$-$MPC when a pre-trained GP is used.

Algorithm 1 Recursive Learning and Control Algorithm Summary

1:initialize $\bm{f}_{\text{DATA}}$ with $\textbf{V}^{+}_{d}$ and
$\tilde{\textbf{A}}^{+}_{d}$

2:initialize MPC with $\bm{f}_{\text{DAPB}}$

3:$\hat{\bm{x}}_{0}\leftarrow$ initial state

4:$k\leftarrow 0$

5:while trajectory ready do

6: procedure RGP$-$MPC STEP

7: $\bm{x}_{k}\leftarrow$ state measurement

8: $\bm{u}_{k,0:0+n_{h}}\leftarrow$ MPC optimal control

9: send control command $\bm{u}_{k,0}$

10:
$\hat{\bm{x}}_{k+1}\leftarrow\bm{f}_{\text{PHYS}}\left(\bm{x}_{k},\bm{u}_{k,0}\right)$

11:
$\tilde{\bm{a}}^{B}_{k}\leftarrow\frac{\bm{v}^{B}_{k}-\hat{\bm{v}}^{B}_{k}}{\Delta
t_{k}}$

12:
$\bm{o}_{k}\leftarrow\left\\{\bm{v}^{B}_{k},\tilde{\bm{a}}^{B}_{k}\right\\}$

13: $p(\tilde{\textbf{A}}^{+}_{d}|\bm{o}_{1:k})\leftarrow$ update with
$\bm{o}_{k}$ from prior

14: $\bm{\mu}^{+,k}\leftarrow$ marginalize
$p(\tilde{\textbf{A}}^{+}_{d},\tilde{\bm{a}}^{B}_{k}|\bm{o}_{1:k})$

15: update $\bm{f}_{\text{DAPB}}$ with $\bm{\mu}^{+,k}$ inside MPC

16: $k\leftarrow k+1$

17: end procedure

18:end while

##  V SIMULATION EXPERIMENTS

The proposed data-augmented modelling and and predictive control of quadrotor
is numerically evaluated on two trajectories using a realistic quadrotor model
provided by the Gazebo simulator. Details on trajectory generation, MPC
implementation, and the results follows. A simplified simulation environment
is also provided as described in V-E.

###  V-A Trajectory Generation

As a tracking reference we generate trajectories illustrated in Fig. 2 in
$\left\\{W\right\\}$ frame using a trajectory
generator555https://github.com/whoenig/uav_trajectories [14]. We start by
randomly generating waypoints in a $3$-dimensional cube of size
$\text{hsize}=10$\mathrm{m}$$ raised above the $xy$ ground plane by hsize.
Then we provide these waypoints to the genTrajectory bash script together with
the maximum allowed velocity $v_{max}$ and acceleration $a_{max}$. The script
then generates a trajectory that satisfies the velocity and acceleration
constraints as a polynomial which we then sample with the desired control
frequency $f_{s}=\frac{1}{\Delta t}$.

((a)) Trajectory created by interpolating between randomly generated waypoints
with a polynomial

((b)) Circular trajectory with radius $r=10m$ with velocity increasing
linearly from $0$ to $v_{\text{max}}$

Figure 2: Reference trajectories used for the experiments.

inline]The view is different inline, color=yellow]Add lemniscate trajectory?

###  V-B Nonlinear Model Predictive Control Implementation

The MPC algorithm is implemented using the Python interface to acados [15], a
solver for nonlinear optimal control problems for embedded applications. The
system model $\bm{f}_{\text{DAPB}}$ is implemented as a symbolic CasADi[16]
function which is used to construct the OCP for the MPC, for which acados
generates a solver.

The MPC is parametrized with a desired horizon look–forward time $t_{h}$ and
the number of prediction steps $n_{h}$. The length of one prediction step is
thus given by $T_{h}=\frac{t_{h}}{n_{h}}$.

To solve the MPC problem at each time step, we use the Sequential quadratic
programming real-time iteration (SQP$-$RTI) solver type [15]. This solver
attempts to solve the optimization problem iteratively, but instead of
iterating until a desired optimality condition is satisfied, it performs only
a single iteration and then returns the current solution. This non-optimal
solution is then used instead of the optimal quadrotor control. At the next
time step, the system state is updated and the solver begins to solve the
optimization problem again, but this time using the previous solution as an
initial guess. The SQP$-$RTI solver scheme allows us to control the quadrotor
in real time, but it is not guaranteed to find the optimal solution,
especially during the first few control loops.

###  V-C Gazebo Simulation

Using the RotorS [8] package for the Gazebo simulator [11] we simulate the
quadrotor in a high fidelity environment. RotorS provides a AscTec Hummingbird
quadrotor model together with an interface using which we supply the control
inputs $\bm{u}_{k}$.

Gazebo provides the quadrotor odometry to our MPC controller node with a
frequency of $100$\mathrm{H}\mathrm{z}$$. The controller node then extracts
the state $\bm{x}_{k}$ from the odometry and runs the procedure RGP$-$MPC STEP
in Alg. 1 to obtain the control action $\bm{u}_{k}$. The control action is
then sent to the quadrotor model in Gazebo, which then updates the quadrotor
state. The procedure is repeated until the trajectory is completed.

###  V-D Experimental Setup

The experiments were performed on the circle trajectory shown in Fig. 2(b) and
on a randomly pre-generated trajectories shown in Fig. 2(a). We set the
parameters of the MPC control algorithm as follows, horizon length
$t_{h}=1$\mathrm{s}$$, $n_{h}=5$ and control interval of $\Delta
t=0.01$\mathrm{s}$$. Both the GP and the RGP models used $m=20$ inducing
points.

The GP in the GP$-$MPC approach was pre-trained by selecting the $m$ most
informative inducing points from a dataset generated in a training run on the
random trajectory using Gaussian mixture modelling (GMM). The hyperparameters
of the GP $\eta$ were then optimized using the maximum likelihood estimation
method [17]. Since the GP$-$MPC approach is trained on the random trajectory
it is expected that it will achieve greater reduction of the error for the
random trajectory than RGP$-$MPC.

The RGP in the RGP$-$MPC approach was not pre-trained before performing the
trajectory, and was trained online only. Its $m$ basis vectors were uniformly
chosen on $[-v_{\text{max}},v_{\text{max}}]$ with corresponding initial drag
acceleration estimates being $0$ as shown in Fig. 4. Since the RGP does not
perform hyperparameter estimation during learning, its hyperparameters were
set at the start of the trajectory as $\eta=\left(1.0,0.1,0.1\right)$. The
need to set hyperparameters beforehand limits the RGP$-$MPC approach, as
discussed further.

###  V-E Simplified Simulation

A simplified simulation environment implemented in Python with the drag model
as described in (4) is included in the provided source code. This environment
can be used to test the correspondence between the known drag equation and the
fitted RGP$-$MPC relation. Due to space constraints, it is, however, not
evaluated in this article.

###  V-F Results

Having description of the simulation environment, the simulation results of
the experiments performed in Gazebo are discussed further.

|  | RMSE pos [mm]
---|---|---
Trajectory |  $v_{\text{max}}$ [ms-1] | Nominal | GP$-$MPC | RGP$-$MPC
Random | 3 | 75.9 | 30.9 (41%) | 40.6 (53%)
6 | 110.1 | 52.9 (48%) | 65.1 (59%)
9 | 128.5 | 70.3 (55%) | 88.2 (69%)
12 | 142.9 | 81.9 (57%) | 99.6 (69%)
Circle | 3 | 57.5 | 22.5 (39%) | 23.6 (41%)
6 | 102.7 | 50.5 (49%) | 43.5 (42%)
9 | 145.1 | 88.1 (61%) | 69.7 (47%)
12 | 183.9 | 128.4 (70%) | 98.2 (53%)
| avg. opt. dt [ms] | 0.60 | 0.66 | 1.21

TABLE I: RMSE position error for the circle trajectory generated using Gazebo
with both $\bm{f}_{\text{PHYS}}$ and $\bm{f}_{\text{DAPB}}$. Both the
pretrained GP and the RGP augmented controllers have consistently lower RMSE
position error at the cost of increased optimization time.

The results shown in Table I indicate that both the GP$-$MPC and RGP$-$MPC
approaches significantly reduce the RMSE position tracking error in comparison
to the nominal, unaugmented physics-based $\bm{f}_{\text{PHYS}}$ MPC.
Performance of MPCs with the augmented models is rather comparable, but the
proposed RGP$-$MPC do not rely on any pre-training. On the other hand, both
GP$-$MPC and RGP$-$MPC increase the optimization time needed to calculate the
next MPC output666Simulations performed on Ubuntu 20.04 with an Intel Core
i5-8300H CPU and 8GB of RAM.. However, this increase is more significant for
RGP$-$MPC, since it performs additional computations to update the RGP and
then to re-parameterize the OCP.

Fig. 3 shows the absolute value of covariance between the velocity $v_{d}$ and
the position tracking error $e_{d}$ for each $d\in\\{x,y,z\\}$ based on the
peak velocity during the simulated trajectory $v_{\text{peak}}$. It can be
seen, that the RGP$-$MPC approach reduces this value w.r.t the nominal
approach due to the fact that using both $\bm{f}_{\text{PHYS}}$ and
$\bm{f}_{\text{DATA}}$ leads to improved forecasting than using
$\bm{f}_{\text{PHYS}}$ only.

Figure 3: Absolute values of covariance between the peak tracking velocity
$v_{\text{peak}}$ and position tracking error $e_{x,y,z}$ for the circle
trajectory captured using the Gazebo simulation.

Fig. 4 visualises the RGP inference distribution at the start (upper plot) and
the end (bottom plot) of a simulation. The training of the RGP is done in real
time is able to fit a distribution to the observations $\bm{o}_{k}$ online at
the same time as being used to make real-time predictions.

Figure 4: RGP inference space at the start (top) and at the end (bottom) of
the trajectory using the Gazebo simulator. Since the Gazebo simulator only
models rotor drag (5), the RGP predicts much lower drag in the $z_{B}$
direction.

inline, color=yellow]Use a better name for the 2xstd. Bigger Sample marker
size.

##  VI CONCLUSIONS AND FUTURE WORK

###  VI-A Observations, Notes, and Future Work

Since we are using estimated disturbance acceleration $\tilde{\bm{a}}^{B}_{k}$
calculated in the $\left\\{B\right\\}$ frame of the quadrotor to fit
$\bm{f}_{\text{DATA}}$, we only have a limited ability to distinguish
disturbances that are not time-invariant with respect to $\left\\{B\right\\}$.
For example, even for a constant wind in $\left\\{W\right\\}$, the disturbance
acceleration $\tilde{\bm{a}}^{B}_{k}$ will be time-varying with respect to
$\left\\{B\right\\}$ due to the rotation of the quadrotor. The presence of
such seemingly time-varying disturbances can induce adaptation that causes
increasing $\tilde{\bm{a}}^{B}_{k}$, which might potentially lead to control
loop instability.

In the current implementation, we do not perform any hyperparameter
optimization in the RGP$-$MPC approach. We found the hyperparameter estimation
method[5] augmenting the RGP state by the hyperparameters $\eta$ and
estimating them using the Kalman filter to be too unstable to be used safely.
Thus, the suitable hyperparameters was selected a priori. On-line adaptation
can further increase the ability of the proposed RGP$-$MPC method.

We use the estimated disturbance acceleration (6) caused by the drag to fit
the RGP model without any pre-processing. This can lead to unstable control
behaviour when the real quadrotor state differs from the predicted state due
to faulty or rare-normal behaviour. A possible solution can be based on
gathering data in a training run and then selects the data points valid for
“nominal” conditions to fit the GP model GP$-$MPC[4].

###  VI-B Conclusions

This paper dealt with the predictive control of a quadrotor described by the
data-augmented physics-based model. The data-driven part of the model si
realised by a Recursive Gaussian Process regression model that gives an
estimate of the air drag force learned online without the need to gather data
in a training run. During each control interval, we calculate the residual air
drag acceleration as the difference between the commanded and measured
velocities divided by the time interval. This residual, together with the
current velocity, is used to update the RGP model. The updated RGP model is
then used by the MPC to predict the future position of the quadrotor, where
the drag force impact is inherently respected. We have shown in realistic
simulations that the proposed method is able to quickly adapt to never-before-
seen velocities and achieves better position tracking performance than the MPC
with the non-augmented (physics-based) model.

inline, color=yellow]The control loop is also able to run in real time with
very fast optimization time.

inline]THIS PAPER MUST HAVE SIX (6) PAGES.

## References

  * [1] A. Carron, E. Arcari, M. Wermelinger, L. Hewing, M. Hutter, and M. N. Zeilinger, “Data-driven Model Predictive Control for trajectory tracking with a robotic arm,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3758–3765, 2019.
  * [2] J. Kabzan, L. Hewing, A. Liniger, and M. N. Zeilinger, “Learning-based Model Predictive Control for autonomous racing,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3363–3370, 2019.
  * [3] L. Hewing, J. Kabzan, and M. N. Zeilinger, “Cautious Model Predictive Control using Gaussian Process Regression,” _IEEE Transactions on Control Systems Technology_ , vol. 28, no. 6, pp. 2736–2743, 2019.
  * [4] G. Torrente, E. Kaufmann, P. Foehn, and D. Scaramuzza, “Data-Driven MPC for Quadrotors,” _IEEE Robotics and Automation Letters_ , 2021.
  * [5] M. F. Huber, “Recursive Gaussian process: On-line regression and learning,” _Pattern Recognition Letters_ , vol. 45, pp. 85–91, 2014.
  * [6] N. Wahlström and E. Özkan, “Extended target tracking using Gaussian Processes,” _IEEE Transactions on Signal Processing_ , vol. 63, no. 16, pp. 4165–4178, 2015.
  * [7] M. Mehndiratta and E. Kayacan, “Gaussian Process-based Learning Control of Aerial Robots for Precise Visualization of Geological Outcrops,” in _2020 European Control Conference (ECC)_ , 2020, pp. 10–16.
  * [8] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart, _Robot Operating System (ROS): The Complete Reference (Volume 1)_. Cham: Springer International Publishing, 2016, ch. RotorS—A Modular Gazebo MAV Simulator Framework, pp. 595–625. [Online]. Available: http://dx.doi.org/10.1007/978-3-319-26054-9_23
  * [9] G. R. Center. (2022) Drag equation. [Online]. Available: https://www1.grc.nasa.gov/beginners-guide-to-aeronautics/drag-equation/
  * [10] T. Imbiriba, A. Demirkaya, J. Duník, O. Straka, D. Erdoğmuş, and P. Closas, “Hybrid Neural Network Augmented Physics-based Models for Nonlinear Filtering,” in _2022 25th International Conference on Information Fusion (FUSION)_ , 2022, pp. 1–6.
  * [11] “Design and use paradigms for Gazebo, an open-source multi-robot simulator,” in _2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(IEEE Cat. No. 04CH37566)_ , vol. 3. IEEE, pp. 2149–2154.
  * [12] C. Rasmussen and C. Williams, _Gaussian Processes for Machine Learning_ , ser. Adaptive Computation and Machine Learning series. MIT Press, 2005. [Online]. Available: https://books.google.fr/books?id=Tr34DwAAQBAJ
  * [13] L. Magni, D. M. Raimondo, and F. Allgöwer, “Nonlinear Model Predictive Control,” _Lecture Notes in Control and Information Sciences_ , vol. 384, 2009.
  * [14] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in _Robotics Research_. Springer, 2016, pp. 649–666.
  * [15] R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. v. Duijkeren, A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, and M. Diehl, “acados—a modular open-source framework for fast embedded optimal control,” _Mathematical Programming Computation_ , vol. 14, no. 1, pp. 147–183, 2022.
  * [16] J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi: a software framework for nonlinear optimization and optimal control,” _Mathematical Programming Computation_ , vol. 11, pp. 1–36, 2019.
  * [17] L. Ljung, _System Identification: Theory for the User_. Pearson Education, 1998. [Online]. Available: https://books.google.cz/books?id=fYSrk4wDKPsC