Model Predictive Control for Trajectory

Tracking of Unmanned Aerial Vehicles

Using Robot Operating System

Mina Kamel, Thomas Stastny, Kostas Alexis and Roland Siegwart

Abstract In this chapter, strategies for Model Predictive Control (MPC) design and
implementation for Unmaned Aerial Vehicles (UAVs) are discussed. This chapter is
divided into two main sections. In the first section, modelling, controller design and
implementation of MPC for multi-rotor systems is presented. In the second section,
we show modelling and controller design techniques for fixed-wing UAVs. System
identification techniques are used to derive an estimate of the system model, while
state of the art solvers are employed to solve the optimization problem online. By
the end of this chapter, the reader should be able to implement an MPC to achieve
trajectory tracking for both multi-rotor systems and fixed-wing UAVs.

1 Introduction

Aerial robots are gaining great attention recently as they have many advantages
over ground robots to execute inspection, search and rescue, surveillance and goods
delivery tasks. Depending on the task required to be executed, a multi-rotor system
or fixed-wing aircraft might be a more suitable choice. For instance, a fixed-wing
aircraft is more suitable for surveillance and large-scale mapping tasks thanks to
their long endurance capability and higher speed compared to a multi-rotor system,
while for an inspection task that requires flying close to structures to obtain detailed
footage a multi-rotor UAV is more appropriate.

M. Kamel (B) · T. Stastny · R. Siegwart

Autonomous System Lab, ETH Zurich, Zurich, Switzerland
e-mail: [email protected]
T. Stastny
e-mail: [email protected]
R. Siegwart
e-mail: [email protected]
K. Alexis
University of Nevada, Reno, NV, USA
e-mail: [email protected]

© Springer International Publishing AG 2017 3

A. Koubaa (ed.), Robot Operating System (ROS), Studies in Computational
Intelligence 707, DOI 10.1007/978-3-319-54927-9_1
4 M. Kamel et al.

Precise trajectory tracking is a demanding feature for aerial robots in general in

order to successfully perform required tasks, especially when operating in realistic
environments where external disturbances may heavily affect the flight performance
and when flying in the vicinity of structure. In this chapter, several model predictive
control strategies for trajectory tracking are presented for multi-rotor systems as well
as for fixed-wing aircraft. The general control structure followed by this chapter is a
cascade control approach, where a reliable and system-specific low-level controller
is present as inner loop, and a model-based trajectory tracking controller is running
as an outer loop. This approach is motivated by the fact that many critical flight
software is running on a separate navigation hardware which is typically based on
micro-controllers, such as Pixhawk PX4 and Navio [1, 2] while high level tasks are
running on more powerful -but less reliable- on-board computers. This introduces a
separation layer to keep critical tasks running despite any failure in the more complex
high-level computer.
By the end of this chapter, the reader should be able to implement and test various
Model Predictive Control strategies for aerial robots trajectory tracking, and integrate
these controllers into the Robot Operating System (ROS) [3]. Various implementation
hints and practical suggestions are provided in this chapter and we show several
experimental results to evaluate the proposed control algorithms on real systems.
In Sect. 2 the general theory behind MPC is presented, with focus on linear MPC
and robust linear MPC. In Sect. 3 we present the multi-rotor system model and
give hints on model identification approaches. Moreover, linear and robust MPC
are presented and we show how to integrate these controller into ROS and present
experimental validation results. In Sect. 4 we present a Nonlinear MPC approach for
lateral-directional position control of fixed-wing aircraft with implementation hints
and validation experiments.

2 Background

2.1 Concepts of Receding Horizon Control

Receding Horizon Control (RHC) corresponds to the historic evolution in control

theory that aimed to attack the known challenges of fixed horizon control. Fixed
horizon optimization computes a sequence of control actions {u 0 , u 1 , . . . , u N −1 }
over a horizon N and is characterized by two main drawbacks, namely: (a) when
an unexpected (unknown during the control design phase) disturbance takes place
or when the model employed for control synthesis behaves different than the actual
system, then the controller has no way to account for that over the computed control
sequence, and (b) as one approaches the final control steps (over the computer fixed
horizon) the control law “gives up trying” since there is too little time left in the fixed
horizon to go to achieve a significant objective function reduction. To address these
limitations, RHC proposed the alternative strategy of computing the full control
Model Predictive Control for Trajectory Tracking … 5

sequence, applying only the first step of it and then repeating the whole process
iteratively (receding horizon fashion). RHC strategies are in general applicable to
nonlinear dynamics of the form (considering that state update is available):

ẋ = f(x, u) (1)

where the vector field f : Rn × Rm , x ∈ Rn×1 represents the state vector, and u ∈
Rm×1 the input vector. The general state feedback-based RHC optimization problem
takes the following form:

N −1
ref
min F(xt+N ) + ||xt+k − xt+k || + ||ut+k ||
z
k=0
s.t. xt+k+1 = f (xt+k , ut+k ) (2)
ut+k ∈ UC
xt+k ∈ XC
xt = x(t)

where z = {ut , ut+1 , . . . , ut+N −1 } is the optimization variables, denotes some

(penalized) metric used for per-stage weighting, F(xt+N ) represents the terminal
ref
state weighting, xt+k is the reference signal, the subscript t + k is used to denote the
sample (using a fixed sampling time Ts ) of a signal at k steps ahead of the current
time t, while t + k + 1 indicates the next evolution of that, UC represents the set of
input constraints, XC the state constraints and x(t) is the value of the state vector at
the beginning of the current RHC iteration. The solution of this optimization problem
leads again to an optimal control sequence {ut , ut+1 , . . . , ut+N −1 } but only the first
step of that ut is applied while the whole process is then repeated iteratively.
Within this formulation, the term F(xt+N ) has a critical role for the closed-loop
stability. In particular, it forces the system state to take values within a particular set
at the end of the prediction horizon. It is relatively easy to prove stability per local
iteration using Lyapunov analysis. In its simplest case, this essentially means that
ref
considering the regulation problem (xt+k = 0 for k = 0, . . . , N − 1), and a “decres-
cent” metric , then the solution of the above optimization problem makes the system
stable at xt = 0, ut = 0 – that is that a terminal constraint xt+k = 0 is introduced (a
simplified illustration is provided in Fig. 1). However, the question of global stability

Fig. 1 Illustration of the terminal constraint set (Ω)

6 M. Kamel et al.

is in general not guaranteed. For that one has to consider the problem of introducing
both a terminal cost and a terminal constraint for the states [4]. However, general
constrained optimization problems can be extremely difficult to solve, and simply
adding terminal constraints may not be feasible. Note that in many practical cases,
the terminal constraint is not enforced during the control design procedure, but rather
verified a posteriori (by increasing the prediction horizon if not satisfied).
Furthermore, one of the most challenging properties of RHC is that of recur-
sive feasibility. Unfortunately, although absolutely recommended from a theoretical
standpoint, it is not always possible to construct a RHC that has a-priori guarantee
of recursive feasibility, either due to theoretical or practical implications. In general,
a RHC strategy lacks recursive feasibility –and is therefore invalidated– even when
it is possible to find a state which is feasible, but where the optimal control action
moves the state vector to a point where the RHC optimization problem is infeasible.
Although a general feasibility analysis methodology is very challenging, for spe-
cific cases powerful tools exist. In particular, for the case of linear systems then the
Farkas’ Lemma [5] in combination with bilevel programming can be used to search
for problematic initial states which lack recursive feasibility – thus invalidating an
RHC strategy.

2.2 Linear Model Predictive Control

In this subsection we briefly present the theory behind MPC for linear systems. We
formulate the optimal control problem for linear systems with linear constraints in the
input and state variables. Moreover, we discuss the control input properties, stability
and feasibility in the case of linear and quadratic cost function. To achieve offset
free tracking under model mismatch, we adopt the approach described in [6] where
the system model is augmented with additional disturbances state d(t) to capture
the model mismatch. An observer is employed to estimate disturbances in steady
state. The observer design and the disturbance model will be briefly discussed in this
subsection.
min J0 x0 , U, Xr e f , Ur e f
U
subject to xk+1 = Axk + Buk + Bd dk ;
dk+1 = dk , k = 0, . . . , N − 1 (3)
xk ∈ XC , uk ∈ UC
x N ∈ XC N
x0 = x (t0 ) , d0 = d (t0 ) .

The optimal control problem to achieve offset-free state tracking under linear
state and and input constraints is shown in (3), where J0 is the cost function, Xr e f =
ref ref
{x0 , . . . , x N } is the reference state sequence, U = {u0 , . . . , u N −1 } and Ur e f =
ref ref
{u0 , . . . , u N −1 } are respectively the control input sequence and the steady state
input sequence, Bd is the disturbance model and dk is the external disturbances, XC ,
Model Predictive Control for Trajectory Tracking … 7

UC and XC N are polyhedra. The choice of the disturbance model is not a trivial
task, and depends on the system under consideration and the type of disturbances
expected. The optimization problem is defined as

N −1
ref ref
J0 x0 , U, X , U
ref ref
= (xk − xk )T Qx (xk − xk )+
k=0
ref ref
(uk − uk )T Ru (uk − uk )+ (4)

(uk − uk−1 )T RΔ (uk − uk−1 ) +
ref ref
(x N − x N )T P(x N − x N ),

where Qx 0 is the penalty on the state error, Ru 0 is the penalty on control input
error, RΔ 0 is a penalty on the control change rate and P is the terminal state error
penalty.
In general, stability and feasibility of receding horizon problems are not ensured
except for particular cases such as infinite horizon control problems as in Linear
Quadratic Regulator (LQR) case. When the prediction horizon is limited to N steps,
the stability and feasibility guarantees are disputable. In principle, longer prediction
horizon tends to improve stability and feasibility properties of the controller, but the
computation effort will increase, and for aerial robot application, fast control action
needs to be computed on limited computation power platforms. However, the terminal
cost P and terminal constraint XC N can be chosen such that closed-loop stability and
feasibility are ensured [6]. In this chapter we focus more on the choice of terminal
weight P as it is easy to compute, while the terminal constraint is generally more
difficult and practically stability is achieved with long enough prediction horizon.
Note that in our cost function (4), we penalize the control input rate Δuk . This
ensures smooth control input and avoids undesired oscillations. In the cost func-
tion (4), u−1 is the actual control input applied on the system in the previous time
step.
As previously mentioned, offset-free reference tracking can be achieved by
augmenting the system model with disturbances dk to capture the modeling error.
Assuming that we want to track the system output yk = Cxk and achieve steady state
offset free tracking y∞ = r∞ . A simple observer that can estimate such disturbance
can be achieved as follows

x̂k+1 A Bd x̂k B Lx
= + uk + Cx̂k − ym,k (5)
d̂k+1 0 I d̂k 0 Ld

where x̂ and d̂ are the estimated state and external disturbances, Lx and Ld are the
observer gains and ym,k is the measured output at time k.
Under the assumption of stable observer, it is possible to compute the MPC state
at steady state xr e f and control input at steady state ur e f by solving the following
system of linear equations:
8 M. Kamel et al.

A−I B xr e f,k −Bd d̂k
= (6)
C 0 ur e f,k rk

2.3 Nonlinear Model Predictive Control

Aerial vehicles behavior is better described by a set of nonlinear differential equations

to capture the aerodynamic and coupling effects. Therefore in this subsection we
present the theory behind Nonlinear MPC that exploits the full system dynamics,
and generally achieve better performance when it comes to aggressive trajectory
tracking. The optimization problem for nonlinear MPC is formulated in Eq. (7).
T

min
h (x(t), u(t)) − yr e f (t)
2 dt +
m (x(T )) − yr e f (T )
2
U Q P
t=0
subject to ẋ = f(x(t), u(t));
(7)
u(t) ∈ UC
x(t) ∈ XC
x(0) = x (t0 ) .

A direct multiple shooting technique is used to solve the Optimal Control Problem
(OCP) (7). In this approach the system dynamics
are discretized over a time grid
t0 , . . . , t N within the time intervals tk , tk+1 . The inequality constraints and control
action are discretized over the same time grid. A Boundary Value Problem (BVP)
is solved for each interval and additional continuity constraints are imposed. Due
to the nature of the system dynamics and the imposed constraints, the optimization
problem becomes a Nonlinear Program (NLP). This NLP is solved using Sequential
Quadratic Programming (SQP) technique where the Quadratic Programs (QPs) are
solved by active set method using the qpOASES solver [7].
Note that, in case of infeasibility of the underlying QP, 1 penalized slack variables
are introduced to relax all constraints.
The controller is implemented in a receding horizon fashion where only the first
computed control action is applied to the system, and the rest of the predicted state
and control trajectory is used as initial guess for the OCP to solve in the next iteration.

2.4 Linear Robust Model Predictive Control

Despite the robustness properties of the nominal MPC formulation, specific robust
control variations exist when further robustness guarantees are required. The problem
of linear Robust Model Predictive Control (RMPC) may be formulated as a Mini-
max optimization problem that is solved explicitly. As an optimality metric we may
Model Predictive Control for Trajectory Tracking … 9

Objective Function
Multiparametric
Optimizer

- Derivation of Convex Optimization Problem

State Space
representation
Predictions
Feedback

using the concate-

Relaxations -
nated vectors over
the prediction Explicit
horizon Piecewise Affine
form

Constraints Robustification
Extended
Sequential Table
State and Input Constraints Traversal

Fig. 2 Overview of the explicit RMPC optimization problem functional components

select the Minimum Peak Performance Measure (MPPM) for its known robustness
properties. Figure 2 outlines the relevant building blocks [8].
Within this RMPC approach, the following linear time invariant representation of
the system dynamics may be considered:

xk+1 = Axk + Buk + Gwk (8)

yk+1 = Cxk

where xk ∈ X, uk ∈ U and the disturbing signals wk are unknown but bounded (wk ∈
W). Within this paper, box-constrained disturbances are considered (W∞ = {w :
||w||∞ ≤ 1}). Consequently, the RMPC problem will be formulated for the system
representation and additive disturbance presented above. Let the following denote
the concatenated versions of the predicted output, states, inputs and disturbances,
where [k + i|k] marks the values profile at time k + i, from time k.

Y = yk|k
T T
yk+1|k . . . yk+N
T
−1|k (9)

X = xk|k
T T
xk+1|k . . . xk+N
T
−1|k (10)

U = uk|k
T T
uk+1|k ... T
uk+N −1|k (11)

W = wk|k
T T
wk+1|k ... T
wk+N −1|k (12)
10 M. Kamel et al.

where X ∈ X N = X × X · · · × X, U ∈ U N = U × U · · · × U, W ∈ W N = W ×
W × · · · × W. The predicted states and outputs present linear dependency on the
current state, the future control input and the disturbance, and thus the following
holds:
X = Axk|k + BU + GW (13)
Y = CX

where A, B , C , G are the stacked state vector matrices as in [8]. Subsequently, the
RMPC problem based on the MPPM (MPPM–RMPC) may be formulated as:

min max ||Y ||∞ , ||Y ||∞ = max ||yk+ j|k ||∞ (14)
u w j
s.t. uk+ j|k ∈ U, ∀ w ∈ W
xk+ j|k ∈ X, ∀ w ∈ W
wk+ j|k ∈ W

2.4.1 Feedback Predictions

Following the aforementioned formulation, the optimization problem will tend to

become conservative as the optimization essentially computes an open-loop control
sequence. Feedback predictions is a method to encode the knowledge that a receding
horizon approach is followed. Towards their incorporation, a type of feedback control
structure has to be assumed. Among the feedback parametrizations that are known
to lead to a convex problem space, the following is selected [9, 10]:

U = LW + V , V = υk|k T υT
k+1|k · · · υk+N −1|k
T
(15)
⎛ ⎞
0 0 0 ··· 0
⎜ L 10 0 0 ··· 0⎟
⎜ ⎟
⎜ L 20 L 0 · · · 0 ⎟
TL = ⎜ 21 ⎟ (16)
⎜ .. .. .. . . ⎟
⎝ . . . . . .
.⎠
L (N −1)0 L (N −1)1 · · · L (N −1)(N −2) 0

Employing this feedback predictions parameterization, the control sequence is now

parameterized directly in the uncertainty, and the matrix L describes how the control
action uses the disturbance vector. Inserting this parametrization yields the following
representation, where V becomes now the RMPC-manipulated action:

X = Axk|k + BV + (G + BL)W (17)

U = LW + V (18)

and the mapping from L, V to X , U is now bilinear. This allows the formulation of
the minimax MPC as a convex optimization problem [9]. Furthermore, let:
Model Predictive Control for Trajectory Tracking … 11

Fu = f uT f uT · · · f uT (19)

F x = f xT f xT · · · f xT (20)

denote the concatenated –over the prediction horizon– versions of the input and
state constraints f u and f x . More specifically, f u = [ f u (1)max , f u (1)min . . .] and
f x = [ f x (1)max , f x (1)min . . .] where f u (i)max , f u (i)min represent the maximum and
minimum allowed input values of the i-th input, while f x ( j)max , f x ( j)min represent
the maximum and minimum acceptable/safe state configurations of the j-th state.

min τ
V ,L,τ

s.t. ||C (A xk|k + BV + (G + BL)W )||∞ ≤ τ, ∀ W ∈ W N (21)

Eu (V + LW ) ≤ Fu , ∀ W ∈ W N
E x (A xk|k + BV + (G + BL)W ) ≤ Fx , ∀ W ∈ W N
Eu = diag N Eu , E x = diag N Ex , τ > 0

within which: (a) Ex , Eu are matrices that allow the formulation of the state and
input constraints in Linear Matrix Inequality (LMI) form, (b) diag N i is a block
diagonal matrix with i being the matrix that fills each diagonal block and allows
the incorporation of the state and input constraints. Within this formulation τ is
defined as a positive scalar value that bounds (from above) the objective function.
The peak constraint may be equivalently reformulated as:

C (A xk|k + BV ) + C (G + BL)W ≤ τ 1, ∀ W ∈ W N (22)

−C (A xk|k + BV ) − C (G + BL)W ≤ τ 1, ∀ W ∈ W N (23)

where 1 is a vector of ones (1 1 · · · 1)T with suitable dimensions. Satisfaction of

these uncertain inequalities is based on robust optimization methods.

2.4.2 Robust Uncertain Inequalities Satisfaction

Since box-constrained disturbances (w ∈ W∞ ) are assumed, the following holds:

max c T x = ||c||1 = |c T |1 (24)

|x|≤1

This equation holds as max|x|≤1 c T x = max|x|≤1 ci xi = ci sign(ci ) = ||c||1 .
Consequently, the uncertain constraints with w ∈ W∞ are satisfied as long as [9]:

C (A xk|k + BV ) + |C (G + BL)|1 ≤ τ 1 (25)

−C (A xk|k + BV ) + |C (G + BL)|1 ≤ τ 1 (26)
12 M. Kamel et al.

To handle these constraints in a linear programming fashion [5], the term |C (G +

BL)| is bounded from above by introducing a matrix variable Γ 0:

C (G + BL) ≤ Γ (27)
−C (G + BL) ≤ Γ (28)

and the peak constraint is guaranteed as long as:

C (A xk|k + BV ) + Γ 1 ≤ τ 1 (29)
−C (A xk|k + BV ) + Γ 1 ≤ τ 1 (30)

2.4.3 Robust State and Input Constraints

To robustly satisfy hard constraints on the input and the states along the prediction
horizon, a new matrix Ω 0 is introduced and the constraints are reformulated as:

E x (A xk|k + BV ) Fx
+ Ω1 ≤ (31)
Eu V Fu

E x (G + BL)
≤Ω (32)
Eu L
E (G + BL)
− x ≤Ω (33)
Eu L

Optimizing the control sequence, while robustly satisfying the state and input con-
straints is of essential importance for the flight control of micro aerial vehicles.

2.4.4 Minimum Peak Performance Robust MPC Formulation

Based on the aforementioned derivations, the total MPPM–RMPC formulation

is solved subject to element-wise bounded disturbances and feedback predictions
through the following linear programming problem:

min τ (34)
V ,L,τ,Ω,Γ
s.t. C (A xk|k + BV ) + Γ 1 ≤ τ 1
−C (A xk|k + BV ) + Γ 1 ≤ τ 1
C (G + BL) ≤ Γ
−C (G + BL) ≤ Γ
E x (A xk|k + BV ) (35)

E x (A xk|k + BV ) Fx
+ Ω1 ≤
Eu V Fu

E x (G + BL)
≤Ω
Eu L
Model Predictive Control for Trajectory Tracking … 13

E x (G + BL)
− ≤Ω
Eu L

2.4.5 Multiparametric Explicit Solution

The presented RMPC strategy requires the solution of a linear programming problem.
However, a multiparametric-explicit solution is possible due to the fact that the
control action takes the general form [6]:

uk = Fr xk + Zr , if xk ∈ Π r (36)

where Π i , r = 1, . . . , N r are the regions of the receding horizon controller. The r -th
control law is valid if the state vector xk is contained in a convex polyhedral region
Π r = {xk |Hr xk ≤ Kr } computed and described in h-representation [11]. Such a fact
enables fast real-time execution even in microcontrollers with very limited computing
power. In this framework, the real-time code described in [8] is employed.

3 Model-Based Trajectory Tracking Controller for

Multi-rotor System

In this section, we present a simplified model of multi-rotor system that can be used
for model-based control to achieve trajectory tracking, and we present a linear and
nonlinear model predictive controller for trajectory tracking.

3.1 Multirotor System Model

The 6DoF pose of the multi-rotor system can be defined by assigning a fixed inertial
frame W and body frame B attached to the vehicle as shown in Fig. 3. We denote by
p the position of the origin of frame B in frame W expressed in frame W, by R the
rotation matrix of frame B in frame W expressed in frame W. Moreover, we denote
by φ, θ and ψ the roll, pitch and yaw angles of the vehicle. In this model we assume
a low level attitude controller that is able to track desired roll and pitch φd , θd angles
with a first order behavior. The first order inner-loop approximation provides suffi-
cient information to the MPC to take into account the low level controller behavior.
The inner-loop first order parameters can be identified through classic system identi-
fication techniques. The non-linear model used for trajectory tracking of multi-rotor
system is shown in Eq. (37).
14 M. Kamel et al.

Fig. 3 Illustration of the Firefly hexacopter from Ascending Technologies with attached body fixed
frame B and inertial frame W

ṗ(t) = v(t)
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
0 0 Ax 0 0
v̇(t) = R (ψ, θ, φ) ⎝ 0 ⎠ + ⎝ 0 ⎠ − ⎝ 0 A y 0 ⎠ v(t) + d(t)
T −g 0 0 Az
(37)
1
φ̇(t) = K φ φd (t) − φ(t)
τφ
1
θ̇ (t) = (K θ θd (t) − θ (t))
τθ

where v indicates the vehicle velocity, g is the gravitational acceleration, T is the

mass normalized thrust, A x , A y , A z indicate the mass normalized drag coefficients,
d is external disturbance. τφ , K φ and τθ , K θ are the time constant and gain of inner-
loop behavior for roll angle and pitch angle respectively.
The cascade controller structure assumed in this chapter is shown in Fig. 4.

3.2 Linear MPC

In this subsection we show how to formulate a linear MPC to achieve trajectory

tracking for multi-rotor system and integrate it into ROS. The optimization problem
presented in Eq. (3) is solved by generating a C-code solver using the CVXGEN
framework [12]. CVXGEN generates a high speed solver for convex optimization
problems by exploiting the problem structure. For clarity purposes, we rewrite the
optimization problem here and show how to generate a custom solver using CVX-
GEN. The optimization problem is given by
Model Predictive Control for Trajectory Tracking … 15

Fig. 4 Controller scheme for multi-rotor system. n 1 . . . n m are the i − th rotor speed and y is the
measured vehicle state

N −1
ref ref ref ref
min (xk − xk )T Qx (xk − xk ) + (uk − uk )T Ru (uk − uk ) + (uk − uk−1 )T RΔ (uk − uk−1 )
U
k=0
ref ref
+ (x N − x N )T P(x N − x N )
subject to xk+1 = Axk + Buk + Bd dk ;
dk+1 = dk , k = 0, . . . , N − 1
uk ∈ UC
x0 = x (t0 ) , d0 = d (t0 ) .
(38)
To generate a solver for the aforementioned optimization problem, the following
problem description is used in CVXGEN.
1 dimensions
2 m = 3 # dimension of i n p u t s .
3 nd = 3 # dimension of d i s t u r b a n c e s .
4 nx = 8 # dimension of s t a t e vector .
5 T = 18 # horizon − 1.
6 end
7

8 parameters
9 A ( nx , nx ) # dynamics m a t r i x .
10 B ( nx ,m) # t r a n s f e r matrix .
11 Bd ( nx , nd ) # disturbance t r a n s f e r matrix
12 Q_x ( nx , nx ) psd # s t a t e c o s t , p o s i t i v e s e m i d i f i n e d .
13 P ( nx , nx ) psd # f i n a l s t a t e p e n a l t y , p o s i t i v e s e m i d i f i n e d .
14 R_u (m,m) psd # i n p u t p e n a l t y , p o s i t i v e s e m i d i f i n e d .
15 R _ d e l t a (m,m) psd # d e l t a i n p u t p e n a l t y , p o s i t i v e s e m i d i f i n e d .
16 x [ 0 ] ( nx ) # initial state .
17 d ( nd ) # disturbances .
18 u_prev (m) # p r e v i o u s i n p u t a p p l i e d t o t h e system .
19 u_max (m) # input amplitude l i m i t .
20 u_min (m) # input amplitude l i m i t .
21 x _ s s [ t ] ( nx ) , t = 0 . . T+1 # reference state .
22 u _ s s [ t ] (m) , t = 0 . . T # reference input .
23 end
16 M. Kamel et al.

24

25 variables
26 x [ t ] ( nx ) , t = 1 . . T+1 # state .
27 u [ t ] (m) , t = 0 . . T # input .
28 end
29

30 minimize
31 quad ( x[0] − x _ s s [ 0 ] , Q_x ) + quad ( u[0] − u _ s s [ 0 ] , R_u ) + quad ( u [ 0 ] −
u_prev , R _ d e l t a ) + sum [ t = 1 . . T ] ( quad ( x [ t ]− x _ s s [ t ] , Q_x ) + quad (
u [ t ]− u _ s s [ t ] , R_u ) + quad ( u [ t ] − u [ t −1] , R _ d e l t a ) ) +quad ( x [ T+1]−
x _ s s [ T+ 1 ] , P )
32 subject to
33 x [ t +1] == A∗x [ t ] + B∗u [ t ] + Bd∗d , t = 0 . . T # dynamics
34 u_min <= u [ t ] <= u_max , t = 0 . . T # input constraint .
35 end

Before we show the integration of the generated solver into ROS, we discuss
how to estimate the attitude loop parameters τφ , K φ , τθ , K θ and the derivation of the
discrete system model A, B, Bd .

3.2.1 Attitude Loop Parameters Identification

The attitude loop identification is a recommended process when no knowledge is

available about the attitude controller used onboard of the vehicle. This is the case
for many commercially available platforms. To perform this system identification,
typically a pilot excites the vehicles axes in free flight. Attitude command and
actual vehicle attitude (estimated using motion capture system if available or Inertial
Measurement Unit (IMU)) are logged with accurate time stamp. Typically two
datasets are collected, one is used for parameters estimation and the other dataset
is used for validation purpose. We will not go into details of system identification,
interested readers can find more details in [13]. A set of scripts for attitude dynamics
identification will be made open source upon acceptance of the chapter.
To perform parameters identification using the available scripts please follow the
following steps:
1. Prepare the system and make sure you are able to log time stamped attitude
commands and actual vehicle attitude.
2. Perform a flight logging the commands and vehicle attitude in a bag file. During
the flight excite as much as possible each axis of the vehicle.
3. Repeat the flight test to collect validation dataset.
4. Set the correct bag files name and topics name in the provided script and run it.
5. The controller parameters will be displayed on screen with validation percentage
to confirm the validity of the identification.
Model Predictive Control for Trajectory Tracking … 17

3.2.2 Linearization, Decoupling and Discretization

For controller design, the vehicle dynamics can be approximated around hovering
condition where small attitude angles are assumed and vehicle heading aligned with
inertial frame x axis (i.e. ψ = 0). The linearized system around hovering condition
can be written as

⎛ ⎞
⎛ ⎞ 0 0 0 ⎛ ⎞
0 0
0 1 0 0 0 0 ⎜ 0 0 0⎟ 000
⎜0 0
0 0 1 0 0 0 ⎟⎟ ⎜ ⎟ ⎜0 0 0⎟
⎜ ⎜ 0 0 0⎟ ⎜ ⎟
⎜0 0
0 0 0 1 0 0 ⎟ ⎜ ⎟ ⎜0 0 0⎟
⎜ ⎟ ⎜ 0 0 0⎟ ⎜ ⎟
⎜0 0 −A x 0
0 0 g 0 ⎟ ⎜ ⎟ ⎜1 0 0 ⎟
⎜ ⎟ ⎜ 0 0 0⎟ ⎜ ⎟
ẋ(t) = ⎜0 0 0 −A y 0
0 0 −g ⎟ x(t) + ⎜ ⎟ u(t) + ⎜0 1 0⎟ d(t),
⎜ ⎟ ⎜ 0 0 0⎟ ⎜ ⎟
⎜0 0
0 0 0 −A z 0 0 ⎟ ⎜ ⎟ ⎜0 0 1⎟
⎜ ⎟ ⎜ 0 0 1⎟ ⎜ ⎟
⎜0 1
0 − τφ 0 ⎠⎟ ⎜ ⎟ ⎝0 0 0⎠
⎝ 0
0 0 0 ⎜ Kφ 0 0⎟
1 ⎝ τφ ⎠
000 0 0 0 0 − τθ Kθ
000
0 τθ 0
Ac Bd,c
Bc
(39)
where the state vector is x=(pT , v T ,W φ,W θ )T , the input vector u = (W φd ,W θd , T )T
and the disturbance vector d = (dx , d y , dz )T . The c subscription indicates that this
is a continuous time model. Note that in this linearization we marked the attitude to
be in inertial frame W to get rid of the yaw angle ψ from the model. The attitude
control action W φd ,W θd is computed in inertial frame and then converted to body
frame by performing a rotation around z axis. The control action in the vehicle body
frame B is given by
W
φd cos ψ sin ψ φd
= . (40)
θd − sin ψ cos ψ W
θd

After the computation of the control input, it is recommended to add a feed-forward

term to compensate for coupling effects and to achieve better tracking performance.
To compensate for vehicle non-zero attitude effects on thrust, and to achieve better
tracking of dynamic trajectory, the following compensation scheme is employed.

T +g
T̃ = +B z̈ d
cos φ cos θ
gφd −B ÿd
φ̃d = (41)
T̃
gθd +B ẍd
θ̃d =
T̃

where B ẍd ,B ÿd ,B z̈ d are the desired trajectory acceleration expressed in vehicle body
frame and quantities with tilde sign are the actual applied control input.
18 M. Kamel et al.

Given that the controller is implemented in discrete time, it is necessary to dis-

cretize the system dynamics (39). This can be done as follows

A = eAc Ts
Ts
B= eAc τ dτ Bc
0
(42)
Ts
Bd = eAc τ dτ Bd,c
0

where Ts is the sampling time. The computation of the terminal cost P matrix is done
by solving the Algebraic Riccati Equation iteratively.

3.2.3 ROS Integration

The strategy followed for the ROS integration of the solver is to create a ROS node to
interface the controller to ROS environment, while the controller, estimator and other
components are implemented as C++ shared libraries that get linked to the node at
compilation time. The controller node expects the vehicle state as a nav odometry
message and publishes a custom message of type RollPitchYawRateThrust
command message. The desired trajectory can be sent to the controller over a topic of
type MultiDOFJointTrajectory or as single desired point over a topic of type
PoseStamped. The advantage of passing the whole desired trajectory over single
point is that the MPC can take into consideration the future desired trajectory and
react accordingly. Figure 5 gives an overview of nodes and topics communication
through a ros graph diagram.

Fig. 5 ROS graph diagram showing various nodes and topics to control multi-rotor system
Model Predictive Control for Trajectory Tracking … 19

Each time the controller receives a new odometry message, a control action is
computed and published. Therefore it is important to guarantee that the state estimator
is publishing an odometry message at the desired rate of control. We recommend to
use this controller with the Modular Framework for MultiSensor Fusion [14]. An
important point to consider when implementing such a controller is to reduce as
much as possible the delays in the loop. A hint to minimize communication delay
is to use the ROS transportation hints by passing ros::TransportHints().
tcpNoDelay() flag to the odometry subscriber.
The controller node publishes the following information:
1. Current desired reference as tf.

Fig. 6 Through dynamic reconfiguration it is possible to change the controller parameters online
for tuning purposes
20 M. Kamel et al.

2. Desired trajectory as rviz marker.

3. Predicted vehicle state as rviz marker.
The controller parameters can be easily changed in run time from the dynamic recon-
figure as shown in Fig. 6.
An open source implementation of the presented controller can be found in https://
github.com/ethz-asl/mav_control_rw

3D trajectory tracking with linear MPC Position tracking performance

3
vehicle trajectory x
reference trajectory y
z
2 x ref
y ref
zref
1

position [m]
1 0
-3.5
0.8
-3
z [m]

0.6 -1
-2.5
0.4
-2 y [m]
0.2 -2
-1.5
0
3.5 3 2.5 -1
2 1.5 1 -3
0.5 0 -0.5 0 5 10 15 20 25 30 35
x [m] -1
time [s]
(a) Aggressive trajectory tracking using
linear MPC.3D trajectory plot. (b) Position tracking per formance
using linear MPC

Velocity tracking performance

2.5
vx
2 vy
vz
v x,ref
1.5
v y,ref
v z,ref
1
velocity [m/s]

0.5

-0.5

-1

-1.5

-2

-2.5
0 5 10 15 20 25 30
time [s]
(c) Velocity tracking performance using linear MPC

Fig. 7 Aggressive trajectory tracking performance using linear MPC controller running onboard
of Firefly hexacopter from Ascending Technologies
Model Predictive Control for Trajectory Tracking … 21

3.2.4 Experimental Results

To validate the controller performance, we track an aggressive trajectory. The

controller is running onboard a Firefly hexacopter from Ascending Technologies
(AscTec) with a NUC i7 computer onboard. An external motion capture system
Vicon [15] is used as pose sensor and fused with onboard IMU using the Multisensor
Fusion Framework (MSF). The controller is running at 100 Hz and the prediction
horizon is chosen to be 20 steps.
Figure 7 shows the tracking performance of the controller.

3.3 Nonlinear MPC

In this subsection we use the full vehicle nonlinear model to design a continuous
time nonlinear model predictive controller. The toolkit employed to generate the
nonlinear solver is ACADO [16] which is able to generate very fast custom C code
solvers for general optimal control problems OCP. The optimization problem can be
written as
T
min (x(t) − xr e f (t))T Q x (x(t) − xr e f (t)) + (u(t) − ur e f (t))T Ru (u(t) − ur e f (t))dt
U t=0
+ (x(T ) − xr e f (T ))T P(x(T ) − xr e f (T ))
(43)
subject to ẋ = f(x, u);
u(t) ∈ UC
x(0) = x (t0 ) .

To generate a solver for the aforementioned optimization problem, the following

problem description is used in ACADO’s Matlab interface.
1 clc ;
2 clear all ;
3 close a l l ;
4

5 Ts = 0 . 1 ; %s a m p l i n g t i m e
6 EXPORT = 1 ;
7

8 D i f f e r e n t i a l S t a t e p o s i t i o n ( 3 ) v e l o c i t y ( 3 ) r o l l p i t c h yaw ;
9 Control r o l l _ r e f pitch_ref t h r u s t ;
10

11 %o n l i n e d a t a r e p r e s e n t d a t a t h a t can be p a s s e d t o t h e s o l v e r o n l i n e .
12 OnlineData r o l l _ t a u ;
13 OnlineData r o l l _ g a i n ;
14 OnlineData p i t c h _ t a u ;
15 OnlineData p i t c h _ g a i n ;
16 O n l i n e D a t a yaw_rate_command ;
17 OnlineData d r a g _ c o e f f i c i e n t ( 3 ) ;
22 M. Kamel et al.

18 OnlineData e x t e r n a l _ d i s t u r b a n c e s ( 3 ) ;
19

20 n_XD = l e n g t h ( d i f f S t a t e s ) ;
21 n_U = l e n g t h ( c o n t r o l s ) ;
22

23 g = [0;0;9.8066];
24

25 %% D i f f e r e n t i a l E q u a t i o n
26

27 %d e f i n e v e h i c l e body z−a x i s e x p r e s s e d i n i n e r t i a l frame .

28 z_axis = [ ( cos ( yaw ) ∗ s i n ( p i t c h ) ∗ cos ( r o l l ) + s i n ( yaw ) ∗ s i n ( r o l l ) ) ; . . .
29 ( s i n ( yaw ) ∗ s i n ( p i t c h ) ∗ cos ( r o l l )−cos ( yaw ) ∗ s i n ( r o l l ) ) ; . . .
30 cos ( p i t c h ) ∗ cos ( r o l l ) ] ;
31

32 d r o l l = ( 1 / r o l l _ t a u ) ∗( r o l l _ g a i n ∗ r o l l _ r e f − r o l l ) ;
33 d p i t c h = ( 1 / p i t c h _ t a u ) ∗( p i t c h _ g a i n ∗ p i t c h _ r e f − p i t c h ) ;
34

35 f = d o t ( [ p o s i t i o n , v e l o c i t y ; r o l l ; p i t c h ; yaw ] ) == . . .
36 [ velocity . . . ;
37 z_axis ∗ t h r u s t − g − diag ( d r a g _ c o e f f i c i e n t )∗ v e l o c i t y +
external_disturbances ; . . .
38 droll ; . . .
39 dpitch ; . . .
40 yaw_rate_command ] ;
41

42 h = [ position ; velocity ; r o l l ; pitch ; r o l l _ r e f ; pitch_ref ; z_axis (3) ∗

t h r u s t −g ( 3 ) ] ;
43

44 hN = [ p o s i t i o n ; v e l o c i t y ] ;
45

46

47 %% NMPCexport
48 a c a d o S e t ( ’ problemname ’ , ’ n m p c _ t r a j e c t o r y _ t r a c k i n g ’ ) ;
49

50 N = 20;
51 ocp = acado . OCP( 0 . 0 , N∗Ts , N ) ;
52

53 W_mat = eye ( l e n g t h ( h ) ) ;
54 WN_mat = eye ( l e n g t h (hN) ) ;
55 W = acado . BMatrix ( W_mat ) ;
56 WN = acado . BMatrix (WN_mat) ;
57

58 ocp . minimizeLSQ ( W, h ) ;
59 ocp . minimizeLSQEndTerm ( WN, hN ) ;
60 ocp . s u b j e c t T o (− d e g 2 r a d ( 4 5 ) <= [ r o l l _ r e f ; p i t c h _ r e f ] <= d e g 2 r a d ( 4 5 ) ) ;
61 ocp . s u b j e c t T o ( g ( 3 ) / 2 . 0 <= t h r u s t <= g ( 3 ) ∗ 1 . 5 ) ;
62 ocp . setModel ( f ) ;
63

64 mpc = acado . OCPexport ( ocp ) ;

65 mpc . s e t ( ’HESSIAN_APPROXIMATION ’ , ’GAUSS_NEWTON’ );
66 mpc . s e t ( ’DISCRETIZATION_TYPE ’ , ’MULTIPLE_SHOOTING ’ ) ;
67 mpc . s e t ( ’SPARSE_QP_SOLUTION ’ , ’FULL_CONDENSING_N2 ’ ) ;
Model Predictive Control for Trajectory Tracking … 23

68 mpc . s e t ( ’INTEGRATOR_TYPE ’ , ’INT_IRK_GL4 ’ );

69 mpc . s e t ( ’NUM_INTEGRATOR_STEPS ’ , N );
70 mpc . s e t ( ’QP_SOLVER ’ , ’QP_QPOASES ’ );
71 mpc . s e t ( ’HOTSTART_QP ’ , ’NO’ );
72 mpc . s e t ( ’LEVENBERG_MARQUARDT’ , 1e−10 );
73 mpc . s e t ( ’LINEAR_ALGEBRA_SOLVER ’ , ’GAUSS_LU ’ );
74 mpc . s e t ( ’IMPLICIT_INTEGRATOR_NUM_ITS ’ , 4 );
75 mpc . s e t ( ’CG_USE_OPENMP’ , ’YES ’ );
76 mpc . s e t ( ’CG_HARDCODE_CONSTRAINT_VALUES ’ , ’NO’ );
77 mpc . s e t ( ’CG_USE_VARIABLE_WEIGHTING_MATRIX ’ , ’NO’ );
78

79

80

81 i f EXPORT
82 mpc . exportCode ( ’ m a v _ N M P C _ t r a j e c t o r y _ t r a c k i n g ’ ) ;
83

84 cd m a v _ N M P C _ t r a j e c t o r y _ t r a c k i n g
85 make_acado_solver ( ’ . . / mav_NMPC_trajectory_tracking ’ )
86 cd . .
87 end

3.3.1 ROS Integration

The ROS integration of the nonlinear controller follows the same guidelines of the
linear MPC previously presented. An open source implementation of the previously
presented controller can be found here. https://github.com/ethz-asl/mav_control_rw.

3.3.2 Experimental Results

The same setup used for the linear MPC is used to evaluate the nonlinear MPC. The
only difference is that we are currently evaluating the controller on a more aggressive
trajectory as shown in Fig. 8.

3.3.3 Robust Linear Model Predictive Control for Multirotor System

In order to evaluate the proposed RMPC, a set of test-cases were conducted using the
structurally modified AscTec Hummingbird quadrotor (ASLquad) shown in Fig. 9.
For the implementation of the RMPC, a software framework around ROS was devel-
oped. In particular, a set of low-level drivers and nodes handle the communication
with the attitude and motor control on-board the aerial robot and therefore enabling
us to provide attitude and thrust references through the RMPC. MATLAB was used
to derive and compute the explicity formulation of the RMPC, while the complete
explicit algorithm overviewed in Sect. 2.4.5 was implemented within a SIMULINK
block. Auto-code generation was then employed to extract the C-code equivalent
24 M. Kamel et al.

Position tracking performance Velocity tracking performance

4
2
3

1 2

velocity [m/s]
1
position [m]

0
0

-1 -1
vx
x vy
y -2
z vz
-2 x ref v x,ref
y ref -3 v y,ref
zref v z,ref
-3 -4
0 5 10 15 20 25 0 5 10 15 20 25
time [s] time [s]
(a) Position tracking performance using (b) Velocity tracking performance using
nonlinear MPC nonlinear MPC

(c) Sequence of rviz screen shot showing the reference trajectory in

green and predicted vehicle state in red.

Fig. 8 Aggressive trajectory tracking performance using nonlinear MPC controller running
onboard of Firefly hexacopter from Ascending Technologies

of this controller, which was then wrapped around a ROS node and integrated into
the overall software framework. For the described experiments with the RMPC, full
state feedback is provided through external motion capture, while an alignment step
to account for relative orientation of the on-board attitude and heading estimation
system also takes place.
Below we present the results on (a) trajectory tracking subject to wind distur-
bances, and (b) slung load operations, while further results are available at [8]. For
Model Predictive Control for Trajectory Tracking … 25

Fig. 9 Instance of an RMPC test for slung load operations

the presented experiments, the sampling time was set to Ts = 0.08 s, the predic-
tion horizon was set to N = 6 for both of them, while the following state and input
constraints were considered:

⎡ ⎤ ⎡ ⎤
ẋ 1.5 m/s
⎡ ⎤ ⎡ ⎤
⎢ ẏ ⎥ ⎢ 1.5 m/s ⎥ ẋ 1.5 m/s
⎢ ⎥ ⎢ ⎥
⎢ ż ⎥ ⎢ 1.5 m/s ⎥ ⎢ ẏ ⎥ ⎢ 1.5 m/s ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ θ ⎥ ⎢π/4 rad⎥ ⎢ ż ⎥ ⎢ 1.5 m/s ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ φ ⎥ ⎢π/4 rad⎥ ⎢ φ ⎥ ⎢π/4 rad⎥
⎢ r⎥ ⎢ ⎥ ⎢ r⎥ ⎢ ⎥
⎢ θ
⎢ ⎥ ⎢ ⎥ ⎢ π/4 rad ⎥ ⎢γ ⎥ ⎢ π/6 rad ⎥
⎥ ⎢ ⎥ ⎢ ⎥
I7×7 07×7 ⎢φ r ⎥ ⎢π/4 rad⎥ I6×6 06×6 ⎢φ r ⎥ ⎢π/4 rad⎥
⎢ ⎥≤⎢
07×7 −I7×7 ⎢ ẋ ⎥ ⎢ 1.5 m/s ⎥
⎥, ⎢ ⎥≤⎢ ⎥
06×6 −I6×6 ⎢ ẋ ⎥ ⎢ 1.5 m/s ⎥
(44)
⎢ ẏ ⎥ ⎢ 1.5 m/s ⎥ ⎢ ẏ ⎥ ⎢ 1.5 m/s ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ż ⎥ ⎢1/5 m/s⎥ ⎢ ż ⎥ ⎢1/5 m/s⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ θ ⎥ ⎢π/4 rad⎥ ⎢ φ ⎥ ⎢π/4 rad⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ φ ⎥ ⎢π/4 rad⎥ ⎣γ r ⎦ ⎣π/6 rad⎦
⎢ ⎥ ⎢ ⎥
⎣ θ r ⎦ ⎣π/4 rad⎦ φr π/4 rad
φr π/4 rad

Regarding the first experimental test-case, an 80 W electric fan was pointed to

the ASLquad, while the RMPC was acting in order to track a helical path despite
the turbulent wind disturbance. As can be observed in Fig. 10, the tracking response
remains precise and only a minor influence from the external disturbance is observed.
Note that the box-constraints selection is sufficient to account –up to some extent–
for the kind of disturbance as although the dynamics of the wind disturbance are
unknown to the controller, its effect may be considered as bounded.
Consequently, the capabilities of performing slung load operations were consid-
ered. As shown in Fig. 11 where a forcible external disturbance is also applied onto
the slung load (a hit on the load), highly precise position hold and disturbance rejec-
tion results were achieved. This is despite the fact that the slung load introduces
disturbances that are phase-delayed compared to the vehicle states and the controller
is not augmented regarding its state to incorporate the load’s motion. For this exper-
26 M. Kamel et al.

0.5
0.4
0.3
0.2
0.1
z (m)

0
−0.1
−0.2
−0.3
−0.4
−0.5

0.6
0.4
0.2
0
x (m) −0.2 0.6
−0.4 0.4
−0.6 0.2
−0.8 0
−0.2 y (m)
−1 −0.4
−1.2 −0.6

Fig. 10 Trajectory tracking subject to wind disturbances using the RMPC framework applied to
the ASLquad

iment, a 0.16 kg load was utilized and was attached through a compressible string
with a length of 0.65 m.
Once the capabilities of the proposed RMPC to handle slung load operations
even while subjected to strong disturbances were verified, the problem of trajectory
tracking during slung load operations (i.e. transportation and delivery of goods in a
Search-and-Rescue scenario or as part of a product delivery mission), was examined.
Figure 12 presents the results achieved when continuously tracking a square refer-
ence trajectory for 10 times while the same slung load is attached onto the quadrotor
platform. As shown, efficient and robust results were derived, indicative of the capa-
bilities of the proposed control scheme to effectively execute such operations.
In the last presented trajectory tracking experiments, a slow response was com-
manded. However, more agile maneuvers could also be considered even during slung
load operations. To this purpose, a step reference of 1.25 m was commanded with the
velocity and attitude references being set to zero. The results are shown in Fig. 13
and as depicted, a satisfactory response was derived although some overshoot is
observed.
Model Predictive Control for Trajectory Tracking … 27

0.2
x ,y
r

0.1 x
y
0
−0.1
−0.2
0 1 2 3 4 5 6 7 8 9 10
Time (s)

zr
1.1 z

1
0.9
0.8
0 1 2 3 4 5 6 7 8 9 10
Time (s)
0.5
φr
φ
0

−0.5
0 1 2 3 4 5 6 7 8 9 10
Time (s)

0.1 r
θ
0.05 θ

0
−0.05
−0.1
0 1 2 3 4 5 6 7 8 9 10
Time (s)

Fig. 11 RMPC performance against disturbance of the slung load which induces unpredicted
disturbances on the vehicle. At time t ≈ 3 s a forcible external disturbance (hit) is applied on the
load, however the RMPC manages to quickly and accurately stabilize the vehicle

4 Model-Based Trajectory Tracking Controller for

Fixed-Wing UAVs

In this section, we describe the modeling of simplified fixed-wing aircraft flight

dynamics, closed-loop low-level system identification methodology, and control
objective design necessary for a general-form high-level nonlinear model predictive
trajectory tracking controller. As the vast majority of fixed-wing flight is conducted
at fixed altitudes, we focus our presentation on planar lateral-directional position
control; though, the techniques employed can easily be extended towards three-
dimensional applications, assuming a suitably stabilizing longitudinal low-level con-
troller.

4.1 Fixed-Wing Flight Dynamics and Identification

Lateral-directional dynamics for a fixed-wing system are defined in the intertial frame
I, in local coordinates composed of Northing n̂ and Easting ê components, and body
28 M. Kamel et al.

1.5
1.4
1.3
1.2
1.1
z (m)

1
0.9
0.8
0.7
0.6
0.5

0.5

0.6
x (m) 0 0.4
0.2
0
−0.5 −0.2 y (m)
−0.4
−0.6

Fig. 12 Trajectory tracking during slung load operations using the ASLquad: the controller tracks
the same square trajectory for 10 times with small deviations from the reference despite the signif-
icant and phase-delayed disturbances introduced from the load

frame B. In reality, aerodynamic effects result in an additional “wind” frame, where

the aircraft may slip, causing the airspeed vector v to deviate from the body-x̂-axis.
In our simplified model, we will assume low-level control is designed such that slip
is regulated appropriately, and we will assume wind and body frames are identical.
It is worth also noting our planar assumption further entails the assumption that we
fly at a fixed altitude. The dynamic equation are defined in Eq. (45).

ṅ = V cos ψ + wn
ė = V sin ψ + we
φ
ψ̇ = g tan
V
(45)
φ̇ = p
ṗ = b0 φr − a1 p − a0 φ

where n and e are the Northing and Easting positions, respectively, ψ is the yaw
angle, V is the air-mass-relative airspeed, φ is the roll angle, p is the roll rate, g is the
acceleration of gravity, and wn and we are the Northing and Easting components of
the wind vector w, respectively. Note that roll φ and yaw ψ angles are defined about
the body frame B. This distinction is important when considering flight dynamics
in wind, where the ground-relative flight path of the vehicle is defined as the course
angle χ from North n̂ to the ground speed vector, v g , see Fig. 14. Additionally, note
Model Predictive Control for Trajectory Tracking … 29

0.2 xr
0.1 x
0
−0.1
−0.2
0 1 2 3 4 5 6 7 8 9 10

Time (s)

2 yr
1.5 y
1
0.5
0
0 1 2 3 4 5 6 7 8 9 10
Time (s)

zr
1.1 z
1
0.9
0.8
0 1 2 3 4 5 6 7 8 9 10
Time (s)

0.4
φr
0.2 φ
0
−0.2
−0.4
0 1 2 3 4 5 6 7 8 9 10
Time (s)

0.1
θr
0.05 θ
0
−0.05
−0.1
0 1 2 3 4 5 6 7 8 9 10
Time (s)

Fig. 13 Lateral step response during slung load operations: minimal overshoot is observed

Fig. 14 Fixed-wing
modeling definitions
30 M. Kamel et al.

that we choose a second-order model with no zeros to describe the rolling dynamic
with respect to roll references φr , where a0 , a1 , and b0 are model coefficients. Higher
order dynamics could be used, however we have found through the identification
procedures outlined in Sect. 4.1.1, that second-order fits appropriately model the
closed loop low-level autopilot attitude control response; further, each increase in
order in turn increases the dimension of the control optimization problem, increasing
computational expense.

4.1.1 Model Identificaiton

Here, we will outline the basic methodology for closed loop model identification
on fixed-wing aircraft. Towards these ends, it is assumed a low-level autopilot with
onboard state estimation and attitude, airspeed, and altitude control functionality.
Such capabilities are present in commercially available autopilot hardware and soft-
ware such as the Pixhawk Autopliot, an open source/open hardware project started
at ETH Zurich [1]. Tuning of the low-level loops will not be discussed, though these
procedures are well documented in the literature as well as in practice on the Pixhawk
website.
The control architecture shown in Fig. 15 demonstrates a typical cascaded PID
structure with attitude PI control and angular rate PD control. Additional compensa-
tion for slipping effects is considered for coordinated turns, i.e. a yaw damper signal,
φ
rr = g sin
V
. The TECS block indicates the use of Total Energy Control System for
airspeed and altitude control, again fully implemented and documented on the pix-
hawk website. In three-dimensional extensions of the proposed lateral-directional
nonlinear MPC, the high-level TECS block could be replaced.
The aim is to identify the closed loop low-level autpilot dynamic response when
reacting to attitude commands. We will specifically discuss the roll dynamic here,
however the same procedures discussed could be used for identification of pitch-
ing dynamics as well as airspeed, given that well-tuned low-level controllers are in
place. Depending on the hardware used, autopilot source code could be modified for
an identification option commanding repeatable excitation inputs, or in the case of

Fig. 15 Fixed-wing control architecture

Model Predictive Control for Trajectory Tracking … 31

Fig. 16 Two concatenated 30

2-1-1 maneuvers from a
flight experiment with a 20
fixed-wing UAV
10

-10

-20 input
output

-30
0 2 4 6 8 10 12
Time [s]

utilizing a Linux operating system, a simple ROS node could be written to generate
the same. For roll channel identification, pitch references to the low level controller
should be held constant for holding altitude. Depending on the operational airspeed,
the pitch reference may vary. 2-1-1 maneuvers, a modified doublet input consisting
of alternating pulses with pulse widths in the ratio 2-1-1, are recommended, see
Fig. 16. Morelli [17] demonstrated that flight time required for the 2-1-1 maneuver
is approximately one-sixth of the time required for the standard frequency sweep,
enabling one to gather more data in the same flight time, which is often limited by
battery capacity. At the same time, concatenated 2-1-1 maneuvers make for suit-
able identification inputs for both frequency and time domain system identification
approaches, on par with frequency sweeps.
It is good practice to perform all identification experiments on calm days with
no wind and to persistently excite the control inputs. Data collection should consist
of several 2-1-1 maneuvers for each of several setpoint magnitudes throughout the
desired range. A similar set of data for both training and validation is required. To
test the generalizability of the fit parameters, it is also worthwhile to include non-
2-1-1 maneuvers in the validation set, e.g. arbitrary piloting (still within attitude
control augmentation mode), to test the generalizability of the fit parameters. The
authors provide a set of Matlab scripts that can perform the parameter identification
after data collection and format conversion. Further literature on closed loop system
identification for fixed-wing vehicles can be found in [17, 18].

4.2 Nonlinear MPC

In this section, we formulate a nonlinear MPC for general high-level fixed-wing

lateral-directional trajectory tracking utilizing the model developed in the previous
section. The generalized form involves augmentation of the vehicle model with trajec-
tory information, including discretely defined sequential tracks. We use the ACADO
32 M. Kamel et al.

Toolkit [16] for generation of a fast C code based nonlinear solver and integration
scheme. The optimization problem OCP takes the continuous time form

min
h (x(t), u(t)) − yr e f (t)
2 dt +
m (x(T )) − yr e f (T )
2
U Q P
t=0
subject to ẋ = f(x(t), u(t));
(46)
u(t) ∈ UC
x(t) ∈ XC
x(0) = x (t0 ) .

The state vector is defined as x = (n, e, φ, ψ, p, xsw )T , and control input u = φr ,

where the augmented state xsw is a switch state used within the horizon in the case
that desired trajectories are piece-wise continuously, or generally discretely, defined.
The switch variable has no dynamic until a switch condition is detected within the
horizon, at which point an arbitrary differential α is applied for the remainder of the
horizon calculations, see Eq. (47). This assumes we only consider a maximum of
two track segments in a given horizon, and ensures the optimization does not revert
back to a previous track after switching within the horizon.

α switch condition met

xsw > threshold
ẋsw = (47)
0 else

A general tracking objective is constructed for minimizing the position error to a

given track,

et = (d − p) × T̄d (48)

where T̄d is the tangent unit tangent vector at the closest point d from the UAV
position p to the current path P, while also aligning the vehicle course with the
desired trajectory direction, i.e. minimize

eχ = χd − χ (49)

where χd = atan2 T̄de , T̄dn ∈ [−π, π ]. Here, we use the atan2 function from the
standard C math library. See also Fig. 17. Use of this general objective formulation
allows inputting any path shape, so long as the nearest point from the UAV position
can be calculated and a direction of motion along the path is given for minimization
throughout the horizon.
A relevant example of switching trajectories is that of Dubins Car, or Dubins
Aircraft in the three-dimensional case, path following, see [19]. Dubins paths can be
used to describe the majority of desired flight maneuvers in a typical fixed-wing UAV
mission. Further, using continuous curves such as arcs and lines allow time-invariant
Model Predictive Control for Trajectory Tracking … 33

Fig. 17 Trajectory tracking

objectives

Fig. 18 Dubins path

definitions

trajectory tracking, as oppose to desired positions in time, a useful quality when only
spatial proximity to the track is desired and timing is less important; for instance,
if energy conservation is required and a single low airspeed reference is given to
be tracked. For the remainder of the section, we will consider Dubins segments as
path inputs to the high-level controller, though it should be noted that the objective
formulation is not limited to these.
Using the definitions in Eqs. (48) and (49), we formulate the objective vector
y = (et , eχ , φ, p, φr )T . We assume a fixed air-mass-relative airspeed V and two-
dimensional wind vector w, held constant throughout the horizon. These values
are estimated and input to the optimization as online data. Also input as online
data, the current and next sets of Dubins path parameters P cur , P next , where line
parameters include P = {t ype = 0, a, b}, a and b are two waypoints defining a
straight segment, and arc parameters include P = {t ype = 1, c, R, dir, ψ0 , Δψ},
c is the center point of the arc, R is the radius, dir is the loiter direction, and ψ0 , is
the heading pointing towards the entrance point on the arc, and Δψ is the arclength
traveled. The path segments are managed and switched based on an acceptance radius
and heading direction criteria, see Fig. 18.
34 M. Kamel et al.

All references are set to zero, except for the control input references. As the
continuous time formulation does not allow slew rate penalties, it is possible to
instead store the previous control horizon for input as weighted control references
in the next nonlinear MPC iteration. This is done to avoid bang–bang control action
when the nonlinear MPC iterations occur at relatively large steps, for fixed-wing
we run the high-level controller at either 10 or 20 Hz. As only the second step in
the current control horizon is sent to the vehicle for tracking, the early horizon is
penalized more heavily than the latter horizon values. This can be accomplished by
weighting the horizon of controls with a decreasing quadratic function. More insights
on the control formulation may be found in [20].
The following MATLAB script may be run to generate the ACADO solver in
C code. Note when dealing with discontinuous functions in the model formulation,
external models with accompanying external jacobians must be supplied, written in
C. Further, if the objective function contains discontinuous functions, this may also
be input externally. Here, we implement a numerical jacobian, though one could find
individual analytic expressions for each discontinuous case and supply them to the
code generation.
1 clc ;
2 clear all ;
3 close a l l ;
4

5 Ts = 0 . 1 ; % model d i s c r e t i z a t i o n
6 N = 40; % horizon length
7

8 % STATES − − − − − − − −
9 DifferentialState n; % ( northing ) [m]
10 DifferentialState e; % ( easting ) [m]
11 D i f f e r e n t i a l S t a t e phi ; % ( r o l l angle ) [ rad ]
12 DifferentialState psi ; % ( yaw a n g l e ) [ rad ]
13 DifferentialState p; % ( roll rate ) [ rad / s ]
14 D i f f e r e n t i a l S t a t e x_sw ; % ( switching s t a t e ) [~]
15

16 % CONTROLS − − − − − − −
17 Control phi_r ; % ( r o l l angle reference ) [ rad ]
18

19 % ONLINE DATA − − − − − −
20 O n l i n e D a t a V; % ( airspeed ) [m/ s ]
21 O n l i n e D a t a pparam1 ; % t y p e =0 t y p e =1
22 O n l i n e D a t a pparam2 ; % a_n c_n
23 O n l i n e D a t a pparam3 ; % a_e c_e
24 O n l i n e D a t a pparam4 ; % b_n R
25 O n l i n e D a t a pparam5 ; % b_e dir
26 O n l i n e D a t a pparam6 ; % −− psi0
27 O n l i n e D a t a pparam7 ; % −− dpsi
28 O n l i n e D a t a pparam1_next ; % t y p e =0 t y p e =1
29 O n l i n e D a t a pparam2_next ; % a_n c_n
30 O n l i n e D a t a pparam3_next ; % a_e c_e
31 O n l i n e D a t a pparam4_next ; % b_n R
32 O n l i n e D a t a pparam5_next ; % b_e dir
33 O n l i n e D a t a pparam6_next ; % −− psi0
Model Predictive Control for Trajectory Tracking … 35

34 O n l i n e D a t a pparam7_next ; % −− dpsi
35 O n l i n e D a t a wn ; % ( n o r t h i n g wind ) [m/ s ]
36 O n l i n e D a t a we ; % ( e a s t i n g wind ) [m/ s ]
37

38 % OPTIMAL CONTROL PROBLEM − − − − − −

39

40 % lengths
41 n_X = l e n g t h ( d i f f S t a t e s ) ; % states
42 n_U = l e n g t h ( c o n t r o l s ) ; % controls
43 n_Y = 4 ; % state / outputs objectives
44 n_Z = 1 ; % control objectives
45 n_OD = 1 7 ; % online data
46

47 % weights
48 Q = eye ( n_Y+n_Z , n_Y+n_Z ) ;
49 Q = acado . BMatrix (Q) ;
50
51 QN = eye ( n_Y , n_Y ) ;
52 QN = acado . BMatrix (QN) ;
53

54

55

56 % o p t i m a l c o n t r o l problem
57 a c a d o S e t ( ’ problemname ’ , ’nmpc ’ ) ;
58 ocp = acado . OCP( 0 . 0 , N∗Ts , N ) ;
59

60 % minimization
61 ocp . minimizeLSQ ( Q, ’ evaluateLSQ ’ ) ;
62 ocp . minimizeLSQEndTerm ( QN, ’ evaluateLSQEndTerm ’ ) ;
63

64 % e x t e r n a l model
65 ocp . setModel ( ’ model ’ , ’ r h s ’ , ’ r h s _ j a c ’ ) ;
66 ocp . s e t D i m e n s i o n s ( n_X , n_U , n_OD , 0 ) ;
67

68 ocp . s u b j e c t T o ( −35∗ p i / 1 8 0 <= p h i _ r <= 35∗ p i / 1 8 0 ) ;

69

70 setNOD ( ocp , n_OD) ;

71

72 % export s e t t i n g s
73 nmpc = acado . OCPexport ( ocp ) ;
74 nmpc . s e t ( ’HESSIAN_APPROXIMATION ’ , ’GAUSS_NEWTON’ ) ;
75 nmpc . s e t ( ’DISCRETIZATION_TYPE ’ , ’MULTIPLE_SHOOTING ’ ) ;
76 nmpc . s e t ( ’SPARSE_QP_SOLUTION ’ , ’FULL_CONDENSING ’ ) ;
77 nmpc . s e t ( ’INTEGRATOR_TYPE ’ , ’INT_IRK_GL4 ’ ) ;
78 nmpc . s e t ( ’NUM_INTEGRATOR_STEPS ’ , N ) ;
79 nmpc . s e t ( ’QP_SOLVER ’ , ’QP_QPOASES ’ ) ;
80 nmpc . s e t ( ’HOTSTART_QP ’ , ’YES ’ ) ;
81 nmpc . s e t ( ’LEVENBERG_MARQUARDT’ , 1e−10 ) ;
82 nmpc . s e t ( ’GENERATE_MAKE_FILE ’ , ’YES ’ ) ;
83 nmpc . s e t ( ’GENERATE_TEST_FILE ’ , ’YES ’ ) ;
84 nmpc . s e t ( ’GENERATE_SIMULINK_INTERFACE ’ , ’YES ’ ) ;
85 nmpc . s e t ( ’CG_HARDCODE_CONSTRAINT_VALUES’ , ’YES ’ ) ;
86 nmpc . s e t ( ’CG_USE_VARIABLE_WEIGHTING_MATRIX ’ , ’YES ’ ) ;
36 M. Kamel et al.

87

88 % export
89 c o p y f i l e ( ’ . . / acado / e x t e r n a l _ p a c k a g e s / q p o a s e s ’ , . . .
90 ’ export_nmpc / q p o a s e s ’ )
91 nmpc . e xportCode ( ’ export_nmpc ’ ) ;
92

93 cd export_nmpc
94 m a k e _ a c a d o _ s o l v e r ( ’ . . / acado_nmpc_step ’ , ’ model . c ’ )
95 cd . .

4.2.1 ROS Integration

As described in Sect. 3.2.3, we integrate the ACADO solver into a ROS node for
generating control solutions in real-time on the aircraft. However, our approach
for the fixed-wing UAV differs slightly from the MAV as all low-level control and
state estimation is performed on the low-level autopilot’s micro-controller (we use
the Pixhawk Autopilot [1]). As processing power on the Pixhawk micro-controller
is somewhat limited, an additional onboard ODROID-U3 computer with 1.7 GHz
Quad-Core processor and 2 GB RAM, running Robotic Operating System (ROS) [3]
is integrated into the platform for experimentation with more computationally tax-
ing algorithms. High-level controllers can be run within ROS node wrappers which
communicate with the Pixhawk via UART serial communication; average commu-
nication latency was observed <3 µs. The nonlinear MPC is run within a ROS node
on the ODROID-U3. MAVROS [21], an extendable communication node for ROS,
is used to translate MAVLink Protocol messages containing current state estimates
from the Pixhawk, and similarly send back control references from the nonlinear
MPC implemented in ROS. As high-level fixed-wing dynamics are somewhat slow,
we choose a fixed rate loop for control generation using a simple while loop, shown
as an example in the following code excerpt.
1 w h i l e ( r o s : : ok ( ) ) {
2

3 / ∗ empty c a l l b a c k queues ∗ /
4 r o s : : spinOnce ( ) ;
5

6 / ∗ nmpc i t e r a t i o n s t e p ∗ /
7 r e t = nmpc . n m p c I t e r a t i o n ( ) ;
8

9 i f ( r e t != 0 ) {
10 ROS_ERROR( ’ ’ n m p c _ i t e r a t i o n : e r r o r i n qpOASES QP s o l v e r . ’ ’ ) ;
11 return 1;
12 }
13

14 /∗ sleep ∗/
15 nmpc_rate . s l e e p ( ) ;
16 }
Note the nonlinear MPC iteration step is only called once per loop, and the sleep
function regulates the timing of the loop. The ros::spinOnce() updates any
Model Predictive Control for Trajectory Tracking … 37

Fig. 19 Fixed-wing test platform

subscriptions with the most recent state estimates for use in the controller, and con-
trol action is subsequently published as a geometry_msgs::TwistStamped
message for processing within the MAVROS attitude setpoint plugin. This plugin is
integrated for off-board control functionality within the Pixhawk standard firmware.
Messages from the pixhawk are streamed at a rate of ∼40 Hz, and it is reasonable to
assume the callback functions will contain up-to-date values in their queues at every
nonlinear MPC iteration; as mentioned previously, typical high-level control rates
for fixed-wing vehicles are often 10 or 20 Hz. All augmented states, i.e. not directly
measured or estimated, used within the controller are also stored and kept for the
next iteration.

4.2.2 Experimental Results

System identification, controller design, and flight experiments described in this

section are performed on a small, 2.6 m wingspan, light-weight 2.65 kg, low-altitude,
and hand-launchable fixed-wing UAV, Techpod, see Fig. 19.
Two flight experiments were conducted using the described framework. A hori-
zon length of N = 40, or 4 s was used with objective weights Q ydiag = Pdiag =
[0.01, 10, 0.1, 0.01, 10], Q u = 100. The discretization time step within the hori-
zon is Tstep = 0.1 s, and the nonlinear MPC is iterated every 0.05 s. The average
solve time for the nonlinear MPC running on the ODROID-U3 was observed to be
∼13 ms. Both experiments took place during very calm conditions, and the wind
speed was negligible.
In Fig. 20, Techpod is commanded towards a box pattern until returning to a
loiter circle. Minimal overshoot is observed, considering the set acceptance radius
of 35 m, and convergence within less than 1 m of position error is observed for each
38 M. Kamel et al.

40
Path
Reference
UAV Trajectory 30 Actual
50
20
start 10
0
0
Northing [m]

-10
-50 0 10 20 30 40 50 60 70

60
-100
40
end 20
-150 0
-20
-200 -40
-100 -50 0 50 100 150 200 250 0 10 20 30 40 50 60 70
Easting [m] Time [s]

Fig. 20 Flight experiment: box tracking

100 40
Path
UAV Trajectory 20
50
0
start
-20 Reference
Northing [m]

0 Actual
-40
0 10 20 30 40 50 60 70 80
-50
60
-100 40
end
20
-150 0
-20
-40
-50 0 50 100 150 200 250 0 10 20 30 40 50 60 70 80
Easting [m] Time [s]

Fig. 21 Flight experiment: Dubins tracking

line segment and the final loiter circle. The commanded and actual roll angles as well
as the roll rate, are both kept within acceptable bounds.
In Fig. 21, an arbitrary sequence of Dubins segments were given to the high-level
nonlinear MPC. Again, good convergence to the path is seen, with acceptable state
responses. Position error of less than 3 m was observed after convergence to the path.
The end of the shown flight path is stopped just before converging to the final loiter
due to rain fall starting during the flight experiment and manual take-over of the
aircraft for landing.

5 Conclusion

In this chapter we presented an overview of many model-based control strategies for

multiple classes of unmanned aerial vehicles. The strategies presented are: Robust
MPC for multi-rotor system, Linear MPC for multi-rotor system and Nonlinear
MPC for multi-rotor system and fixed-wing UAVs. These control strategies have
Model Predictive Control for Trajectory Tracking … 39

been evaluated in real experiments and performance evaluation has been shown in
this chapter. The presented controllers are available as open source ROS package
on https://github.com/ethz-asl/mav_control_rw for rotary wing MAVs and https://
github.com/ethz-asl/mav_control_fw for fixed wing MAVs.

References

1. Px4 autopilot. https://pixhawk.org.

2. Navio autopilot. https://emlid.com.
3. Robot operating system. http://www.ros.org.
4. Mayne, D.Q., J.B. Rawlings, C.V. Rao, and P.O. Scokaert. 2000. Constrained model predictive
control: Stability and optimality. Automatica 36 (6): 789–814.
5. Boyd, S., and L. Vandenberghe. 2004. Convex Optimization. Cambridge: Cambridge University
Press.
6. Borrelli, F., A. Bemporard, and M. Morari. 2015. Predictive Control for Linear and Hybrid
Systems. Cambridge: Cambridge University Press.
7. Ferreau, H., C. Kirches, A. Potschka, H. Bock, and M. Diehl. 2014. qpOASES: A parametric
active-set algorithm for quadratic programming. Mathematical Programming Computation 6
(4): 327–363.
8. Alexis, K., C. Papachristos, R. Siegwart, and A. Tzes. 2015. Robust model predictive flight
control of unmanned rotorcraft. Journal of Intelligent & Robotic Systems 1–27.
9. Loefberg, J. 2003. Minimax approaches to robust model predictive control. Ph.D. dissertation
Linkoping, Sweden: Linkoping University.
10. Cannon, M., S. Li, Q. Cheng, and B. Kouvaritakis. 2011. Efficient robust output feedback mpc.
In Proceedings of the 18th IFAC World Congress, vol. 18, 7957–7962.
11. Kvasnica, M. 2009. Real-Time Model Predictive Control via Multi-Parametric Programming:
Theory and Tools. Saarbrücken: VDM Verlag.
12. Mattingley, J., and S. Boyd. 2012. Cvxgen: A code generator for embedded convex optimiza-
tion. Optimization and Engineering 13 (1): 1–27.
13. Ljung, L. 1999. System identification - Theory for the User. Englewood Cliffs: Prentice-Hall.
14. Lynen, S., M.W. Achtelik, S. Weiss, M. Chli, and R. Siegwart. 2013. A robust and modu-
lar multi-sensor fusion approach applied to mav navigation. In 2013 IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 3923–3929. IEEE.
15. vicon systems. http://www.vicon.com.
16. Houska, B., H. Ferreau, M. Vukov, and R. Quirynen. 2009–2013. ACADO Toolkit User’s
Manual. http://www.acadotoolkit.org.
17. Morelli, E.A. 2003. Low-order equivalent system identification for the tu-144ll supersonic
transport aircraft. Journal of guidance, control, and dynamics 26 (2): 354–362.
18. Luo, Y., H. Chao, L. Di, and Y. Chen. 2011. Lateral directional fractional order (pi) π control
of a small fixed-wing unmanned aerial vehicles: controller designs and flight tests. Control
Theory & Applications, IET 5 (18): 2156–2167.
19. Beard, R.W., and T.W. McLain. 2013. Implementing dubins airplane paths on fixed-wing uavs.
In Contributed Chapter to the Springer Handbook for Unmanned Aerial Vehicles.
20. Stastny, T., A. Dash, and R. Siegwart. 2017. Nonlinear mpc for fixed-wing uav trajectory
tracking: Implementation and flight experiments. In AIAA Guidance, Navigation, and Control
(GNC) Conference.
21. MAVROS. 2016. http://wiki.ros.org/mavros.