Quadrotor prototype

Jorge Miguel Brito Domingues

Dissertao para obteno do Grau de Mestre em
Engenharia Mecnica
Jri:
Presidente: Prof. Joo Rogrio Caldas Pinto (DEM)
Orientadores: Prof. Jos Ral Carreira Azinheira (DEM)
Prof. Alexandra Bento Moutinho (DEM)
Vogal: Prof. Agostinho Rui Alves da Fonseca
Outubro de 2009
Imagination is everything. It is the preview of lifes coming attractions.
Albert Einstein
i
Abstract
There is currently a limited diversity of vehicles capable of Vertical Take-Off
and Landing. However there
is a visible interest by scholars and admirers of Unmanned Aerial Vehicles for a
particular type of aircraft
called quadrotor. This work aims precisely at the design and control of a quad
rotor prototype having a
tri-axis accelerometer and a compass as its sensors.
This project started with the design of the quadrotor having in mind a list of o
bjectives to be ful lled
by it. Then followed the modeling of the dynamics and kinematics of the aircra
fts motion, as well as
of its motors and sensors. This led to the implementation of the qu
adrotor in a computer simulation
environment called Simulink

.
Given the outputs provided by the available sensors, a Kalman lter was developed
capable of estimating the angular velocities and Euler angles of the quadrotor, being therefore
possible to have knowledge of its attitude. This lter was combined with a Linear-Quadratic Regulator
in order to make the
system follow a given attitude reference. Finally, the results of the simulati
ons and real implementation
in the quadrotor are presented.
ii
iii
Resumo
Existe actualmente uma variedade reduzida de veculos capazes de descolagem e ater
ragem vertical.
No entanto existe um interesse visvel por parte de investigadores e admiradores d
e Veculos Areos
No Tripulados num tipo especial de aeronave denominada de quadrirotor. Este tra
balho tem precisamente como objectivo desenvolver e controlar um prottipo de um quadrirotor, possu
indo apenas como
sensores um acelermetro triaxial e uma bssola.
Comeou-se por conceber o quadrirotor tendo em vista uma lista de ob
jectivos a cumprir pelo
mesmo. Seguiu-se a modelao da dinmica e cinemtica da aeronave, assim co
mo tambm dos
seus motores e sensores. Da resultou a implementao do sistema em ambiente de simu
lao computacional Simulink

.
Tendo em conta as sadas fornecidas pelos sensores disponveis, foi dese
nvolvido um
ltro de
Kalman capaz de estimar apenas as velocidades angulares e os ngulos de Euler do q
uadrirotor, sendo
portanto possvel ter conhecimento da atitude do mesmo. Este ltro foi combinado co
m um controlador
LQR por forma fazer o sistema seguir uma dada referncia de atitude.
Finalmente, os resultados das simulaes e da implementao real no quadrirotor so aprese
ntados.
iv
v
Keywords
Quadrotor modelling
Quadrotor simulation
Kalman lter
Linear-Quadratic Regulator
vi
vii
Acknowledgments
First of all, I would like to thank my teachers, Professor Jos Raul Azinheira and
Professor Alexandra
Bento Moutinho for their advice, support and availability to speak with me throu
ghout the course of this
thesis.
I must not forget to thank my colleague Tiago Rita, who shared with me the dif cul
ty of working with a
UAV, Tiago Gonalves also for offering his aid and knowledge on UAVs, and Mr. Rap
oseiro who helped
me with the quadrotor construction.
Last but not least, I want to give special thanks to my family for all their lov
e, support, and for encouraging me to work my way through college, a path that made me gain precious life
experiences, which i
would have missed otherwise.
viii
Contents
Abstract i
Resumo iii
Keywords v
Acknoledgments vii
List of Tables xiii
List of Figures xvii
Notation xix
Acronyms xix
Subscripts xix
Superscripts xix
List of symbols xxi
1 Introduction 1
1.1 De nition and purpose of a quadrotor . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 1
1.2 Quadrotor operation . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 1
1.3 Historical role of quadrotors . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 2
1.4 Motivation . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . 6

1.5 State of the art . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 7
1.6 Thesis contributions . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 11
1.7 Structure of this thesis . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 11
2 Quadrotor design 13
2.1 General concept . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 13
2.2 Airframe . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . 14
2.3 Propellers . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . 14
2.4 Electronics . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . 18
2.4.1 Motors and Electronic Speed Controllers . . . . . . . .
. . . . . . . . . . . . . . . 18
ix
x CONTENTS
2.4.2 Microcontroller Unit . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 18
2.4.3 Wireless communication . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
19
2.4.4 Sensors . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 19
2.4.5 Batteries . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 20
2.4.6 Electronic circuit schematics . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
21
2.5 Flight autonomy . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 22
3 Quadrotor modelling 25
3.1 System dynamics . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 25
3.2 Kinematic equations and Euler angles . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
29
3.3 Quaternion differential equations . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 30
3.4 Modelling of a motor-propeller assembly . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 3
1
3.5 Moments of inertia . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 36

3.6 Calculation of the center of gravity . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .
. . 38
3.7 Sensors modelling . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 38
3.7.1 Accelerometer . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 39
3.7.2 Compass . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 40
3.8 Model implementation in MATLAB

and Simulink

. . . . . . . . . . . . . . . . . . . .
. . 41
4 Kalman Filter 45
4.1 State-space system linearization . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 46
4.2 System analysis . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 47
4.3 Mathematical formulation . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 48
4.4 Kalman lter in MATLAB

and Simulink

. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 49
4.5 Kalman lter for Arduino . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 54
5 Optimal Control - The Linear-Quadratic Regulator 55
5.1 Mathematical formulation . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 55
5.2 LQR control in MATLAB

and Simulink

. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 56
5.3 LQR control in the Arduino . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 58
6 Implementation and results 59
6.1 Full 12 states control . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 60
6.2 6 states control . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 62
6.2.1 LQG control with sensors . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
62

6.2.2 LQG control with sensors and motors dynamics . . . . . . .

. . . . . . . . . . . . . 65
6.3 Practical implementation . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 67
6.3.1 Telemetry and joystick control software . . . . . . . .
. . . . . . . . . . . . . . . . 67
6.3.2 Kalman lter and LQR controller . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 69
6.4 Using gyroscopes to improve state estimation . . . . . . .
. . . . . . . . . . . . . . . . . . 72
6.5 Technical issues . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 73
7 Conclusions and future work 75
Bibliography 77
CONTENTS xi
A Electronic Circuits 79
B Matlab

les 81
B.1 loadvars.m . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 81
B.2 quadsys.m . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 87
C Arduino source code 93
xii CONTENTS
List of Tables
3.1 Dead zone pulse width of each motor. . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
34
3.2 Data points gathered for understanding the motor-propeller dynamics. .
. . . . . . . . . . 34
3.3 Gains of the transfer functions describing the motors dynamics. . .
. . . . . . . . . . . . . 35
3.4 Quadrotors mass and main geometric variables. . . . . . . .
. . . . . . . . . . . . . . . . 37
3.5 Relevant accelerometer data. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 40
3.6 Relevant compass data . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 41
5.1 Maximum state values for designing matrix

Q. . . . . . . . . . . . . . . . . . . .
. . . . . 56
5.2 Maximum input values for designing matrix

R. . . . . . . . . . . . . . . . . . . .
. . . . . 57
xiii
xiv LIST OF TABLES
List of Figures
1.2.1Yaw, pitch and roll rotations of a common quadrotor. . . . . .
. . . . . . . . . . . . . . . . 2
1.2.2Illustration of the various movements of a quadrotor. . . . . .
. . . . . . . . . . . . . . . . 2

1.3.13D model of the Gyroplane No. 1. . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .
. 3
1.3.2Brguet-Richet Gyroplane No. 1 of 1907. . . . . . . . . .
. . . . . . . . . . . . . . . . . . 3
1.3.3The Oemichen No.2 of 1922
1
. . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 4
1.3.4Quadrotor designed by Dr. Bothezat an Ivan Jerome. This photograph was take
n at takeoff during its rst ight in October 1922
2
. . . . . . . . . . . . . . . . . . . . .
. . . . . . . . 4
1.3.5Convertawings Model A helicopter
3
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . 4
1.3.6V-22 Ospray. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 5
1.3.7Concept of Bells quad tiltrotor. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 5
1.3.8Skycar during a test ight
4
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 5
1.4.1Ambulance stuck in a traf c jam in Lisbon, Portugal. . . . . . .
. . . . . . . . . . . . . . . 6
1.5.1OS4 Unmanned Air Vehicle
5
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . 8
1.5.2X-4 Flyer Mark II [1]. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 8
1.5.3STARMAC II quadrotor [2]. . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 9
1.5.4Oakland University quadrotor system [3]. . . . . . . . .
. . . . . . . . . . . . . . . . . . . 10
1.5.5ALIV quadrotor. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 10
1.5.6ALIV control simulation in FLIGHTGEAR. . . . . . . . . .
. . . . . . . . . . . . . . . . . . 10
2.2.1Vario-43 quadrotor airframe. . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 14
2.3.1EPP1045 propellers. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 16
2.3.2Typical propeller thrust curves as a function of advance ratio J and blade
angle [4]. . . . . 16
2.3.3Typical propeller power curves as a function of advance ratio J and blade a
ngle [4]. . . . 17
2.3.4Theoretical thrust and power of an EPP1045 propeller. . . . . .
. . . . . . . . . . . . . . . 17

2.4.1Thunderbird-9 ESC. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 18
2.4.2Electric DC Brushless motor Robbe ROXXY BL-Outrunner 2824-34. . . .
. . . . . . . . . 18
2.4.3Arduino Duemilanove. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 19
2.4.4XBee 802.15.4 1mW wireless module. . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 19
2.4.5ADXL330 accelerometer. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 20
2.4.6HMC6352 compass. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 20
1
http://www.thingsmagazine.net/blogimages/
2
http://www.aviastar.org/helicopters\_eng/bothezat.php
3
http://www.aviastar.org/helicopters\_eng/convertawings.php
4
http://www.symscape.com/node/622
5
http://aero.ep .ch/page57689.html
xv
xvi LIST OF FIGURES
2.4.7Vislero LiPo 11,1V 2200mAh 20C battery. . . . . . . . . .
. . . . . . . . . . . . . . . . . . 21
2.5.1Example illustration of the angular velocity of a DC motor as a function of
power supply
voltage. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 22
3.1.2NED and ABC reference frames. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. 26
3.1.1Visualization of the North-East-Down reference frame. . . . . .
. . . . . . . . . . . . . . . 26
3.4.1Schematic of the input-output sequence for identi cation purposes. . . .
. . . . . . . . . . 32
3.4.2Microphone positioned to record the propeller sound. . . . . . .
. . . . . . . . . . . . . . . 33
3.4.3Propeller sound. Each step in amplitude is a consequence of a propeller bla
de passage. . 33
3.4.4Dynamic behaviour of the motors. The grey areas show a zoom on of the lin
earization
zone in red. . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 34
3.4.5Input-output data used with the system identi cation toolbox to calculate the
time constant. 35
3.4.6Blocks used in Simulink

to simulate motor 1. . . . . . . . . . . . . . . .
. . . . . . . . . . 36
3.5.1Distance from the motors to the center of gravity. . . . . . .
. . . . . . . . . . . . . . . . . 37
3.6.1The red dot in the center of the gure indicates the center of gravity. .

. . . . . . . . . . . 38
3.8.1Block diagram of quadrotor dynamics. . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
42
3.8.2Block diagram of the sensors. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 42
3.8.3Inside view of sensors Simulink

block. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . 43
4.4.1Kalman lter schematic. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 54
5.2.1LQR control. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 57
5.2.2LQG control block diagram in Simulink

. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . 58
6.1.112 states LQR control loop. . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 60
6.1.2Time response of the LQR control loop using with 12 states feedback. .
. . . . . . . . . . 61
6.1.3Linear positions with increasing weight matrix

Q. . . . . . . . . . . . . . . . . . . .
. . . . 62
6.1.4Linear positions with increasing weight matrix

R. . . . . . . . . . . . . . . . . . . .
. . . . 62
6.2.16 states Simulink

LQR control loop. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . 63
6.2.2Time response of the LQR control loop using 6 states. . . . . .
. . . . . . . . . . . . . . . 63
6.2.36 states Simulink

LQR control loop with sensors. . . . . . . . . . . . .

. . . . . . . . . . 64
6.2.4Time response of the LQR control loop using 6 states and sensors. . . .
. . . . . . . . . . 65
6.2.56 states Simulink

LQR control loop with sensors and motors dynamics. . . . . . . .

. . . 66
6.2.6Time response of the LQR control loop using 6 states, sensors and motors dy
namics. . . 66
6.3.1Quadrotor prototype. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 67
6.3.2Quadjoy user interface. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 68
6.3.3Quadrotor control diagram. . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .

. . . 68
6.3.4Kalman lter estimation of the yaw angle in the quadrotor prototype. . .
. . . . . . . . . . 69
6.3.5Time response of the LQR control loop using 6 states, sensors and motors dy
namics with
PWM resolution. . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 70
6.3.6Sensor noise data. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 71
6.4.1Time response of the LQR control loop using 6 states and gyros while in the
presence of
very noisy sensor readings. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 73
A.1 Accelerometer wiring. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 79
LIST OF FIGURES xvii
A.2 Compass and electronic speed controllers wiring. . . . . . . .
. . . . . . . . . . . . . . . . 80
A.3 Arduinos battery check circuit. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 80
xviii LIST OF FIGURES
Notation xix
Notation
Acronyms
ALIV - Autonomous Locomotion Individual Vehicle
COG - Center Of Gravity
DC - Direct Current
ESC - Electronic Speed Controller
GPS - Global Positioning System
LED - Light-emitting diode
LQR - Linear-Quadratic Regulator
LQ - Linear-Quadratic
LQG - Linear-Quadratic Gaussian
MIMO - Multiple-Input Multiple-Output
PD - Proportional-Derivative
PID - Proportional-Integral-Derivative
PWM - Pulse-Width Modulation
RC - Remote Control
SISO - Single-Input Single-Output
UAV - Unmanned Aerial Vehicle
VTOL - Vertical Take-Off and Landing
Subscripts
B - Regarding the aircrafts body centered coordinate frame
g - Regarding gravity
net - Resultant (e.g. resultant force)
Superscripts
T - Matrix transpose
xx Notation
Notation xxi
List of symbols
Symbol Domain Unit Description
R - Quat rnion angl of rotation
R rad Pitch angl

R rad.s

1
Pitch angle rate
R kg.m
3
Ai
density at sea level and 20

C
R s Time consan
R rad Roll angle

R rad.s
1
Roll angle rae
R rad Yaw angle

R rad.s
1
Yaw angle rae
R
ad.s
1
P
opelle
angula
velocity

R
31

ad.s
1
Quad
oto
s angula
velocities
_
P Q R
_

1
,
2
,
3,

4
R
ad.s
1
Angula
velocity of each moto

A R
nn
 State mat
ix (n states)
A
K
R
nn
 Kalman lte
state mat
ix (n states)
a
m
R
31
m.s
2
Accele
ation measu
ed by the accele
omete

a
p
R

31
m.s
2
Absolute accele
ation at the point he
e the senso
is located,
a
p
=
_
a
px
a
py
a
pz
_
a
B
R
31
m.s
2
Accele
ation in the quad
oto
s body
B R
nm
 Input mat
ix (n states, m inputs)
B
K
R
nm
 Kalman lte
input mat
ix
c
T
R none Th
ust coef cient of a p
opelle

c
P
R 
Poe
coef cient of a p
opelle

C R
qn
 Output mat
ix (q outputs, n states)
C
K
R
qn
 Kalman lte
output mat
ix (q outputs, n states)
D
P
R m P
opelle
diamete

D R
qm
 Feedfo
a
d mat
ix (q outputs, m inputs)
D
K
R
qm
 Kalman lte
feedfo
a
d mat
ix (q outputs, m inputs)
d
cg
R m Distance beteen the quad
oto
s cente
of g
avity and a moto

e
x

R
n1
 E

o
of the dynamics de
ivative (n states)
e
y
R
q1
 Output e

o
(q outputs)
F
net
R
31
N Combination of all the fo
ces acting on the quad
oto
,
F
net
=
_
F
x
F
y
F
z
_
F
P
R
31
N Total th
ust gene
ated by the p
opelle
s,
F
P
=
_
F
Px
F
Py
F
Pz
_
f
t
R s Flight autonomy
f R  Quad
oto
s model function to linea
ize
G(s) R 
Moto
dynamics t
ansfe
function
G R
np
 Obse
vation mat
ix (n states, p p
ocess noise measu
ements)
g R m.s
2
Ea
ths g
avity (constant value of 9.81m.s
2
)
xxii Notation
H R
qp
 Mat
ix that
elates the p
ocess noise ith the outputs of a
system in the state space fo
m (q outputs, p p
ocess noise
measu
ements)

I R
33
kg.m
2
Ine
tia mat
ix of the quad
oto

I R
nn
 Identity mat
ix (n states)
I
x
, I
y
, I
z
R kg.m
2
Elements of a moto
s ine
tia tenso

espectively
elated to u
x
,
u
y
and u
x
I
R
I
0

Elect
ical cu

ent consumption of the moto

R A Elect
ical cu

ent consumption of the moto
in noload condition

 P
opelle
advance
atio
J R
J
LQR
R 
LQR cost function
 T
ansfe
function gain
K R
K
T
R kg.m
2
.
ad
2
Constant value to calculate the th
ust p
ovided by a p
opelle

given its angula
speed
K
M
R kg.m
2
.
ad
2
Constant value to calculate the moment of a p
opelle
given its
angula
speed
K
TM
R m Constant value that
elates th
ust and moment of a p
opelle

K
mq
R
 LQR gain mat
ix (m inputs, q outputs)
k
v
R RPM.V

1
P
opelle
s speed in
evolutions pe
minute pe
unit of voltage
k R  Time instant k
L
nq
R
 Kalman gain (n states, q outputs)
M
net
R
31
 Sum of all the moments acting on the quad
oto
,
M
net
=
_
M
x
M
y
M
z
_
M
P
R N.m P
opelle
s moment
m R kg Mass of the quad
oto

N
c
R
ad No
th magnetic pole
N R  O
de
of the state space system
n R s
1
P
opelle
s speed in
evolutions pe
second
n
1
, n
2
, n
3
R 
Elements of a unit vecto
used to build a quate
nion
O
ABC
R
3
 Ai
c
aftBodyCente
ed
efe
ence f
ame
O
NED
R
3
 No
thEastDon
efe
ence f
ame
O R
nqn
 Obse
vability mat
ix (n states, q outputs)
P R
ad.s
1
Angula
speed a
ound the u
x
axis of the quad
oto

P R
ad.s

2
Angula
accele
ation of the quad
oto
along the u
x
axis of the
ine
tial
efe
ence f
ame
P
P
R W P
opelle
poe

P R
n1
 Steadystate e

o
cova
iance mat
ix (n states)
p
R
 Linea
ization point of the system

P R
mq
 Unique positivede nite solution fo
the LQR cont
olle
Riccati
equation (m inputs, q outputs)
P R s Pulse idth of the PWM signal d
iving a moto

P
0
R s PWM signal pulse idth of the moto
s dead zone
Q R
ad.s
1
Angula
speed a
ound the u
y
axis of the quad
oto

Notation xxiii

Q R
ad.s
2
Angula
accele
ation of the quad
oto
along the u
y
axis of the
ine
tial
efe
ence f
ame
Q
R
Q
k
R
Q
kk


Q
nn


Q
k
R

Q
q
41

_
q

A.s

Elect
ic cha
ge of the batte
y (usually p
esented in mAh)

P
ocess noise cova
iance int
oduced th
ough input k

P
ocess noise cova
iance mat
ix (k obse
vable states)

R
LQR cont
olle
eighting mat
ix (n states to cont
ol)

Diagonal elements of

R
Quate
nion, q =

0
q
1
q
2
q
3
_
q R
41
 Angula
velocity quate
nion
R R
ad.s
1
Angula
speed a
ound the u
z
axis of the quad
oto

R
m
R Moto
s coppe
coil elect
ic
esistance

R R
ad.s
2
Angula
accele
ation of the quad
oto
along the u
z
axis of the
ine
tial
efe
ence f
ame
R
() R
33
 Roaion marix ha expresses he orienaion o he O
ABC
rame around he u
x
axis o O
NED
R
() R
33
 Roaion marix ha expresses he orienaion o he O
ABC
rame around he u
x
axis o O
NED
R
() R
33
 Roaion marix ha expresses he orienaion o he O
ABC
rame around he u
x
axis o O
NED
R
acc
R m.s
2

Acceleromeer reading resoluion o he Arduino board

R
k
R 
Sensor noise covariance rom sensor reading k
R R
ii
 Measuremen noise covariance marix (i sensor measuremens)

R R
mm
 LQR conroller weighing marix (m inpus)

R
k
R 
Diagonal elemens o

R
r R m Propeller radius

r R
31
m Posiion o he O
ABC
coordinae rame relaive o O
NED
S R
33
 Roaion marix (also known as direcion cosine marix)
S

R
33
 Quaernion euivalen o he roaion marix
T R N Propeller hrus
T R
33
 Marix ha relaes he bod xed angular veloci vecor wih he
rae o change o he Euler angles
T
P
R s Time period beween he passage o wo propeller blades

s
R s Sample ime.

U R m.s
2
Linear acceleraion o he uadroor along he u
x
axis o he
inerial reerence rame
u
0
R m.s
1
Aircra igh veloci
u
x
R 
Xaxis o he O
NED

u
x

reerence rame

 Xaxis o he O
R
ABC
reerence rame
u

R 
Yaxis o he O
NED
reerence rame
u

R
 Yaxis o he O
ABC
reerence rame
u
z
R 
Zaxis o he O
NED
reerence rame.
u
z
 Zaxis o he O
R
ABC
reerence rame
u R
41
 Vecor conaining he inpus o he ssem a an euilibrium poin
xxiv Noaion
u
K
R
41
 LQR vecor o conrol acions
u
k,max
R 
Highes olerable value or inpu k

V R m.s
2
Linear acceleraion o he uadroor along he u

axis o he
inerial reerence rame
V
m
R V Moors power suppl volage
V
0
R V Minimum power suppl reuired or propeller roaion
v
R
31
m.s
1
Vecor conaining he uadroors linear velociies
_
U V W

_
v
p
R
31
m.s
1
Vecor o linear velociies a he poin where he acceleromeer is
locaed
v
cg
R
31
m.s
1
Vecor o linear velociies o he uadroors cener o gravi
v R
i
 Measuremen noise (i sensor measuremens)

W R m.s
2
Linear acceleraion o he uadroor along he u
z
axis o he
inerial reerence rame
w
v
R rad.s
1
.V
1
Propeller angular speed in S.I. unis per uni o volage
w R
p1
 Process noise (p process noise measuremens)
X R m Posiion o he uadroors cener o gravi along he xaxis he
inerial reerence rame O
NED

X R m.s
1
Time derivaive i he O
ABC
rames posiion along u
x
x R
n1
 Sae vecor (n saes)
x R
n1
 Vecor conaining he saes o he ssem a an euilibrium poin
(n saes)
x
k,max
R 
Highes olerable value or sae k
x R
n1
 Esimaed saes b he Kalman ler (n saes)
Y R m Posiion o he uadroors cener o gravi along he axis he

inerial reerence rame O

NED

Y R m.s
1
Time derivaive i he O
ABC
rames posiion along u

R

 Oupu vecor

R
1
 Esimaed oupus b he Kalman ler ( oupus)
Z R m Posiion o he uadroors cener o gravi along he zaxis he
inerial reerence rame O
NED

Z R m.s
1
Time derivaive i he O
ABC
rames posiion along u
z
Chaper 1
Inroducion
1.1 De niion and purpose o a uadroor
A uadroor, or uadroor helicoper, is an aircra ha becomes airborne due 
o he li orce provided b
our roors usuall mouned in cross con guraion, hence is name. I is an enire
l dieren vehicle when
compared wih a helicoper, mainl due o he wa boh are conrolled. Helicop
ers are able o change
he angle o aack o is blades, uadroors canno.
A presen, here are hree main areas o uadroor developmen: miliar, ra
nsporaion (o goods
and people) and Unmanned Aerial Vehicles (UAVs) [5]. UAVs can be classi ed ino
wo major groups:
heavierhanair and ligherhanair. These wo groups sel divide in man ohe
r ha classi aircras ac
cording o moorizaion, pe o lio and man oher parameers. Verical Tak
eO and Landing (VTOL)
UAVs like uadroors have several advanages over xedwing airplanes. The can mo
ve in an direcion
and are capable o hovering and  a low speeds. In addiion, he VTOL capabili
 allows deplomen
in almos an errain while xedwing aircra reuire a prepared airsrip or ak
eo and landing. Given
hese characerisics, uadroors can be used in search and rescue missions, me
eorolog, peneraion
o hazardous environmens (e.g. exploraion o oher planes) and oher applic
aions suied or such an
aircra. Also, he are plaing an imporan role in research areas like con
rol engineering, where he
serve as proopes or real lie applicaions.
1.2 Quadroor operaion
Each roor in a uadroor is responsible or a cerain amoun o hr
us and orue abou is cener o
roaion, as well as or a drag orce opposie o he roorcras direcion o igh
. The uadroors propellers
are no all alike. In ac, he are divided in wo pairs, wo pusher and w

o puller blades, ha work in

conraroaion. As a conseuence, he resuling ne orue can be n
ull i all propellers urn wih he
same angular veloci, hus allowing or he aircra o remain sill around is
cener o gravi.
In order o de ne an aircras orienaion (or aiude) around is cener o mass,
aerospace engineers
usuall de ne hree dnamic parameers, he angles o aw, pich and roll. This
is ver useul because
ac
around is cener o
he orces used o conrol he aircra
mass, causing i
o pich, roll or aw.
Changes in he pich angle are induced b conrar variaion o speeds in propel
lers 1 and 3 (see Figure
1.2.1), resuling in orward or backwards ranslaion. I we do his same acion
or propellers 2 and 4, we
1
2 CHAPTER 1. INTRODUCTION
can produce a change in he roll angle and we will ge laeral ranslaion. Yaw
is induced b mismaching
he balance in aerodnamic orues (i.e. b oseing he cumulaive hrus be
ween he counerroaing
blade pairs). So, b changing hese hree angles in a uadroor we are able o m
ake i maneuver in an
direcion (Figure 1.2.2).
Figure 1.2.1: Yaw, pich and roll roaions o a common uadroor.
Figure 1.2.2: Illusraion o he various movemens o a uadroor.
1.3 Hisorical role o uadroors
This sor begins in he 20h cenur, when Charles Riche, a French scienis a
nd academician, buil a
small, unpiloed helicoper (see reerence [6]). Alhough his aemp was no
a success, Louis Brgue,
one o
Riches sudens, was inspired b his uors example. Laer i
n 1906, Louis and his broher,
1.3. HISTORICAL ROLE OF QUADROTORS 3
uadroor. Louis ex
Jacues Brgue
began he consrucion o he rs
ecued man ess on airoil
shapes, proving ha he had a leas some basic undersanding o he reuiremen
s necessar o achieve
verical igh.
In 1907 he had nished he consrucion o he aircra which was named BrgueRi
che Groplane
No. 1 (Figures 1.3.1 and 1.3.2), a uadroor wih propellers o 8.1 meers in
diameer each, weighing
578 kg (2 pilos included) and wih onl one 50 hp (37.3 kW) inernal combusion
engine, which drove
he roors hrough a bel and pulle ransmission. O course a ha ime he h
ad no idea how he would
conrol i, he main concern was o ensure he aircra would achieve verical ig
h. The rs aemp o
igh was done in beween Augus and Sepember o 1907 wih winesses saing he
saw he uadroor
li 1.5 m ino he air or a ew momens, landing immediael aerwards. Tho
se same winesses also
menioned he aircra was sabilized, and perhaps even lied b men assising
on he ground.
Discouraged b he lack o success o he Groplane No. 1, Brgue and his meno
r coninued heir
pursui
igh
machines and aerwards also emporari
o build verical
l dedicaed hemselves o he
developmen o xedwing aircra, area where he became ver successul. Louis n

ever abandoned his

passion or verical igh aircra and in 1932 he became one o he pioneers o h
elicoper developmen
[6].
Figure 1.3.1: 3D model o he Groplane No. 1
1
.
Figure 1.3.2: BrgueRiche
Groplane No. 1 o
1907
2
.
Eienne Oemichen, anoher engineer, also began experimening wih roaingwing
designs in 1920.
He designed a grand oal o
six dieren
verical li
machines
. The
rs
model
ailed in liing rom
he ground bu Oemichen was a deermined person, so he decided o add a hdrogen
 lled balloon o
provide boh sabili and li. His second aircra, he Oemichen No. 2 (Fi
gure 1.3.3), had our roors
and eigh propellers, suppored b a cruciorm seelube ramework laou. Fi
ve o he propellers were
mean o sabilize he machine laerall, anoher or seering and wo or orwa
rd propulsion. Alhough
rudimenar, his machine achieved a considerable degree o sabili and conr
ollabili, having made
more han a housand es
ighs in he middle o ha decade. I wa
s even possible o mainain he
aircra several minues in he air. In he 14h o Ma he machine was airborne
or oureen minues and
i ew more han a mile. Bu Oemichen was no sais ed wih he poor heighs he was
able o , and he
nex machines had onl a main roor and wo exra aniorue roors.
1
hp://www.hingsmagazine.ne/blogimages/
2
hp://www.cie.monash.edu.au/hargrave/images
4 CHAPTER 1. INTRODUCTION
Figure 1.3.3: The Oemichen No.2 o 1922
3
.
The arm also had an ineres or verical li machines. In 1921, Dr. Georg
e de Boheza and Ivan
Jerome were hired o develop one or he US Arm Air Corps. The resul was a 1
678 kg srucure wih
9 m arms and our 8.1 m sixblade roors (Figure 1.3.4). The arm conrac re
uired ha he aircra
would hover a 100 m high, bu he bes he achieved was 5 m. A he end o
he projec Bohezad
demonsraed he vehicle could be uie sable, however i was underpowered and
unresponsive, among
oher echnical problems.
Figure 1.3.4: Quadroor designed b Dr. Boheza an Ivan Jerome. This phoograph
was aken a akeo
during is rs igh in Ocober 1922
4
.
Laer in 1956, a uadroor helicoper proope called Converawings Model A (see
Figure 1.3.5)
was designed boh or miliar and civilian use. I was conrolled b varing
he hrus beween roors,

and is ighs were a success, even in orward igh. The projec ended mainl due 
o he lack o demand
or he aircra.
Figure 1.3.5: Converawings Model A helicoper
5
.
3
hp://www.hingsmagazine.ne/blogimages/
4
hp://www.aviasar.org/helicopers_eng/boheza.php
5
hp://www.aviasar.org/helicopers_eng/converawings.php
1.3. HISTORICAL ROLE OF QUADROTORS 5
Recenl here has been an increasing ineres
in uadroor designs.
Bell is working on a uad
ilroor o overcome he V22 Ospra (see Figure 1.3.6), capable o carring a
large paload, achieving
high veloci and while using a shor amoun o space or Verical TakeO and
Landing (VTOL). Much
o is ssems come direcl rom he V22 excep or he number o engines. Als
o, he wing srucure on
he new design has some improvemens, i has a wider wing span on he rear roor
s. As a conseuence,
he Bell uad ilroor (Figure 1.3.7) aims or higher perormance and uel econ
om [7].
Figure 1.3.6: V22 Ospra
6
. Figure 1.3.7: Concep o Bells uad ilroor
7
.
Anoher recen and amous uadroor design is he Moller Skcar (Figure 1.3.8),
a proope or a
personal VTOL ing car. The Skcar has our duced ans allowing or a saer and
e cien operaion
a low speeds. I was a arge or much criicism because he onl demonsrai
ons o igh were hover
ess wih he Skcar ehered o a crane [8]. Is invenor, Paul Moller alrea
d ried o sell he Skcar
b aucion wihou success. Nowadas he ocuses his work on he precursor o he
Skcar, he M200G
Volanor, a ing saucersle hovercra. This laer model uses eigh ans conrol
led b a compuer and
is capable o hovering up o 3 m above he ground. This limiaion is imposed b
he onboard compuer
due o regulaions o he Federal Aviaion Adminisraions, saing ha an veh
icle ha ies above 3 m is
regulaed as an aircra [9].
Figure 1.3.8: Skcar during a es igh
8
.
Quadroors are also available o he public hrough radio conrolled os. Some
enhusiass as well as
researches have been developing heir own uadroor proopes. This is possible
due o he availabili
6
hp://zerosix.wordpress.com/2007/10/06/v22insearchandrescuemodearizonano
ira/
7
hp://sisemadearmas.sies.uol.com.br/ca/aco01il.hml
8

hp://www.smscape.com/node/622
6 CHAPTER 1. INTRODUCTION
Figure 1.4.1: Ambulance suck in a ra c jam in Lisbon, Porugal.
o cheap elecronics and lighweigh resisan maerials available o he public
. Be i or personal sais
acion, enerainmen, miliar or civilian use, uadroors have plaed an impo
ran role in he evoluion
o aircras and ma prove hemselves as imporan means o ransporaion in a
near uure.
1.4 Moivaion
In siuaions where peoples lives are a danger here is regularl he need or 
ransporaion o injured or
ill paiens o a hospial or an oher medical acili capable o providing co
mpeen medical care. Oen,
when an ambulance canno rapidl reach he emergenc sie, medical aircra are
usuall deploed in
alernaive, a use which has proven o be uie valuable so ar. The rs recor
d o his kind o acion
daes back o 1870, where during he FrancoPrussian War 160 French
soldiers were ranspored b
hoair balloon o France [10]. Since hen, he concep o medical use aircra
 has evolved uie a lo,
eiher in he miliar or civilian perspecives. Nowadas, medical assisance
helicopers and airplanes
are ver well euipped (e.g. some pes o euipmen he have onboard: venila
ors, elecrocardiogram
moniors, de brillaors, ec.), allowing or immediae reamen o criical cases
, avoiding in man o hem
deah beore reaching he hospial. These aircras serve grea and noble purpos
es, especiall in remoe
and isolaed inhabied areas, bu wha abou pracical uncion in developing c
iies? Surel here is a
large concenraion o medical aciliies inside a big ci, bu here are also
oher acors o consider like
ra c jams (Figure 1.4.1).
This causes a as ambulance rip o become a oal nighmare, aking hours o r
each he desinaion,
no speaking o he risk o acciden where crew and paien can be injured sever
el. We can o course
emplo helicopers, bu heir dimensions are a serious obsacle due o he high
diameer o he propeller
which can collide ino a ree or elecrici pole. Wha
i we cou
ld arrange an aircra
wih he same
characerisics o a helicoper, bu wih smaller size? A possible answer o h
is uesion is he use o a
uadroor vehicle. Here are he advanages:
No gearing necessar beween he moor and he roor;
No variable propeller pich is reuired or alering he uadroor angle o a
ack;
No roor sha iling reuired;
1.5. STATE OF THE ART 7
4 smaller moors insead o one big roor resuling in less sored
kineic energ and hus less
damage in case o accidens;
Minimal mechanical complexi,
Quadroors reuire less mainenance compared o boh helicopers and planes;
Roor mechanics simpli caion;
Paload augmenaion;
Groscopic eecs reducion.
Alhough here are he drawbacks o weigh augmenaion and high energ consump

ion, hese can be

reduced hanks o more e cien energ sources and maerials engineering research.
Thus arose he moivaion or his hesis: he design o a proope air vehicl
e ha can be conrolled
b a pilo, where some conrol echniues ma be esed in order o sud he ro
busness o such ssems
and hus conribue in some wa or he developmen o saer uadroors. Such
an endeavor will also
serve as an opporuni o experience he challenges o aircra modelling and r
eal lie applicaion.
1.5 Sae o he ar
Roorcras have winessed an incredible evoluion in he las ears.
Universiies, sudens and re
searchers coninuousl work o inroduce more robus conrollers and modelling 
echniues, so ha he
can provide deailed and accurae represenaions o reallie uadroors. Thi
s secion inroduces some
o he work presened in recen ears.
Progress in sensor echnolog, daa processing and inegraed acuaors has made
he developmen
o miniaure ing robos possible. Bouabdallah e al. [11] described a possib
le approach owards he
developmen o an indoor micro uadroor called OS4 (Figure 1.5.1), including he
mechanical design,
dnamic modelling, sensing and conrol o he orienaion angles. Laer, Bo
uabdallah e al. [12] pre
sened he resuls o classic PID (ProporionalInegralDerivaive) and LQ (Lin
earQuadraic) conrollers
implemened in he OS4, based on a more complee model han he previousl aai
ned. The LQ con
roller revealed o be problemaic, i was hard o nd weigh marices o sais
conrol sabili. However,
he classical approach (PID) perormed avorabl compared o he LQ due o he s
impler mehods oler
ance or model uncerain.
In reerence [13], researchers also developed a compuaional ool designed o s
imulae he dnamics
o he OS4. Implemened in MaLab using Simulink, he soware allows o es di
eren sensors and
lers, as well as various conrol echniues. The simulaion environmen was sp
eci call used, in his
case, o es obsacle avoidance approaches using ulrasound sensors, beore p
roceeding wih acual
experimenaion on he OS4.
8 CHAPTER 1. INTRODUCTION
Figure 1.5.1: OS4 Unmanned Air Vehicle
9
.
Aiming o design a pracical uadroor helicoper, Pounds e
al. [
1], rom he Ausralian Naional
Universi, creaed he X4 FlerMark II plaorm (Figure 1.5.2), a 4
kg srucurall robus uadroor
wih cusom buil chassis and avionics. An aeroelasic blade was designed and
is perormance resuls
presened. The uadroor also includes a sprung eeering roor hub ha allow
s he adjusmen o he
blade apping characerisics. This eec was also inroduced in he dnamic mod
el o he roorcra. A
his poin, MaLab simulaions showed ha an invered roorcra con guraion is b
ene cial o uadroor

ers. Two ears laer, in reerence [14] he analsis o he aircras aiude d
namics allowed he uning
o he mechanical design o improve disurbance rejecion and conrol sensiivi
. A linear SISO conroller
was hen implemened or aiude conrol, aiming o sabilize dominan decouple
d pich and roll modes,
while using a model o disurbance inpus o esimae he perormance o he pla
n. The model o he
plan also implemened blade apping o he propellers, having he experimenal re
suls in he X4 Fler
illusraed ha blade apping inroduces signi can sabili eecs on he vehicl
e. Resuls showed ha
he compensaor was able o conrol aiude a low roor speeds .
Figure 1.5.2: X4 Fler Mark II [1].
Mokhari e al. [15] presened a mixed robus eedback linearizaion wih linear
GH

conroller applied
o a nonlinear uadroor. Acuaor sauraion and consrain on sae
space oupu
was implemened
case scenario o
conrol law design. Also,
o analze he wors
he conrollers behavior was esed
when subjugaed o parameer uncerainies, exernal disurbances and measuremen
 noise. Simulaions
revealed ha he overall ssem becomes robus when weighing uncions are cho
sen judiciousl.
Ari cial vision has also proven o be a resourceul ool when conr
olling uadroors. Tournier e
10
hp://aero.ep .ch/page57689.hml
1.5. STATE OF THE ART 9
al. [16] presened visionbased esimaion and conrol o a uadroor using a
single camera relaive o
a arge ha incorporaes moir paerns. The purpose o his research was o a
cuire six degree o
reedom esimaion, which is essenial or he operaion o vehicles in close pr
oximi o oher aircra and
landing plaorms. Tess showed he usabili o he ssem o auonomousl hove
r he aircra, given ha
he arge remains wihin he cameras eld o view.
Taking ino accoun ha curren designs oen consider onl nominal operaing c
ondiions or vehicle
conrol design, Homann e al. [2] seeks o engage in he sud o he issues
ha arise when deviaing
signi canl romhe hover igh regime. Three dieren uadroor aerodnamic eec
s were invesigaed
and heir relaion o vehicular veloci, angle o aack and airrame design. T
he rs eec sudied was he
wa how oal hrus varies no onl wih inpu power, bu wih he ree sream
veloci and angle o aack
wih respec o he ree sream. The second phenomenon was blade apping, a resul o
 dieren in ow
velociies experienced b he advancing and rereaing blades, which induces rol
l and pich momens on
he roor hub as well as a de ecion o he hrus vecor. Lasl, he eec o
inererence caused b he
vehicles bod componens locaed near he slip sream o he propellers was inves
igaed because i is
responsible or unsead hrus, rendering aiude racking di cul. The ull ex
en o hese aerodnamic

eecs was uncovered wih he help o he STARMAC II uadroor (see Figure 1.5.
3), which possesses
auonomous aiude and aliude conrol. Resuls proved ha exising models an
d conrol echniues are
inadeuae or accurae rajecor racking a high speed and in unconrolled en
vironmens.
Figure 1.5.3: STARMAC II uadroor [2].
Mehme Ee [17] provided he ull dnamical model o a commerciall available u
adroor roorcra
and presened is behavior a low aliudes hrough sliding mode conrol, which
is a conrol echniue
known b is robusness agains disurbances and invariance during he sliding r
egime. The plan was
modeled as being nonlinear, wih sae variables ighl coupled. The assumpion
s made included unsa
uraed conrols, negligible weaher disurbances and reasonabl as acuaion p
eripher. The simulaions
showed ha he algorihm successull drives he ssem owards he desired ra
jecor wih bounded
conrol signals.
In he Universi o Caliornia, Berkle, Kaie Miller [18] sough o design con
rol laws or a uadroor
helicoper o rack a desired pah b implemenaion o wapoins, more speci cal
l a she used a PD
(ProporionalDerivaive) or posiioning conrol and a PID or roaion conrol
. The conrol laws were de
veloped on a model o he uadroor dnamics, which was obained b linearizing
is nonlinear euaions
o moion. The conrol laws were sudied while in he presence o modeling unc
erainies and exernal
disurbances like wind guss. The resuls showed ha while he linear conrol
laws are adeuae under
perec condiions, when uncerain is inroduced, he do no perorm well.
10 CHAPTER 1. INTRODUCTION
Suden compeiions are anoher wa o promoing uadroor developmen. AbouS
leiman e al. [3]
designed a uadroor robo or he Sixh Annual Unmanned Aerial Ssems Compei
ion
11
rom scrach
(see Figure 1.5.4). The uadroor uses GPS or wapoin racking and aliude,
onboard camera and a
dual processor capable o auonomous pah navigaion and daa exchange wih he
ground saion. Con
rol was made wih 4 PID conrollers (respecivel o conrol pich, roll, aw a
nd hrole) and heir gains
were ound experimenall. During he sar o he compeiion, immediael a
er akeo he uadroor
was hi b srong wind urbulence bu he conroller perormed exremel well an
d was able o sabilize
he ssem.
Figure 1.5.4: Oakland Universi uadroor ssem [3].
Srgio Pereira [19] has recenl presened his maser hesis where he proposed a m
odel o simulae
he ALIV (Auonomous Locomoion Individual Vehicle) uadroor UAV (Figu
re 1.5.5). He was able o
es he capabiliies o he ALIV b using a model buil in MaLab and Simulink, w
here he designed
a Kalman Filer o esimae he uadroor sae b modelling is real sensors.
Then, he implemened

a Linear Quadraic Regulaor (LQR) conroller o sabilize he roorcra. Pos

iioning conrol was aided
eiher hrough a josick or a simulaed onboard camera. A Simulink realime si
mulaion o he ssem
was successull inegraed wih he FLIGHTGEAR igh simulaor (Figure 1.5.6), wh
ere a user can pilo
he uadroor o a cerain arge using he josick, and, aer locking on he
arge, he roorcra perorms
auonomous igh hrough esablished wapoins.
Figure 1.5.5: ALIV uadroor [19]. Figure 1.5.6: ALIV conrol simulaion
in FLIGHT
GEAR [19].
11
hp://65.210.16.57/sudencomp2010/deaul.hml
1.6. THESIS CONTRIBUTIONS 11
1.6 Thesis conribuions
In reerence [19], he auhor shows how o model and conrol a uadroor in a si
mulaion environmen.
Conrol is made hrough wo possible was: wih user inpu (using a josick) an
d auonomousl, showing
how o ake ull advanage o he uadroor as an UAV. The sensors used include
a riaxis acceleromeer,
a riaxis magneomeer and a camera used o perorm posiion conrol.
This hesis uses reerence
[19] as a basis or he consrucion o a uadroor proope, ocusing more on
he issues o pracical
implemenaion. During he developmen o his hesis, a se o conribuions
were made ha are worh
menioning:
Ideni caion o he dnamics o a moorpropeller assembl using sound acuired
romhe spinning
propeller;
Modeling o a brushless moor as a linear ssem, conrolled b a PWM signal;
Design o a Kalman ler ha allows or sensor usion o daa coming rom a ri
axis acceleromeer
and compass (insead o a riaxis magneomeer), hereore providing an esima
e or he aiude
o a uadroor.
The implemenaion o a sae space Kalman ler and LQR conroller in he Ardui
no elecronics
prooping plaorm.
Pracical demonsraion o he impac o sensor noise in he conrol process o
 a uadroor.
1.7 Srucure o his hesis
In Chaper 1, Inroducion, we begin b de ning wha a uadroor is, and wha is
main applicaions are.
Aer de ning he maneuvering capabiliies o his kind o air vehicle, is hisor
ical evoluion is presened,
rom he beginning o he wenieh cenur o he presen da. Nex, he reas
ons ha led o he choice
o his hesis are explained, and a brie descripion o he main areas o resea
rch wih hese vehicles is
made.
Moving on o Chaper 2, hroughou which he design process o uadroor is desc
ribed, i sars wih
he de niion o he objecives o be ul lled b he aircra. Follows he anals
is and jusi caion o he
chosen componens or he aircras assembl.
Chaper 3 ocuses on uadroor modeling. The used reerence rames are de ned, 
ollowed b he

dnamic and kinemaic analsis o he aircra. The ideni caion o

he moorpropeller assembl is
made b using recorded sound les o he propellers sound. The calculaion o mo
mens o ineria o
he aircra is also presened, as well as o he cener o gravi locaion. Th
is chaper ends wih sensors
modeling and wih a brie inroducion o he simulaion o he uadroor and i
s sensors in MATLAB

and
Simulink

.
Assuming he uadroor as a linear sae space ssem, in Chaper 4 a Kalman le
r is described
according o is mahemaical ormulaion and design or his paricular case s
ud. I is also explained
how i is implemenaion in Simulink

as well as in he Arduino.
Chaper 5 ocuses on he mahemaical de niion and design o a conroller Linear
Quadraic Regu
laor o allow or conrol over he uadroor saes. Jus as or he Kalman ler
, he conroller implemen
aion in MATLAB

and embedded conrol plaorm Arduino is also revealed.
12 CHAPTER 1. INTRODUCTION
Chaper 6 begins he simulaions sage, beginning wih an ideal case where we ha
ve access o all o
he uadroors saes, o see i i is possible o have ull conrol over hem.
Nex, a se o simulaions is
execued where no all o he ssems saes are available o he conroller. I
n his se, he sensors d
namics, Kalman ler, and nall he moors dnamics are added o he simulaions, g
raduall increasing
is degree o realism. Aer he simulaions were compleed, an aemped is m
ade o appl he Kalman
ler and LQR conroller o he uadroor proope.
This hesis ends in Chaper 7 wih he nal conclusions and suggesions or uure
improvemens.
Chaper 2
Quadroor design
Correc execuion o an aircra design sage provides he earl deerminaion
o wha he drawbacks
in design are, allowing us o save no onl mone, bu also ime. Tha wa, e
wer changes will need o
be implemened aer building he uadroor. This chaper will provide
an example on how o build a
uadroor proope, rom he mechanical componens o he avionics, culminaing
in he consrucion o
a uncional aircra.
2.1 General concep
The mos imporan arge o his paricular design process is o arrive a he
correc se o reuiremens
or he aircra, which are oen summarized in a se o speci caions. In his
secion we will de ne our
mission o build a uadroor proope suiable or indoor igh, as well jusi
he decisions and euipmen
chosen or achieving his purpose. The speci caions or our uadroor proope a

re:
Overall mass no superior o 1 kg. When i comes o uadroors, he heavier
he ge, he more
expensive he are. Man uadroors used or research do no exceed his mass. S
o, aiming or a
maximum mass o 1 kg seems o be a suiable arge;
Fligh auonom beween 10 and 20 minues. There is no poin in using he uad
roor or 2 minues
and hen wai a couple o hours o recharge he baeries, wasing precious ime
;
Onboard conroller should have is separae power suppl o preven
simulaneous engine and
processor ailure in case o baer loss;
Abili o ransmi live elemer daa and receive movemen orders rom a gro
und saion wirelessl,
hereore avoiding he use o cables which could become enangled in he aircra
 and cause an
acciden;
The uadroor should no  ver ar rom ground saion so here is no need o
long range elemer
hardware and he exra power reuiremens associaed wih long range ransmissio
ns.
Conseuenl, he main componens should be:
4 elecric moors and 4 respecive Elecronic Speed Conrollers (ESC);
13
14 CHAPTER 2. QUADROTOR DESIGN
4 propellers;
1 onboard processing uni;
1 riaxis analog acceleromeer. Measuring acceleraion in hreedimen
sional space aboard an
aircra is a necessar ea o undersand he magniude o he orces acing on
his aircra. I ma
also be possible o use his sensor o calculae he angles o aw and pich wi
h knowledge o he
gravi acceleraion vecor;
1 elecronic compass o measure he aw angle;
2 onboard power supplies (baeries), one or he moors and anoher o he p
rocessing uni. A
his poin we will assume ha beond he need o provide elecrical power o h
e moors, we mus
assure ha he brain o he uadroor (i.e. he onboard processing uni) remai
ns working well aer
he baer o he moors has discharged;
1 airrame or supporing all he aircras componens.
2.2 Airrame
The airrame is he mechanical srucure o an aircra ha suppors
all he componens, much like a
skeleon in Human Beings. Designing an airrame rom scrach involves imporan co
nceps o phsics,
aerodnamics, maerials engineering and manuacuring echniues o achieve cer
ain perormance, re
liabili and cos crieria. The main purpose o his hesis is no airrame des
ign, so, because consrucion
ime o he uadroor is criical, i is preerable o acuire, i possible, par
s alread available or sale.
The chosen airrame or he uadroor was he Vario43 model (Figure 2.2.1
1
), wih 258g o mass
and made o GPR (GlassReinorcedPlasic), posessing a cagelike srucure in i
s cener ha will oer

exra proecion o he elecronics. This paricular deail ma prove isel ver
 useul when i comes o he
es igh sage, when accidens are more likel o happen.
Figure 2.2.1: Vario43 uadroor airrame.
2.3 Propellers
The pical behavior o a propeller can be de ned b hree parameers [4]:
Thrus coe cien c
T
;
1
hp://www.mikrokoper.de/ucwiki/VarioFrame_43, Januar 2009.
2.3. PROPELLERS 15
Power coe cien c
P
;
Propeller radius r.
These parameers allow he calculaion o a propellers hrus T:
T = c
T
4

4

2
(2.1)
and o r P
P
:
P
P
= c
P
4

5

3
(2.2)
h r is th ro ll r angular s d and the density of ai
.
These fo
mulas sho that both th
ust and poe
inc
ease g
eatly ith p
ope
lle
s diamete
. If the
diamete
is big enough, then it should be possible to get suf cient th
ust hile d
emanding lo
otational
speed of the p
opelle
. Consequently, the moto
d
iving the p
opelle
ill have
loe
poe
consumption,
giving the quad
oto
highe
ight autonomy.
Available models of counte

otating p
opelle
s a
e sca
ce in the ma
ket of
adi
o cont
olled a
ic
afts.
The EPP1045 (see Figu
e 2.3.1), a p
opelle
ith a diamete
of 10 (25
.4 cm) and eighting 23g,
p
esented itself as a possible candidate fo
implementation in the quad
oto
.
To check its compatibility
ith the p
oject
equisites it is necessa
y to calculate the
espectiv
e th
ust and poe
coef cient. In

efe
ence [20] e have data f
om tests done on the EPP1045, f
om hich e can ex
t
act the mean th
ust
and poe
coef cients by using equations (2.1) and (2.2):
c

T
= 0.1154

(2.3)

c
P
= 0.0743 (2.4)
In
eality, neithe
the th
ust no
the poe
coef cients a
e constant values, they
a
e both functions of the
advance
atio J [4]:
J =
u
0
nD
P
(2.5)
he
e u
0
is the ai
c
aft ight velocity, n the p
opelle
s speed in
evolutions pe
second a
nd nally D
P
is the p
opelle
diamete
. Hoeve
, hen obse
ving the cha
acte
istic cu
ves f
o
both these coef cients
(see Figu
es 2.3.2 and 2.3.3), it is clea
that hen an ai
c
afts
i
ght velocity is ve
y lo (e.g. in a
constant altitude hove
) the advance
atio is almost ze
o and the to coef cients
can be app
oximated
as constants, hich is the cu

ent case, fo
at this point the
e is no inte
est
in achieving high t
anslation
velocity.
Assuming the quad
oto
s maximum eight is 9.81N (1kg) and that e ha
ve fou
p
opelle
s, it is
mandato
y that each p
opelle
is able to p
ovide at least 2.45N (
1
/4 of the quad
oto
eight) in o
de

to achieve liftoff. Taking this data into conside
ation leads us to onde
ab
out the minimum p
opelle


otational speed involved, as ell as the magnitude of the poe

equi
ed fo
igh
t. Figu
e 2.3.4 helps us
ith some of these questions. We can see that a p
opelle
ill have to achieve a
pp
oximately 412
ad.s
1
,
hich is equivalent to 3934
evolutions pe
minute, to p
ovide the minimum 2.45N

equi
ed fo
liftoff. The

espective p
opelle
poe
is 26W. Afte
this sho
t analysis e can state that t
he EPP1045 p
opelle
s a
e
16 CHAPTER 2. QUADROTOR DESIGN
suitable fo
implementation in the quad
oto
p
ototype, a statement that can be
p
oved by expe
imental
data
2
, shoing that ith the
ight moto
e can p
oduce the necessa
y th
ust fo
Lift
Off.
Figu
e 2.3.1: EPP1045 p
opelle
s.
Figu
e 2.3.2: Typical p
opelle
th
ust cu
ves as a function of advance
atio J a
nd blade angle [4].
2
.
ad
ota
y.com/moto
_test.xls, Novembe
2008.
2.3. PROPELLERS 17
Figu
e 2.3.3: Typical p
opelle
poe
cu
ves as a function of advance
atio J an
d blade angle [4].

Figu
e 2.3.4: Theo
etical th
ust and poe
of an EPP1045 p
opelle
.

18 CHAPTER 2. QUADROTOR DESIGN
2.4 Elect
onics
2.4.1 Moto
s and Elect
onic Speed Cont
olle
s
The moto
s usually implemented in this kind of application a
e elect
ic Di
ect C
u

ent (DC) moto
s. They
a
e lighte
than combustion engines and do not need a combustible fuel, hich, a
mong othe
bene ts,
dec
eases the
isk of explosion.
DC moto
s available in the
adio cont
ol hobby ma
ket a
e eithe
b
ushed o
b
us
hless. B
ushless
moto
s a
e expensive but have highe
ef ciency, poe
, and do not need
egula
mai
ntenance. B
ushed
moto
s a
e cheap but have a sho
te
lifetime and thei
b
ushes need
egula

epl
acements. Fo
these

easons it is p
efe
able to use b
ushless moto
s, because loss of st
uctu
al int
eg
ity of the quad
oto
due
to moto
failu
e should be avoided by using mo
e
eliable equipment.
The
e a
e cases hen a moto
does not have the necessa
y to
que to
spin the p
opelle
at the

equi
ed speed, o
even hen the
e is the need to
educe the p
opelle
speed to
an optimum velocity
infe
io
to that of the main d
ive shaft. These a
e situations he
e a PSRU (P

opelle
Speed Reduction
Unit  a gea
box speed
eduction system) is
equi
ed. Although these units a
e
available to use in RC
(Radio Cont
ol) ai
c
afts, in the quad
oto
e ant to have a st
uctu
e as light
as possible. One ay to
have the bene ts of high to
que ithout using gea
boxes is by using a design of b

ushless DC moto

called Out
unne
. The selected moto
as the BLOut
unne
282434 model f
om the m
anufactu
e

Robbe ROXXY (Figu
e 2.4.2). This moto
is able to
otate at 12100 RPM hen f
ee
of load, eights 48g
and has a maximum ef ciency of 79%
3
.
The speed of a b
ushless moto
is cont
olled by an Elect
onic Speed
Cont
olle
s (o
ESC). This
ha
da
e
eceives the poe
f
om the batte
y and d
ives it to the moto
acco
din
g to a PWM (Pulse
Width Modulation) signal that is p
ovided by the cont
olle
unit. The thunde
b
i
d9 ESC f
om Castle
C
eations is ell suited fo
the job at hand (Figu
e 2.4.1). It has a mass of 9g
and is capable of p
oviding
up to 9A of cu

ent
4
(hich is also the maximum alloable cu

ent of the BLOut
unne
282434 moto
).
Figu
e 2.4.1: Thunde
bi
d9 ESC. Figu
e 2.4.2: Elect
ic DC B
ushless mot
o

Robbe
ROXXY BLOut
unne
282434.
2.4.2 Mic
ocont
olle
Unit
A stabilization system is necessa
y to d
ive the quad
oto
because it
is a natu
ally unstable vehicle.
The implementation of stabilization cont
ol algo
ithms has not been possible unt
il ve
y
ecently ith the
3
http://data.
obbeonline.net/
obbe_pdf/P1101/P1101_14777.pdf, Feb
ua
y 2009.
4
http://.castlec
eations.com/suppo
t/documents/Thunde
bi
d9use
guide.pdf, F

eb
ua
y 2009.
2.4. ELECTRONICS 19
development of small size mic
ocont
olle
s.
One mic
ocont
olle
that has gotten special attention f
om the
obotics communit
y o
ldide is the
A
duino. This mic
ocont
olle
platfo
m is quite inexpensive and has a Cbased
language development
envi
onment that is ve
y intuitive to use. F
om the diffe
ent ve
sions of the
A
duino, the selected one
fo
this p
oject as the A
duino Duemilanove (Figu
e 2.4.3). With a mass of 35
g, the Duemilanove has
6 analog inputs ith 10 bit
esolution, se
ial po
t communication, 14 I/O pins (
of hich 6 p
ovide PWM
output) and many othe
featu
es
5
.
Figu
e 2.4.3: A
duino Duemilanove.
2.4.3 Wi
eless communication
Wi
eless communications a
e alays a challenge. One has to eight impo
tant fa
cto
s like poe
con
sumption, eight, t
ansmission speed and
eliability. Fo
tunately it is possib
le to use ha
da
e ith the
A
duino that allos to satisfy all the p
evious conditions, e.g. the XBee 802.
15.4 1mW (Figu
e 2.4.4).
This module can be 
apped into a se
ial command set (useful becaus
e the A
duino can use se
ial
communication), consumes only 50mA of cu

ent, has a maximum data
ate of 250kbp
s (using
adio f
e
quency) and a
ange of 100m (la
ge

ange telemet
y is not a
equi
ement of this
thesis)
6
. To XBee
modules a
e going to be used: one fo
the quad
oto
and anothe
in the g
ound
station compute
(that
ill handle all telemet
y fo
system identi cation and cont
ol pu
poses).
Figu
e 2.4.4: XBee 802.15.4 1mW i
eless module.
2.4.4 Senso
s
The senso
s of a
oto
c
aft a
e a key element of the cont
ol loop. They a
e

esponsible fo
p
oviding
info
mation like ai
c
aft attitude, accele
ation, altitude, global position, and
othe

elevant data. Mode
n
ai
c
aft often ca

y
obust and expensive senso
technology, hich is
often a synonymous of big and
5
http://a
duino.cc/en/Main/A
duinoBoa
dDuemilanove, Feb
ua
y 2009.
6
http://.inmotion.pt/sto
e/p
oduct_info.php?cPath=7&p
oducts_id=63, Feb
ua
y 2
009.
20 CHAPTER 2. QUADROTOR DESIGN
heavy elect
omechanical systems. In ou
quad
oto
p
ototype, the keyo
ds a
e
light and small, thus
hen it comes to small sensing e should conside
MEMS (Mic
oElect
oMechanical
Systems) tech
nology. The use of locost senso
s of this type implies less ef cient data p
oc
essing and thus a bad
o
ientation data p
ediction in addition to a eak d
ift
ejection, making the c
ont
ol p
ocess a lot mo
e
challenging. Also, and in spite of the latest advances in miniatu
e actuato
s,
the scaling las
7

a
e still
unfavo
able and one has to face sometimes the p
oblem of actuato
satu
ation.
Ou
p
ototype has to senso
s, a t
iple axis accele
omete
p
oviding
th
ee accele
ations (one fo

each axis of a Ca
tesian coo
dinate system) and a dual axis magneto
mete
p
oviding the magnetic
no
th di
ection. The accele
omete
of choice is the ADXL330 (Figu
e 2.
4.4) and some of its main
cha
acte
istics a
e: maximum cu

ent consumption of 320A, an accele
ation
ange
of 29.4m.s
2
and
a mass of app
oximately 2g. The magnetomete
is an HMC6352 (Figu
e 2.4.4), ith a
mass of 0.14g, a
1mA cu

ent consumption, update
ate up to 20Hz and a selectable heading
esolut
ion of eithe
0.5

o

1

.
Figu
e 2.4.5: ADXL330 accele
omete
. Figu
e 2.4.6: HMC6352 compass.
2.4.5 Batte
ies
Poe
sto
age has expe
ienced g
eat advances in the last decades, mainly due to
the sea
ch of lighte

and longlasting poe
sou
ces fo
the mobile devices indust
y and
ecently, fo

the eme
ging elect
ic
ca
ma
ket. The ai
c
aft indust
y hoeve
as not yet had a noticeable inte
est
in elect
ic poe
sou
ces.
Inte
ested pa
ties in this eld of development a
e mostly
elated to a fe
esea
c
h p
ojects (some of hich
make use of sola
ene
gy as a poe
sou
ce) o
even some hobby develope
s that a

e al
eady on thei

ay to selling thei
elect
ic ai
planes (e.g. the Elect
aFlye
C
8
and the Sunseeke
II
9
). In the RC ma
ket,
elect
ic batte
ies have p
oven to be a long te
m cheape
solution to combustion
engines. Speci cally,
Lithiumion Polyme
(o
LiPo) batte
ies, a
ecent advance in poe
s
to
age technology, p
ovide high
capacity, light and
obust poe
sou
ce that has a la
ge ma
ket spect
um of appl
ications, including RC
ai
c
aft. Fo
these
easons, the quad
oto
p
ototype ill use to s
epa
ate LiPo batte
ies, one fo
the
moto
s and anothe
fo
A
duino (a sepa
ate poe
sou
ce fo
the cont
olle
unit
assu
es that the cont
olle

has poe
, even if the main moto
batte
y
eaches c
itical levels hich is usefu
l fo
eme
gency situations).
The A
duino is the
efo
e poe
ed ith a 15g LiPo batte
y, capable of p
oviding 2
40mAh of cu

ent at
7.4V . As fo
the moto
s, they a
e poe
ed by a 179g batte
y, elect
ic cha
ge of
2200mAh at 11.1V (Figu
e
7
Scaling las a
e an enginee
ing concept that
efe
s to va
iables hich change d

astically depending on the scale being con
side
ed. Fo
example, if e t
y to use a mechanical gy
oscope aboa
d an ai
c
a
ft subject to a st
ong elect
omagnetic eld, e
may expe
ience diffe
ent
eadings than hen using a small piezo elect
ic gy
osco

pe (hose
eadings a
e highly susceptible to

elect
omagnetic elds).
8
http://.elect
a ye
.com/elect
a ye
c.php
9
http://sola
 ight.com/
2.4. ELECTRONICS 21
2.4.7).
Figu
e 2.4.7: Visle
o LiPo 11,1V 2200mAh 20C batte
y.
2.4.6 Elect
onic ci
cuit schematics
Afte
desc
ibing the pa
ts used in the const
uction of the quad
oto
, it is impo

tant to mention ho the
elect
onic components a
e assembled (the elect
onic ci
cuit schematics can be fo
und in Appendix A).
Each one of the moto
s has th
ee i
es hich a
e di
ectly connected to the
espe
ctive elect
onic speed
cont
olle
(o
ESC). The speed cont
olle
s e
e attached to the main batte
y th

ough a MPX connecto
.
These speed cont
olle
s also
equi
e a connection to a 5V DC poe
supply, g
ou
nd i
ing, and the use
of the PWM pins on the p
ocessing boa
d (all of these pins a
e available on the
A
duino).
The i
ing of the accele
omete
uses the 3.3V pin as the senso
poe
supply,
a g
ound pin, and
th
ee analog inputs fo
the accele
ations. Tu
ning ou
attention to the compas
s, its poe
supply uses
the 5V pin on the A
duino, a g
ound pin and the to analog inputs fo
establish
ing data t
ansactions by
I2CBus p
otocol.
Supplying the A
duino ith DC means e need to use at least a 6V batte
y. To
monito
this poe

sou
ce, a ci
cuit as devised to light up a LED in case the voltage d
ops belo
6.2V . The basic philoso
phy in this batte
y check system is the fact that elect
icity follos the easies
t ay possible to the g
ound.
So, hile voltage is above 6.2V , elect
ic cu

ent os th
ough a Zene
diode and
activates the base col
lecto
of a NPN t
ansisto
, and allos cu

ent to o f
om the collecto
to the em
itte
of this component,
di
ectly to the g
ound. If the voltage d
ops belo 6.2V then no mo
e cu

ent
passes th
ough the Zene

diode, meaning it has to go th
ough the LED, lighting it up in the p
ocess. Wh
ile testing the quad
oto
,
it as discove
ed that the A
duino
emained poe
ed, even hen its batte
y as n
ot connected. This
happened because the chosen elect
onic speed cont
olle
s have the pa
ticula
abi
lity to use poe
f
om
the moto
s batte
y to feed 5V to the ha
da
e connected to it, usually a RC
ec
eive
but in ou
case, an
A
duino boa
d. This made the batte
y fo
the A
duino dispensable, but at the sam
e time useful if e ant
to keep the enti
e system online fo
a fe mo
e minutes.
Rega
ding the autonomy of the main batte
y, a voltage divide
as implemented so
that an analog
po
t on the A
duino could be used to measu
e its voltage. This app
oach as di
sca
ded due to the fact
that the speed cont
olle
s al
eady possess a built in secu
ity system
that sloly cuts poe
f
om the
moto
s as soon as they detect its time to
echa
ge the main batte
y (note that th
e
e is no
isk of the

A
duino stop o
king befo
e the moto
s because the ESCs p
events the moto
s of 
o
king hen the
batte
y
eaches 9V, hich is mo
e than enough fo
ou
p
ocessing unit).
22 CHAPTER 2. QUADROTOR DESIGN
2.5 Flight autonomy
Flight autonomy is an impo
tant speci cation hen designing a quad
oto

. The majo
facto
di
ectly
in uencing the ight autonomy is the high poe
consumption of the moto
s, hich inc

eases ith the
value of the p
opelle
angula
velocity. An elect
ic DC moto
does not have a
linea
behavio
(Figu
e
2.5.1). Usually, it is cha
acte
ized by angula
speed satu
ation, as ell as a
dead zone p
eceding the
minimum voltage
equi
ed fo
p
opelle

otation [19]. The satu
ation, p
events
to su
pass the maximum
speed alloable by the moto
.
Figu
e 2.5.1: Example illust
ation of the angula
velocity of a DC moto
as a
function of poe
supply
voltage.
The p
ope
ty that cha
acte
izes ho the angula
velocity of a ce
tain DC moto
e
volves ith poe

supply voltage is the k
v
(p
onounced exactly as kv). This p
ope
ty simply allos to kno ho many
RPM ill the moto
p
ovide pe
each Volt of supplied elect
icity. Thus, to calcu
late ho a moto
s angula

velocity changes ith voltage e can use:

v
=
2k
v
60
(2.6)
=
v
(V
m
V
0
) (2.7)
where
v
is the p
opelle
s angula
speed pe
Volt, V
m
is the moto
s poe
supply voltage and V
0
is the
poe
supply voltage co

esponding to the moto
s dead zone.
At this point, it is only a matte
of combining equations (2.1) (assuming that t
h
ust is equal to the total
eight of the vehicle) and (2.7) ith Ohms la to get the total elect
ic cu

ent
consumption I

equi
ed
to maintain a quad
oto
of a given mass m in the ai
:
I
=
V

0
R
m
+
1
R
m
k
v
_

2
mg
4c
T


4
_
1
2
(2.8)
Fo
p
actical
easons e can
e
ite 2.8 into:
I
= I
0
+

2R
m
k
v
r
2
_
mg
c
T

_1
2
(2.9)
2.5. FLIGHT AUTONOMY 23
he
e g is the accele
ation of g
avity, R
m
the moto
s coppe
coil elect
ic
esistance and I
0
the elect
ic
cu

ent consumption of the moto
in noload condition. No that e can calcula
te the moto
s elect
ical
consumption needs, it is time to conjugate this info
mation ith ce
tain batte
y
p
ope
ties that ill allo
the calculation of the ight autonomy f
t
, given the elect
ic cha
ge Q
of the batte
y:
f
t
=

Q
I
(2.10)
The BLOut
unne
282434 moto
has a coil
esistance of 0.3136 and is capable of

otating at 1100
RPM pe
Volt. At this point it is not possible to p
edict the exact mass of
the ai
c
aft neithe
to have
knoledge of the noload cu

ent, so let us assume a mass of 1kg a
nd a minimum elect
ical cu

ent
of 3A (a conse
vative value fo
this type of moto
). Equation (2.9
) the
efo
e shos that each moto

consumes 3.23A. If e inse
t this data into equation (2.7) (not fo
getting tha
t e have fou
moto
s) e
can p
edict a ight autonomy of app
oximately 614s (10.2 minutes), a
easonable ti
me given that e a
e
unce
tain about some p
ope
ties of the moto
s and ai
c
aft at this stage.
24 CHAPTER 2. QUADROTOR DESIGN
Chapte
3
Quad
oto
modelling
The
st step befo
e the cont
ol stage is the adequate modeling of the system dyna
mics. This phase ill
hopefully facilitate the cont
ol of the ai
c
aft as it ill p
ovide us ith a be
tte
unde
standing of the ove
all
system capabilities and limitations. The cu

ent chapte
ill guide us th
ough
the equations and tech
niques used to model ou
quad
oto
and its senso
s, p
oviding the mathematical b
asis fo
the application
of the system dynamics in a simulation envi
onment.
3.1 System dynamics
W
iting the equations that po
t
ay the complex dynamics of an ai
c
aft implies
s
t de ning the system
of coo
dinates to use. Only to
efe
ence f
ames a
e
equi
ed, an ea
th xed f
ame
and a mobile f
ame
hose dynamic behavio
can be desc
ibed
elative to the xed f
ame. The ea
th xed
axis system ill
be
ega
ded as an ine
tial
efe
ence f
ame: one in hich the
st la of Neton
1
is valid. Expe
ience
indicates this to be acceptable even fo
supe
sonic ai
planes but not fo
hype
s
onic vehicles
2
[21]. We
shall designate this
efe
ence f
ame by O
NED
(No
thEastDon) because to of its axis (u
x
and u
y
) a
e
aligned
espectively ith the No
th and East di
ection, and the thi
d axis (u
z
) is di
ected don, aligned
toa
ds the cente
of the Ea
th (Figu
e 3.1.1). The mobile f
ame is designated
by O
ABC
, o
Ai
c
aft
BodyCente
ed, and has its o
igin coincident ith the quad
oto
s cente
of g
avit
y (Figu
e 3.1.2).

1
An object that is not moving ill not move until a net fo
ce acts upon it. An
object that is moving ill not change its velocity
(accele
ate) until a net fo
ce acts upon it.
2
The
otational velocity of Ea
th must not be neglected fo
hype
sonic ight.
25
26 CHAPTER 3. QUADROTOR MODELLING
Figu
e 3.1.2: NED and ABC
efe
ence f
ames.
Figu
e 3.1.1: Visualization of the No
thEastDon
efe
ence f
ame.
In cont
ol theo
y, knoledge about the dynamic behavio
of a given system can be
acqui
ed th
ough
its states. Fo
a quad
oto
, its attitude about all 3 axis of
otation is knon
ith 6 states: the Eule
angles
[ ] (Roll Pich Yaw as seen beore in Figure (1.2.1)) and he angular velociies a
round each axis
o he O
ABC
rame [P Q R].
Ye
anoher 6 saes are necessar: he posiion o he cener o
 gravi (or COG) [X Y Z] and
respecive linear veloci componens [U V W] relaive o he xed rame. In
sum, he uadroor has
12 saes ha describe 6 degrees o reedom.
Unsurprisingl, we mus deduce he euaions describing he orienaion o he m
obile rame relaive
o he xed one, which can be achieved b using a roaion marix. This marix
resuls o he produc
beween hree oher marices (R
(), R
() and R
()), each o hem represening he roaion o he
ABC rame around each one o he O
NED
axis [22]:
3.1. SYSTEM DYNAMICS 27
R
() =
_

_
1 0 0
0 cos
0 sin
_

_ R

sin
cos

() =
_

_
cos 0 sin
0 1 0
sin 0 cos
_


_ R
() =
_

_
cos sin 0
sin cos 0
0 0 1
_

_ (3.1)
S = R
() R
() R
() (3.2)
S =
_

_
cos cos cos sin sin
sin sin cos cos sin cos cos + sinsin sin sincos
cos sin cos + sinsin sin cos sin sincos cos cos
_

_ (3.3)
where S is he roaion marix ha expresses he orienaion o he coordinae
rame O
ABC
relaive o he
reerence rame O
NED
. To mahemaicall wrie he movemen o an aircra we mus emplo Newons
second law o moion. As such, he euaions o he ne orce and momen aci
ng on he uadroors
bod (respecivel F
ne
and M
ne
) are provided:
F
ne
=
d
d
[mv]
B
+
[mv]
B
(3.4)
M
net
=
d

dt
[I
]
B
+

_
I
_
B
(3.5)
he
e I is the ine
tia mat
ix of the quad
oto
,
locities and

v is the vecto
of linea
ve

the vecto
of
angula
velocities. If the equation of Netons second la is to be as complete
as possible, e should
add ext
a te
ms such as the Co
iolis, Eule
and ae
odynamic fo
ces (e.g. ind)
, but to keep the model
simple, and also because the quad
oto
is not supposed, at this sta
ge, to go ve
y fa
aay f
om the
g
ound station, these fo
ces ill not be inco
po
ated in the modeling p
ocess. T
he fo
ce of g
avity (F
g
) is
too signi cative to be neglected, thus it is de ned by:
F
g
= mS
_
0 0 g
_
T
= mg
_
sin cos sin cos cos
_
T
B
(3.6)
The orce o gravi ogeher wih he oal hrus generaed b he propellers
(F
P
) have hereore o be
eual o he sum o orces acing on he uadroor:
F
g
+F
P
= F
ne
(3.7)
Combining euaions (3.4), (3.6) and (3.7) we can wrie he vecor o linear acc
eleraions acing on he
vehicles bod:
_

W
_

_ =
1
m
_

_
F
Px
F
P
F
Pz
_

_ +g
_

_
sin
cos sin
cos cos
_

_
_

_
QW RV
RU PW
PV QU
_

_ (3.8)
where [F
Px
F
P
F
Pz
] are he vecor elemens o F
P
. Assuming he aircra is in a hovered igh, in such
a scenario here are orces acing onl in he z axis o uadroor, correspondi
ng o he siuaion where
28 CHAPTER 3. QUADROTOR MODELLING
we have he engines ring o overcome he orce o gravi o keep he aircra
sable a a given aliude:
F
Pz
= (T
1

+T
2
+T
3
+T
4
) (3.9)
Noe ha he minus sign means he liing orce is acing upwards, awa rom h
e surace (noe ha he
posiive axis o he O
NED
is poin downwards).
Working now on Newons second law or roaion, he ineria marix is given b:
I =
_

_
I
11
I
12
I
13
I
21
I
22
I
23
I
31
I
32
I
33
_

_ (3.10)
Assuming he uadroor is a rigid bod wih consan mass and axis aligned wih
he principal axis o
ineria, hen he ensor I becomes a diagonal marix conaining onl he princip
al momens o ineria:
I =
_

_
I
11
0 0
0 I
22
0
0 0 I
33
_

_ (3.11)
Joining euaions 3.11 and 3.5 we ge:
M
ne

=
_

_
I
11
0 0
0 I
22
0
0 0 I
33
_

_
_

R
_

_ +
_

_
P
Q
R
_

_
_

_
I
11
0 0
0 I
22
0
0 0 I
33
_

_
_

_
P
Q
R
_

_ (3.12)
Conseuenl, we will have:

M
ne
=
_

_
I
11

P
I
22

Q
I
33

R
_

_ +
_

_
(I
33
I
22
) QR
(I
11
I
33
) RP
(I
22
I
11
) PQ
_

_ (3.13)
_

R
_

_ =
_

_
Mx
I11
M

I22
Mz
I33
_

_
_

_
(I33I22)QR
I11
(I11I33)RP
I22
(I22I11)PQ
I33
_

_ (3.14)
Inormaion on he momens acing on he aircra can be provided b:
M
x
= (T
4
T
2
) d
cg
(3.15)
M

= (T
1
T
3
) d
cg
(3.16)
M
z
= (T
1
+T
3
T
2
T
4
) K
TM
(3.17)
where d
cg
is he disance o he aircras COG (see Table 3.4) and K
TM
is a consan ha relaes momen
and hrus o a propeller (see Secion 3.4).
3.2. KINEMATIC EQUATIONS AND EULER ANGLES 29
3.2 Kinemaic euaions and Euler angles
In his secion we will sud he kinemaics o he uadroor. The rs sage o k
inemaics analsis consiss

o deriving posiion o obain veloci. Le us hen consider he posiion vec
or

r , which indicaes he
posiion o he origin o he O
ABC
rame relaive o O
NED
:

r = X

i +Y

j +Z

k (3.18)
I we derive each o he componens in

r we can obain he insananeous veloci o O
ABC
relaive o
O
NED
:

r =

X

i +

Y

j +

Z

k (3.19)
To nd ou he componens o he aircras linear veloci v
in he
_

_
U
V
W
_

_ = S
_

xed rame we can use:

_ (3.20)
where we can ake ull advanage o he orhogonali o S, meaning is inverse
is eual o is ranspose:
_

Z
_

_ = S
T
_

_
U
V
W
_

_ (3.21)
_

Z
_

_ =
_

_
cos cos U + (sinsin cos cos sin) V + (cos sin cos + sinsin) W
cos sinU + (cos cos + sinsin sin) V + (sin cos sin sincos ) W
sinU + sincos V + cos cos W
_

_
(3.22)
The igh pah o he uadroor in erms o [X Y Z] can be ound b inegraion
o euaion 3.22. To per
orm his inegraion, he Euler angles , and mus be known. However, he Euler a
ngles hemselves
are uncions o ime: he Euler raes

, and

depend on he bod axis angular raes P, Q and R. To

esablish he relaionship beween

_

_
and [P Q R] he ollowing euali mus be sais ed:

= P

i +Q

j +R

k =

(3.23)
Noe ha alhough i ma appear ha he Euler raes are he same as angular ve
lociies, his is no he
case. I a solid objec is roaing a a consan rae, hen is angular veloci


ill be constant, hoeve

the Eule

ates ill be va
ying because they depend on the instantaneous angles
beteen the coo
dinate
f
ame of the body and the ine
tial
efe
ence system (e.g. beteen O
ABC
and O
NED
). The Eule
angle
sequence is made up of th
ee successive
otations: Roll, Pitch and Ya. In othe

o
ds, the angula

ate

needs one roaion,

needs wo and

needs hree:

= R() R() R()
_

_
0
0

_ +R() R()
_

0
_

_ +R()
_

0
0
_

_ (3.24)
30 CHAPTER 3. QUADROTOR MODELLING
Thereore:
_

_
P
Q
R
_

_ =
_

_
1 0 sin
0 cos sincos
0 sin cos cos
_

_
_

_ (3.25)
Solving or he Euler angular raes ields he desired dierenial euaions [2
3]:
_

_ = T
_

_
P
Q
R
_

_ (3.26)
T =
_

_
1 an sin an cos
0 cos sin
0 sin/ cos cos / cos
_

_ (3.27)
where T is he marix ha relaes he bod xed angular veloci vecor

and the
ate of change of the
Eule
angles.
3.3 Quate
nion diffe
ential equations
A p
oblem ith the implementation of Eule
angles , and is ha or = 90

the Roll angle loses

is meaning. In oher words, I he aircra piches up 90 degrees, he aircra
roll axis becomes parallel o
he aw axis, and here is no axis available o accommodae aw roaion (one de
gree o reedom is los).
This phenomenon is designaed b gimbal lock. In simulaions where complee loop
ing maneuvers ma
have o be perormed ha is no accepable. To overcome his problem, he so ca
lled uaernion mehod
ma be used. Quaernions are vecors in ourdimensional space ha oer a mah
emaical noaion ha
allows he represenaion o hree dimensional roaions o objecs. Quaernio
ns also avoid he problem
o gimbal lock and, a he same ime, are numericall more e cien
and sable when compared wih
radiional roaion marices (e.g. S) [24]. A roaion uaernion is hen prese
ned b:
 =
_

_
cos (/2)
sin(/2) n
1
sin(/2) n
2
sin(/2) n
3

_
(3.28)
h r  r r s nts a rotation about th unit v ctor [n
1
n
2
n
3
] through an angl . Th tim d rivativ of
th rotation uat rnion is also rovid d by [23]:
 =
1
2

q
q +q =
1
2
_

_
0 P Q R
P 0 R Q
Q R 0 P
R Q P 0
_

_
_

_
q
0
q
1
q
2
q
3
_

_
+

_
q
0
q
1
q
2
q
3
_

_
(3.29)
= 1
_
q
2
1
+q
2
2
+q
2
3
+q
2
4
_
(3.30)
3.4. MODELLING OF A MOTORPROPELLER ASSEMBLY 31
For practical reasons, the initialization of equation (3.29) requires that we ex
press the quaternion com
ponents in terms of Euler an
les because doin
it with quaternions is not so str
ai
htforward. Therefore,
expressin
the quaternion vector elements as a function of Euler an
les yields:
_

_
q
0

= cos
_

2
_
cos
_

2
_
cos
_

2
_
+ sin
_

2
_
sin
_

2
_
sin
_

2
_

1
= cos
_

2
_
cos
_

2
_
cos
_

2
_
sin
_

2
_
sin
_

2
_
sin
_

2
_

2
= cos
_

2
_
sin
_

2
_
cos
_

2
_
+ sin
_

2
_
cos
_

2
_
sin
_

2
_

3
= sin
_

2
_
cos
_

2
_
cos
_

2
_
cos
_

2
_
sin
_

_
sin
_

2
_
(3.31)
Oherwise, i we wan o exrapolae he Euler angles rom he uaernion dier
enial euaions we can
achieve jus ha b aking inermediae seps o he roaion ensor S and rela
e hem wih he elemens
o he roaion uaernion marix, raher han calculaing hem rom he uaern
ion direcl:
S

=
_

_

2
0
+
2
1

2
2

2
3
2 (
1

2
+
0

3
) 2 (
1

3

0

2
)
2 (
1

2

0

3
)

2
0


2
1

+
2
2

2
3
2 (
2

3
+
0

1
)
2 (
1

3
+
0

2
) 2 (
2

3

0

1
)

2
0

2
1

2
2
+
2
3
_

_ (3.32)
where S

is he uaernion euivalen o he roaion marix (3.3).
can hereore be
calculaed b using:
_

The Euler angles


_ =
_

_
arcan
_
2(23+01)

2
0

2
1

2
2

2
3
_
arcsin(2 (
1

3

0

2
))
arcan
_
2(23+01)

2
0
+
2
1

2
2

2
3
_
_

_
(3.33)
In analog o (3.21), we can also ge he absolue veloci using he uaernion
roaion ensor:
_

Z
_

_ = S
T

_

_
U
V
W
_

_ (3.34)
To emplo he uaernion mehod we mus
rs use he iniial Euler an
gles in (3.31), and use he
resuling uaernion elemens in (3.32). I is also possible o combine he prev
ious uaernion euaions
wih he dnamics euaions o compose he vehicle acceleraion on he aircra
local rame:
F
g
= mS

_
0 0 g
_
T
= mg
_
2 (
1

3

0

2
) 2 (
2

3
+
0

1
)

2
0

2
1

2
2
+
2

3
_
T
B
(3.35)
_

W
_

_ =
1
m
_

_
F
Px
F
P
F
Pz
_

_ +g
_

_
2 (
1

3

0

2
)
2 (
2

3
+
0

1
)

2
0

2
1

2

+
2
3
_

_
_

_
QW RV
RU PW
PV QU
_

_ (3.36)
3.4 Modelling o a moorpropeller assembl
The moorpropeller assemblies are essenial componens o our aircra since h
e are responsible or
he producion o he liing orce ha allows igh. This secion ocuses on m
odeling he dnamics o
32 CHAPTER 3. QUADROTOR MODELLING
hese componens.
As we have seen previousl in he uadroor design secion here are
man variables o accoun
or when choosing he righ moorpropeller assembl. This is mosl because 
he hrus is a uncion
o
several properies such as he propellers diameer, hrus
coe cie
n, air densi and he moors
k
v
. Moreover, alhough we have 4 eual moors, in reali each one o hem has a d
ieren dnamic
behavior. This dissimilari reuires a more precise modelling o he moors,
a process ha oen sars
b esablishing he relaion beween he volage ed o he moor and he machi
ng speed o roaion.
A permanen magne DC moor ranser uncion is usuall described b a gain and
wo real poles
(also known as a secondorder model), respecivel associaed wih mechanical an
d elecrical ime con
sans, being he mechanical consan commonl higher han he elecrical one b
a leas one order o
magniude. In oher words, he pole associaed wih he elecrical consan is
he aser one and as a
conseuence he dnamic behavior o such a moor is dominaed b he slower pole
. Having his knowl
edge ino consideraion, a common wa o de ne he dnamic behavior o a DC moor
is b using a rs
order ranser uncion, having onl a gain and a pole associaed wih he mecha
nical ime consan.
To ideni he ranser uncion o each moor we have o conceive
a mehod o daa acuisiion,
paricularl beween he inpu and oupu o each moor. The propeller roaio
n speed (oupu) can be
acuired hrough a achomeer device. For he inpu we could use he volage p
rovided o a moor bu
or pracical reasons i is b ar simpler o use he PWM signal because i is a
variable we have direc
access o (measuring volage is also possible using he analog pins on he Ardui

no board, bu in his case

hese are alread being used or sensor readings). Figure 3.4.1 shows an idealiz
aion o he ideni caion
process.
Figure 3.4.1: Schemaic o he inpuoupu seuence or ideni caion purposes.
Here arises our rs big problem: we know our inpus because we can program he pu
lse widh o he
signals ha conrol he speed o he moors, bu how can we know our oupus i
one does no have direc
access o a digial achomeer device? The devised soluion or his problem wa
s o build a rudimenar
achomeer o ge he job done. This device consised o a simple m
icrophone placed approximael
3cm awa rom he ip o he propeller and high enough no o ge
hi
b i
(see Figure 3.4.2). The
echnolog isel does no seem o be new, a simple search on he Inerne revea
ls ha here are alread
a ew digial microphone achomeers available o he general public. Nonehel
ess, he concep behind
i is uie simple: a each pass o he propeller, air ges pulled downwards o
 he propeller plane and a
microphones membrane ma be able o regiser he conseuen sucion eec. Ano
her advanage o
using a microphone is he ac ha modern da compuers can capure sound daa
o a microphone in
realime, which makes i ver useul or an MATLAB

daa processing or Simulink

daa acuisiion ha
ma be reuired. Following his logic, b using Simulink

i was possible o provide orders o an Arduino
program and capure he sound daa o each propeller.
3.4. MODELLING OF A MOTORPROPELLER ASSEMBLY 33
Figure 3.4.2: Microphone posiioned o record he propeller sound.
Sound processing o he capured sound les was made as simple as possible. Usin
g a MATLAB

scrip
i
was possible o plo
he sound daa and measure he per
iod beween each passage o he
propeller blades T
p
(see Figure 3.4.3), calculaing he speed o roaion immediael aerwards:
=

T
P
(3.37)
It is im ortant to m ntion that although this m thod for m asuring th ro ll r
out ut is v ry ch a
and acc ssibl , it do s not ork v ry  ll for lo s ds of rotation as as v r
i d during x rim ntal
trials, mostly du to v ry lo signal-to-nois ratios (i. . nois am litud is
so high that it g ts v ry hard
to distinguish hat is ambi nt nois from ro ll r sound). Figur 3.4.3 shos
a v ry cl ar signal form
of th ro ll r sound, h r  can cl arly id ntify th sound r ssur av s o
f th assag of th
ro ll r. Proc ding ith th motor- ro ll r syst m id nti cation, m asur m n

ts took lac to nd
th r s ctiv d ad zon (Tabl 3.1) and dynamic r s ons (Tabl 3.2).
Figur 3.4.3: Pro ll r sound. Each st in am litud is a cons u nc of a ro
ll r blad assag .
34 CHAPTER 3. QUADROTOR MODELLING
Tabl 3.1: D ad zon uls idth of ach motor.
Motor i D ad zon uls idth (s)
1 1262
2 1252
3 1250
4 1252
Tabl 3.2: Data oints gath r d for und rstanding th motor- ro ll r dynamics.
In ut PWM signal (s) 1500 1700 1800 2000
Motor 1 out ut (rad.s
1
) 4268.6 5926.5 6504.8 6729.5
Motor 2 output (rad.s
1
) 4083.3 5732.8 6361.3 6645.9
Motor 3 output (rad.s
1
) 4336.5 6132.5 6746.1 6925.2
Motor 4 output (rad.s
1
) 4329.6 6064.2 6684.5 6890.2
Fi
ure 3.4.4 shows plotted data of tables 3.1 and 3.2 revealin
that the dynamic
s of the motors is
nonlinear, meanin
that direct implementation of a rstorder transfer function to
model the dynamic of
each motor is not adequate.
To solve this issue it was decided that to obtain a linear model
(to allow implementation of
rst
order transfer functions) the model should be linearized around an ad
equate operation point. Initial
experimental i
ht tests were executed and revealed that shortly beyond the dead z
one the quadrotor
took off from the
round. Takin
this observation into consideration led to th
e choice of a relatively low
operation point (pulse width) of 1300s.
Fi
ure 3.4.4: Dynamic behaviour of the motors. The
rey areas show a zoom on of
the linearization zone
in red.
(x collected data points,  second order polynomial data t, linearization)
3.4. MODELLING OF A MOTORPROPELLER ASSEMBLY 35
First order transfer functions require a
ain K and a time constant :
G(s) =
K
s + 1
(3.38)
The gains are eual o he slope o he linearized dnamic response
o each moor (dashed lines o
Figure 3.4.4; slope values in Table 3.3). Inpuoupu
daa was ga
hered or moor 1 and he Open
Ssem Ideni caion Tool Graphical User Inerace in MATLAB

was used o esimae he respecive
ime consan, wih value 0.136s. A sample o he inpuoupu daa
used can be observed in Figure
3.4.5. Time consans or he our moors were assumed o be all eual o he on
e o moor 1, which also

serves as uure es or robusness o he conroller.

Table 3.3: Gains o he ranser uncions describing he moors dnamics.
Moor 1 Moor 2 Moor 3 Moor 4
Gain K 2.0276 1.8693 2.0018 1.996
Figure 3.4.5: Inpuoupu daa used wih he ssem ideni caion oolbox o calc
ulae he ime consan.
( pulse widh o he inpu signal;  speed o roaion)
B using Simulink

o simulae he moors (Figure 3.4.6) we can exend he uali o i i we add
exra parameers like sauraion o he moors speed (i.e. maximum and lowes
possible speeds), he
dead zone o each moor, and sampling reuenc (i.e. he Arduino is a digial
plaorm and hereore i
is no a coninuousime processing device).
36 CHAPTER 3. QUADROTOR MODELLING
Figure 3.4.6: Blocks used in Simulink

o simulae moor 1.
Now ha we have a wa o calculae he speed o roaion o a moorpropeller a
ssembl in a sim
ulaion environmen, we also need o calculae he orce and momens produced b
each one o hese
assemblies. Toal orce can be calculaed b adding he hrus produced b a p
ropeller as in euaion
(2.1). As or he momen M
P
produced b he same propeller we have:
P
P
= M
P
(3.39)
Replacing this equation (3.39) in (2.2) leads to:
M
P
= c
P
4

5

2
(3.40)
Assuming all variabl s in s. (2.1) and (3.40) ar constant ith xc tion fo
r angular s d, ro ll r
mom nt and thrust,  can r rit th s uations:
T = K
T

2
(3.41)
K
T
= c
T
4

4

2
= 0.1154
4 1.2 (5 25.4 10
3
)
4

2
= 1.46 10
5
k
.m.rad
2
(3.42)
M
T
= K
M

2
(3.43)
K
M
= c
P
4

5

3
= 0.0743
4 1.2 (5 25.4 10
3
)
5

3
= 3.8 10
7
k
.m
2
.rad
2
(3.44)
where K
M
and K
T
are constants that respectively relate a propeller moment and thrust with an
u
lar
speed. Moment and thrust can therefore also be related throu
h the constant K
TM
:
M
T
=
K
M
K
T
T = K
TM

T =
c
P
r
c
T

T (3.45)
K
TM
= 0.026m (3.46)
3.5 Mom nts of in rtia
Mass and its g om tric distribution in an aircraft is som thing of xtr m im or
tanc b caus it aff cts
th ntir dynamics of th syst m. Th n, aft r building th uadrotor, it is t
im to valuat som of its
most im ortant f atur s lik its mom nts of in rtia and mass.
W ill assum th in rtia matrix is diagonal and ositiv -d nit , ith th u
r os of sim lifying
3.5. MOMENTS OF INERTIA 37
th calculations and also du to th articular symm tric g om try of uadrotors
(s Figur 3.5.1). As
such, th calculation of in rtia mom nts ill only includ th g om try and mass
of th motors, as  ll
as th ir g om tric osition on th uadrotor.
Figur 3.5.1: Distanc from th motors to th c nt r of gravity.
Each motor  ights a roximat ly 0.048kg. Oth r im ortant variabl s r lat d to
th aircraft such as
th l m nts of ach motor in rtia t nsor (l
x
, l
y
and l
z
) can b consult d in tabl 3.4.
Tabl 3.4: Quadrotors mass and main g om tric variabl s.
Variabl
Valu
l
x
(m) 0.0288
l
y
(m) 0.0288
l
z
(m) 0.026
d
cg
(m) 0.29
m (kg) 0.82
It follos that th in rtia matrix l m nts of th aircraft ar :
_

I
x1
= I
x3
=
1
12
m
m
(l
2
y
+l
2
z
) = 6.0218 10
6
k
.m
2
I
x2
= I
x4
=
1
12
m
m
(l
2
y
+l
2
z
) +m
m
d
2
c

= 0.004 k
.m
2
I
11
= 2I
x1
+ 2I
x2
= 0.0081 k
.m
2
(3.47)
38 CHAPTER 3.
_

QUADROTOR MODELLING

I
y1
= I
y3
=
1
12
m
m
(l
2
x
+l
2
z
) +m
m
d
2
c

= 0.004 k
.m
2
I
y2
= I
y4
=
1
12
m
m
(l
2
x
+l
2
z
) = 6.0218 10
6
k
.m
2
I
22
= 2I
y1
+ 2I
y2
= 0.0081 k
.m
2
(3.48)
_
_
_
I
z1
= I
z2
= I
z3
= I

z4
=
1
12
m
m
(l
2
x
+l
2
y
) +m
m
d
2
c

= 0.004 k
.m
2
I
33
= 4I
z1
= 0.0162 k
.m
2
(3.49)
And the inertia matrix itself is:
I =
_

_
I
11
0 0
0 I
22
0
0 0 I
33
_

_ =
_

_
0.0081 0 0
0 0.0081 0
0 0 0.0162
_

_ k
.m
2
(3.50)
3.6 Calculation of the center of
ravity
It has been clear so far that we have assumed the quadrotor to ha
ve its center of
ravity located in
the center of the XY plane of the O
ABC
reference frame. Nonetheless it is also important to calculate
its position alon
the Z axis because we will need it to know the relative pos

ition of the accelerometer

in the next section. This task was carried out in the most pract
ical way possible throu
h the use of
the SolidWorks

software, in which some of the most heavy parts of the quadrotor were modeled (e
.
.
motors, sensors, main battery and aluminum beams) and assembled. Fi
ure 3.6.1 sh
ows the computed
position of the COG, located 6.7cm above the base of the quadrotor.
Fi
ure 3.6.1: The red dot in the center of the
ure indicates the center of
ravi
ty.
3.7 Sensors modellin

Inertial navi
ation is implemented in cases where the use of an external referen
ce to measure position
is impractical. Typical inertial navi
ation systems used in reallife applicat
ions such as aircraft or space
rockets are hi
hly advanced, and expensive, pieces of technolo
y. H
owever, inexpensive sensorin

3.7. SENSORS MODELLING 39

equipment can be used to make a less accurate inertial navi
ation u
nit, hence the need for sensor
modellin
to increase the precision of computational simulations.
3.7.1 Accelerometer
As the name implies, the accelerometer is a sensor that measures accelerations.
Typically, accelerom
eters like the one installed onboard our quadrotor work throu
h piezo resistive
materials, that when de
formed by an external force, chan
e the intensity of the electric current owin
t
hrou
h them, providin

in this manner a quanti cation of that force.

Ideally, an accelerometer should be located near the center of
ravity of the ai
rcraft. In the case of
our quadrotor it is positioned in the coordinates indicated in table 3.5.
Havin
the position of the accelerometer relative to the center of
ravity, the
measured acceleration
a
m
is
iven by [19]:
a
m
= a
p
S
_
0 0

_
T
(3.51)
where a
p
is the absolute acceleration at point p, usually the point where the accelero
meter is located.
Also, the instantaneous velocity v
p
at point p is:
v
p
= v
c

+
B


s
(3.52)
he
e

s
is the position of the accele
omete

elative to the quad
oto
s cente
of g
avi
ty (see Table 3.5
fo
data on the accele
omete
location) and v
cg
is the vecto
of linea
velocities at the COG. Applying the
Co
iolis theo
em yields:
a
p
= a
cg
+
B


s
+
B
(
B


s
) + 2
B
v
cg
(3.53)
Given that:
ma
cg
= ma
B
+mS
_
0 0 g
_
T
(3.54)
e can
e
ite equation (3.53) to p
ovide absolute accele
ation of the accele
om
ete
located at point p:
a
p
=
a
B
m
+S
_
0 0 g
_
T
+
B


s

+
B
(
B


s
) (3.55)
Equation (3.55) does not contain the Co
iolis accele
ation. The
eason fo
thi
s is that the Co
iolis Fo
ce
is p
opo
tional to the velocity of the ai
c
aft. Because e do not ant to ach
ieve ext
eme velocities, e
a
e going to neglect this te
m of the equation. As e have a t
iaxis accele
o
mete
, e need to
ead
th
ee components of accele
ation:
_

_
a
px
a
py
a
pz
_

_ =
1
m
_

_
F
x
F
y
F
z
_

_+g
_

_
sin
cos sin
cos cos
_

_+
_

Qr
sz

Rr
s

Rr
sx

Pr
sz

Pr
s

Qr
sx
_

_+
_

_
Q(Pr
s
Qr
sx
) R(Rr
sx
Pr
sz
)
R(Qr
sz
Rr
s
) P(Pr
s
Qr
sx
)
P(Rr
sx
Pr
sz
) Q(Qr
sz
Rr
s
)
_

_
(3.56)
40 CHAPTER 3. QUADROTOR MODELLING
Table 3.5: Relevan acceleromeer daa.
Variable Value
Number o bis used b he Arduino or A/D conversion N
b
10
Sensor measuremen range 

3g
Sensiivi
0.3V.g
1
Posiion along he xaxis relaive o he cener o gravi r
sx
0.003m
Posiion along he axis relaive o he cener o gravi r
s
0m
Posiion along he zaxis relaive o he cener o gravi r
sz
0.049m
Using daa rom Table 3.5, he sensor reading resoluion o he Arduino board wi
ll be:
R
acc
=

smax

smin
2
N
b
=
3 (3)
2
10
= 0.00586g = 0.0575m.s
2
(3.57)
Oher variables such as variaion o sensor sensibili wih emperaure can be
simulaed wih he addiion
o whie Gaussian noise.
As alread noiced, we do no
have o gros onboard he uadroor.
Their absence also makes
he deerminaion o he angles o roll and pich uie rick because gros are
used or sensor usion
ogeher wih acceleromeers, providing ver sead oupus, hus maki
ng hem suiable or vibraion
inensive applicaions.
A raher less robus bu direc pah o obain hese angles is hrough he veco
r o graviaional accel
eraion provided b he acceleromeer, based on he iniial approach ha we do
no inend o move he
uadroor a high speeds (oherwise ver high acceleraions will indicae roll a
nd pich angles wih ver
high error) :
= arcan
_
a
p
a
pz
_
(3.58)
= arcan
_
a

px
a
pz
_
(3.59)
3.7.2 Compass
Compasses are scieni c insrumens used o measure he direcion o he magneic e
ld in he vicini o
he sensor. Among oher applicaions, he are used in aircra o provide a dir
ecional bearing componen
cenered on he norh or souh poles. The proope o his hesis uses a comp
ass which serves his
exac purpose. Elecronic compasses operae using he Halleec echnolog, wh
ich is o sa ha in is
inerior here are maerials which var he volage a is erminals when crosse
d b a magneic eld. Le
us assume ha he sensor can be modeled as ollows:
= N
c
(3.60)
where N
c
is he direcion o he magneic Norh Magneic Pole mapped beween and radians.
Tabl
3.6 shos th k y charact ristics of th s nsor to im rov th simulation v raci
ty.
Similarly to th acc l rom t r  can add hit Gaussian nois to th com ass r
adings to simulat
th ff cts of arasit magn tic
lds.
3.8. MODEL IMPLEMENTATION IN MATLAB

AND SIMULINK

41
Tabl 3.6: R l vant com ass data
Variabl
Valu
S nsor m asur m nt rang
360

Selected
esolution 1

3.8 Model implementation in MATLAB


and Simulink

Sta
ting ith the de nition of all the va
iables that a
e impo
tant to the simulat
ions, e can load them all
into the MATLAB

envi
onment by using the le loadva
s.m (see Appendix B.1). As fo
the dynamics
of
the quad
oto
s body, its modeling as accomplished by applying the equations p
ev
iously explained in
this section th
ough a function named quadsys.m (see Appendix B.2). He
es ho t
his function o
ks,
step by step:
1. Use the p
ovided state vecto
of the quad
oto
to build the quate
nion
ota
tion mat
ix S
q
necessa
y
fo
the next steps using equation 3.32;

2. Calculate the fo
ces and moments applied on the quad
oto
by the p
opelle
s
using equations (3.41)
and (3.43);
3. Compute the linea
and angula
accele
ations using equations (3.36) and (3.
14);
4. Compute the linea
and quate
nion angula
velocities th
ough equations (3.3
4) and (3.29);
5. Output the states vecto
de
ivative
_

_
.
Now, when we impor he uncion ino he Simulink

environmen we mus use an inegraor block o
produce linear and angular velociies rom he linear and angular acceleraions,
and we can also use he
same logic o obain he linear posiion and uaernion angle represenaion res
pecivel rom he linear
velociies and angular veloci uaernion. O course angles in he orm o a
uaernion are no ver
inelligible, so in order o ransorm a uaernion ino Euler angles we can us
e a ransormaion block
available in Simulink

or his ask (he name o he block is Qua2Ang). One should be ver careul n
o
o misake he angles order o roaion, in our case we are working wih he ord
er ZYX (or YawPichRoll)
and hereore i we wish o obain, as an example, he roll, we should ge i in
he hird oupu o his block.
A diagram illusraing he uadss.m uncion is presened in gure 3.8.1.
42 CHAPTER 3. QUADROTOR MODELLING
Figure 3.8.1: Block diagram o uadroor dnamics.
Turning our aenion o he sensors, he uadsens.m uncion was designed o si
mulae he sensor
readings. I reads he oupus (i.e. he sae vecor) rom he uadroor dnami

cs blocks and oupus he

sensor readings. Here is a descripion o he algorihm:
1. Use he provided sae vecor o he uadroor o assemble he roaion ma
rix S essenial or he
nex seps rom euaion (3.3);
2. Calculae he orces and momens applied on he uadroor b he propellers
using euaions (3.41)
and (3.43);
3. Compue he acceleraions el b he acceleromeer using euaion (3.56);
4. Use he read acceleraions o deermine he roll and pich angles using eu
aions (3.58) and (3.59);
5. Ge he compass reading rom he aw angle o he provided sae vecor usi
ng euaion (3.60);
6. Oupu he sensor readings
_
a
px
a
p
a
pz

_
.
Now ha we have he sensor readings i is imporan no o orge he applicai
on o he sensor charac
erisics, which can be easil done in he Simulink

environmen. This can be seen in gure 3.8.2 and
he Simulink

implemenaion in gure 3.8.3.
Figure 3.8.2: Block diagram o he sensors.
3.8. MODEL IMPLEMENTATION IN MATLAB

AND SIMULINK

43
Figure 3.8.3: Inside view o sensors Simulink

block.
44 CHAPTER 3. QUADROTOR MODELLING
Chaper 4
Kalman Filer
The Kalman ler [25] is a recursive ler
1
creaed in 1960 b engineer Rudol Emil Kalman ha esimaes
he saes o a linear ssem rom readings corruped b noise (e.g. sensor read
ings).
The word recursive in he ormer descripion means ha, disincl rom cerain
daa processing con
ceps, he Kalman ler does no need all previous daa o be kep in sorage and
reprocessed ever ime
a new measuremen is aken. This will be o vial imporance o he pracicali
 o ler implemenaion
(e.g. Arduino memor resricions).
Regardless o he pical connoaion o ler as a blackbox conaining elecrical
neworks, he ac is
ha in mos pracical applicaions, he Kalman ler is jus a compuer program i
n a cenral processor. As

such, i inherenl incorporaes discreeime measuremen samples raher han c

oninuous ime inpus.
Oen, he variables o ineres, some nie number o uaniies o
describe he sae o he ssem
canno be measured correcl, and some means o esimaing hese values rom he
available daa mus
be generaed (i.e. someimes no all o he saes are available). Thus deducio
n is problemaical b he
acs ha a ssem driven b inpus oher han our own known conro
ls and he relaionships among
he various sae variables and measured oupus are known onl wih
some degree o uncerain.
Furhermore, an measuremen will be degraded o some degree b noise, biases
, and some device
inaccuracies, and so a means o exracing valuable inormaion rom a nois sig
nal mus be provided
also. There ma as well be a number o dieren measuremen devices, each w
ih is own paricular
dnamics and error characerisics, ha suppl some inormaion abou a paricu
lar variable, and i would
be desirable o combine heir oupus in a mehodical and opimal manner. A Kalm
an ler combines an
available measuremen daa, plus prior knowledge abou he ssem and measuring
devices, o generae
an esimae o he desired variables in such a manner ha he error is minimize
d saisicall (e.g. i we
were o run a number o candidae lers man imes or he same applicaion, hen
he average resuls
o he Kalman ler would be beer han he average o an oher) [26].
In resume, he Kalman ler algorihm incorporaes all inormaion ha can be pro
vided o i, regard
less o he precision, o esimae he curren value o he variables o ineres
, wih use o:
Knowledge o he ssem ;
Measuremen device dnamics ;
Saisical descripion o he ssem noises;
1
Filer ha uses one or more o is oupus as inpus.
45
46 CHAPTER 4. KALMAN FILTER
Measuremen errors;
Uncerain in he dnamics models;
An available inormaion abou iniial condiions o he variables o ineres
.
Provided ha we can expec nois measuremens rom our sensors, in his chaper
we aim o design a
Kalman ler o esimae as man o he ssem saes as possible rom hose senso
r readings.
4.1 Saespace ssem linearizaion
The presen euaions in secion 3 ha describe he dnamics o he ssem are
no linear. As alread
menioned beore, in such a siuaion i is common o linearize he euaions ar
ound an operaing poin.
Linearizing he ssem will aciliae he consrucion o he Kalman ler and u
adroor conrol, as we
shall see laer.
Le
us ake as a saring poin
or he process o linearizaion
he ollowing sae vecor x, which
illusraes nohing less han he uadriroor in
igh, sabilized a a
heigh
z rom he ground, wha is

usuall known as an euilibrium poin:

x = [U V W P Q R X Y Z ] = [0 0 0 0 0 0 0 0 z 0 0 0] (4.1)
where he R, X, Y and can assume an random value because he do no inerere
in he linearizaion
process. Le us also consider he ollowing euilibrium inpu vecor u (i.e.
he vecor wih he speed o
roaion o each one o he moors):
u =
_

4
_
(4.2)
As an example, in the case of the quad
oto
u can be built by nding the angula
s
peed of each moto

so that the total th
ust p
oduced is equal to the fo
ce of g
avity. Implementing
the
st o
de
Taylo
se
ies
expansion, the linea
ization of the quad
oto
s model:
_
_
_
x = f (x, u)
y = h(x, u)
(4.3)
a
ound an ope
ating point ( x, u) is [27]:
_
_
_
x f ( x, u) +
f(x,u)
x
(x x) +
f(x,u)
u
(u u)
y h( x, u) +
h(x,u)
x
(x x) +
h(x,u)
u
(u u)
(4.4)
he
e f(x, u) a
e the set of equations p
oviding the dynamic behavio

of the system (see equations
(3.36), (3.14), (3.34) and (3.29)) and h(x, u) a
e the equations tha
t simulate the senso
outputs (see
equations (3.56), (3.58), (3.59) and (3.60)). Assuming that the p
e
sence of ve
y small distu
bances
a
ound the equilib
ium point ( x, u), (x x) and (u u) a
e both almost ze
o, e ca
n ve
ify:
_
_
_

 x = 0 f ( x, u) = 0
y = 0 h( x, u) = 0
(4.5)
4.2. SYSTEM ANALYSIS 47
The
efo
e:
_
_
_
x =
f(x,u)
x
(x x) +
f(x,u)
u
(u u)
y =
h(x,u)
x
(x x) +
h(x,u)
u
(u u)
(4.6)
ith:
A =
f (x, u)
x
(4.7)
B =
f (x, u)
u
(4.8)
C =
h(x, u)
x
(4.9)
D =
h(x, u)
u
(4.10)
Equation (4.6) is the gene
ic state space
ep
esentation:
_
_
_

x = A x +B u
y = C x +D u
(4.11)
ith x = x x and y = y y.
4.2 System analysis
If e ish the Kalman lte
to be conve
gent, e must analyze some p
ope
ties of d
isc
ete state space
systems such as
eachability, cont
ollability and obse
vability. By conve
gence
e mean the Kalman lte

estimated states e

o
t
ies to go to ze
o hile unde
the in uence of noise. Th
at noise cannot push a
state ve
y fa
and e can expect the va
iance of that state to
emain bounded, a
nd this is p
ecisely hat
happens hen the system is stable. If the system is not stable, then the va
ian
ce of a state ill explode

hen the system gets affected by noise. Hence, if e ish the Kalman lte
to be
conve
gent, e must
analyze some p
ope
ties of disc
ete state space systems such as
eachability, co
nt
ollability and obse
v
ability [28].
By de nition,
eachability tells us if it is possible to nd a cont
ol
sequence such that an a
bit
a
y
state can be
eached f
om any initial state in nite time. The
e a
e seve
al met
hods fo
dete
mining if a
system is
eachable, depending on hethe
the system mat
ix A is o
isnt singula

(i.e. if A isnt o
is
inve
tible). This p
ope
ty can be investigated by checking the dete
minant of ma
t
ix A, hich in this case
equals ze
o, meaning this mat
ix is nonsingula
. In this case e can immediate
ly say the system is not

eachable.
Cont
ollability aims to tell us if it is possible to nd a cont
ol sequence such t
hat the o
igin (i.e. hen
a state is equal to ze
o) can be
eached in nite time. Fo
systems in hich mat

ix A is nonsingula
, e
can say the system is cont
ollable if the
e is a value fo
i such that A
i
equals ze
o. In this case this is
48 CHAPTER 4. KALMAN FILTER
t
ue fo
i = 4, the
efo
e ou
system is fully cont
ollable.
Obse
vability
efe
s to the possibility of a given state being dete
mined in nite
time using only the
system outputs. This p
ope
ty can be ve
i ed if the
ank of the obse
vability mat

ix O:
O =
_

_
C
CA
CA
2
.
.
.
CA
N1
_

_
(4.12)
is equal to the systems o
de
. The equivalent MATLAB


command is:

ank((obsv(A,C));
The
esult is 6 meaning e have 6 unobse
vable states (f
om the total 12) in the
system.
4.3 Mathematical fo
mulation
To const
uct a Kalman lte
e must meet 3 conditions:
1. The model of the system must be linea
. Ve
y
egula
ly, even in the p
esenc
e of nonlinea
ities, the
common app
oach is to engage in system linea
ization a
ound an ope
ating point,
given that it is
easie
to o
k ith linea
systems;
2. The noise is hite, hich means it has equal poe
ove
all f
equencies.
This also simpli es the
math involved in the lte
, fo
hite noise is ve
y simila
to the actual noise af
fecting the system,
ithin limited
ange of f
equencies;
3. The noise has Gaussian density. Typically the
e a
e to statistical p
ope
t
ies that a
e easily asce

tainable and that cha
acte
ize a noise signal, the mean and va
iance, to assume
that the noise is
Gaussian and simplify the p
ocess of lte
ing f
om the mathematical point of vie.
Sta
ting ith the linea
ized continuoustime state space fo
m of the system:
_
_
_
x = Ax +Bu +G
y = Cx +Du +H+v
(4.13)
he
e the stochastic va
iables v and 
espectively
ep
esent the noise affectin
g the senso
measu
e
ments and the ai
c
aft, cha
acte
ized by its
espective noise cova
iances R
k
and Q
k
:
Q
k
= E
_

T
_
(4.14)
R
k
= E
_
vv
T
_
(4.15)
4.4. KALMAN FILTER IN MATLAB

AND SIMULINK

49
The p
ocess noise cova
iances Q
k
and measu
ement cova
iances R
k

might change ith each time step

o
measu
ement, hoeve
, he
e e assume they a
e constant. The optimal solution
fo
the Kalman lte

is then p
ovided by:
_

_
x(n) = A
k
x(n 1) +B
k
_
_
u(n)
y(n)
_
_
y(n) = C
k
x(n 1) +D
k
_
_
u(n)
y(n)
_
_
(4.16)
A
k
= I LC (4.17)
B
k
=
_
LD L
_
(4.18)
C
k
= C(I LC) (4.19)
D
k
=
_
(I CL)D CL
_
(4.20)

he
e the Kalman gain L updates the estimate x(n 1) using the ne measu
ement y(n
):
x(n) = x(n 1) +L(y(n) C x(n 1) Du(n)) (4.21)
Notice that this last equation is the same equation used fo
states estimation i
n (4.16), only
e
itten in
diffe
ent fo
m. The main ta
get of the Kalman estimato
design is to minimize th
e e

o
s:
e
x
= x x = C(x x) +v (4.22)
e
y
= y y = (ALC) (x x) +GLv (4.23)
This is accomplished by nding the lte
gain L that p
ovides the stateestimate x
that minimizes the
steadystate e

o
cova
iance P:
P = E
_
(x x) (x x)
T
_
(4.24)
The
efo
e, the gain L can be calculated by solving an algeb
aic Riccati simpli ed
equation [29]:
L = PC
T
_
CPC
T
+R
_
1
(4.25)
he
e R is the measu
ement noise cova
iance mat
ix.
4.4 Kalman lte
in MATLAB

and Simulink

To p
oduce a Kalman lte
in MATLAB

e need the state space mat
ices as ell as the cova
iance
mat
ices of the p
ocess and senso
noises. The nume
ical calculus of
the state space mat
ices as
50 CHAPTER 4. KALMAN FILTER
achieved by designing a MATLAB

function named quadss.m. This function o
ks ith a ve
y basic
algo
ithm:
1. Use the quadsys.m function to get mat
ices A and B;
2. Use the quadsens.m function to calculate mat
ices C and D.
Afte
executing this code e get the folloing mat
ices:
A =
_

_
0 0
0 0
0 0
0 0
0 0
0 0
1 0
0 1
0 0
0 0
0 0
0 0
_

_
(4.26)
B =
_

0
0
0
0
0
0
0
0
1
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
1
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
0
0
0
0
0
0
0
0
0
0
0

0
0
0
0
0
0
0
0
0
0
0
0

0 9.81 0
9.81 0 0
0.0005 0.0005
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0

_
0 0 0 0
0 0 0 0
0.0132 0.0132 0.0132 0.0132
0 0.3881 0 0.3881
0.3881 0 0.3881 0
0.0174 0.0174 0.0174 0.0174
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
_


_
(4.27)
C =
_

_
0 0 0 0 0 0 0 0 0 0 19.62 0
0 0 0 0 0 0 0 0 0 19.62 0 0
0 0 0 0 0 0 0 0 0 0.001 0.001
0 0 0 0 0 0 0 0 0 2 0 0
0 0 0 0 0 0 0 0 0 0 2 0
0 0 0 0 0 0 0 0 0 0 0 1
_

_
(4.28)
4.4. KALMAN FILTER IN MATLAB

AND SIMULINK

51
D =
_

_
0 0.019 0 0.019
0.0001 0.0191 0.0001 0.019
0.0144 0.0132 0.0121 0.0132
0 0.0019 0 0.0019
0 0.0019 0 0.0019
0 0 0 0
_

_
(4.29)
Mat
ix D shos that the Eule
angles pitch and
oll
inputs, a consequence of the ac
cele
omete
not being positioned at the COG of the quad
oto
.
liminate all unobse
v
able o
uncont
ollable states. The MATLAB

command fo
this ope
ation is:
sysm=min
eak(ss(A,B,C,D));
The
esultant state space
ealization of the system is:
A =
_

_
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
_

_
(4.30)
B =
_

_
0 0.3881 0 0.3881

depend on the
No e have to e

0.3881 0 0.3881 0
0.0174 0.0174 0.0174 0.0174
0 0 0 0
0 0 0 0
0 0 0 0
_

_
(4.31)
C =
_

_
0 0 0 0 19.62 0
0 0 0 19.62 0 0
0 0 0 0.001 0.001 0
0 0 0 2 0 0
0 0 0 0 2 0
0 0 0 0 0 1
_

_
(4.32)
he
e mat
ix D suffe
s no changes f
om equation (4.29). This
ealization also h
as no
eachable states,
but is fully cont
ollable and obse
vable. Afte
close obse
vation e can also se
e that the eliminated states
a
e the linea
velocities and linea
positions, leaving us ith the attitude of
the quad
oto
and angula

velocities. The linea
positions a
e no su
p
ise due to the absence of position
senso
s. About the linea

velocities, ould be possible to ext
act them by integ
ation of the accele
ation
s? Well, this is something
e can not expect to do ith cu

ent lte
ing techniques because by integ
ating th
e accele
ations e a
e
also doing the same to the noise associated ith it, thus the linea
velocities

d
ift.
The state space fo
m of this system can be implemented in MATLAB

using the command:
52 CHAPTER 4. KALMAN FILTER
sys = ss(A,[B G],C,[D H]);
Whe
e the mat
ices G and H a
e de ned as
G = eye(12,6);
H = ze
os(6,6);
Mat
ix Gbasically says that e can make measu
ements on only 6 states (in a pe
f
ect situation he
e all
12 states measu
ed, G ould be an identity mat
ix of size [12 12]). Mat
ix H is d
e ned as null because
e a
e assuming the p
ocess noise has no in uence in the system outputs (see equat
ion (4.13)). As
fo
the cova
iance mat
ices,
e assume the noise cova
iances of both
the system inputs and senso


eadings a
e the same (because e do not kno its
eal values):
Q = 10
4
I(6) (4.33)
R = 10
4
I(6) (4.34)
Notice that R is a [6 6] mat
ix, because e can only measu
e 6 outputs (3 accele

ations and the
Eule
angles ya, pitch and
oll). Also, the p
ocess noise cova
iance mat
ix Q
has size [6 6] because
e a
e o
king only ith the 6 cont
ollable and obse
vable states of the system.
When designing a Kalman lte
, e can also choose f
omto possible types of estima
to
s: continuous
time o
disc
etetime. As e plan to use the Kalman estimato
on the A
duino (a
digital system), e must
choose the disc
etetime option, meaning e have to use anothe
ve
y impo
tant p
a
amete
, the sample
time t
s
. In MATLAB

the disc
ete Kalman lte
is obtained ith:
Kest = kalmd(sys,Q,R,t
s
);
he
e Kest p
ovides contains the lte
mat
ices A
k
, B
k
C
k
and D
k
.
T
ying to p
oduce a disc
ete Kalman lte
ith the cu

ent state space mat
ices an
d cova
iance ma
t
ices
esults in the folloing state space
ealization of the lte
:
A
k
=
_

_
1 0 0 0.5985 0 0
0 1 0 0 0.5985 0
0 0 1 0 0 0.0479
0.05 0 0 0.3384 0 0
0 0.05 0 0 0.3384 0
0 0 0.05 0 0 0.9147
_

_
(4.35)
4.4. KALMAN FILTER IN MATLAB

AND SIMULINK

53
B
k
=
_

_
0 0.0188 0 0.0188 0 0.0302 0 0.0031
0.0194 0.0006 0.0194 0.0006 0.0302 0
0.0009 0.0009 0.0009 0.0009 0 0 0 0
0 0.0002 0 0.0002 0 0.0334 0 0.0034
0.0005 0.0006 0.0005 0.0006 0.0334 0
0 0 0 0 0 0 0 0 0 0.0853
_

0.0031
0.0479

0
0
0

0
0

0.0034

_
(4.36)
C
k
=
_

_
0 0
0 0
0 0
0 0
0 0
0 0
1 0
0 1
0 0
0 0
0 0
0 0
_

0
0
0
0
0
0
0
0
1
0
0
0

0 7.2272 0
7.2272 0 0
0.0004 0.0004
0.7367 0 0
0 0.7367 0
0 0 0.917
0.5985 0 0
0 0.5985 0
0 0 0.0479
0.3684 0 0
0 0.3684 0
0 0 0.917

_
(4.37)
D
k
=
_

_
0 0.007 0 0.007 0.6251 0 0
0 0.007 0 0.007 0 0.6251 0
0.0144 0.0132 0.0121 0.0132
0 0.0007 0 0.0007 0 0.0637
0 0.0007 0 0.0007 0.0637 0
0 0 0 0 0 0 0 0 0 0
0 0.0006 0 0.0006 0 0.0302
0 0.0006 0 0.0006 0.0302 0
0 0 0 0 0 0 0 0 0 0
0 0.0006 0 0.0006 0 0.0319
0 0.0006 0 0.0006 0.0319 0
0 0 0 0 0 0 0 0 0 0
_

0 0 0.0637
0.0637 0.0637 0
0 0 0 0 0 0
0 0.0065 0.0065 0
0 0 0 0.0065
0
0
0
0

0.0031
0 0

0.0031
0.0031

0.0032
0 0

0.0032
0.0032

_
(4.38)
No that e have ou
state space Kalman lte
e can implement it in Simulink

using the block dispo
sition in gu
e 4.4.1, th
ough a disc
ete state space block.
54 CHAPTER 4. KALMAN FILTER
Figu
e 4.4.1: Kalman lte
schematic.
4.5 Kalman lte
fo
A
duino
Implementation of a state space Kalman lte
in the A
duino is quite easy, mostly
because e only need
mat
ices A
k
and B
k
to estimate the states
_
P Q R
_
. The euaion we are going o use
or his purpose is:
x(k) = A
k
x(k 1) +B
k
u(k) (4.39)
where we esimae he saes a ime insan
k are calculaed b us
ing he saes a he ime insan
k 1 and inpus a he curren ime insan
k. Consanl ring ou dieren
 sae space marices in
he Arduino can be a dilaor work. To ease his ask a MATLAB

uncion was developed o conver
an marix variable o Arduino code read or cop/pase operaions. We also n
eed o remember he
necessi o providing he iniial saes x(0) or he lers sarup calculaion.
In our case we are going o
assume he uadroor is leveled and heading norh, which makes us assume he ini
ial sae vecor as:
x(0) =
_
0 0 0 0 0 0

_
(4.40)
The esimaed saes will be used o eed a LQR conroller, which will be he ma
in subjec o he nex
chaper.
Chaper 5
Opimal Conrol  The
LinearQuadraic Regulaor
In opimal conrol one endeavors on nding a conroller ha provides he bes pos
sible perormance wih
respec o some given measure o perormance (e.g. he conroller ha uses he
leas amoun o conrol
signal energ o ake he oupu o zero). In his case he measure o perorm
ance (also called opimal
crierion) would be he conrolsignal energ. In his secion we r o implem
en opimal conrol hrough
he use o a LQR conroller (LinearQuadraic Regulaor) in which he conrol si
gnal energ is measured
b a cos uncion conaining weighing acors provided b he conroller desig
ner.
Moreover, he LQR conroller is applicable o MIMO (MulipleInpu MulipleOup
u) ssems (e.g. in
he uadroor we have he speeds o 4 moors as inpus, and he sensor readings
as oupus) or which
classical designs are di cul o appl (see reerence [30]).
5.1 Mahemaical ormulaion
For a coninuousime linear ssem described b:
x = Ax +Bu (5.1)
The cos uncion J
LQR
is [29]:
J
LQR
=

0
_
x
T

Qx +u
T

Ru
_
d
(5.2)
where

Q (size n n, where n corresponds o he number o saes o conrol) and

R (size m m, wih
m he number o process inpus) are boh smmeric posiivede nie weighing mar
ices. Conseuenl,
he euivalen discreeime eedback conrol law (noe ha he conroller is
o be implemened in he
Arduino, which b i sel is a digial plaorm, hence i is no a coninuousi
me ssem) ha minimizes he
cos value is:
u

K
= Kx (5.3)
where u
K
is he vecor o conrol acions. The LQR gain marix K is provided b:
55
56 CHAPTER 5. OPTIMAL CONTROL  THE LINEARQUADRATIC REGULATOR
K =

R
1
B
T

P (5.4)
and

P is derived b means o he algebraic Riccai euaion:

A
T

P+

PA

PB

R
1
B
T

P+

Q = 0 (5.5)
The LQR design echniue presened so ar assumes he order o he conroller wi
ll be eual o he order
o he ssem. This ma no be our case. In ac, because, as we have seen beor
e, we have onl 6 saes
available o conrol rom he Kalman ler (angular velociies and Euler angles),
he order o our conroller
will be lower han he order o he ssem. This suggess ha we have wo possi
ble case sudies or he
LQR conroller: 12 saes conrol and 6 saes conrol. The rs one assumes w
e have no Kalman ler
and have ull access o all saes, as or he second we will be ineresed in 
esing he perormance o
he conroller using he Kalman ler. This implies respecivel eiher we use a
[12 12] marix A and
[12 6] marix B(or 12 sae conrol) as opposie o he lower size Aand Bmarice
s when using Kalman
lering (see secion 4.4). As or marix

Q we will have o assume he same logic respecivel or 12 or 6

saes conrol, where i will have dieren size accordingl o each case.
A rs choice or marices

Q and

R is provided b he Brsons rule which saes hese marices as

being diagonal:

Q
k
=
1
x
2
i,max
(5.6)

R
k
=
1
u
2
i,max
(5.7)
where x
i,max
is he highes olerable value or he sae x
i
, and u
i,max
is he highes olerable value or
he inpu u
i
. The core objecive o Brsons rule is hereore o scale he variables in
J
LQR
so ha
he highes olerable value or each erm is one. Paricularl, his is ver
mporan when x and u are
numericall ver dieren rom each oher [31].
5.2 LQR conrol in MATLAB

and Simulink

A discree LQR conroller can be produced in MATLAB

using he command:
Klr=lrd(A,B,

Q,

R,
s
);
which will reurn he gain marix K, according o he sample ime 
s
. Tables 5.1 and 5.2 presen he
values chosen o building he weigh marices

Q and

R. The values presened in hese ables are no

ver eas o choose. In ac, he were sough ieraivel, showing ha he
mplemenaion o Brsons
rule is no ver inuiive, and does no guaranee a working conroller on he

he

i
rs

 r.
Table 5.1: Maximum sae values or designing marix

Q.
Saes U
max
V
max
W
max
P
max
Q
max
R
max
X
max
Y
max
Z
max

max

max

max
12 0.1 0.1 0.1 0.001 0.001 0.001 0.4 0.4 0.4 0.1 0.1 0.1
1 1 1 
0.1 0.1 0.1
6
  
 
5.2. LQR CONTROL IN MATLAB

AND SIMULINK

57
Table 5.2: Maximum inpu values or designing marix

R.

1 max

2 max

3 max

4 max
12 states cont
ol 3 3 3 3
6 states cont
ol 3 3 3 3
No all e have to do is to build mat
ices

Q and

R by app
op
iately using equations (5.6) and (5.7).

The folloing gain mat
ices a
e obtained fo
each cont
ol scena
io of table 5.1
by using a sample time of
0.05s (20Hz):
K
12 states
=
_

_
0.6649 0.0001 18.7694 0.0001 32.9579 338.8566
0.0001 0.6652 18.7694 32.9577 0.0001 338.8566
0.6652 0.0001 18.7694 0.0001 32.9577 338.8566
0.0001 0.6649 18.7694 32.9579 0.0001 338.8566
0.0803 0 3.6567 0.0057 20.6969 3.3871
0 0.0804 3.6567 20.6855 0.0057 3.3871
0.0804 0 3.6567 0.0057 20.6855 3.3871
0 0.0803 3.6567 20.6969 0.0057 3.3871
_

_
(5.8)
K
6 states
=
_

_
0 7.103 20.4163 0 18.2839 14.4659
7.103 0 20.4163 18.2839 0 14.4659
0 7.103 20.4163 0 18.2839 14.4659
7.103 0 20.4163 18.2839 0 14.4659
_

_
(5.9)
Fo
simulation pu
poses, a LQR cont
olle
can be implemented in Simulink

ith a simple gain block
as schematically po
t
ayed in
gu
e 5.2.1. When a LQR cont
olle
is co
mbined ith a Kalman
lte
,
they fo
m a LQG (Linea
Quad
aticGaussian) cont
olle
(see Figu
e 5.2.2). Bot
h elements of an LQG
cont
olle
can be designed and computed independently, as in this case.
Figu
e 5.2.1: LQR cont
ol.
58 CHAPTER 5. OPTIMAL CONTROL  THE LINEARQUADRATIC REGULATOR
Figu
e 5.2.2: LQG cont
ol block diag
am in Simulink

.
5.3 LQR cont
ol in the A
duino
The cont
olle
implementation in the A
duino is quite easy. We only have to de n
e the gain mat
ix K in
the A
duino p
og
am envi
onment and multiply it by the estimated states p
oduced
by the Kalman lte

as in equation (4.39). We have to be ca
eful as the cont
ol actions of the LQR c
ont
olle
a
e the angula

speeds of the moto
s. This value has to be conve
ted back into a PWM signal. We
can achieve this goal
by using data in equation (3.38) to conve
t angula
speeds to pulse idths and c
ompensating ith the

espective moto
s dead zone:
P =

K
+P
0
(5.10)
he
e P
0
is the pulse idth of a moto
s dead zone, K the gain of its
st o
de
t
ansfe

function, the
angula
speed (f
om the LQR cont
olle
) and P is the consequent pulse idth to d

ive the moto
.
Chapte
6
Implementation and
esults
This chapte
shos the evolution of ou
attempts to cont
ol the quad
oto
system
. The add
essed case
studies a
e:
12 states LQR cont
ol. Sta
ting f
om the ideal case, e ill study the situa
tion he
e a LQR con
t
olle
handles all 12 states of the system. This ill sho us the best scena

io possible ith this
kind of cont
olle
.
LQG cont
ol ith senso
s. Although e ould like to have all 12 states of th
e quad
oto
system
at ou
disposal, this is not alays possible. In this case, a Kalman lte
is us
ed to estimate th
ee
angula
velocities and th
ee Eule
angles f
om the senso
s outputs. These six st
ates a
e afte
a
ds
fed to a LQR cont
olle
.
LQG cont
ol ith senso
s and moto
s dynamics. This case is the continuation of
the p
evious one,
but this time the cont
olle

obustness is put to the test hen the simulation i
ncludes the dynamics
of the moto
p
opelle
assemblies.
P
actical implementation. Taking ou
cont
olle
and Kalman lte
into ou
quad
o
to
p
ototype is the
next logical step, he
e e can test ho the LQG cont
olle
pe
fo
ms in the
eal
o
ld.
Using gy
oscopes to imp
ove state estimation. Sometimes the intensity of sen
so
noise is unde

estimated in simulation envi
onments. In this case study, the use of gy
oscopes
(togethe
ith the
t
iaxis accele
omete
and compass) is evaluated as a means of p
oviding ext
a i
nfo
mation to the
Kalman lte
and obse
ve hat is its effect on the quality of the state estimates
(hich accu
acy is
c
ucial fo
system cont
ol).
59
60 CHAPTER 6. IMPLEMENTATION AND RESULTS
6.1 Full 12 states cont
ol
One of the most impo
tant lessons to be d
an f
om the p
ocess of simulating any
cont
ol loop is that it
is unise to sta
t immediately ith the most complex situation he
e e have the
system as complete as
possible. This case is often mo
e dif cult to cont
ol, hich in case of the eme
ge

nce of some unexpected

esults (e.g. the appa
ent inability to stabilize a given system in the simula
tion envi
onment) can make
the detection of any implementation e

o
s ve
y ha
d to detect. The
e is nothin
g bette
than to sta
t ith
a simple system and g
adually p
og
ess to its most complete (and complex) fo
m.
Folloing this philosophy, e shall sta
t ith by analyzing hethe
o
not it is
possible to cont
ol ou

system using only the dynamics of quad
oto
togethe
ith the LQR cont
olle
.
This case is conside
ed
to be ideal because e a
e assuming that all 12 states of the system a
e availab
le fo
cont
ol pu
poses,
hich does not alays co

espond to a
eal situation. The cont
ol loop in Si
mulink used fo
this task
is p
esented in Figu
e 6.1.1, he
e e can see that the LQR cont
olle
(in o
an
ge)
eceives the e

o

beteen the actual states and the desi
ed ones, and p
oduces fou
angula
speeds
(one fo
each moto
)
that ill make the quad
oto
follo a given
efe
ence.
Figu
e 6.1.1: 12 states LQR cont
ol loop.
Knoing that this cont
ol loop uses the cont
olle
in equation (6.1.1), e also
have to specify a
ef
e
ence fo
ou
system in o
de
to analyze the pe
fo
mance of this cont
olle
.
Taking into account that
e ant to take the ai
c
aft f
om a situation he
e it is landed on the g
ound,
ith the initial state vecto

equal to x = [0 0 0 0 0 0 0 0 0 0 0 0] to a
efe
ence x = [0 0 0 0 0 0 1 1 1 0 0 0
] (i.e. one mete
up, one
mete
West and one mete
No
th). Figu
e 6.1.2 illust
ates the
esults f
om the
cont
olle
pe
fo
mance
using these
efe
ences.
6.1. FULL 12 STATES CONTROL 61
Figu
e 6.1.2: Time
esponse of the LQR cont
ol loop using ith 12 states feedbac
k.
We can see that ith this cont
olle
, the systemcan follo the
efe
ence ithout
any p
oblems, sta
ting
ith TakeOff and then moving No
theast hile alays keeping the No
th heading.
The time
esponse
in te
ms of the pitch and
oll angles is the same. Figu
es 6.1.3 and 6.1.4 sho
 fu
the
analysis on this
cont
olle
, alloing us to see hat happens hen e tune the

Q and

R mat
ices.
Figu
e 6.1.3 illust
ates that inc
easing the value of the eighting mat
ix

Q also augments the system

speed of
esponse, hile also making it a little bit unde
damped. On the othe

hand, Figu
e 6.1.4
e ects
that an inc
ease in mat
ix

R leads to a dec
ease in the cont
ol action of the moto
s, as it also appea
s to

have a signi cant in uence only in the position of the quad
oto
along u
Z
.
62 CHAPTER 6. IMPLEMENTATION AND RESULTS
Figu
e 6.1.3: Linea
positions ith inc
easing eight mat
ix

Q.

(eight mat
ices

Q
1
>

Q
2
>

Q
3
: 
oll

Q
1
;  
oll

Q
2
; .
oll

Q
3
;  pitch

Q
1
;   pitch

Q
2
; . pitch

Q
3
;  ya

Q
1
;   ya

Q
2
; . ya

Q
3
)
Figu
e 6.1.4: Linea
positions ith inc
easing eight mat
ix

R.
(eight mat
ices

R
1
>

R
2

>

R
3
: 
oll

R
1
,

R
2
and

R
3
;  pitch

R
1
,

R
2
and

R
3
;  ya

R
2
;   ya

R
3
; . ya

R
1
)
6.2 6 states cont
ol
6.2.1 LQG cont
ol ith senso
s
Afte
ou
analysis on the design of the Kalman estimato
fo
ou
system (see Cha
pte
4) e concluded
that e can only o
k ith 6 of the 12 states available f
om ou
system, the ang
ula
velocities and and
the Eule
angles. It is the
efo
e app
op
iate to investigate the behavio
of ou

system in the p
esence of
the LQR cont
olle
in equation (5.9), hich is designed to cont
ol only these 6
states.
The 6 states cont
ol loop is displayed in Figu
es 6.2.1 (Simulink

) and 6.2.2, he
e ou
state vecto

is x =
_
P Q R
_
and our reerence is x =
_

0
_

/4

(i. .  ant to k
th uadrotor l v l d and rotat d at an angl = 0.7854 radians, or 45

).
6.2. 6 STATES CONTROL 63
Figu
e 6.2.1: 6 states Simulink

LQR cont
ol loop.
Figu
e 6.2.2: Time
esponse of the LQR cont
ol loop using 6 states.
64 CHAPTER 6. IMPLEMENTATION AND RESULTS
The Figu
e above shos that the system follos the
efe
ence
easonably fast (le
ss than 10 seconds).
At the same time e can see that both the linea
velocities and linea
positions
a
e not unde
the in uence
of the cont
olle
, since they a
e not cont
ollable states. In a
eal case,
if e ould like to cont
ol the
altitude,
e ould have to do it ou
selves only by cont
olling the poe
p
ovi
ded to the moto
s. This
ould not be necessa
y if, fo
example, e had an altitude senso
.
Figu
e 6.2.3 illust
ates Simulink block diag
am of the next stage of simulations
, he
e e include the
senso
s (ith noise) in the cont
ol loop to see ho the LQR cont
ol pe
fo
ms (se
e Figu
e 6.2.4). The
contents of the LQG block of Figu
e 6.2.3 a
e also p
esented in Figu
e 5.2.2.
We can see (in Figu
e 6.2.4, he
e the Eule
angles a
e no g
aphically
ep
esen
ted in deg
ees to
imp
ove
eadability) that the LQR cont
olle
continues to
ende
the system stab
le, ithout any need of
adjustments to the Q and R mat
ices (note that the pitch and ya angles a
e ove

lapped).
Figu
e 6.2.3: 6 states Simulink

LQR cont
ol loop ith senso
s.
6.2. 6 STATES CONTROL 65
Figu
e 6.2.4: Time
esponse of the LQR cont
ol loop using 6 states and senso
s.
6.2.2 LQG cont
ol ith senso
s and moto
s dynamics
In this section e ill obse
ve hat is the effect p
oduced by the int
oduction
of the moto
s dynamics in
the cont
ol loop (see Figu
e 6.2.5), and check if the LQR cont
olle
continues t
o maintain the quad
oto

leveled. The
efe
ence used in this case continues to be x =
_
0 0 0 0 0 /4
_
and th simulation
r sults ar availabl in Figur 6.2.6.
66 CHAPTER 6. IMPLEMENTATION AND RESULTS
Figur 6.2.5: 6 stat s Simulink

LQR control loo ith s nsors and motors dynamics.

Figur 6.2.6: Tim r s ons of th LQR control loo using 6 stat s, s nsors and
motors dynamics.
6.3. PRACTICAL IMPLEMENTATION 67
W continu to v rify that th syst m r mains stabl , noting that th tim r s o
ns is v ry similar
to th on h r th motors dynamics had not y t b n includ d (s Figur 6.2.4
). This ff ct can
only b x lain d by th introduction of th motors dynamics, hich d s it not

b ing consid r d in th
controll r d sign, continu s to b dominat d by it.
6.3 Practical im l m ntation
No that  hav rov d that it is ossibl to stabiliz th uadrotor in a simu
lat d nvironm nt ith th
curr nt combination of s nsors,  ill try to validat our r sults through th
im l m ntation of both th
LQR controll r and Kalman lt r in our uadrotor rototy (s Figur 6.3.1), and
s if  can hav
control ov r its attitud .
Figur 6.3.1: Quadrotor rototy .
6.3.1 T l m try and joystick control softar
In this has of our ork  can x ct to b n c ssary th d v lo m nt of a sof
tar modul hich
can coll ct information from th Arduino and, if n c ssary, s nd control ord rs
to it. With this obj ctiv in
mind, a small softar modul as built to assist us. This a lication is d sign
at d by Quadjoy and as
d sign d to coll ct not only ASCII t xt m ssag s s nt by th Arduino, as it can
also s nd commands
from a gam controll r that has 2 joysticks, hich tog th r hav a suf ci nt numb
r axis to control th
Eul r angl s and o r su li d to th motors (i. .  n d four ax s, thr for
th Eul r angl s and 1
for motors o r). Th a lication int rfac is visibl in Figur 6.3.2 and th
control sch m in Figur
6.3.3.
68 CHAPTER 6. IMPLEMENTATION AND RESULTS
Figur 6.3.2: Quadjoy us r int rfac .
Figur 6.3.3: Quadrotor control diagram.
Th r s nt d int rfac has b n lann d to b as intuitiv as ossibl , also al
loing us to acuir
data to a l that can b load d in Matlab

. As an xam l for a s ssion of data acuisition,  initiat

communications ith th Arduino by rst choosing th COM ort that is conn ct d to
ir l ss ant nna,
and th n s l cting th ch ckbox that indicat s h th r  ant to rit th coll
ct d data to a l . Finally,  hav to r ss th button O n to o n th COM ort and start giving ord
rs to th uadrotor
using our joystick. Aft r 
nish d our ying s ssion 
clos th COM
ort and nd out that th
rogram dir ctory ill no hav a data l nam d data.txt. To load th data con
tain d in this l in
Matlab

,  sim ly us th command:
6.3. PRACTICAL IMPLEMENTATION 69
load data.txt
6.3.2 Kalman lt r and LQR controll r
Th rst has of rototy t sts consists in v rifying th b havior of th Kalman
lt r. Th cod that as
d v lo d to im l m nt th Kalman lt r and LQR controll r in th Arduino can b f
ound in A ndix C.
This t st consists in grabbing th uadrotor ith both hands and tilting it arou
nd th roll, itch and ya
axis to nsur that it is roviding an acc tabl stimation of th 6 stat s an
t d. This x ri nc  nt
fairly  ll, shoing that th Kalman lt r can stimat th d sir d 6 stat . As

an xam l , Figur 6.3.4

illustrat s on of thos situations h r  can s th stimation of th ya a
ngl h n th uadrotor
is rotat d 45 d gr s from North to W st.
Figur 6.3.4: Kalman lt r stimation of th ya angl in th uadrotor rototy .
(- Ya angl ; - Ya angl r f r nc )
Knoing that th Kalman lt r s ms to b functional,  roc d to th stability
t st of our uadrotor
using th LQR controll r. Th
int nd d r f r nc to follo is x = [
0 0 0 0 0 0 ]. Thus this
r f r nc as rogramm d in th Arduino and, for saf ty r asons, th uadrotor 
as lac d at a saf
distanc
from any hysical obstacl . Th n, ith th h l of th
j
oystick, o r as rovid d to th
motors so that th
aircraft as abl
to gain altitud (not
that
th cod im l m nt d in th
Arduino
alr ady includ s a s curity syst m that sto s th motors in th abs nc of a sig
nal from th joystick).
It as obs rv d that th uadrotor, just b for r aching Tak -Off, b gan to slo
ly s in around th u
z
axis, sugg sting that th control of th ya angl as not functioning as x ct
d. Giving mor o r
to th motors r sult d in an uncontroll d ight. It is th r for r f rabl to in
v stigat hat is th caus
of this instability. W start d by v rifying th ord rs that th controll r as
giving to th motors. For this
ur os , th uadrotor as lift d into th air, and ithout giving o r to th
motors,  tri d to ch ck if
th control ord rs corr s ond d to thos shon in Figur 1.2.2 of s ction 1.2. B
as d on this Figur , if for
70 CHAPTER 6. IMPLEMENTATION AND RESULTS
xam l , in a situation h r th uadrotor is l v l d but ith = /4 rad, It is th
r for x ct d that
motors to and four hav high r angular s d than motors on and thr in ord r
for th uadrotor to
r ach our r f r nc . This situation as ind d v ri d, ho v r it as obs rv d th
at th chang s in th
angular s d of th motors ( roduc d by th controll r) did not xc d to radi
ans r s cond, hich
l ads us to b li v that a th r is an im ortant variabl that as not tak n int
o account: th r solution of
th PWM signal. This m ans that, for xam l , if th motors ar not s nsitiv
to variations of 2 radians
r s cond and th controll r only issu s control ord rs hich do not vary by mo
r than 1 radian r
s cond (hich is lo r than th r solution of th PWM signal) th n control ov r
th uadrotor might b
v ry dif cult. W n d to ch ck if th controll r is aggr ssiv nough to d al 
ith this n  as ct, and
if n c ssary, mak adjustm nts to its  ighting matric s. Aft r consulting th
r solution of th s rvo
signal of th Arduino, it as conclud d that it is only s nsitiv to chang s of
4 micros conds. This n 
f atur as introduc d in th Simulink

block of th motors and, according to th r sults obs rv d in

Figur 6.3.5,  can still s that th controll r can k th syst m stabl , a
lthough th r ar no id
uctuations in th itch and ya angl s.

Figur 6.3.5: Tim r s ons of th LQR control loo using 6 stat s, s nsors and
motors dynamics ith
PWM r solution.
This n  data m ans  n d to mak th folloing u stion: If by mani ulation
of th uadrotor
6.3. PRACTICAL IMPLEMENTATION 71
attitud ith th motors turn d off  hav control ord rs and stimat d stat s
that a ar to b fram d
ithin valu s that s m a ro riat to th stability sc nario  ish to hav ,
th n hat is ha ning
h n th motors ar orking that l ads to instability? Th Arduino cod as th r
for chang d to s nd
us data dir ctly from th s nsors in a situation h r th motors  r orking.
Figur 6.3.6 shos th
acc l rom t r data coll ct d, h r  can s th nois a aring 4 s conds fro
m th start of data
collation, mom nt at hich th motors start d to ork, roducing vibrations acro
ss th structur of th
uadrotor.
Figur 6.3.6: S nsor nois data.
(- x-axis acc l ration; - y-axis acc l ration; - z-axis acc l ration; com ass ya
 angl )
At this mom nt  can only say our bigg st robl m is nois (as shon by th Fig
ur abov ). This
is articularly s rious for th acc l rom t r, hos data is us d to stimat th
itch and ya angl s.
To try to r duc th nois coming from th s nsors, a n  Kalman lt r as d sign
d ith a covarianc
matrix R calculat d from th coll ct d nois data. Cons u ntly, th nois as
also introduc d in th
simulation block of s nsors in ord r to b tt r ortray r ality. Th stimation
rror rov d to b so high
that it as im ossibl to nd a functional LQR controll r for th syst m. Ev n th
addition of a lo ass
72 CHAPTER 6. IMPLEMENTATION AND RESULTS
lt r aft r th s nsors, such as a Butt rorth lt r, r nd r d th syst m im ossibl
to control.
6.4 Using gyrosco s to im rov stat stimation
W hav s n that  hav too much nois for our Kalman lt r to handl . It is th
r for tim to a roach
th robl m from a diff r nt rs ctiv in ord r to achi v a stabiliz d ight.
This cha t r focus on
x loring th o tion of s nsor fusion using th curr nt s nsor con guration (th t
ri-axis acc l rom t r
and magn tic com ass) tog th r ith thr angular rat gyrosco s (on for ach
axis of rotation) to
rovid mor accurat stat stimation from noisy m asur m nts.
Angular rat s nsors, also knon as gyrosco s (or gyros), s rv th ur os of
m asuring th rat
of rotation around th s nsor axis. Th or tically, h n int grating th signal
from th gyro, on can acuir th angular chang ov r a riod of tim , but on has to consid r th rob
l m of knoing th initial
angl at tim z ro, and also th signal bias hich vari s in an un r dictabl a
y. As a cons u nc , long
t rm gyro signal int gration is v ry oor, and th calculat d angl drifts aay
from its r al valu . Hov r, gyros rovid v ry good r adings of th angular rat s, and onc  g t som
knol dg on th
s nsor signal bias,  can accurat ly int grat th signal ov r a short riod o

f tim to d t rmin r liabl

angular m asur m nts. This m ans that a gyrosco has a v ry good r s ons for
high fr u nci s,
but a v ry oor on for lo fr u nci s (corr s onding to th o osit cas of a
n acc l rom t r). By
fusing th lo fr u ncy r s ons of th acc l rom t r ith th high fr u ncy m
asur m nts from th
gyros yi lds accurat stimat s for th attitud of an aircraft. Furth rmor , 
can also us th com ass
to h l th Kalman lt r g tting an stimat on th ya angl .
Having collat d a s ri s of nois data on th s nsors (s Subs ction 6.3.2), 
can us this information to build a mor accurat covarianc matrix R, as r s nt d:
R =
_

_
29.3 0 0 0 0 0 0
0 22.2 0 0 0 0 0
0 0 8 0 0 0 0
0 0 0 0.18 0 0 0
0 0 0 0 0.15 0 0
0 0 0 0 0 0.05 0
0 0 0 0 0 0 0.05
_

_
(6.1)
Notic that th matrix abov as not built ith angular gyro data, but ith data
from th acc l rom t r
and com ass. W ill also assum that th covarianc of th ya gyro r ading i
s th sam as th on
from th com ass. This a roach is not v ry accurat in th as ct that  sho
uld hav gyro data to
calculat its covarianc , but b caus  do not hav any at th tim ,  shall u
s th curr nt data as
a m ans of t sting th b havior of th Kalman lt r in th simulations. As usual
so far,  shall us a
r f r nc h r th uadrotor is in stationary hov r hil rotat d 45

deg
ees a
ound its ya axis. Figu
e

6.4.1 shos the simulation
esults, he
e the simulation no includes noisy meas
u
ements based on the
late
collated senso
data. It is clea
the ou
t
iad of senso
s is ve
y poe
fu
l in the sense that it p
ovides
suf cient data to the Kalman lte
to at least keep the quad
oto
stabilized, althou
gh e can clea
ly see
some distu
bances in the signal that a
e neve
theless handled by the LQR cont
ol
le
.
6.5. TECHNICAL ISSUES 73
Figu
e 6.4.1: Time
esponse of the LQR cont
ol loop using 6 states and gy
os 
hile in the p
esence of
ve
y noisy senso

eadings.
6.5 Technical issues
Du
ing the const
uction and testing of the quad
oto
, one is faced
ith many technical issues, such
as the p
oblem of tting the avionics inside a p
otective hull and imp
oving vib
a
tion abso
ption of the
st
uctu
e amongst othe
s. One p
oblem in pa
ticula
, conce
ning the batte
y feed
ing the moto
s, is o
th
of mention. Th
ough the cou
se of cont
ol testing ith the quad
oto
p
ototype,
it as ve
i ed that afte
a
batch of tests, the batte
y as discha
ged, as expected. Du
ing the folloing te
st ight, afte
the batte
y
as
echa
ged, it as obse
ved that the quad
oto
as no longe
able to achieve
takeoff, even though
the joystick cont
olling the p
opelle
s poe
as at full th
ottle. Measu
ing
the voltage in each one of
th
ee cells of the batte
y
evealed that to of them e
e
eading 3 Volts and th
e thi
d one as
eading 0
Volts. LiPo batte
ies a
e the top of the line hen it comes to poe
toeight

atio, but they a
e ext
emely
sensitive and
equi
e special ca
e. It is not advisable to let the voltage of ea
ch cell to d
op belo 3 Volts
74 CHAPTER 6. IMPLEMENTATION AND RESULTS
because it is impossible to
echa
ge the damaged cell again ithout an ext
eme

isk of causing a batte
y

e. This
e is ext
emely dange
ous in natu
e, and it cannot be extinguished ith
ate
. The
efo
e, it
is advisable to alays keep the voltage of each cell beteen 3 and 3.7 Volts.
As the batte
y could no
longe
be used, the
emaining cells e
e discha
ged by connecting a LED and a sm
all
esisto
in se
ies
ith each
emaining batte
y cell. The batte
y as safely disca
ded afte
a
ds an
d
eplaced ith a model
having simila
cha
acte
istics.
Chapte
7
Conclusions and futu
e o
k
The main objective of this thesis as to design and cont
ol a quad

oto
p
ototype, having a t
iaxis
accele
omete
and a compass as its senso
s.
In the
st phase of this thesis, objectives e
e de ned fo
the quad
oto
design, a
nd based on them
a study as ca

ied out on the elect
onics that ould be pa
t of the ai
c
aft. T
his analysis
esulted in the
const
uction of a quad
oto
capable of achieving TakeOff ithout using the moto

s at full th
ottle, leaving
plenty of ext
a space fo
maneuve
ing.
Knoing the cha
acte
istics of ou
quad
oto
, e p
oceeded to the modeling of th
e ai
c
afts dynamics

and kinematics fo
implementation of the
esulting equations in a compute
simul

ation envi
onment. The
modeling of the moto
s as achieved due to the use of data collected fo
identi ca
tion hich o
iginated
f
om the captu
e of sound of the p
opelle
s. This technique as based on the f
act that a mic
ophone
is capable of sensing the p
essu
e ave
esulting f
om the passage of
a p
opelle
, and the
efo
e, by
measu
ing the pe
iod of each
otation, it is possible dete
mine the angula
spee
d of the moto
p
opelle

assembly. This technique p
oved to be effective only fo
high angula
velociti
es. Folloing this logic,
the accele
omete
and the compass e
e also modeled so that the compute
simulat
ions could bette

po
t
ay
eality.
Anticipating the need to estimate as many states as possible f
om the available
senso
s, a Kalman
lte
as designed, limited to the hypothesis that the quad
oto
is a linea
syste
m. It as ve
i ed that the
Kalman lte
could only estimate 6 of the 12 states initially conside
ed fo
ai
c

aft. In this stage, it as
also discove
ed that none of the systems states is
eachable.
Folloed the const
uction of to LQR cont
olle
s fo
the system, one fo
the sit
uation he
e all 12
states a
e obse
vable (pe
fect situation) and anothe
fo
the
eal situation he

e e only have 6 states
available f
om the Kalman lte
(i.e. e only have the angula
speeds and Eule
an
gles).
Finally e p
oceeded to the implementation phase, hich evolved f
om the simples
t case of cont
ol in
the Simulink envi
onment, to the
eal application in the quad
oto
. Thus, sta
ti
ng ith the 12 states LQR
cont
olle
, e found that the system is completely cont
ollable hen all states
a
e available. Continuing
to the situation he
e e can only
ead 6 of those 12 states, the LQR cont
olle

designed fo
this case
as also able to cont
ol the available states, hile the othe
6 visibly d
ifted
ith time. The int
oduction
of the Kalman lte
, senso
s and moto
s dynamics in the cont
ol loop as also domi
nated by the same 6
states LQR cont
olle
, ithout any need fo
adjustment to its eighting mat
ices
. We then sta
ted ith the
actual implementation, he
e a softa
e p
og
am as developed to allo fo
commu
nication beteen a
75
76 CHAPTER 7. CONCLUSIONS AND FUTURE WORK
joystick and the quad
oto
s cont
ol unit, the A
duino. Unfo
tunately, the quad
ot
o
displayed an unstable
behavio
. It as assumed that the lack of inclusion of the PulseWidth Modulat
ion signal
esolution in
the simulations could the
eason hy this instability had not been fo
eseen. A
fte
int
oducing this ne
element in the simulation, e found neve
theless that the cont
olle
could stil
l d
ive the system to the
intended
efe
ence, although ith a little mo
e dif culty. This info
ma
tion led us to analyze the noise
coming f
om the senso
s ith the moto
s
unning, hich p
oved to be the sou
ce o
f the p
oblem. Not
even the Kalman
lte
as able to lte
out that much noise. This may
be explained by the fact that

the system is not
eachable. In othe
o
ds, if the senso
data is co

upted 
ith noise of such high in
intensity, it may be impossible fo
the Kalman lte
to estimate the desi
ed state
s.
All the o
k done so fa
shos only that the
e a
e seve
al aspects of the cont
o
l p
ocess that need
to be co

ected/imp
oved in o
de
to make a cont
olled ight fo
this p
ototype po
ssible. In pa
ticula
,
the inclusion of 3 gy
oscopes (one fo
each axis of
otation) should be conside

ed, hich togethe
ith
the t
iaxis accele
omete
, fo
m a combination of senso
s that has al
eady been
tested in quad
oto
s,
alloing fo
ve
y good cont
ol ove
its pitch and
oll angles. The idea is tha
t ith gy
oscopes e can
dete
mine o
ientation, but they d
ift ove
time (longte
m e

o
s), hich is som
ething e can co

ect ith
accele
omete
s (sho
tte
m e

o
s, i.e. they a
e ve
y noisy). But even then,
e ould need something
else to co

ect the d
ifting ya angle f
om the gy
oscope, hich could be done 
ith a magnetic compass.
The longte
m idea is that adding a g
eate
numbe
of senso
s, in addition to th
ose suggested, can only
imp
ove the quality and quantity of states estimated by the Kalman lte
.
It ould also be inte
esting to develop the simulations fu
the
by in
cluding ne elements such as
ae
odynamic fo
ces (i.e.
ind), collisions and othe
types of cont
o
l methods, such as P
opo
tional
De
ivative cont
olle
s. The quad
oto
p
ototype should also be taken to an outsi
de envi
onment to see its
pe
fo
mance tested unde
mo
e ext
eme conditions.
Bibliog
aphy
[1] P. Pounds, R. Mahony, J. G
esham, P. Co
ke, and J. Robe
ts.
Toa
ds DynamicallyFavou
able
QuadRoto
Ae
ial Robots. 2004.
[2] G.M. Hoffmann, H. Huang, S.L. Waslande
, and C.J. Tomlin. Quad
oto
Helic
opte
Flight Dynamics
and Cont
ol: Theo
y and Expe
iment. 2007.
[3] Gjioni E. AbouSleiman R., Ko
ff D. and Yang H. The oakland unive
sity un
manned ae
ial quad
oto

system. 2008.
[4] W. Ba
nes and W. McCo
mick. Ae
odynamics, Ae
onautics and Flight Mechanics
. Ne Yo
k: Wiley,
2nd. edition, 1995.
[5] http://en.ikipedia.o
g/iki/Quad
oto
, Octobe
2009.
[6] J.G. Leishman. The B
eguetRichet QuadRoto
Helicopte
of 1907. Ve
ti ite
, 47(3):14, 2001.
[7] http://www.nationmaster.com/encyclopedia/Quadrotor, January 2009.
[8] http://en.wikipedia.or
/wiki/Moller_Skycar_M400, January 2009.
[9] http://en.wikipedia.or
/wiki/M200G_Volantor, January 2009.
[10] http://www.accessmylibrary.com/article1G183761490/airmedicaltransport
takin
.html, Septem
ber 2009.
[11] S. Bouabdallah, P. Murrieri, and R. Sie
wart. Desi
n and control of an
indoor micro quadrotor. In
Robotics and Automation, 2004. Proceedin
s. ICRA04. 2004 IEEE International Co
nference on,
volume 5.
[12] S. Bouabdallah, A. Noth, and R. Sie
wart. PID vs LQ control techniques ap
plied to an indoor micro
quadrotor. In Intelli
ent Robots and Systems, 2004.(IROS 2004). Procee

din
s. 2004 IEEE/RSJ
International Conference on, volume 3.
[13] M. Becker, S. Bouabdallah, and R. Sie
wart. Desenvolvimento de um con
trolador de desvio de
obstculos para um minihelicptero Quadrirotor autnomo  1 fase: Simulao.
[14] P. Pounds, R. Mahony, and P. Corke. Modellin
and Control of a QuadRoto
r Robot. 2006.
[15] A. Mokhtari, A. Benalle
ue, and B. Daachi. Robust feedback linearizatio
n and
h8 controller for a
quad rotor unmanned aerial vehicle. Journal of Electrical En
ineerin
, 57(1):202
7, 2006.
77
78 BIBLIOGRAPHY
[16] G.P. Tournier, M. Valenti, J.P. How, and E. Feron. Estimation and control
of a quadrotor vehicle usin

monocular vision and moir patterns. 2006.

[17] E. Courses and T. Surveys. Robust low altitude behavior control of a qu
adrotor rotorcraft throu
h
slidin
modes. pa
es 16, 2007.
[18] Katie Miller. Path trackin
control for quadrotor helicopters. July 200
8.
[19] Sr
io Eduardo Aurlio Pereira da Costa. Controlo e simulao de um quadrirotor
convencional.
Masters thesis, october 2008.
[20] www.radrotary.com/motor_test.xls, November 2008.
[21] J. Roskam. Airplane i
ht dynamics and automatic i
ht controls. DARcorpora
tion, 2001.
[22] http://mathworld.wolfram.com/EulerAn
les.html, May 2009.
[23] HZ Peter. Modelin
and simulation of aerospace vehicle dynamics. Vir

inia: American Institute of

Aeronautics and Astronautics, pa
es 17242, 2000.
[24] http://en.wikipedia.or
/w/index.php?title=Quaternions_and_spatial_rotatio
n&oldid=296655109,
June 2009.
[25] A new approach to linear lterin
and prediction problems. Transactions o
f the ASMEJournal of
Basic En
ineerin
, 82(Series D):3545, 1960.
[26] P.S. Maybeck. Stochastic Models, Estimation and Control, Volume 1 Chapte
r 1, 1979.
[27] Seth Hutchinson. Lecture notes on linearization via taylor series, unive
rsity of illinois, Sprin
2009.
[28] Jean Walrand. Kalman lter: Conver
ence. Lecture Notes, U. C. Berkeley, A
u
ust 2009.
[29] G.F. Franklin, M.L. Workman, and D. Powell. Di
ital control of dynamic
systems. AddisonWesley
Lon
man Publishin
Co., Inc. Boston, MA, USA, 1997.
[30] B. D. O. Anderson and J. B. Moore. Optimal Control: Linear
Quadratic Methods. PrenticeHall,
En
lewood Cliffs, New Jersey, 1990.
[31] http://www.ece.ucsb.edu/ roy/classnotes/147c/lqrlq
notes.pdf, July 2009.
Appendix A
Electronic Circuits
Fi
ure A.1: Accelerometer wirin
.
79
80 APPENDIX A. ELECTRONIC CIRCUITS
Fi
ure A.2: Compass and electronic speed controllers wirin
.
Fi
ure A.3: Arduinos battery check circuit.
Appendix B
Matlab

les
B.1 loadvars.m
81
82 APPENDIX B. MATLAB

FILES
%Description: This mfile initializes all necessary variables required t
o
%buid the dynamic model of a quadrotor in the statespace form.
%The motor confi
uration is as follows:
% X axis
% _|_
% / \
% | 1 |
% \___/
% |
% |
% ___ | ___
% / \ || / \
% | 4 || xZ || 2 |Y axis
% \___/ || \___/
% |
% |
% _|_
% / \
% | 3 |
% \___/
clear all;
%% Physical properties of the environment

=9.81; %Acceleration of
ravity (m)
rho=1.2; %Density of air (m3.k
1)
%% Physical properties of the quadrotor
mq=0.82; %Total mass of the quadrotor
mm=0.048; %Mass of a motor (k
). All motors have equal mass.
lx=28.8e3; %Motor len
th alon
xaxis (m). All motors have equal size
s.
ly=28.8e3; %Motor len
th alon
yaxis (m)
lz=26e3; %Motor len
th alon
zaxis (m)
dc
=0.29; %Distance from the center of
ravity to the center of a mo
tor (m).
%The quadrotor is symmetric re
ardin
the XZ and YZ planes, so
%dc
is the same for all motors.
Ix1=(1/12)*mm*(ly2+lz2); %Moment of inertia (xaxis) for motors 1 and 3
%(k
.m2).
B.1. LOADVARS.M 83
Ix2=(1/12)*mm*(ly2+lz2)+mm*dc
2; %Moment of inertia (xaxis) for motors
%2 and 4 (k
.m2).
Ixx=2*Ix1+2*Ix2; %Total moment of inertia alon
the xaxis (k
.m2)
Iy1=(1/12)*mm*(lx2+lz2)+mm*dc
2; %Moment of inertia (yaxis) for motors
%1 and 3 (k
.m2).
Iy2=(1/12)*mm*(lx2+lz2); %Moment of inertia (yaxis) for motors 2 and 4
%(k
.m2).
Iyy=2*Iy1+2*Iy2; %Total moment of inertia alon
the yaxis (k
.m2)
Iz1=(1/12)*mm*(lx2+ly2)+mm*dc
2; %Moment of inertia (zaxis) for motors
%1 and 3 (k
.m2).
Iz2=(1/12)*mm*(lx2+ly2)+mm*dc
2; %Moment of inertia (zaxis) for motors
%2 and 4 (k
.m2).
Izz=2*Iz1+2*Iz2; %Total moment of inertia alon
the zaxis (k
.m2)
I=dia
([Ixx Iyy Izz]); %Inertia matrix
cp=0.0743; %Thrust coefficient of the propeller

ct=0.1154; %Power coefficient

rp=5*25.4e3; %Propeller radius (m)
Km=cp*4*rho*rp5/pi()3; %Constant value to calculate the moment provided
%by a propeller
iven its an
ular speed (k
.m2.rad1)
Kt=ct*4*rho*rp4/pi()2; %Constant value to calculate the thrust provided
%by a propeller
iven its an
ular speed (k
.m.rad1)
Ktm=Km/Kt; %Constant that relates thrust and moment of a propeller.
k1=2.028; %Transferfunction
ain of motor 1
k2=1.869; %Transferfunction
ain of motor 2
k3=2.002; %Transferfunction
ain of motor 3
k4=1.996; %Transferfunction
ain of motor 4
tc=0.136; %Timeconstant of the motors (assumed equal for all motors)
dz1=1247.4; %PWM deadzone of motor 1
iven its firstorder transfer
%function (microseconds)
dz2=1247.8; %PWM deadzone of motor 2
iven its firstorder transfer
%function (microseconds)
dz3=1246.4; %PWM deadzone of motor 3
iven its firstorder transfer
%function (microseconds)
dz4=1247.9; %PWM deadzone of motor 4
iven its firstorder transfer
84 APPENDIX B. MATLAB

FILES
%function (microseconds)

_m1=tf(k1,[tc 1]); %Firstorder transferfunction of motor 1

_m2=tf(k2,[tc 1]); %Firstorder transferfunction of motor 2

_m3=tf(k3,[tc 1]); %Firstorder transferfunction of motor 3

_m4=tf(k4,[tc 1]); %Firstorder transferfunction of motor 4

%Note: If the behavior of the motors is not linear, linearization aro
und
%an operatin
point is required to attain these transferfunctions.
PWM_max=2200; %Upper limit of the PWM si
nal provided to
%the motors (microseconds)
%% Arduino variables
freq = 20; %Samplin
frequency (Hz);
ardres = 10; %Arduino analo
resolution (bits)
%% Sensors
daccel_x = 0.003; %Accelerometer distance to the quadrotors center of
%
ravity alon
the xaxis (m)
daccel_y = 0; %Accelerometer distance to the quadrotors center of
%
ravity alon
the yaxis (m)
daccel_z = 0.0490; %Accelerometer distance to the quadrotors center of
%
ravity alon
the zaxis (m)
daccel=[daccel_x daccel_y daccel_z];
accel_max = 3*
; %Accelerometer maximum possible readin

accel_min = 3*
; %Accelerometer minimum possible readin

ares = (accel_maxaccel_min)/(2ardres); %Accelerometer readin

resolution
cres = 1*pi()/180;
%Compass readin
resolution (rad) = i de
ree x 180 de
rees / pi() ra
d.
%Note: this can be confi
ured in the Arduino to be either 1 or 0.5
de
rees
%% Initial states
k1 = 2.0276; %Constant that relates Rotation speed of propeller 1 wit
h the
%correspondin
PWM pulse of the Arduino
k2 = 1.8693; %Constant that relates Rotation speed of propeller 2 wit
h the
B.1. LOADVARS.M 85
%correspondin
PWM pulse of the Arduino
k3 = 2.0018; %Constant that relates Rotation speed of propeller 3 wit

h the
%correspondin
PWM pulse of the Arduino
k4 = 1.9986; %Constant that relates Rotation speed of propeller 4 wit
h the
%correspondin
PWM pulse of the Arduino
PWMdz1 = 1256; %Dead zone of the motor 1 (PWM pulsewidth in microse
conds)
PWMdz2 = 1264; %Dead zone of the motor 2 (PWM pulsewidth in microse
conds)
PWMdz3 = 1256; %Dead zone of the motor 3 (PWM pulsewidth in microse
conds)
PWMdz4 = 1256; %Dead zone of the motor 4 (PWM pulsewidth in microse
conds)
%Calculate the PWM pulse necessary to beat the force of
ravity
syms PWM
1 PWM
2 PWM
3 PWM
4
PWM
1=solve((PWM
1PWMdz1)2(1/4)*mq*
/(Kt*(k12)),PWM
1);
PWM
1=double(PWM
1);
PWM
1=PWM
1(PWM
1>PWMdz1);
PWM
2=solve((PWM
2PWMdz2)2(1/4)*mq*
/(Kt*(k22)),PWM
2);
PWM
2=double(PWM
2);
PWM
2=PWM
2(PWM
2>PWMdz2);
PWM
3=solve((PWM
3PWMdz3)2(1/4)*mq*
/(Kt*(k32)),PWM
3);
PWM
3=double(PWM
3);
PWM
3=PWM
3(PWM
3>PWMdz3);
PWM
4=solve((PWM
4PWMdz4)2(1/4)*mq*
/((Kt*k42)),PWM
4);
PWM
4=double(PWM
4);
PWM
4=PWM
4(PWM
4>PWMdz4);
%Compute the necessary rotation speed (rad.s1) of each propeller to be
at
%the force of
ravity
wm1 = double(k1*(PWM
1PWMdz1));
wm2 = double(k2*(PWM
2PWMdz2));
wm3 = double(k3*(PWM
3PWMdz3));
wm4 = double(k4*(PWM
4PWMdz4));
w0=[wm1 wm2 wm3 wm4]; %Initial an
ular speed of motors 1 to 4 (rad.s
1)
U0=0; %Initial linear velocity alon
the xaxis (m.s1)
V0=0; %Initial linear velocity alon
the yaxis (m.s1)
W0=0; %Initial linear velocity alon
the zaxis (m.s1)
P0=0; %Initial an
ular velocity around the xaxis of the quadrotor
%(rad.s1)
Q0=0; %Initial an
ular velocity around the yaxis of the quadrotor
%(rad.s1)
R0=0; %Initial an
ular velocity around the zaxis of the quadrotor
%(rad.s1)
86 APPENDIX B. MATLAB

FILES
X0=0; %Initial quadrotor position alon
the xaxis of the inertial fra
me of
%reference (m)
Y0=0; %Initial quadrotor position alon
the yaxis of the inertial fra
me of
%reference (m)
Z0=0; %Initial 1uadrotor position alon
the zaxis of the inertial fra
me of
%reference (m)
PHI0=0; %Initial pitch an
le of the quadrotor (rad)
THETA0=0; %Initial roll an
le of the quadrotor (rad)
PSI0=0; %Initial yaw an
le of the quadrotor (rad)

q0=an
le2quat(PHI0,THETA0,PSI0); %Initial state quaternion
X0=[U0 V0 W0 P0 Q0 R0 X0 Y0 Z0 q0(1) q0(2) q0(3) q0(4)]; %Initial
state
%vector
Z0lin = 1; %Linearization point with the quadrotor stabilized (m)
PHIlin = 0; %Linearization point pitch an
le of the quadrotor (rad)
THETAlin = 0; %Linearization point roll an
le of the quadrotor (rad)
PSIlin = 0; %Linearization point yaw an
le of the quadrotor (rad)
qlin=an
le2quat(PHIlin,THETAlin,PSIlin); %Initial state quaternion
X0lin=[0 0 0 0 0 0 0 0 Z0lin PHIlin THETAlin PSIlin]; %Linearization
point
%for
eneratin
the statespace system representation of the quadrotor
[A,B,C,D]=quadss(X0lin,mq,I,w0,Kt,Ktm,dc
,daccel); %Get linearized state
%space matrices of the quadrotor
Y0ref = [0 0 0 0 0 0]; %Initial quadrotor reference
quadkalm; %Get Kalman filter
quadLQRcontrol; %Get LQR controller
%quadLQRcontrol_12states %Uncomment this line if you wish 12 states con
trol
%with pure feedback (i.e. no Kalman filter nor sensors)
damp(AB*[zeros(4,3) Klqr(:,1:3) zeros(4,3) Klqr(:,4:6)]) %Ei
en values of
%the closed loop. If an ei
envalue is not stable, the dynamics of th
is
%ei
envalue will be present in the closedloop system which therefore
will
%be unstable.
B.2. QUADSYS.M 87
B.2 quadsys.m
88 APPENDIX B. MATLAB

FILES
function [Xout] = quadsys(Xin,m,I,wm,Kt,Ktm,dc
)
%Function to simulate the quadrotor dynamics and outputs its current
%state space vector.
%The motor confi
uration is as follows:
% X axis
% _|_
% / \
% | 1 |
% \___/
% |
% |
% ___ | ___
% / \ || / \
% | 4 || xZ || 2 |Y axis
% \___/ || \___/
% |
% |
% _|_
% / \
% | 3 |
% \___/
% Inputs:
%
% Xin  Initial state vector of the quadrotor:
% Xin = [U V W P Q R X Y Z q1 q2 q3 q4]
% U  Linear velocity in the xaxis direction of the inertial
% reference frame (m.s1);
% V  Linear velocity in the yaxis direction of the inertial
% reference frame (m.s1);

% W  Linear velocity in the zaxis direction of the inertial

% reference frame (m.s1);
% P  An
ular speed around the xaxis of the quadrotor (rad.s1);
% Q  An
ular speed around the yaxis of the quadrotor (rad.s1);
% R  An
ular speed around the zaxis of the quadrotor (rad.s1);
% X  Position of body frame of the quadrotor alon
the xaxis of
% the inertial reference frame (m);
% Y  Position of body frame of the quadrotor alon
the xaxis of
% the inertial reference frame (m);
% Z  Position of body frame of the quadrotor alon
the xaxis of
% the inertial reference frame (m);
% q1...q4  Quaternion containin
the quadrotors attitude
% (conversion made from Euler an
les).
%
B.2. QUADSYS.M 89
% m  Quadrotors mass (k
).
%
% I  Inertia matrix assumin
the quadrotor is a ri
id body with
% constant mass of which its axis are ali
ned with the principal
% axis of inertia. The matrix I becomes a dia
onal matrix
% containin
only the principal moments of inertia:
%
% I = [I1 0 0;0 I2 0;0 0 I3]
% I1  Principal moment of inertia alon
the xaxis of the
% quadrotors body frame (k
.m2);
% I2  Principal moment of inertia alon
the yaxis of the
% quadrotors body frame (k
.m2);
% I3  Principal moment of inertia alon
the zaxis of the
% quadrotors body frame (k
.m2).
%
% wm  An
ular speed array of the propellers:
% wm = [w1 w2 w3 w4]
% w1  Current an
ular speed of propeller 1 (North propeller)
% (rad.s1);
% w2  Current an
ular speed of propeller 2 (East propeller)
% (rad.s1);
% w3  Current an
ular speed of propeller 3 (South propeller)
% (rad.s1);
% w4  Current an
ular speed of propeller 4 (West propeller)
% (rad.s1).
%
% Kt  Constant used to express the thrust and prop. an
ular speed
%
% Ktm  Constant used to express the thrust with prop. momentum
%
% dc
 Distance from the quadrotors COG to the motor
%
% Outputs:
%
% Xout  Current system states:
% Xout = [U V W P Q R X Y Z q1 q2 q3 q4]
%
% Example:
% Xin = [0 0 0 0.1 0 0 0 0 0 1 0 0 0];
% wm = [0 0 0 0];
% m = 1;
% I = [0.1 0 0;0 0.1 0;0 0 0.1];
% Kt=0.1;
90 APPENDIX B. MATLAB

FILES
% Ktm=0.01;
% dc
=0.5;
%
% Xout = quadsys(Xin,m,I,wm,Kt,Ktm,dc
) outputs:
%
% Xout = [0 0 9.8100 0 0 0 0 0 0 0 0.0500 0 0]
U=Xin(1); %Linear velocity xaxis (m)
V=Xin(2); %Linear velocity yaxis (m)
W=Xin(3); %Linear velocity zaxis (m)
P=Xin(4); %An
ular velocity around the xaxis (rad.s1)
Q=Xin(5); %An
ular velocity around the yaxis (rad.s1)
R=Xin(6); %An
ular velocity around the zaxis (rad.s1)
X=Xin(7); %Position of the body axes frame alon
the xaxis of the i
nertial
%reference system (m)
Y=Xin(8); %Position of the body axes frame alon
the yaxis of the i
nertial
%reference system (m)
Z=Xin(9); %Position of the body axes frame alon
the zaxis of the i
nertial
%reference system (m)
q0=Xin(10); %Quaternion element 1 calculated from Euler an
le conversion
q1=Xin(11); %Quaternion element 2 calculated from Euler an
le conversion
q2=Xin(12); %Quaternion element 3 calculated from Euler an
le conversion
q3=Xin(13); %Quaternion element 4 calculated from Euler an
le conversion
Rm=[q02+q12q22q32 2*(q1*q2+q0*q2) 2*(q1*q3q0*q2);
2*(q1*q2q0*q3) q02q12+q22q32 2*(q2*q3+q0*q1);
2*(q1*q3+q0*q2) 2*(q2*q3q0*q1) q02q12q22+q32]; %rotation matrix R
%usin
quaternions
Tm(1:4)=Kt.*(wm(1:4).2); %calculate thrust produced by each propeller (N)
Fx=0; %xaxis force actin
on the quadrotor while hoverin
steady at
%constant altitude
Fy=0; %yaxis force actin
on the quadrotor while hoverin
steady at
%constant altitude
Fz=(Tm(1)+Tm(2)+Tm(3)+Tm(4)); %zaxis force actin
on the quadrotor whil
e
%hoverin
steady at constant altitude
Mx=(Tm(4)Tm(2))*dc
; %Momentum responsible for rotation around the xaxi
s
%(N.m)
My=(Tm(1)Tm(3))*dc
; %Momentum responsible for rotation around the yaxi
s
%(N.m)
Mz=Ktm*(Tm(1)+Tm(3)Tm(2)Tm(4)); %Momentum responsible for rotation around
B.2. QUADSYS.M 91
%the zaxis (N.m)

=9.81; %Acceleration of
ravity (m.s2)
Lin_a=(1/m).*[Fx Fy Fz]+Rm*[0 0
]([Q*WR*V R*UP*W P*VQ*U]); %Calc.
%the linear acceleration of the quadrotor
An
P=(Mx/I(1,1))(I(3,3)I(2,2))*Q*R/I(1,1); %An
ular acceleration around
%the xaxis of the body reference frame
An
Q=(My/I(2,2))(I(1,1)I(3,3))*R*P/I(2,2); %An
ular acceleration around
%the yaxis of the body reference frame
An
R=(Mz/I(3,3))(I(2,2)I(1,1))*P*Q/I(3,3); %An
ular acceleration around
%the zaxis of the body reference frame
Pos=Rm*[U V W]; %Calculate the position of the quadrotor
Omq=[0 P Q R; %Rotation matrix to calculate the time drivative of
the
P 0 R Q; %rotation quaternion

Q R 0 P;
R Q P 0];
epsilon=1[q0 q1 q2 q3]*[q0 q1 q2 q3];
dq=(1/2)*Omq*[q0 q1 q2 q3]+epsilon*[q0 q1 q2 q3]; %Time derivative of th
e
%rotation quaternion
Xout=[Lin_a(1) Lin_a(2) Lin_a(3) An
P An
Q An
R Pos(1) Pos(2) Pos(3)...
dq(1) dq(2) dq(3) dq(4)]; %Output state vector
end
92 APPENDIX B. MATLAB

FILES
Appendix C
Arduino source code
93
94 APPENDIX C. ARDUINO SOURCE CODE
#include <Wire.h> //Library for I2C implementation usin
analo
pins 4
(SDA) and 5 (SCL)
#include <Me
aServo.h>
#define NBR_SERVOS 4 // the maximum number of servos (up to 12 on A
rduino Diecimilia)
#define FIRST_SERVO_PIN 6 // define motor pins on the Arduino board
#define SECOND_SERVO_PIN 9
#define THIRD_SERVO_PIN 10
#define FOURTH_SERVO_PIN 11
Me
aServo Servos[NBR_SERVOS] ; // max servos
//Accelerometer variables
float ax,ay,az; //Acceleration values in m/s^2 for all three axis
const float as=0.00488; //Arduino analo
resolution = 5Volts/1024 (10bit
s)
const float z
=1.63; //Sensor zero
bias = Supply Volta
e/2 =3.3/2 (
V)
const float sR=0.3; //Sensor resolution (V/
)
//Compass variables
int compassAddress = 0x42 > > 1; // compass I2C slave adress: 0x42
// because the wire.h library only uses the 7 bits most si
nificant
bits
// a shift necessary is to
et the most si
nificant bits.
int psi = 0; // Compass headin
= yaw (direct readin
s from the com
pass in de
rees)
float psirad; // Compass headin
= yaw in radians
//Euler an
les Roll and pitch
float phi; // (rad)
float theta; // (rad)
//Kalman filter data
const float A[6][6] = {
{1.0000,0.0000,0.0000,0.5985,0.0000,0.0000},
{0.0000,1.0000,0.0000,0.0000,0.5985,0.0000},
{0.0000,0.0000,1.0000,0.0000,0.0000,0.0479},
95
{0.0500,0.0000,0.0000,0.3384,0.0000,0.0000},
{0.0000,0.0500,0.0000,0.0000,0.3384,0.0000},
{0.0000,0.0000,0.0500,0.0000,0.0000,0.9147}
};//(Kalman statespace matrix Ak)
const float B[6][10] = {
{0.0000,0.0188,0.0000,0.0188,0.0000,0.0302,0.0000,0.0031,0.0000,0.0000},
{0.0194,0.0006,0.0194,0.0006,0.0302,0.0000,0.0000,0.0000,0.0031,0.0000},
{0.0009,0.0009,0.0009,0.0009,0.0000,0.0000,0.0000,0.0000,0.0000,0.0479},
{0.0000,0.0002,0.0000,0.0002,0.0000,0.0334,0.0000,0.0034,0.0000,0.0000},
{0.0005,0.0006,0.0005,0.0006,0.0334,0.0000,0.0000,0.0000,0.0034,0.0000},

{0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0853}
}; //(Kalman statespace matrix Bk)
float x[6] = {0,0,0,0,0,0}; //state vector: Roll speed, Pitch Speed,
Yaw speed, Roll an
le, Pitch an
le, Yaw an
le
float u[10] = {0,0,0,0,0,0,0,0,0,0}; //input vector: w1 (motor 1 speed
),w2,w3,w4,ax,ay,az,phi,theta,yaw
float At[6] = {0,0,0,0,0,0}; //temp variable
float Bt[6] = {0,0,0,0.01,0.04,0}; //temp variable
//LQR
ain matrix
const float K[4][6] = {
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{7.1030,0.0000,20.4163,18.2839,0.0000,14.4659},
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{7.1030,0.0000,20.4163,18.2839,0.0000,14.4659}
};
float w1,w2,w3,w4; //Motor inputs from LQR controller (rad.s^1)
float w0; //Throttle to the motors (from joystick)
//Motor inputs for PWM si
nal
float wc1=0;
96 APPENDIX C. ARDUINO SOURCE CODE
float wc2=0;
float wc3=0;
float wc4=0;
//PWM si
nals
int p1 = 200;
int p2 = 200;
int p3 = 200;
int p4 = 200;
//Reference
float yr[6]={0,0,0,0,0,0};
void setup()
{
Wire.be
in();
Serial.be
in(9600); // Initialize serial communications with setup
Servos[0].attach( FIRST_SERVO_PIN, 800, 2200); //Motor 1  North  Cloc
kwise rotation
Servos[1].attach( SECOND_SERVO_PIN, 800, 2200); //Motor 2  East  Coun
terclockwise rotation
Servos[2].attach( THIRD_SERVO_PIN, 800, 2200); //Motor 3  South  Cloc
kwise rotation
Servos[3].attach( FOURTH_SERVO_PIN, 800, 2200); // Motor 4  West  Co
unterclockwise rotation
//Put the motors in full stop
Servos[0].write(0);
Servos[1].write(0);
Servos[2].write(0);
Servos[3].write(0);
}
void loop()
{
//
et the most recent acceleration values for all three axis
97
ax = ((analo
Read(0)*asz
)/sR)*9.81; //acceleration xaxis (m.s^2)
ay = ((analo
Read(1)*asz
)/sR)*9.81; //acceleration yaxis (m.s^2)
az = ((analo
Read(2)*asz
)/sR)*9.81; //acceleration zaxis (m.s^2)
ax=ax; //This correction allows the accelerometer to provide positive
acceleration in the xaxis direction
// Compass task 1: connect to the HMC6352 sensor
Wire.be
inTransmission(compassAddress);
Wire.send(A); // The ascii character "A" tells the compass sensor to se

nd data
Wire.endTransmission();
// Compass task 2: wait for data processin

delay(50); // Compass datasheet says we need to wait at least 6000 m

icrose
undos (0.006s) for data processin
in the sensor
//We can also take this opotunity to implement the samplin
time (20H
z)
// Compass task 3: Request headin

Wire.requestFrom(compassAddress, 2); // request 2 bytes of data

// Compass task 4: Get headin
data
if(2 <= Wire.available()) // if 2 bytes are available
{
//16bit numbers can be broken in two 8 bit chunks. The first is ca
lled hi
h byte and the second low byte
psi = Wire.receive(); //
et hi
h byte
psi = psi < < 8; // shift hi
h byte
psi += Wire.receive(); //
et low byte
psi /= 10; // comment this line if you wish to
et the headin
in
decide
rees instead of de
rees
}
psirad = de
2rad((float)psi); // Map the compass readin
from 0 to 35
9 de
rees to pi radians to pi radians
phi=atan(ay/az); //Calculate roll an
le from the accelerations provided
by the accelerometer (rad)
theta=atan(ax/az); //Calculate pitch an
le from the accelerations provid
ed by the accelerometer (rad)
//Perform Kalman filterin

At[0]=A[0][0]*x[0]+A[0][1]*x[1]+A[0][2]*x[2]+A[0][3]*x[3]+A[0][4]*x[4]+A[0][5]*x
[5];
98 APPENDIX C. ARDUINO SOURCE CODE
At[1]=A[1][0]*x[0]+A[1][1]*x[1]+A[1][2]*x[2]+A[1][3]*x[3]+A[1][4]*x[4]+A[1][5]*x
[5];
At[2]=A[2][0]*x[0]+A[2][1]*x[1]+A[2][2]*x[2]+A[2][3]*x[3]+A[2][4]*x[4]+A[2][5]*x
[5];
At[3]=A[3][0]*x[0]+A[3][1]*x[1]+A[3][2]*x[2]+A[3][3]*x[3]+A[3][4]*x[4]+A[3][5]*x
[5];
At[4]=A[4][0]*x[0]+A[4][1]*x[1]+A[4][2]*x[2]+A[4][3]*x[3]+A[4][4]*x[4]+A[4][5]*x
[5];
At[5]=A[5][0]*x[0]+A[5][1]*x[1]+A[5][2]*x[2]+A[5][3]*x[3]+A[5][4]*x[4]+A[5][5]*x
[5];
//Get throttle from the joystick
if (Serial.available() > 0)
{
// read the incomin
byte:
w0 = map((float)Serial.read(), 0, 100, 0, 500);
w0=constrain(w0,0,500); //Limit maximum velocity to 600 rad/s
Serial.flush();
}
else
{
w0 = 0;
}
//place known inputs and measurements in the input vector u
u[0]=wc1w0;
u[1]=wc2w0;
u[2]=wc3w0;
u[3]=wc4w0;
u[4]=axyr[0];
u[5]=ayyr[1];
u[6]=azyr[2];

u[7]=phiyr[3];
u[8]=thetayr[4];
99
u[9]=psiradyr[5];
Bt[0]=B[0][0]*u[0]+B[0][1]*u[1]+B[0][2]*u[2]+B[0][3]*u[3]+B[0][4]*u[4];
Bt[0]=Bt[0]+B[0][5]*u[5]+B[0][6]*u[6]+B[0][7]*u[7]+B[0][8]*u[8]+B[0][9]*u[9];
Bt[1]=B[1][0]*u[0]+B[1][1]*u[1]+B[1][2]*u[2]+B[1][3]*u[3]+B[1][4]*u[4]
Bt[1]=Bt[1]+B[1][5]*u[5]+B[1][6]*u[6]+B[1][7]*u[7]+B[1][8]*u[8]+B[1][9]*u[9];
Bt[2]=B[2][0]*u[0]+B[2][1]*u[1]+B[2][2]*u[2]+B[2][3]*u[3]+B[2][4]*u[4]
Bt[2]=Bt[2]+B[2][5]*u[5]+B[2][6]*u[6]+B[2][7]*u[7]+B[2][8]*u[8]+B[2][9]*u[9];
Bt[3]=B[3][0]*u[0]+B[3][1]*u[1]+B[3][2]*u[2]+B[3][3]*u[3]+B[3][4]*u[4]
Bt[3]=Bt[3]+B[3][5]*u[5]+B[3][6]*u[6]+B[3][7]*u[7]+B[3][8]*u[8]+B[3][9]*u[9];
Bt[4]=B[4][0]*u[0]+B[4][1]*u[1]+B[4][2]*u[2]+B[4][3]*u[3]+B[4][4]*u[4]
Bt[4]=Bt[4]+B[4][5]*u[5]+B[4][6]*u[6]+B[4][7]*u[7]+B[4][8]*u[8]+B[4][9]*u[9];
Bt[5]=B[5][0]*u[0]+B[5][1]*u[1]+B[5][2]*u[2]+B[5][3]*u[3]+B[5][4]*u[4]
Bt[5]=Bt[5]+B[5][5]*u[5]+B[5][6]*u[6]+B[5][7]*u[7]+B[5][8]*u[8]+B[5][9]*u[9];
//Get estimated states x(k+1)=A.x(k)+B.u(k)
x[0]=At[0]+Bt[0];
x[1]=At[1]+Bt[1];
x[2]=At[2]+Bt[2];
x[3]=At[3]+Bt[3];
x[4]=At[4]+Bt[4];
x[5]=At[5]+Bt[5];
//Controlo LQR
w1=K[0][0]*x[0]+K[0][1]*x[1]+K[0][2]*x[2]+K[0][3]*x[3]+K[0][4]*x[4]+K[0][5]*x[5]
;
100 APPENDIX C. ARDUINO SOURCE CODE
w2=K[1][0]*x[0]+K[1][1]*x[1]+K[1][2]*x[2]+K[1][3]*x[3]+K[1][4]*x[4]+K[1][5]*x[5]
;
w3=K[2][0]*x[0]+K[2][1]*x[1]+K[2][2]*x[2]+K[2][3]*x[3]+K[2][4]*x[4]+K[2][5]*x[5]
;
w4=K[3][0]*x[0]+K[3][1]*x[1]+K[3][2]*x[2]+K[3][3]*x[3]+K[3][4]*x[4]+K[3][5]*x[5]
;
//Adjust speeds with the throttle
wc1=w0w1;
wc2=w0w2;
wc3=w0w3;
wc4=w0w4;
//Calculate and send pulse width to the motors
p1=(int)((wc1/2.0276)+1252); // (speed of motor 1 (rad/s) / transfer fu
nction
ain) + Deadzone pulse width
p2=(int)((wc2/1.8693)+1262);
p3=(int)((wc3/2.0018)+1255);
p4=(int)((wc4/1.9986)+1255);
if(w0==0) //Forar para
em
{
p1=400;
p2=400;
p3=400;
p4=400;
}
Servos[0].write(p1);
Servos[1].write(p2);
Servos[2].write(p3);
Servos[3].write(p4);
// output states
Serial.print(x[0]);
Serial.print(" ");
101
Serial.print(x[1]);

Serial.print(" ");
Serial.print(x[2]);
Serial.print(" ");
Serial.print(x[3]);
Serial.print(" ");
Serial.print(x[4]);
Serial.print(" ");
Serial.print(x[5]);
Serial.println("");
}
float de
2rad (float x) //function that receives an an
le between 0 a
nd 359 de
rees and resturns the same an
le between pi
and pi radians
{
if(x>=0 && x<=180)
{
x=x*3.14/180;
}
else
{
x = (360x)*3.14/180;
}
return x;
}