# Trajectory Tracking and Stabilization of a Quadrotor Using Model Predictive Control of Laguerre Functions

Authors: Mapopa Chipofya; Deok Jin Lee; Kil To Chong
Journal: Abstract and Applied Analysis
 (2015)

---

## Abstract

This paper presents a solution to stability and trajectory tracking of a quadrotor system using a model predictive controller designed using a type of orthonormal functions called Laguerre functions. A linear model of the quadrotor is derived and used. To check the performance of the controller we compare it with a linear quadratic regulator and a more traditional linear state space MPC. Simulations for trajectory tracking and stability are performed in MATLAB and results provided in this paper.


---

## Body

## 1. Introduction

A quadrotor is a helicopter which has four equally spaced rotors, usually arranged at the corners of a square body. When these rotors spin, they push air downwards and in the process create a thrust force that keeps the quadrotor aloft. Unlike the conventional helicopter (with two rotors) which requires a swashplate mechanism in order to have more degrees of freedom, such mechanism is not needed in quadrotor systems since the two additional rotors provide the same level of control as that with conventional helicopters fixed with swashplate mechanism.The task to control a quadrotor is a fundamentally difficult and interesting problem. With six degrees of freedom (three translational and three rotational) and only four independent inputs (rotor speeds), quadrotors are severely underactuated. In order to achieve six degrees of freedom, rotational and translational motion are coupled. The resulting dynamics are highly nonlinear, especially after accounting for the complicated aerodynamic effects. Finally, unlike ground vehicles, quadrotors have very little friction to prevent their motion, so they must provide their own damping in order to stop moving and remain stable. Put together, these factors create a very challenging control problem.


## 2. Literature Review

Amongst the many control techniques used in quadrotor control PID, LQR and recently MPC have been widely used.While PID is the most popular choice for controlling several types of processes, it turns out that tuning becomes a big challenge especially for MIMO systems like the quadrotor. Several techniques have been used to divide the quadrotor control problem to several SISO systems. While this has worked in some cases it has to be pointed out that this takes away the most natural way of controlling the system and designing many SISO controllers may make maintenance more difficult.LQR is a relatively modern control method that is very powerful yet limited to applications where linear system models are available. To use this method to control nonlinear systems a linear model has to be obtained from the corresponding nonlinear model. To evaluate the performance of the LQR controller we need to compare it with results obtained from other control techniques. This means designing the same system using another control technique which could be cumbersome. Because of this, LQR is only very popular to use in inherently linear systems or where results of control obtained using other control techniques are available for comparison.On the other hand, MPC can handle both linear and nonlinear systems [1]. Comparisons between the nonlinear system and its corresponding linear model can easily be made with very little modification. Compared to PID, tuning MPC is easier even for complex MIMO systems.However, in trying to track the reference trajectory in an optimal way and at the same time obey the constraints imposed MPC makes much more calculations than either PID or LQR. This computation burden makes MPC less suitable for “fast” processes. It requires very powerful processors to be used in hardware implementation. Not surprisingly a lot of effort is being put in to reduce the computations so that MPC can be faster and easily be implemented even on low cost processors.In [2] Wang proposed a method of designing MPC using orthonormal functions. This method makes less computations than the traditional MPC. With reduced number of computations this technique can be used where rapid system dynamics are required. We will use this method in our task to control a quadrotor using MPC.Other MPC applications to quadrotor can be found in [3, 4]. In [4] the control structure consists of a controller based on MPC to track the reference trajectory and a second one based on a nonlinear H
∞ control technique to stabilize the rotational movements. Similarly in [3] an MPC controller is designed to control position, and a second feedforward controller is used to perform stabilization of the quadrotor system.In both [3, 4] the control loop uses two controllers: the controller based on MPC which is used for tracking the position reference trajectory and a second controller (which is designed using other techniques) that is used for stabilization of the rotational movements. In this paper, MPC is used to control both the position and rotational movements. This is good for uniformity and ease of maintenance.


## 3. Kinematics and Dynamics of a Quadrotor

A quadrotor is completely described by twelve states. The first six states describe translational motion of the quadrotor. These states arex
,
y, and z which represent position in inertial frame and u
,
v, and w which denote velocity vectors along the body frame. Similarly, the remaining six variables describe rotational motions. These are ϕ, θ, and ψ commonly known as Euler angles and p, q, and r which denote angular velocities.A free body diagram of a quadrotor is shown in Figure1. The force F
* and moment M
* (where *
=
f
,
b
,
r, and l) produced by the rotors can be described by their angular speed (
ω
) as F
*
=
k
1
ω
2 and M
*
=
k
2
ω
2, respectively. Similarly, torques τ
ϕ
,
τ
θ, and τ
ψ and the thrust force (F) are related to angular speed of rotors as
(1)
F
τ
ϕ
τ
θ
τ
ψ
=
M
ω
f
2
ω
r
2
ω
b
2
ω
l
2
,
where M
=
[
k
1
k
1
k
1
k
1
0
-
l
k
1
0
l
k
1
l
k
1
0
-
l
k
1
0
-
k
2
k
2
-
k
2
k
2
] and l is the length of arm of the quadrotor.Inertial frame (a) and forces and torques acting on the quadrotor (b).
(a)
(b)Equations describing the kinematics of the quadrotor are given by(2)
x
˙
y
˙
z
˙
=
R
ϕ
,
θ
,
ψ
u
v
w
,
ϕ
˙
θ
˙
ψ
˙
=
1
s
ϕ
t
θ
c
ϕ
t
θ
0
c
ϕ
-
s
ϕ
0
s
ϕ
c
θ
c
ϕ
c
θ
p
q
r
,
where R
ϕ
,
θ
,
ψ
=
[
c
θ
c
ψ
s
ϕ
s
θ
c
ψ
-
c
ϕ
s
ψ
c
ϕ
s
θ
c
ψ
+
s
ϕ
s
ψ
c
θ
s
ψ
s
ϕ
s
θ
s
ψ
+
c
ϕ
c
ψ
c
ϕ
s
θ
s
ψ
-
s
ϕ
c
ψ
-
s
θ
s
ϕ
c
θ
c
ϕ
c
θ
], c stands for cos
⁡, s stand for sin, and t stands for tan. Similarly, dynamic equations can be derived using Newtons laws of motion which are listed in
(3)
u
˙
v
˙
w
˙
=
r
v
-
q
w
p
w
-
r
q
q
u
-
p
v
+
-
g
sin
θ
g
cos
⁡
θ
sin
ϕ
g
cos
⁡
θ
cos
⁡
ϕ
+
1
m
0
0
-
F
,
p
˙
q
˙
r
˙
=
J
y
-
J
z
J
x
q
r
J
z
-
J
x
J
y
p
r
J
x
-
J
y
J
z
p
q
+
1
J
x
τ
ϕ
1
J
y
τ
θ
1
J
z
τ
ψ
,
where J
x, J
y, and J
z are the respective moment of inertia about X, Y, and Z axes of the quadrotor. For the simulation of the controller of quadrotor we have considered F, τ
ϕ, τ
θ, and τ
ψ as the inputs to the system.


### 3.1. Linearization

In order to design a linear controller it is vital that (2) to (3) be simplified to linear ones. To do that roll, pitch, and heading angles are assumed to operate within very small angles [5] which leads to the following:
(4)
ϕ
¨
θ
¨
ψ
¨
=
1
J
x
τ
ϕ
1
J
y
τ
θ
1
J
z
τ
ψ
,
z
¨
=
g
-
F
m
,
u
˙
=
-
g
θ
,
v
˙
=
g
ϕ
.


## 3.1. Linearization

In order to design a linear controller it is vital that (2) to (3) be simplified to linear ones. To do that roll, pitch, and heading angles are assumed to operate within very small angles [5] which leads to the following:
(4)
ϕ
¨
θ
¨
ψ
¨
=
1
J
x
τ
ϕ
1
J
y
τ
θ
1
J
z
τ
ψ
,
z
¨
=
g
-
F
m
,
u
˙
=
-
g
θ
,
v
˙
=
g
ϕ
.


## 4. Controller Design

In order to control attitude (ϕ, θ, and ψ), altitude (z), and position (x, y), we propose a control architecture given in Figure 2 where we have utilized MPC to design the three controllers. The attitude controller generates τ
ϕ, τ
θ, and τ
ψ actuator signals whereas the altitude controller generates required thrust for the system. The position controller controls position in the x and y directions and generates control signals θ and ϕ which, when combined with the ψ signal, act as reference signals to the attitude controller.Figure 2
Block diagram showing the structure of the controllers.In the next section we look at the design of each of the three controllers.


### 4.1. Discretization

In this section, discrete-time state-space equations necessary for altitude, position, and attitude control are derived.


#### 4.1.1. Altitude Controller

The linear equations responsible for altitude control are(5)
z
˙
=
w
,
w
˙
=
g
-
F
m
.
These two equations can be combined to give the following second order ODE:
(6)
z
¨
=
g
-
F
m
.
This second order ODE can be simplified to
(7)
z
¨
=
G
,
where G
=
g
-
F
/
m or F
=
m
(
g
-
G
). Equation (7) can be written in state-space form as in the following equation:
(8)
z
˙
z
¨
=
0
1
0
0
z
z
˙
+
0
1
G
.
Using the forward Euler method and choosing a sampling interval Δ
T, we can express (8) in discrete form as in
(9)
z
k
+
1
v
z
k
+
1
=
1
Δ
T
0
1
z
(
k
)
v
z
(
k
)
+
0
Δ
T
G
k
.
The states z
(
k
) and v
z
(
k
) represent the position and velocity, respectively, and we could choose a proper output matrix C depending on which state we are going to measure. In this paper we are interested in the position and, therefore, we will use the output matrix C
=
1
0.


#### 4.1.2. Position Controller

The equations responsible for position and velocity control inx direction are
(10)
x
˙
=
u
,
u
˙
=
-
g
θ
.
These two equations can be combined to give the second order ODE:
(11)
x
¨
=
-
g
θ
.
Similarly the equations responsible for position and velocity control in y direction are
(12)
y
˙
=
v
,
v
˙
=
g
ϕ
,
which give rise to the second order ODE:
(13)
y
¨
=
g
ϕ
.
Equations (11) and (13) can be combined and form a state-space equation such as
(14)
x
˙
x
¨
y
˙
y
¨
=
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
x
x
˙
y
y
˙
+
0
0
-
g
0
0
0
0
g
θ
ϕ
.
As stated before, we can use the forward Euler method to express (14) in discrete form. Consider
(15)
x
k
+
1
v
x
k
+
1
y
k
+
1
v
y
k
+
1
=
1
Δ
T
0
0
0
1
0
0
0
0
1
Δ
T
0
0
0
1
x
k
v
x
k
y
k
v
y
k
+
0
0
-
g
Δ
T
0
0
0
0
g
Δ
T
θ
k
ϕ
k
.
The states x
(
k
) and y
(
k
) represent the position while v
x
(
k
) and v
y
(
k
) represent velocity. Since we are interested in the position, we will use the output matrix C
=
1
0
0
0
0
0
1
0 but we could have used a different 2
×
4 output matrix if we were interested in the velocity. But it is important to note that we can not use a 3
×
4 or 4
×
4 as there is no guarantee that we will be able to control each of the measured outputs independently with zero steady state errors. This is generally the case for a system with m inputs, q outputs, and q
>
m [6].


#### 4.1.3. Attitude Controller

In attitude control we are interested in controlling the rollϕ, pitch θ, and yaw (heading) ψ such that we generate appropriate torque signals responsible for steering the quadrotor in the desired direction with the required attitude. The equations responsible for roll control are
(16)
ϕ
˙
=
p
,
p
˙
=
1
J
x
τ
ϕ
.
These two equations can be combined to give the second order ODE:
(17)
ϕ
¨
=
1
J
x
τ
ϕ
.
Similarly the equations responsible for pitch are given by
(18)
θ
˙
=
q
,
q
˙
=
1
J
y
τ
θ
,
which give rise to the second order ODE:
(19)
θ
¨
=
1
J
y
τ
θ
.
Also the equations responsible for heading are expressed as
(20)
ψ
˙
=
r
,
r
˙
=
1
J
z
τ
ψ
,
which, like in roll and pitch, give rise to the second order ODE:
(21)
ψ
¨
=
1
J
z
τ
θ
.
Equations (17), (19), and (21) can be combined and written in state-space form as in
(22)
ϕ
˙
ϕ
¨
θ
˙
θ
¨
ψ
˙
ψ
¨
=
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
ϕ
ϕ
˙
θ
θ
˙
ψ
ψ
˙
+
0
0
0
J
x
-
1
0
0
0
0
0
0
J
y
-
1
0
0
0
0
0
0
J
z
-
1
τ
ϕ
τ
θ
τ
ψ
,
where τ
ϕ, τ
θ, and τ
ψ are the torques required to give the required roll, pitch, and yaw, respectively.Again, using the forward Euler method we express (22) in discrete form as
(23)
ϕ
k
+
1
v
ϕ
k
+
1
θ
k
+
1
v
θ
k
+
1
ψ
k
+
1
v
ψ
k
+
1
=
1
Δ
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
T
0
0
0
0
0
1
ϕ
k
v
ϕ
k
θ
k
v
θ
k
ψ
k
v
ψ
k
+
0
0
0
Δ
T
J
x
-
1
0
0
0
0
0
0
Δ
T
J
y
-
1
0
0
0
0
0
0
Δ
T
J
z
-
1
τ
ϕ
k
τ
θ
k
τ
ψ
k
.
Since we are interested in measuring ϕ
(
k
), θ
(
k
), and ψ
(
k
), we will use the output matrix C
=
[
1
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
].So far we have formulated the necessary equations used to build the three controllers. Figure2 shows, in block diagram form, what we have done so far. But that is not the whole story. In the next section we will look at exactly how model predictive control is applied to the quadrotor system using the discrete state-space equations formulated in this section.


## 4.1. Discretization

In this section, discrete-time state-space equations necessary for altitude, position, and attitude control are derived.


### 4.1.1. Altitude Controller

The linear equations responsible for altitude control are(5)
z
˙
=
w
,
w
˙
=
g
-
F
m
.
These two equations can be combined to give the following second order ODE:
(6)
z
¨
=
g
-
F
m
.
This second order ODE can be simplified to
(7)
z
¨
=
G
,
where G
=
g
-
F
/
m or F
=
m
(
g
-
G
). Equation (7) can be written in state-space form as in the following equation:
(8)
z
˙
z
¨
=
0
1
0
0
z
z
˙
+
0
1
G
.
Using the forward Euler method and choosing a sampling interval Δ
T, we can express (8) in discrete form as in
(9)
z
k
+
1
v
z
k
+
1
=
1
Δ
T
0
1
z
(
k
)
v
z
(
k
)
+
0
Δ
T
G
k
.
The states z
(
k
) and v
z
(
k
) represent the position and velocity, respectively, and we could choose a proper output matrix C depending on which state we are going to measure. In this paper we are interested in the position and, therefore, we will use the output matrix C
=
1
0.


### 4.1.2. Position Controller

The equations responsible for position and velocity control inx direction are
(10)
x
˙
=
u
,
u
˙
=
-
g
θ
.
These two equations can be combined to give the second order ODE:
(11)
x
¨
=
-
g
θ
.
Similarly the equations responsible for position and velocity control in y direction are
(12)
y
˙
=
v
,
v
˙
=
g
ϕ
,
which give rise to the second order ODE:
(13)
y
¨
=
g
ϕ
.
Equations (11) and (13) can be combined and form a state-space equation such as
(14)
x
˙
x
¨
y
˙
y
¨
=
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
x
x
˙
y
y
˙
+
0
0
-
g
0
0
0
0
g
θ
ϕ
.
As stated before, we can use the forward Euler method to express (14) in discrete form. Consider
(15)
x
k
+
1
v
x
k
+
1
y
k
+
1
v
y
k
+
1
=
1
Δ
T
0
0
0
1
0
0
0
0
1
Δ
T
0
0
0
1
x
k
v
x
k
y
k
v
y
k
+
0
0
-
g
Δ
T
0
0
0
0
g
Δ
T
θ
k
ϕ
k
.
The states x
(
k
) and y
(
k
) represent the position while v
x
(
k
) and v
y
(
k
) represent velocity. Since we are interested in the position, we will use the output matrix C
=
1
0
0
0
0
0
1
0 but we could have used a different 2
×
4 output matrix if we were interested in the velocity. But it is important to note that we can not use a 3
×
4 or 4
×
4 as there is no guarantee that we will be able to control each of the measured outputs independently with zero steady state errors. This is generally the case for a system with m inputs, q outputs, and q
>
m [6].


### 4.1.3. Attitude Controller

In attitude control we are interested in controlling the rollϕ, pitch θ, and yaw (heading) ψ such that we generate appropriate torque signals responsible for steering the quadrotor in the desired direction with the required attitude. The equations responsible for roll control are
(16)
ϕ
˙
=
p
,
p
˙
=
1
J
x
τ
ϕ
.
These two equations can be combined to give the second order ODE:
(17)
ϕ
¨
=
1
J
x
τ
ϕ
.
Similarly the equations responsible for pitch are given by
(18)
θ
˙
=
q
,
q
˙
=
1
J
y
τ
θ
,
which give rise to the second order ODE:
(19)
θ
¨
=
1
J
y
τ
θ
.
Also the equations responsible for heading are expressed as
(20)
ψ
˙
=
r
,
r
˙
=
1
J
z
τ
ψ
,
which, like in roll and pitch, give rise to the second order ODE:
(21)
ψ
¨
=
1
J
z
τ
θ
.
Equations (17), (19), and (21) can be combined and written in state-space form as in
(22)
ϕ
˙
ϕ
¨
θ
˙
θ
¨
ψ
˙
ψ
¨
=
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
ϕ
ϕ
˙
θ
θ
˙
ψ
ψ
˙
+
0
0
0
J
x
-
1
0
0
0
0
0
0
J
y
-
1
0
0
0
0
0
0
J
z
-
1
τ
ϕ
τ
θ
τ
ψ
,
where τ
ϕ, τ
θ, and τ
ψ are the torques required to give the required roll, pitch, and yaw, respectively.Again, using the forward Euler method we express (22) in discrete form as
(23)
ϕ
k
+
1
v
ϕ
k
+
1
θ
k
+
1
v
θ
k
+
1
ψ
k
+
1
v
ψ
k
+
1
=
1
Δ
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
T
0
0
0
0
0
1
ϕ
k
v
ϕ
k
θ
k
v
θ
k
ψ
k
v
ψ
k
+
0
0
0
Δ
T
J
x
-
1
0
0
0
0
0
0
Δ
T
J
y
-
1
0
0
0
0
0
0
Δ
T
J
z
-
1
τ
ϕ
k
τ
θ
k
τ
ψ
k
.
Since we are interested in measuring ϕ
(
k
), θ
(
k
), and ψ
(
k
), we will use the output matrix C
=
[
1
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
].So far we have formulated the necessary equations used to build the three controllers. Figure2 shows, in block diagram form, what we have done so far. But that is not the whole story. In the next section we will look at exactly how model predictive control is applied to the quadrotor system using the discrete state-space equations formulated in this section.


## 4.1.1. Altitude Controller

The linear equations responsible for altitude control are(5)
z
˙
=
w
,
w
˙
=
g
-
F
m
.
These two equations can be combined to give the following second order ODE:
(6)
z
¨
=
g
-
F
m
.
This second order ODE can be simplified to
(7)
z
¨
=
G
,
where G
=
g
-
F
/
m or F
=
m
(
g
-
G
). Equation (7) can be written in state-space form as in the following equation:
(8)
z
˙
z
¨
=
0
1
0
0
z
z
˙
+
0
1
G
.
Using the forward Euler method and choosing a sampling interval Δ
T, we can express (8) in discrete form as in
(9)
z
k
+
1
v
z
k
+
1
=
1
Δ
T
0
1
z
(
k
)
v
z
(
k
)
+
0
Δ
T
G
k
.
The states z
(
k
) and v
z
(
k
) represent the position and velocity, respectively, and we could choose a proper output matrix C depending on which state we are going to measure. In this paper we are interested in the position and, therefore, we will use the output matrix C
=
1
0.


## 4.1.2. Position Controller

The equations responsible for position and velocity control inx direction are
(10)
x
˙
=
u
,
u
˙
=
-
g
θ
.
These two equations can be combined to give the second order ODE:
(11)
x
¨
=
-
g
θ
.
Similarly the equations responsible for position and velocity control in y direction are
(12)
y
˙
=
v
,
v
˙
=
g
ϕ
,
which give rise to the second order ODE:
(13)
y
¨
=
g
ϕ
.
Equations (11) and (13) can be combined and form a state-space equation such as
(14)
x
˙
x
¨
y
˙
y
¨
=
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
x
x
˙
y
y
˙
+
0
0
-
g
0
0
0
0
g
θ
ϕ
.
As stated before, we can use the forward Euler method to express (14) in discrete form. Consider
(15)
x
k
+
1
v
x
k
+
1
y
k
+
1
v
y
k
+
1
=
1
Δ
T
0
0
0
1
0
0
0
0
1
Δ
T
0
0
0
1
x
k
v
x
k
y
k
v
y
k
+
0
0
-
g
Δ
T
0
0
0
0
g
Δ
T
θ
k
ϕ
k
.
The states x
(
k
) and y
(
k
) represent the position while v
x
(
k
) and v
y
(
k
) represent velocity. Since we are interested in the position, we will use the output matrix C
=
1
0
0
0
0
0
1
0 but we could have used a different 2
×
4 output matrix if we were interested in the velocity. But it is important to note that we can not use a 3
×
4 or 4
×
4 as there is no guarantee that we will be able to control each of the measured outputs independently with zero steady state errors. This is generally the case for a system with m inputs, q outputs, and q
>
m [6].


## 4.1.3. Attitude Controller

In attitude control we are interested in controlling the rollϕ, pitch θ, and yaw (heading) ψ such that we generate appropriate torque signals responsible for steering the quadrotor in the desired direction with the required attitude. The equations responsible for roll control are
(16)
ϕ
˙
=
p
,
p
˙
=
1
J
x
τ
ϕ
.
These two equations can be combined to give the second order ODE:
(17)
ϕ
¨
=
1
J
x
τ
ϕ
.
Similarly the equations responsible for pitch are given by
(18)
θ
˙
=
q
,
q
˙
=
1
J
y
τ
θ
,
which give rise to the second order ODE:
(19)
θ
¨
=
1
J
y
τ
θ
.
Also the equations responsible for heading are expressed as
(20)
ψ
˙
=
r
,
r
˙
=
1
J
z
τ
ψ
,
which, like in roll and pitch, give rise to the second order ODE:
(21)
ψ
¨
=
1
J
z
τ
θ
.
Equations (17), (19), and (21) can be combined and written in state-space form as in
(22)
ϕ
˙
ϕ
¨
θ
˙
θ
¨
ψ
˙
ψ
¨
=
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
ϕ
ϕ
˙
θ
θ
˙
ψ
ψ
˙
+
0
0
0
J
x
-
1
0
0
0
0
0
0
J
y
-
1
0
0
0
0
0
0
J
z
-
1
τ
ϕ
τ
θ
τ
ψ
,
where τ
ϕ, τ
θ, and τ
ψ are the torques required to give the required roll, pitch, and yaw, respectively.Again, using the forward Euler method we express (22) in discrete form as
(23)
ϕ
k
+
1
v
ϕ
k
+
1
θ
k
+
1
v
θ
k
+
1
ψ
k
+
1
v
ψ
k
+
1
=
1
Δ
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
T
0
0
0
0
0
1
ϕ
k
v
ϕ
k
θ
k
v
θ
k
ψ
k
v
ψ
k
+
0
0
0
Δ
T
J
x
-
1
0
0
0
0
0
0
Δ
T
J
y
-
1
0
0
0
0
0
0
Δ
T
J
z
-
1
τ
ϕ
k
τ
θ
k
τ
ψ
k
.
Since we are interested in measuring ϕ
(
k
), θ
(
k
), and ψ
(
k
), we will use the output matrix C
=
[
1
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
0
].So far we have formulated the necessary equations used to build the three controllers. Figure2 shows, in block diagram form, what we have done so far. But that is not the whole story. In the next section we will look at exactly how model predictive control is applied to the quadrotor system using the discrete state-space equations formulated in this section.


## 5. Model Predictive Control

One of the many tenets deployed in control of complex systems like the quadrotor using MPC is to use models that reduce the computation burden which is a necessity when it comes to online implementation. It is also agreed upon that linear models perform better in this respect than the nonlinear models they represent. In this and subsequent sections the performance of a qudrotor controller designed using Laguerre-based MPC (LMPC) is compared with that designed using the more common linear state-space approach presented in [7]. We will call the linear state-space method SS-MPC.


### 5.1. Linear State-Space MPC (SS-MPC)

Linear state-space MPC is modeled using a linear state-space relation and plant constraints are modeled using linear equalities and inequalities. When combined with convex quadratic cost function this implies that the desired control action can be obtained, at each sample interval, via the solution of a corresponding quadratic program. This is attractive because the quadratic programs can be solved efficiently online [7]. The general block diagram representing SSMPC is shown in Figure 3.Figure 3
Block diagram of SS-MPC.


#### 5.1.1. Plant

The plant is modeled by using the following state-space system:(24)
x
k
+
1
=
A
x
k
+
B
u
k
+
w
k
,
y
k
=
C
x
k
+
D
u
k
+
v
k
,
where w
k is the state noise and v
k is the measurement noise. Both w
k and v
k are assumed to be Gaussian distributed with zero mean, respective covariances of W and V, and cross covariance Z. This is represented mathematically as
(25)
w
k
v
k
~
N
0
0
,
W
Z
Z
T
V
.


#### 5.1.2. Observer

Using Gaussian assumptions stated above it is possible to make optimal predictions of state and output using the Kalman filter as(26)
x
^
k
+
1
∣
k
=
A
x
^
k
∣
k
-
1
+
B
u
+
K
y
k
-
y
^
k
∣
k
-
1
,
y
^
k
∣
k
-
1
=
C
x
^
k
∣
k
-
1
+
D
u
k
.
The closed loop gain K is found by solving the discrete-time algebraic Riccati equation (DARE):
(27)
K
=
A
P
C
T
+
Z
C
P
C
T
+
V
-
1
,
P
=
W
+
A
P
A
T
-
A
P
C
T
+
Z
C
P
C
T
+
V
-
1
Z
T
+
C
P
A
T
.


#### 5.1.3. Optimal Estimation of State and Output

The general equation representing the optimal estimate of state at instantj and written in terms of the initial state x
^
k
+
1
∣
k and future control inputs u
k
+
i
∣
k is given by
(28)
x
^
k
+
j
∣
k
=
A
j
-
1
x
^
k
+
1
∣
k
+
∑
n
=
1
j
-
1
‍
A
j
-
n
-
1
B
u
k
+
n
∣
k
.
Similarly the output equation is given by
(29)
y
^
k
+
j
∣
k
=
C
A
j
-
1
x
^
k
+
1
∣
k
+
C
∑
n
=
1
j
-
1
‍
A
j
-
n
-
1
B
u
k
+
n
∣
k
+
D
u
k
+
j
∣
k
.
The vector containing all output vectors can be written as
(30)
Y
k
=
y
k
+
1
∣
k
⋮
y
k
+
N
∣
k
,
while that containing all control actions can be written as
(31)
U
k
=
u
k
+
1
∣
k
⋮
u
k
+
N
∣
k
.
Therefore, we can write Y
k as
(32)
Y
k
=
F
x
^
k
+
1
∣
k
+
Φ
U
k
,
where
(33)
F
=
C
C
A
C
A
2
⋮
C
A
N
-
1
,
Φ
=
D
C
B
D
C
A
B
C
B
D
⋮
C
A
N
-
2
B
⋯
⋯
C
B
D
.


#### 5.1.4. Cost Function

The prime goal in MPC is to reject disturbances whilst tracking a reference signal. But at each instant it has to ensure that the control signal is within reasonable range that is practical for actuation. An objective function that combines these two tasks is(34)
J
x
^
k
+
1
∣
k
,
U
k
=
1
2
∑
n
=
1
N
‍
y
^
k
+
n
∣
k
-
r
k
+
n
Q
2
+
u
k
+
n
∣
k
-
u
k
+
n
-
1
∣
k
S
2
,
where both Q and S are assumed to be symmetric and positive definite.


### 5.2. Laguerre-Based MPC (LMPC)

Model predictive control designed using Laguerre functions is developed and summarized in [2]. The author presented this design technique for both single-variable and multivariable systems. Fortunately, the derivations for the single-variable case and the multivariable case are very similar and knowing one would give enough insight into the derivation of the other. For brevity only the single-variable case is presented and it is hoped that this is enough to set forth the LMPC design technique.Consider a plant withp inputs, q outputs, and n states as described by (35) where the subscript m stands for model
(35)
x
m
k
+
1
=
A
m
x
m
k
+
B
m
u
k
+
B
d
ω
k
,
y
k
=
C
m
x
k
,
where u
(
k
) is the input variable, y
(
k
) is the process output, and x
m
(
k
) is the state vector. ω
(
k
) is the input disturbance and is assumed to be a sequence of integrated white noise.The plant described by (35) can be expressed in augmented state-space form [2, 6] as
(36)
x
k
+
1
=
A
x
k
+
B
Δ
u
k
+
B
ϵ
ϵ
k
,
y
k
=
C
x
k
,
where A
=
A
m
0
n
×
q
C
m
A
m
I
q
×
q, B
=
B
m
C
m
B
m, B
ϵ
=
B
d
C
m
B
d
C
=
0
q
×
n
I
q
×
q, and the state x
(
k
)
=
Δ
x
m
(
k
)
y
(
k
)
T. ϵ
(
k
) is a zero mean white noise sequence related to the disturbance by the difference equation ϵ
(
k
)
=
ω
(
k
)
-
ω
(
k
-
1
) [6].


#### 5.2.1. Design Framework

In designing MPC using Laguerre functions the control trajectoryΔ
U is expressed using a set of orthonormal functions calledLaguerre functions. Since the state and output vectors can also be described in terms of Δ
U, it follows that they too can be expressed using Laguerre functions.At sampling instantk
i, the state variable x
(
k
i
) is available through measurement. The future control trajectory is given by
(37)
Δ
U
=
Δ
u
k
i
,
Δ
u
k
i
+
1
,
…
,
Δ
u
k
i
+
N
c
-
1
,
where N
c is the control horizon.Laguerre functions can approximate the incremental terms contained inΔ
U. The z-transfroms of the discrete-time Laguerre functions are written as
(38)
Γ
1
z
=
1
-
a
2
1
-
a
z
-
1
,
Γ
2
z
=
1
-
a
2
1
-
a
z
-
1
z
-
1
-
a
1
-
a
z
-
1
,
⋮
Γ
N
z
=
1
-
a
2
1
-
a
z
-
1
z
-
1
-
a
1
-
a
z
-
1
N
-
1
.
In the discrete-time Laguerre network of (38), a is the pole of the discrete-time Laguerre network and 0
≤
a
<
1 for stability of the network. The parameter a is called the scaling factor and N
=
1,2
,
3
,
… is the number of Laguerre terms used in the network. Note that
(39)
Γ
k
z
=
Γ
k
-
1
z
z
-
1
-
a
1
-
a
z
-
1
,
with the first term as Γ
1
(
z
)
=
1
-
a
2
/
1
-
a
z
-
1.Letl
j
(
k
) be the inverse z-transform of the jth term in the discrete Laguerre network and L
(
k
) the vector containing all inverse z-transform terms. Then taking advantage of (39), successive inverse z-transform vectors are obtained through the difference equation:
(40)
L
k
+
1
=
A
l
L
k
.
The matrix A
l has size N
×
N and is a function of parameters a and β
=
1
-
a
2. The initial condition is given by
(41)
L
0
⊤
=
β
1
-
a
a
2
⋯
-
1
N
-
1
a
N
-
1
.
In the case where N
=
5, matrix A
l and the initial condition L
(
0
) are
(42)
a
0
0
0
0
β
a
0
0
0
-
a
β
β
a
0
0
a
2
β
-
a
β
β
a
0
-
a
3
β
a
2
β
-
a
β
β
a
,
(43)
L
0
=
β
1
-
a
a
2
-
a
3
a
4
,
respectively.At sampling instantk
i, the control trajectory is described using Laguerre functions as in
(44)
Δ
u
k
i
+
k
=
∑
j
=
1
N
‍
c
j
k
i
l
j
k
i
,
where k
=
0,1
,
2
,
…
,
N
p with N
p as the prediction horizon, N terms used in the expansion and l
j
(
k
i
) is the inverse z-transform of the jth term in the discrete Laguerre network.Equation (44) can be rewritten as
(45)
Δ
u
k
i
+
k
=
L
k
⊤
η
,
where L
(
k
)
=
l
1
(
k
)
l
2
(
k
)
⋯
l
N
(
k
)
T, η
=
c
1
c
2
⋯
c
N
T, and the coefficients c
1
,
c
2
,
…
,
c
N are obtained from system data.At an arbitrary future instantm, the state is described using Laguerre functions as
(46)
x
k
i
+
m
∣
k
i
=
A
m
x
k
i
+
∑
i
=
0
m
-
1
‍
A
m
-
i
-
1
B
L
i
T
η
.
Similarly, the output is described as
(47)
y
k
i
+
m
∣
k
i
=
C
A
m
x
k
i
+
∑
i
=
0
m
-
1
‍
C
A
m
-
i
-
1
B
L
i
T
η
.


#### 5.2.2. Cost Function

The cost function is used to choose the optimal control trajectoryΔ
U to bring the predicted output as close as possible to the set-point. The cost function
(48)
J
=
∑
m
=
1
N
p
‍
x
k
i
+
m
∣
k
i
T
Q
x
k
i
+
m
∣
k
i
+
η
T
R
L
η
minimizes the error between the set-point signal and the output in the shortest possible time by carefully tuning the weighting matrices Q
≥
0 and R
L
>
0.


#### 5.2.3. Cost Function Minimization

The state variable in (46) can be rewritten as
(49)
x
k
i
+
m
∣
k
i
=
A
m
x
k
i
+
ϕ
m
T
η
,
where ϕ
(
m
)
T
=
∑
i
=
0
m
-
1
A
m
-
i
-
1
B
L
(
i
)
T.By substituting (49) into (48) and performing the partial derivative ∂
J
/
∂
η
=
0, the Laguerre coefficients vector is found to be
(50)
η
=
-
Ω
-
1
Ψ
x
k
i
,
with Ω
=
∑
m
=
1
N
p
ϕ
(
m
)
Q
ϕ
(
m
)
T
+
R
L and Ψ
=
ϕ
(
m
)
Q
A
m.


#### 5.2.4. Receding Horizon Control

In receding horizon control (RHC) only the first term inΔ
U, that is, Δ
u
(
k
i
), is implemented at instant k
i. The rest of the sequence is ignored. Only the most recent measurement is taken to form the state vector for calculation of the control signal. This procedure is repeated in real-time to give the receding horizon control law [6, 8].


#### 5.2.5. Stability

In stability analysis we make use of the technique of exponential data weighting originated by Anderson and Moore [9] and applied to MPC in [6]. More specifically we will concentrate on the discrete exponential factor e
λ
Δ
t where Δ
t is the sampling interval and the discrete weights form a geometric sequence {
α
j
,
j
=
0,1
,
2
,
…
}.The proposed cost function is similar to the one used in linear quadratic regulator (LQR) systems but with the inclusion of discrete weights(51)
J
w
=
∑
j
=
1
N
p
‍
α
-
2
j
x
k
i
+
j
∣
k
i
T
Q
x
k
i
+
j
∣
k
i
+
∑
j
=
0
N
p
‍
α
-
2
j
Δ
u
k
i
+
j
T
R
Δ
u
k
i
+
j
.
For α
>
1, the exponential weights α
-
2
j, j
=
1,2
,
3
,
…
,
N
p, put more emphasis on the current state x
k
i
+
j
∣
k
i and less emphasis on subsequent future states.The exponentially weighted cost function can be expressed more compactly as(52)
J
w
=
∑
j
=
1
N
p
‍
x
^
k
i
+
j
∣
k
i
T
Q
x
^
k
i
+
j
∣
k
i
+
∑
j
=
0
N
p
‍
Δ
u
^
k
i
+
j
T
R
Δ
u
^
k
i
+
j
,
with state equation
(53)
x
^
k
+
1
=
A
α
x
^
k
+
B
α
Δ
u
^
k
,
and A
α
=
A
/
α and B
α
=
B
/
α.WithQ
≥
0, R
>
0, and N
p
→
∞, minimizing the cost function J
w is equivalent to the DLQR problem which is solved using algebraic Riccati equation (54). The pair (
A
α
,
B
α
) is assumed to be controllable and (
A
α
,
C
) observable with Q
=
C
T
C. Then there is a stabilizing state feedback control gain matrix K
w,
(54)
K
w
=
R
+
α
-
2
B
T
P
w
B
-
1
α
-
2
B
T
P
w
A
,
A
T
α
P
w
-
P
w
B
α
R
+
B
T
α
P
w
B
α
-
1
B
T
α
P
w
A
α
+
Q
-
P
w
=
0
that gives a stable closed loop system with all its eigenvalues inside the unit circle and the the closed loop system being described by
(55)
x
^
k
i
+
j
+
1
∣
k
i
=
α
-
1
A
-
B
K
w
x
^
k
i
+
j
∣
k
i
.
From (55), the transformed system has all its eigenvalues inside the unit circle by taking N
p
→
∞. So
(56)
α
-
1
λ
max
⁡
A
-
B
K
w
<
1
,
which gives
(57)
λ
max
⁡
A
-
B
K
w
<
α
.
Thus by choosing α
>
1 there is a great chance that the system will be stable. Several simulations on the quadrotor system indicate that a choice of α slightly greater than unity makes the system stable almost all the time.


## 5.1. Linear State-Space MPC (SS-MPC)

Linear state-space MPC is modeled using a linear state-space relation and plant constraints are modeled using linear equalities and inequalities. When combined with convex quadratic cost function this implies that the desired control action can be obtained, at each sample interval, via the solution of a corresponding quadratic program. This is attractive because the quadratic programs can be solved efficiently online [7]. The general block diagram representing SSMPC is shown in Figure 3.Figure 3
Block diagram of SS-MPC.


### 5.1.1. Plant

The plant is modeled by using the following state-space system:(24)
x
k
+
1
=
A
x
k
+
B
u
k
+
w
k
,
y
k
=
C
x
k
+
D
u
k
+
v
k
,
where w
k is the state noise and v
k is the measurement noise. Both w
k and v
k are assumed to be Gaussian distributed with zero mean, respective covariances of W and V, and cross covariance Z. This is represented mathematically as
(25)
w
k
v
k
~
N
0
0
,
W
Z
Z
T
V
.


### 5.1.2. Observer

Using Gaussian assumptions stated above it is possible to make optimal predictions of state and output using the Kalman filter as(26)
x
^
k
+
1
∣
k
=
A
x
^
k
∣
k
-
1
+
B
u
+
K
y
k
-
y
^
k
∣
k
-
1
,
y
^
k
∣
k
-
1
=
C
x
^
k
∣
k
-
1
+
D
u
k
.
The closed loop gain K is found by solving the discrete-time algebraic Riccati equation (DARE):
(27)
K
=
A
P
C
T
+
Z
C
P
C
T
+
V
-
1
,
P
=
W
+
A
P
A
T
-
A
P
C
T
+
Z
C
P
C
T
+
V
-
1
Z
T
+
C
P
A
T
.


### 5.1.3. Optimal Estimation of State and Output

The general equation representing the optimal estimate of state at instantj and written in terms of the initial state x
^
k
+
1
∣
k and future control inputs u
k
+
i
∣
k is given by
(28)
x
^
k
+
j
∣
k
=
A
j
-
1
x
^
k
+
1
∣
k
+
∑
n
=
1
j
-
1
‍
A
j
-
n
-
1
B
u
k
+
n
∣
k
.
Similarly the output equation is given by
(29)
y
^
k
+
j
∣
k
=
C
A
j
-
1
x
^
k
+
1
∣
k
+
C
∑
n
=
1
j
-
1
‍
A
j
-
n
-
1
B
u
k
+
n
∣
k
+
D
u
k
+
j
∣
k
.
The vector containing all output vectors can be written as
(30)
Y
k
=
y
k
+
1
∣
k
⋮
y
k
+
N
∣
k
,
while that containing all control actions can be written as
(31)
U
k
=
u
k
+
1
∣
k
⋮
u
k
+
N
∣
k
.
Therefore, we can write Y
k as
(32)
Y
k
=
F
x
^
k
+
1
∣
k
+
Φ
U
k
,
where
(33)
F
=
C
C
A
C
A
2
⋮
C
A
N
-
1
,
Φ
=
D
C
B
D
C
A
B
C
B
D
⋮
C
A
N
-
2
B
⋯
⋯
C
B
D
.


### 5.1.4. Cost Function

The prime goal in MPC is to reject disturbances whilst tracking a reference signal. But at each instant it has to ensure that the control signal is within reasonable range that is practical for actuation. An objective function that combines these two tasks is(34)
J
x
^
k
+
1
∣
k
,
U
k
=
1
2
∑
n
=
1
N
‍
y
^
k
+
n
∣
k
-
r
k
+
n
Q
2
+
u
k
+
n
∣
k
-
u
k
+
n
-
1
∣
k
S
2
,
where both Q and S are assumed to be symmetric and positive definite.


## 5.1.1. Plant

The plant is modeled by using the following state-space system:(24)
x
k
+
1
=
A
x
k
+
B
u
k
+
w
k
,
y
k
=
C
x
k
+
D
u
k
+
v
k
,
where w
k is the state noise and v
k is the measurement noise. Both w
k and v
k are assumed to be Gaussian distributed with zero mean, respective covariances of W and V, and cross covariance Z. This is represented mathematically as
(25)
w
k
v
k
~
N
0
0
,
W
Z
Z
T
V
.


## 5.1.2. Observer

Using Gaussian assumptions stated above it is possible to make optimal predictions of state and output using the Kalman filter as(26)
x
^
k
+
1
∣
k
=
A
x
^
k
∣
k
-
1
+
B
u
+
K
y
k
-
y
^
k
∣
k
-
1
,
y
^
k
∣
k
-
1
=
C
x
^
k
∣
k
-
1
+
D
u
k
.
The closed loop gain K is found by solving the discrete-time algebraic Riccati equation (DARE):
(27)
K
=
A
P
C
T
+
Z
C
P
C
T
+
V
-
1
,
P
=
W
+
A
P
A
T
-
A
P
C
T
+
Z
C
P
C
T
+
V
-
1
Z
T
+
C
P
A
T
.


## 5.1.3. Optimal Estimation of State and Output

The general equation representing the optimal estimate of state at instantj and written in terms of the initial state x
^
k
+
1
∣
k and future control inputs u
k
+
i
∣
k is given by
(28)
x
^
k
+
j
∣
k
=
A
j
-
1
x
^
k
+
1
∣
k
+
∑
n
=
1
j
-
1
‍
A
j
-
n
-
1
B
u
k
+
n
∣
k
.
Similarly the output equation is given by
(29)
y
^
k
+
j
∣
k
=
C
A
j
-
1
x
^
k
+
1
∣
k
+
C
∑
n
=
1
j
-
1
‍
A
j
-
n
-
1
B
u
k
+
n
∣
k
+
D
u
k
+
j
∣
k
.
The vector containing all output vectors can be written as
(30)
Y
k
=
y
k
+
1
∣
k
⋮
y
k
+
N
∣
k
,
while that containing all control actions can be written as
(31)
U
k
=
u
k
+
1
∣
k
⋮
u
k
+
N
∣
k
.
Therefore, we can write Y
k as
(32)
Y
k
=
F
x
^
k
+
1
∣
k
+
Φ
U
k
,
where
(33)
F
=
C
C
A
C
A
2
⋮
C
A
N
-
1
,
Φ
=
D
C
B
D
C
A
B
C
B
D
⋮
C
A
N
-
2
B
⋯
⋯
C
B
D
.


## 5.1.4. Cost Function

The prime goal in MPC is to reject disturbances whilst tracking a reference signal. But at each instant it has to ensure that the control signal is within reasonable range that is practical for actuation. An objective function that combines these two tasks is(34)
J
x
^
k
+
1
∣
k
,
U
k
=
1
2
∑
n
=
1
N
‍
y
^
k
+
n
∣
k
-
r
k
+
n
Q
2
+
u
k
+
n
∣
k
-
u
k
+
n
-
1
∣
k
S
2
,
where both Q and S are assumed to be symmetric and positive definite.


## 5.2. Laguerre-Based MPC (LMPC)

Model predictive control designed using Laguerre functions is developed and summarized in [2]. The author presented this design technique for both single-variable and multivariable systems. Fortunately, the derivations for the single-variable case and the multivariable case are very similar and knowing one would give enough insight into the derivation of the other. For brevity only the single-variable case is presented and it is hoped that this is enough to set forth the LMPC design technique.Consider a plant withp inputs, q outputs, and n states as described by (35) where the subscript m stands for model
(35)
x
m
k
+
1
=
A
m
x
m
k
+
B
m
u
k
+
B
d
ω
k
,
y
k
=
C
m
x
k
,
where u
(
k
) is the input variable, y
(
k
) is the process output, and x
m
(
k
) is the state vector. ω
(
k
) is the input disturbance and is assumed to be a sequence of integrated white noise.The plant described by (35) can be expressed in augmented state-space form [2, 6] as
(36)
x
k
+
1
=
A
x
k
+
B
Δ
u
k
+
B
ϵ
ϵ
k
,
y
k
=
C
x
k
,
where A
=
A
m
0
n
×
q
C
m
A
m
I
q
×
q, B
=
B
m
C
m
B
m, B
ϵ
=
B
d
C
m
B
d
C
=
0
q
×
n
I
q
×
q, and the state x
(
k
)
=
Δ
x
m
(
k
)
y
(
k
)
T. ϵ
(
k
) is a zero mean white noise sequence related to the disturbance by the difference equation ϵ
(
k
)
=
ω
(
k
)
-
ω
(
k
-
1
) [6].


### 5.2.1. Design Framework

In designing MPC using Laguerre functions the control trajectoryΔ
U is expressed using a set of orthonormal functions calledLaguerre functions. Since the state and output vectors can also be described in terms of Δ
U, it follows that they too can be expressed using Laguerre functions.At sampling instantk
i, the state variable x
(
k
i
) is available through measurement. The future control trajectory is given by
(37)
Δ
U
=
Δ
u
k
i
,
Δ
u
k
i
+
1
,
…
,
Δ
u
k
i
+
N
c
-
1
,
where N
c is the control horizon.Laguerre functions can approximate the incremental terms contained inΔ
U. The z-transfroms of the discrete-time Laguerre functions are written as
(38)
Γ
1
z
=
1
-
a
2
1
-
a
z
-
1
,
Γ
2
z
=
1
-
a
2
1
-
a
z
-
1
z
-
1
-
a
1
-
a
z
-
1
,
⋮
Γ
N
z
=
1
-
a
2
1
-
a
z
-
1
z
-
1
-
a
1
-
a
z
-
1
N
-
1
.
In the discrete-time Laguerre network of (38), a is the pole of the discrete-time Laguerre network and 0
≤
a
<
1 for stability of the network. The parameter a is called the scaling factor and N
=
1,2
,
3
,
… is the number of Laguerre terms used in the network. Note that
(39)
Γ
k
z
=
Γ
k
-
1
z
z
-
1
-
a
1
-
a
z
-
1
,
with the first term as Γ
1
(
z
)
=
1
-
a
2
/
1
-
a
z
-
1.Letl
j
(
k
) be the inverse z-transform of the jth term in the discrete Laguerre network and L
(
k
) the vector containing all inverse z-transform terms. Then taking advantage of (39), successive inverse z-transform vectors are obtained through the difference equation:
(40)
L
k
+
1
=
A
l
L
k
.
The matrix A
l has size N
×
N and is a function of parameters a and β
=
1
-
a
2. The initial condition is given by
(41)
L
0
⊤
=
β
1
-
a
a
2
⋯
-
1
N
-
1
a
N
-
1
.
In the case where N
=
5, matrix A
l and the initial condition L
(
0
) are
(42)
a
0
0
0
0
β
a
0
0
0
-
a
β
β
a
0
0
a
2
β
-
a
β
β
a
0
-
a
3
β
a
2
β
-
a
β
β
a
,
(43)
L
0
=
β
1
-
a
a
2
-
a
3
a
4
,
respectively.At sampling instantk
i, the control trajectory is described using Laguerre functions as in
(44)
Δ
u
k
i
+
k
=
∑
j
=
1
N
‍
c
j
k
i
l
j
k
i
,
where k
=
0,1
,
2
,
…
,
N
p with N
p as the prediction horizon, N terms used in the expansion and l
j
(
k
i
) is the inverse z-transform of the jth term in the discrete Laguerre network.Equation (44) can be rewritten as
(45)
Δ
u
k
i
+
k
=
L
k
⊤
η
,
where L
(
k
)
=
l
1
(
k
)
l
2
(
k
)
⋯
l
N
(
k
)
T, η
=
c
1
c
2
⋯
c
N
T, and the coefficients c
1
,
c
2
,
…
,
c
N are obtained from system data.At an arbitrary future instantm, the state is described using Laguerre functions as
(46)
x
k
i
+
m
∣
k
i
=
A
m
x
k
i
+
∑
i
=
0
m
-
1
‍
A
m
-
i
-
1
B
L
i
T
η
.
Similarly, the output is described as
(47)
y
k
i
+
m
∣
k
i
=
C
A
m
x
k
i
+
∑
i
=
0
m
-
1
‍
C
A
m
-
i
-
1
B
L
i
T
η
.


### 5.2.2. Cost Function

The cost function is used to choose the optimal control trajectoryΔ
U to bring the predicted output as close as possible to the set-point. The cost function
(48)
J
=
∑
m
=
1
N
p
‍
x
k
i
+
m
∣
k
i
T
Q
x
k
i
+
m
∣
k
i
+
η
T
R
L
η
minimizes the error between the set-point signal and the output in the shortest possible time by carefully tuning the weighting matrices Q
≥
0 and R
L
>
0.


### 5.2.3. Cost Function Minimization

The state variable in (46) can be rewritten as
(49)
x
k
i
+
m
∣
k
i
=
A
m
x
k
i
+
ϕ
m
T
η
,
where ϕ
(
m
)
T
=
∑
i
=
0
m
-
1
A
m
-
i
-
1
B
L
(
i
)
T.By substituting (49) into (48) and performing the partial derivative ∂
J
/
∂
η
=
0, the Laguerre coefficients vector is found to be
(50)
η
=
-
Ω
-
1
Ψ
x
k
i
,
with Ω
=
∑
m
=
1
N
p
ϕ
(
m
)
Q
ϕ
(
m
)
T
+
R
L and Ψ
=
ϕ
(
m
)
Q
A
m.


### 5.2.4. Receding Horizon Control

In receding horizon control (RHC) only the first term inΔ
U, that is, Δ
u
(
k
i
), is implemented at instant k
i. The rest of the sequence is ignored. Only the most recent measurement is taken to form the state vector for calculation of the control signal. This procedure is repeated in real-time to give the receding horizon control law [6, 8].


### 5.2.5. Stability

In stability analysis we make use of the technique of exponential data weighting originated by Anderson and Moore [9] and applied to MPC in [6]. More specifically we will concentrate on the discrete exponential factor e
λ
Δ
t where Δ
t is the sampling interval and the discrete weights form a geometric sequence {
α
j
,
j
=
0,1
,
2
,
…
}.The proposed cost function is similar to the one used in linear quadratic regulator (LQR) systems but with the inclusion of discrete weights(51)
J
w
=
∑
j
=
1
N
p
‍
α
-
2
j
x
k
i
+
j
∣
k
i
T
Q
x
k
i
+
j
∣
k
i
+
∑
j
=
0
N
p
‍
α
-
2
j
Δ
u
k
i
+
j
T
R
Δ
u
k
i
+
j
.
For α
>
1, the exponential weights α
-
2
j, j
=
1,2
,
3
,
…
,
N
p, put more emphasis on the current state x
k
i
+
j
∣
k
i and less emphasis on subsequent future states.The exponentially weighted cost function can be expressed more compactly as(52)
J
w
=
∑
j
=
1
N
p
‍
x
^
k
i
+
j
∣
k
i
T
Q
x
^
k
i
+
j
∣
k
i
+
∑
j
=
0
N
p
‍
Δ
u
^
k
i
+
j
T
R
Δ
u
^
k
i
+
j
,
with state equation
(53)
x
^
k
+
1
=
A
α
x
^
k
+
B
α
Δ
u
^
k
,
and A
α
=
A
/
α and B
α
=
B
/
α.WithQ
≥
0, R
>
0, and N
p
→
∞, minimizing the cost function J
w is equivalent to the DLQR problem which is solved using algebraic Riccati equation (54). The pair (
A
α
,
B
α
) is assumed to be controllable and (
A
α
,
C
) observable with Q
=
C
T
C. Then there is a stabilizing state feedback control gain matrix K
w,
(54)
K
w
=
R
+
α
-
2
B
T
P
w
B
-
1
α
-
2
B
T
P
w
A
,
A
T
α
P
w
-
P
w
B
α
R
+
B
T
α
P
w
B
α
-
1
B
T
α
P
w
A
α
+
Q
-
P
w
=
0
that gives a stable closed loop system with all its eigenvalues inside the unit circle and the the closed loop system being described by
(55)
x
^
k
i
+
j
+
1
∣
k
i
=
α
-
1
A
-
B
K
w
x
^
k
i
+
j
∣
k
i
.
From (55), the transformed system has all its eigenvalues inside the unit circle by taking N
p
→
∞. So
(56)
α
-
1
λ
max
⁡
A
-
B
K
w
<
1
,
which gives
(57)
λ
max
⁡
A
-
B
K
w
<
α
.
Thus by choosing α
>
1 there is a great chance that the system will be stable. Several simulations on the quadrotor system indicate that a choice of α slightly greater than unity makes the system stable almost all the time.


## 5.2.1. Design Framework

In designing MPC using Laguerre functions the control trajectoryΔ
U is expressed using a set of orthonormal functions calledLaguerre functions. Since the state and output vectors can also be described in terms of Δ
U, it follows that they too can be expressed using Laguerre functions.At sampling instantk
i, the state variable x
(
k
i
) is available through measurement. The future control trajectory is given by
(37)
Δ
U
=
Δ
u
k
i
,
Δ
u
k
i
+
1
,
…
,
Δ
u
k
i
+
N
c
-
1
,
where N
c is the control horizon.Laguerre functions can approximate the incremental terms contained inΔ
U. The z-transfroms of the discrete-time Laguerre functions are written as
(38)
Γ
1
z
=
1
-
a
2
1
-
a
z
-
1
,
Γ
2
z
=
1
-
a
2
1
-
a
z
-
1
z
-
1
-
a
1
-
a
z
-
1
,
⋮
Γ
N
z
=
1
-
a
2
1
-
a
z
-
1
z
-
1
-
a
1
-
a
z
-
1
N
-
1
.
In the discrete-time Laguerre network of (38), a is the pole of the discrete-time Laguerre network and 0
≤
a
<
1 for stability of the network. The parameter a is called the scaling factor and N
=
1,2
,
3
,
… is the number of Laguerre terms used in the network. Note that
(39)
Γ
k
z
=
Γ
k
-
1
z
z
-
1
-
a
1
-
a
z
-
1
,
with the first term as Γ
1
(
z
)
=
1
-
a
2
/
1
-
a
z
-
1.Letl
j
(
k
) be the inverse z-transform of the jth term in the discrete Laguerre network and L
(
k
) the vector containing all inverse z-transform terms. Then taking advantage of (39), successive inverse z-transform vectors are obtained through the difference equation:
(40)
L
k
+
1
=
A
l
L
k
.
The matrix A
l has size N
×
N and is a function of parameters a and β
=
1
-
a
2. The initial condition is given by
(41)
L
0
⊤
=
β
1
-
a
a
2
⋯
-
1
N
-
1
a
N
-
1
.
In the case where N
=
5, matrix A
l and the initial condition L
(
0
) are
(42)
a
0
0
0
0
β
a
0
0
0
-
a
β
β
a
0
0
a
2
β
-
a
β
β
a
0
-
a
3
β
a
2
β
-
a
β
β
a
,
(43)
L
0
=
β
1
-
a
a
2
-
a
3
a
4
,
respectively.At sampling instantk
i, the control trajectory is described using Laguerre functions as in
(44)
Δ
u
k
i
+
k
=
∑
j
=
1
N
‍
c
j
k
i
l
j
k
i
,
where k
=
0,1
,
2
,
…
,
N
p with N
p as the prediction horizon, N terms used in the expansion and l
j
(
k
i
) is the inverse z-transform of the jth term in the discrete Laguerre network.Equation (44) can be rewritten as
(45)
Δ
u
k
i
+
k
=
L
k
⊤
η
,
where L
(
k
)
=
l
1
(
k
)
l
2
(
k
)
⋯
l
N
(
k
)
T, η
=
c
1
c
2
⋯
c
N
T, and the coefficients c
1
,
c
2
,
…
,
c
N are obtained from system data.At an arbitrary future instantm, the state is described using Laguerre functions as
(46)
x
k
i
+
m
∣
k
i
=
A
m
x
k
i
+
∑
i
=
0
m
-
1
‍
A
m
-
i
-
1
B
L
i
T
η
.
Similarly, the output is described as
(47)
y
k
i
+
m
∣
k
i
=
C
A
m
x
k
i
+
∑
i
=
0
m
-
1
‍
C
A
m
-
i
-
1
B
L
i
T
η
.


## 5.2.2. Cost Function

The cost function is used to choose the optimal control trajectoryΔ
U to bring the predicted output as close as possible to the set-point. The cost function
(48)
J
=
∑
m
=
1
N
p
‍
x
k
i
+
m
∣
k
i
T
Q
x
k
i
+
m
∣
k
i
+
η
T
R
L
η
minimizes the error between the set-point signal and the output in the shortest possible time by carefully tuning the weighting matrices Q
≥
0 and R
L
>
0.


## 5.2.3. Cost Function Minimization

The state variable in (46) can be rewritten as
(49)
x
k
i
+
m
∣
k
i
=
A
m
x
k
i
+
ϕ
m
T
η
,
where ϕ
(
m
)
T
=
∑
i
=
0
m
-
1
A
m
-
i
-
1
B
L
(
i
)
T.By substituting (49) into (48) and performing the partial derivative ∂
J
/
∂
η
=
0, the Laguerre coefficients vector is found to be
(50)
η
=
-
Ω
-
1
Ψ
x
k
i
,
with Ω
=
∑
m
=
1
N
p
ϕ
(
m
)
Q
ϕ
(
m
)
T
+
R
L and Ψ
=
ϕ
(
m
)
Q
A
m.


## 5.2.4. Receding Horizon Control

In receding horizon control (RHC) only the first term inΔ
U, that is, Δ
u
(
k
i
), is implemented at instant k
i. The rest of the sequence is ignored. Only the most recent measurement is taken to form the state vector for calculation of the control signal. This procedure is repeated in real-time to give the receding horizon control law [6, 8].


## 5.2.5. Stability

In stability analysis we make use of the technique of exponential data weighting originated by Anderson and Moore [9] and applied to MPC in [6]. More specifically we will concentrate on the discrete exponential factor e
λ
Δ
t where Δ
t is the sampling interval and the discrete weights form a geometric sequence {
α
j
,
j
=
0,1
,
2
,
…
}.The proposed cost function is similar to the one used in linear quadratic regulator (LQR) systems but with the inclusion of discrete weights(51)
J
w
=
∑
j
=
1
N
p
‍
α
-
2
j
x
k
i
+
j
∣
k
i
T
Q
x
k
i
+
j
∣
k
i
+
∑
j
=
0
N
p
‍
α
-
2
j
Δ
u
k
i
+
j
T
R
Δ
u
k
i
+
j
.
For α
>
1, the exponential weights α
-
2
j, j
=
1,2
,
3
,
…
,
N
p, put more emphasis on the current state x
k
i
+
j
∣
k
i and less emphasis on subsequent future states.The exponentially weighted cost function can be expressed more compactly as(52)
J
w
=
∑
j
=
1
N
p
‍
x
^
k
i
+
j
∣
k
i
T
Q
x
^
k
i
+
j
∣
k
i
+
∑
j
=
0
N
p
‍
Δ
u
^
k
i
+
j
T
R
Δ
u
^
k
i
+
j
,
with state equation
(53)
x
^
k
+
1
=
A
α
x
^
k
+
B
α
Δ
u
^
k
,
and A
α
=
A
/
α and B
α
=
B
/
α.WithQ
≥
0, R
>
0, and N
p
→
∞, minimizing the cost function J
w is equivalent to the DLQR problem which is solved using algebraic Riccati equation (54). The pair (
A
α
,
B
α
) is assumed to be controllable and (
A
α
,
C
) observable with Q
=
C
T
C. Then there is a stabilizing state feedback control gain matrix K
w,
(54)
K
w
=
R
+
α
-
2
B
T
P
w
B
-
1
α
-
2
B
T
P
w
A
,
A
T
α
P
w
-
P
w
B
α
R
+
B
T
α
P
w
B
α
-
1
B
T
α
P
w
A
α
+
Q
-
P
w
=
0
that gives a stable closed loop system with all its eigenvalues inside the unit circle and the the closed loop system being described by
(55)
x
^
k
i
+
j
+
1
∣
k
i
=
α
-
1
A
-
B
K
w
x
^
k
i
+
j
∣
k
i
.
From (55), the transformed system has all its eigenvalues inside the unit circle by taking N
p
→
∞. So
(56)
α
-
1
λ
max
⁡
A
-
B
K
w
<
1
,
which gives
(57)
λ
max
⁡
A
-
B
K
w
<
α
.
Thus by choosing α
>
1 there is a great chance that the system will be stable. Several simulations on the quadrotor system indicate that a choice of α slightly greater than unity makes the system stable almost all the time.


## 6. Simulation Parameters and Results

In order to illustrate the controller’s stability we will check if the eigenvalues of each of the three individual controllers appear inside a unit circle. And we will use a trajectory in a 3D plane to check the overall controller’s ability to track a desired trajectory.Table1 shows the parameters of the quadrotor used in simulation.Table 1
Parameter
Value
Mass,m
0.7 kg
Length,l
0.275 m
J
x
0.1063 kg⋅m2
J
y
0.1063 kg⋅m2
J
z
0.2122 kg⋅m2


### 6.1. Stability Analysis

In this section we look at the location of eigenvalues of LMPC and check if they fall within the unit circle for stability of the system. We also compare them to those obtained from the optimal DLQR system. To ensure stability as explained in Section5.2.5 we use α
=
1.2.By looking at Figure4 we note that all the eigenvalues appear inside the unit circle as required. Also the eigenvalues obtained from LMPC closely match with those of the optimal DLQR. This not only shows the system is stable but also shows the controllers perform optimally.Figure 4
Eigenvalues for DLQR and LMPC in altitude control.


### 6.2. Trajectory Tracking

In this section we look at the performance of LMPC when used to track a 3D trajectory. Also LMPC is compared to SS-MPC and individual state trajectories are observed. The starting point of the trajectory is(
x
,
y
,
z
)
=
(
0,0
,
0
) and final point is (
x
,
y
,
z
)
=
(
5,5
,
10
). Parameter a
=
0.8187 while N
=
5. See Figure 5.Figure 5
Tracking a 3D trajectory.Clearly both LMPC and SS-MPC track the desired trajectory very well. See Figures5, 6 and 7. However, LMPC performs better than SS-MPC in that it requires only five parameters (N
=
5) to capture the control trajectory compared to SS-MPC’s minimum of 25 (N
c
=
25) parameters (see Table 2 for relationship of N and N
c with a
=
0.8187).Table 2
Control horizonN
c
N
Requireda
10
5
0.6065
15
5
0.7165
20
5
0.7788
25
5
0.8187
50
5
0.9048Trackingx (a) and y in (b).
(a)
(b)Altitude (z) (a) and heading angle (ψ) (b).
(a)
(b)


### 6.3. Control Horizon in Relation toa and N

The control horizon (N
c) is related to the parameters a and N [2] by
(58)
a
≈
e
-
N
/
N
c
.


#### 6.3.1. ConstantN

Using (58) and choosing a constant parameter N
=
5 we get Table 2.Thus we can increase the control horizon by increasing the parametera without necessarily changing the order N.


#### 6.3.2. Constant Control HorizonN
c

Assuming we want to achieve a control horizon of 30, we choose one parameter usuallyN because it is an integer and directly defines the order of the discrete Laguerre network. Therefater, we simply use (58) to find the corresponding scaling factor a as shown in Table 3.Table 3
Target control horizonN
c
ChosenN
Requireda
30
1
0.9672
30
3
0.9048
30
5
0.8465
30
7
0.7919
30
9
0.7408Figures8, 9, 10, 11, 12, and 13 compare two LMPC designs both of which yield a control horizon of 30. One design uses N
=
1 and the other N
=
9. Thus we can choose a Laguerre network with lower number of terms N (that gives lower computation burden) but with a larger scaling factor a to achieve the same control horizon. The computational cost is lower if a smaller number of parameters are used. For N
=
1, only one Laguerre term is used to capture the control trajectory while N
=
9 requires nine Laguerre terms to capture the same control trajectory.Figure 8
x trajectory N
=
1.Figure 9
x trajectory N
=
9.Figure 10
y trajectory N
=
1.Figure 11
y trajectory N
=
9.Figure 12
z trajectory N
=
1.Figure 13
z trajectory N
=
9.


## 6.1. Stability Analysis

In this section we look at the location of eigenvalues of LMPC and check if they fall within the unit circle for stability of the system. We also compare them to those obtained from the optimal DLQR system. To ensure stability as explained in Section5.2.5 we use α
=
1.2.By looking at Figure4 we note that all the eigenvalues appear inside the unit circle as required. Also the eigenvalues obtained from LMPC closely match with those of the optimal DLQR. This not only shows the system is stable but also shows the controllers perform optimally.Figure 4
Eigenvalues for DLQR and LMPC in altitude control.


## 6.2. Trajectory Tracking

In this section we look at the performance of LMPC when used to track a 3D trajectory. Also LMPC is compared to SS-MPC and individual state trajectories are observed. The starting point of the trajectory is(
x
,
y
,
z
)
=
(
0,0
,
0
) and final point is (
x
,
y
,
z
)
=
(
5,5
,
10
). Parameter a
=
0.8187 while N
=
5. See Figure 5.Figure 5
Tracking a 3D trajectory.Clearly both LMPC and SS-MPC track the desired trajectory very well. See Figures5, 6 and 7. However, LMPC performs better than SS-MPC in that it requires only five parameters (N
=
5) to capture the control trajectory compared to SS-MPC’s minimum of 25 (N
c
=
25) parameters (see Table 2 for relationship of N and N
c with a
=
0.8187).Table 2
Control horizonN
c
N
Requireda
10
5
0.6065
15
5
0.7165
20
5
0.7788
25
5
0.8187
50
5
0.9048Trackingx (a) and y in (b).
(a)
(b)Altitude (z) (a) and heading angle (ψ) (b).
(a)
(b)


## 6.3. Control Horizon in Relation toa and N

The control horizon (N
c) is related to the parameters a and N [2] by
(58)
a
≈
e
-
N
/
N
c
.


### 6.3.1. ConstantN

Using (58) and choosing a constant parameter N
=
5 we get Table 2.Thus we can increase the control horizon by increasing the parametera without necessarily changing the order N.


### 6.3.2. Constant Control HorizonN
c

Assuming we want to achieve a control horizon of 30, we choose one parameter usuallyN because it is an integer and directly defines the order of the discrete Laguerre network. Therefater, we simply use (58) to find the corresponding scaling factor a as shown in Table 3.Table 3
Target control horizonN
c
ChosenN
Requireda
30
1
0.9672
30
3
0.9048
30
5
0.8465
30
7
0.7919
30
9
0.7408Figures8, 9, 10, 11, 12, and 13 compare two LMPC designs both of which yield a control horizon of 30. One design uses N
=
1 and the other N
=
9. Thus we can choose a Laguerre network with lower number of terms N (that gives lower computation burden) but with a larger scaling factor a to achieve the same control horizon. The computational cost is lower if a smaller number of parameters are used. For N
=
1, only one Laguerre term is used to capture the control trajectory while N
=
9 requires nine Laguerre terms to capture the same control trajectory.Figure 8
x trajectory N
=
1.Figure 9
x trajectory N
=
9.Figure 10
y trajectory N
=
1.Figure 11
y trajectory N
=
9.Figure 12
z trajectory N
=
1.Figure 13
z trajectory N
=
9.


## 6.3.1. ConstantN

Using (58) and choosing a constant parameter N
=
5 we get Table 2.Thus we can increase the control horizon by increasing the parametera without necessarily changing the order N.


## 6.3.2. Constant Control HorizonN
c

Assuming we want to achieve a control horizon of 30, we choose one parameter usuallyN because it is an integer and directly defines the order of the discrete Laguerre network. Therefater, we simply use (58) to find the corresponding scaling factor a as shown in Table 3.Table 3
Target control horizonN
c
ChosenN
Requireda
30
1
0.9672
30
3
0.9048
30
5
0.8465
30
7
0.7919
30
9
0.7408Figures8, 9, 10, 11, 12, and 13 compare two LMPC designs both of which yield a control horizon of 30. One design uses N
=
1 and the other N
=
9. Thus we can choose a Laguerre network with lower number of terms N (that gives lower computation burden) but with a larger scaling factor a to achieve the same control horizon. The computational cost is lower if a smaller number of parameters are used. For N
=
1, only one Laguerre term is used to capture the control trajectory while N
=
9 requires nine Laguerre terms to capture the same control trajectory.Figure 8
x trajectory N
=
1.Figure 9
x trajectory N
=
9.Figure 10
y trajectory N
=
1.Figure 11
y trajectory N
=
9.Figure 12
z trajectory N
=
1.Figure 13
z trajectory N
=
9.


## 7. Conclusion

This paper has presented a solution to stability and trajectory tracking of quadrotor systems using a model predictive controller designed by using a special type of orthonormal functions called Laguerre functions. A quadratic cost function similar to that used in linear quadratic regulator (LQR) has been used. In stability analysis LMPC has been compared to the optimal DLQR system whereas in trajectory tracking LMPC has been compared to a popular linear state-space MPC desgin technique in which plant constraints are modeled using linear equalities and inequalities. The results from the simulations indicate that the controller performs very well and is considered feasible. With this perceived feasibility LMPC offers the added advantage that it can handle systems where rapid sampling and more complicated process dynamics are required [5, 10]. By selecting appropriate values of a and N LMPC reduces the number of parameters required for accurate prediction when using the traditional (MPC) approach. This is a big advantage in that, with the reduced number of parameters, online implementation might be possible where the traditional MPC would have failed.



---

*Source: 916864-2015-02-19.xml*