Estimation and Control of a Quadrotor Attitude

Bernardo Sousa Machado Henriques

Dissertac ao para obtenc ao do Grau de Mestre em
Engenharia Mec anica
J uri
Presidente: Professor Jos e Rog erio Caldas Pinto
Orientador: Professora Alexandra Bento Moutinho
Co-Orientador: Professor Jos e Ra ul Carreira Azinheira
Vogal: Professor Paulo Oliveira
Junho 2011
Ce qui sapprend sans peine ne vaut rien et ne demeure pas.
Ren e Barjavel
Abstract
The aim of the present work is to estimate and control the attitude of a quadrotor aircraft prototype
using solely inexpensive sensors: 3-axes accelerometer, 3-axes gyroscope, 2-axes compass and a range
nder.
We rst introduce the concept of the quadrotor, where the motion of the vehicle is explained along with
the set of frames necessary to understand it. The characteristics of the prototype are estimated and the
dynamics and kinematics models of the quadrotor concept are developed. To complete the model, the
actuators are approximated to rst order systems with parameters obtained experimentally. The sensors
are modeled using real sensor measurements to correctly estimate the measurement noise. This led to
the implementation of the model in Simulink.
After linearizing the model around the operating point, a linear state space system was obtained and
used to develop a 12 states LQR controller using ideal sensors. Closing the loop with that controller
generated attitude data which was used to tune the different estimators considered. The Complementary
lter and the different Kalman lters, as the linear and expended Kalman lters, are tested and compared.
An 8 states LQR controller using ideal sensors is then tuned to control the attitude and height of the
quadrotor in continuous and in discrete states. The LQR is also tested with feedback from the chosen
estimator, the EKF. Then, the estimator proves to be well tuned after being implemented on the prototype
and tested in open loop considering the operating conditions: all motors turned on.
Finally the results suggest that the discrete LQR controller along with the EKF are a good strategy for
estimating and controlling the attitude of a quadrotor.
Keywords: Quadrotor, VTOL, Extended Kalman lter, Complementary lter.
v
vi
Resumo
O objectivo deste trabalho e estimar e controlar a atitude de um quadrirotor usando apenas sensors de
baixo custo: aceler ometro e girosc opio de tr es eixos, b ussola de dois eixos e tel emetro.
Inicialmente, o conceito de quadrirotor e introduzido, explicando os movimentos do veculo com os sis-
temas de eixos necess arios para denir esses movimentos. As caractersticas do prot otipo s ao estimadas
e os modelos din amicos desenvolvidos. Para completar o modelo, os actuadores s ao aproximados por
sistemas de primeira ordem com par ametros obtidos experimentalmente. Os sensores s ao modelados
usando medic oes reais para estimar correctamente o rudo dos sensores. Isto resulta na implementac ao
do modelo em Simulink.
Depois de linearizar o modelo em torno da condic ao do ponto de funcionamento, um sistema linear
em espaco de estados e obtido e usado para desenvolver um controlador LQR de 12 estados, utilizando
sensores ideais. Fechando o anel com esse controlador, dados relativos a atitude do quadrirotor s ao
obtidos e usados para anar os estimadores considerados. Filtros complementares e diferentes ltros de
Kalman, como o ltro linear e ltro extendido ou n ao linear, s ao testados e comparados.
Um controlador LQR de 8 estados, utilizando sensores ideais, e anado para controlar a atitude e
altura do quadrirotor no espaco contnuo e discreto. O controlador e tamb em testado com realimentac ao
do estimador escolhido, o EKF. Em seguida, o estimador e implementado no prot otipo provando estar bem
anado para as condic oes de funcionamento: com todos os motores a funcionar.
Finalmente, os resultados sugerem que o LQR discreto juntamente com o EKF formam uma boa es-
trat egia pas estimar e controlar a atitude de um quadrirotor.
Palavras chave: Quadrirotor, VTOL, Filtro de Kalman extendido, Filtros complementares.
vii
viii
Acknowledgments
I would like to thank Prof. Azinheira and Prof. Moutinho for the constant help provided, for being there
at any time of the day ready to help in any way I needed. I also want to thank them for the patience shown
throughout the last year.
I would also like to thank my mother, sister and niece for the great patience, and for helping me in my
free time distracting me whenever I most needed.
I would also like to thank all of my friends from the BunkerGroup and especially Jane, Luigi, Virgilio,
Farinha, Romeu, Miguel and Elisa for their personal help, always ready for a coffee, for a talk or to distract
me in any possible way.
ix
x
Contents
Contents xi
List of Figures xv
List of Tables xvii
Notation xix
1 Introduction 1
1.1 Context and Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Objectives and Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Structure of the Dissertation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
I Modeling the Dynamics of a Quadrotor 5
2 Denitions 7
2.1 Conventions and Characteristics of a Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.1 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.2 Maneuvering a Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2 Prototype Identication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 General Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3 Quadrotor Model 13
3.1 Kinematics and Dynamics Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1.1 Quaternions and Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1.2 Quaternion Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.1.3 Euler Angles Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2 Model of the Ground Reaction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
4 Sensors 19
4.1 Choosing the Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.2 Model of the Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.3 Experimental Identication of the Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
5 Actuators 31
5.1 Model of the Propeller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
xi
5.2 Model of the Motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
5.3 Identication of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
6 State Space Model 37
6.1 Assembling the State Space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
6.2 Linearization Point . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
6.3 Linearization of the Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
7 Quadrotor Simulator 43
7.1 Implementation of the Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.1.1 Dynamic and Output Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.1.2 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.1.3 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.2 Simulation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
II Flight Control of a Quadrotor 47
8 Control Design 49
8.1 Linear Quadratic Regulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
8.1.1 Control Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
8.1.2 Introduction to the LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
8.2 Ideal Full State Control via LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
9 Estimation 55
9.1 Complementary Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
9.1.1 First Complementary Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
9.1.2 Passive Complementary Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
9.1.3 Discussion of the Complementary Filters . . . . . . . . . . . . . . . . . . . . . . . . 60
9.2 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
9.2.1 Simple Linear Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
9.2.2 Improved Linear Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
9.2.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
9.2.4 Discussion of the Kalman Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
9.3 Height Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
9.4 Conclusion about the Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
10 Practical Implementation 69
10.1 Attitude Stabilization Via LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
10.1.1 Continuous LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
10.1.2 Discrete LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
10.2 Discrete LQR with Estimation Feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
10.3 Remote Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
10.4 Practical Validation of the Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
xii
11 Conclusions and Future Work 81
11.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
11.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
Bibliography 83
Appendices 85
A Practical Usage of the Prototype 87
A.1 Components of the Prototype . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
A.2 Wiring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
A.3 Maintenance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
A.4 Arduino Program . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
B Determination of the Center of Mass 103
xiii
xiv
List of Figures
1.1 Development of the Quadrotor Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Further Development of the Quadrotor Concept . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Quadrotors in the Private Sector and in Universities . . . . . . . . . . . . . . . . . . . . . . 3
2.1 Inertial and Body-Fixed Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 Quadrotor Rotation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Determination of the Center of Mass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.4 Determination of the Center of Mass along u
z
. . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.5 Dimensions of the Prototype . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.1 Dynamics and Kinematics Block Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.1 Frame of the Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.2 Functioning Principle of a MEMS Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.3 Functioning Principle of a Capacitive Accelerometer . . . . . . . . . . . . . . . . . . . . . . 23
4.4 Range Finder working Principle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.5 Nonlinear Behavior of the Range Finder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.6 Identication of the Range Finder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.1 Conguration of the Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
5.2 Electrical and Mechanical Systems of the Motor . . . . . . . . . . . . . . . . . . . . . . . . . 32
5.3 Identication of the Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.4 Determination of the Time Constant of the Motor Set . . . . . . . . . . . . . . . . . . . . . . 35
7.1 Simulink Implementation of the System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.2 Takeoff and Landing Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.3 Implementation of the Model of the Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.4 Implementation of the Model of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
7.5 Implementation of the Complete Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
7.6 Open-Loop Response of the System. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
8.1 Reference Set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
8.2 Full-State Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
8.3 Full-state Control Response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
8.4 Closed Loop Poles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
8.5 Closed Loop step Response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
xv
9.1 Block Diagram of the Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
9.2 Complementary Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
9.3 Attitude Estimation with Complementary Filters . . . . . . . . . . . . . . . . . . . . . . . . . 60
9.4 Passive Filter with Bias Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
9.5 Mathematical Formulation of the Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 63
9.6 Block Diagram of the Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
9.7 Bloc Diagram of the EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
9.8 Attitude Estimation with Kalman Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
10.1 Stabilization via Continuous LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
10.2 Continuous Closed Loop Response to Initial Conditions . . . . . . . . . . . . . . . . . . . . 71
10.3 Closed Loop Response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
10.4 Stabilization via Discrete LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
10.5 Discrete Closed Loop Response to Initial Conditions . . . . . . . . . . . . . . . . . . . . . . 73
10.6 Discrete Closed Loop Poles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
10.7 Attitude Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
10.8 Attitude Stabilization with Initial Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
10.9 Communication Set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
10.10Implementation of the Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
A.1 Frame of the Prototype . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
A.2 Propellers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
A.3 Motor and Speed Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
A.4 Arduino Diecimila . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
A.5 Xbee and Shield . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
A.6 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
A.7 Board and Battery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
A.8 Sensors Connections . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
A.9 Actuators Connections . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
B.1 Determination of the Center of Mass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
B.2 Determination of the Center of Mass along u
z
axis . . . . . . . . . . . . . . . . . . . . . . . 104
xvi
List of Tables
4.1 Gyroscopes Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.2 Accelerometers Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.3 Compass Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.4 Range Finder Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.5 Noise Characteristics of the Accelerometers . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.6 Noise Characteristics of the Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.7 Noise Characteristics of the Compass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
5.1 Dead Zone of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.2 Static Gain of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.3 Polynomial Approximation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
6.1 Dead Zone of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
8.1 Closed Loop Poles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
8.2 Rising and Settling Time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
9.1 Comparison of Complementary Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
9.2 Comparison of Kalman Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
10.1 Stabilization Closed Loop Poles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
10.2 Comparison of Continuous and Discrete Controllers . . . . . . . . . . . . . . . . . . . . . . 72
10.3 Attitude Error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
10.4 Attitude Estimation Error with null Reference . . . . . . . . . . . . . . . . . . . . . . . . . . 78
10.5 Attitude Estimation Error with different References . . . . . . . . . . . . . . . . . . . . . . . 79
10.6 Heading Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
10.7 Height Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
xvii
xviii
Notation
The following notation is used throughout this work. The variables are listed by order of appearance.
Acronyms
UAV - Unmanned Aerial Vehicle
MAV - Micro Aerial Vehicle
VTOL - Vertical Takeoff and Landing
NED - North-East-Down, assumed as inertial frame
ABC - Aircraft-Body-Centered, assumed as body xed frame
PWM - Pulse Width Modulation
MEMS - Micro Electro Mechanical Systems
IMU - Inertial Measurement Unit
AD - Analog-Digital Converter
List of Variables
(U
N
, U
E
, U
D
) North, East and Down axes of the NED frame
O Origin of the NED frame
(u
x
, u
y
, u
z
) x, y and z axes of the ABC frame
Oc Origin of the ABC frame
P
I
Position vector of O
c
in the NED frame, P = [X, Y, Z]
T
V
B
Velocity vector, measuring the linear velocity of the craft in the ABC frame,V = [U, V, W]
T

B
Representation of the Euler Angles, = [, , ]
T
Angular velocity of the craft, in ABC frame, = [P, Q, R]
T

B
Angular speed of the propellers; = [
1
,
2
,
3
,
4
]
m Mass of the prototype
I Inertia matrix with diagonal: (I
x
, I
y
, I
z
)
d
cm
Distance from the center of mass to the motor, in the u
x
u
y
plane (m)
h
feet
Distance between the base of the feet to the center of mass of the Quadrotor along u
z
d
feet
Distance between the base of the feet to the center of mass of the Quadrotor along u
x
or u
y
xix
R()
T
Roll rotation
R()
T
Pitch rotation
R()
T
Yaw rotation
S Transformation matrix from NED frame to ABC frame
S
q
Transformation matrix in the quaternion form
q Quaternion element ; q = [q
0
, q
1
, q
2
, q
3
]
T
F
B
Forces applied to the quadrotor (excluding gravity); F
B
= [F
x
, F
y
, F
z
]
T
F
i
Thrust produced by motor number M
i
M
B
Moments applied on the quadrotor in the ABC frame; M
B
= [M
x
, M
y
, M
z
]
T
a
B
Acceleration vector, in the body-xed frame: a
B
= [

U,

V ,

W]
T
g
I
Gravity vector dened in the inertial frame;
g
B
Gravity vector dened in the body-xed frame
g
0
Value of the gravitational constant
K
spring
Spring constant of the undercarriage model
K
damp
Damping constant of the undercarriage model
Damping Ratio of the undercarriage model

0
Natural frequency of the undercarriage model
z
i
Coordinate along U
D
of the foot under motor M
i
z
i
Vertical speed of the foot under motor M
i
x
i
Linear velocity along u
x
of the foot under motor M
i
y
i
Linear velocity along u
y
of the foot under motor M
i
f
c
Friction coefcient between the ground and the quadrotor
F
floor
Ground reaction on the quadrotor; F
floor
= [F
floor
x
, F
floor
y
, F
floor
z
]
T
M
floor
Moments applied on the quadrotor in the ABC frame; M
floor
= [M
floor
x
, M
floor
y
, M
floor
z
]
T
K Constant which relates thrust and torque produced by a propeller

B
Measured angular rates, in the body-xed frame;
B
= [g
x
, g
y
, g
z
]
T

g
Gaussian measurement noise of the gyroscope;
g
= [
gx
,
gy
,
gz
]
T
b
g
Slowly time-varying bias of the gyroscope; b
g
= [b
gx
, b
gy
, b
gz
]
T
r Location of the IMU relative to the center of mass of the prototype; r = [r
x
, r
y
, r
z
]
T
a
B
Measured acceleration vector, in the body-xed frame; a
B
= [a
x
, a
y
, a
z
]
T

a
Gaussian measurement noise of the accelerometer;
a
= [
ax
,
ay
,
az
]
T
b
a
Constant bias of the accelerometer
N
B
Measured magnetic vector, dened in the body-xed frame; N
B
= [N
x
, N
y
, N
z
]
N
I
Magnetic vector, dened in the inertial frame

m
Gaussian measurement noise of the compass;
m
= [
mx
,
my
,
mz
]
T
b
m
Bias of the compass; b
m
= [b
mx
, b
my
, b
mz
]
T
z
B
Measured height of the quadrotor
b
z
Bias of the range nder

z
Gaussian noise of the range nder
AccRes Resolution of the accelerometer
xx
GyroRes Resolution of the gyroscope
MagRes Resolution of the compass
RangeFinderRes Resolution of the range nder
freq Considered sampling frequency of the sensors
K
T
Constant which relates thrust and angular velocity of the propeller
K
M
Constant which relates torque and the angular velocity of the propeller
T
i
Thrust produced by the propeller on motor M
i
M
zi
Torque around u
z
produced by the propeller on motor M
i
D Diameter of the propeller
Density of the air
c
T
Thrust coefcient
c
P
Power coefcient
PWM PWM signal sent; PWM= [PWM
1
, PWM
2
, PWM
3
, PWM
4
]
k
i
Static gain of the model of motor i
J Moment of inertia of the rotors
b Mechanical damping ratio of the motors
L Electric inductance of the motors
re Electric resistance of the motors
Time constant of the model of the motors
X State vector of the quadrotor
U Input vector of the quadrotor system
Y Output vector of the quadrotor system
g() Dynamic equation of the nonlinear state space model
h() Output equation of the nonlinear state space model
X
k
Discrete state vector of the quadrotor
U
k
Discrete input vector of the quadrotor system
Y
k
Discrete output vector of the quadrotor system
A, B, C, D Matrices of the linear state space system
A
d
, B
d
, C
d
, D
d
Matrices of the discrete linear state space system
X
0
Linearization point

0
i
Angular velocity of the propeller at the linearization condition.
PWM
d
i
PWM dead zone of motor M
i
PWM
0
i
Input linearization point
PWM
deq
i
Equivalent PWM dead zone
U
trim
Trim condition regarding the input vector
Y
trim
Trim condition regarding the output vector
X
trim
Trim condition regarding the state vector
T = 1/freq Time interval
J
LQR
Cost function of the LQR controller
X
ref
Reference State
xxi
E Error vector; E = X
ref
X
K
lqr
Optimal gain matrix
Q
lqr
and R
lqr
Weighting matrices

X Estimated state vector;

X = [

W,

P,

Q,

R,

Z,

,

,

]
T

X
acc
Estimated state vector based on accelerometers;

X
acc
= [

,

]
T

X
gyro
Estimated state vector based on the integration of the gyroscopes;

X
gyro
= [

,

]
T
T(s) General low pass lter of the complementary lter
S(s) General high pass lter of the complementary lter
T
c
Cut-off time of the generic high or low pass lter
K
k
Kalman gain of the Kalman lter
Q
k
and R
k
Weighting matrices of the Kalman lter
Superscripts
B Dened in the Body-Fixed frame (ABC frame)
I Dened in the Inertial frame (NED frame)
xxii
Chapter 1
Introduction
Ever since their appearance in the 1980

s, Unmanned Aerial Vehicles (UAV) have been the subject of

interest for the military. The Israeli and US military were amongst the rst to recognize the advantage of
the concept of aerial vehicles either remotely controlled or totally autonomous.
Research and investment in these machines were bolstered by the advance of miniaturization, maturing
of the technologies, more powerful processors and more reliable and cheaper sensors; thus motivating the
creation of innovative vehicles in the private sector and in Universities. That miniaturization favored the
creation of mini UAV or micro UAV (MAV), weighting less than a kilogram; in particular, some universities
turned their attention to the potential of Vertical Takeoff and Landing (VTOL) vehicles, which are used in
situations where traditional xed-wing crafts are unable to operate properly. As an example one might think
of rescue operations or goods delivery in restricted areas. In those situations vehicles such as helicopters
or quadrotors are essential.
1.1 Context and Motivation
The concept of a quadrotor is actually not new; amongst the rst sketches of rotorcrafts, appeared the
rst ideas for the concept of quadrotors. In 1907, Louis and Jacques Breguet associated to Professor
Charles Richet developped the Gyroplane No.1, propelled by four 4-blade biplane rotors mounted on the
extremity of a cross-shaped structure. To obtain stability, the rotorcraft counted on diagonally opposed
rotors to rotate in opposite directions thus being able to cancel the torque produced by each pair with the
other pair of rotors. Although able to achieve lift, the stability necessary to consider it a proper ight was
not attained and the short 60cm ight was only made possible thanks to the four arms supporting and
stabilizing the craft. Later that year, a free ight was accomplished, reaching the height of over a meter.
However the pilot had no steering nor forward propulsion means and the gyroplane could not be considered
to be controllable nor perfectly stabilized.
In the 1920s, Etienne Oehmichen underwent several experiments concerning rotorcrafts. His second
prototype (see Figure 1.1(a)), baptized Oehmichen No.2 had four rotors and eight propellers mounted on
a cross shaped frame. Five propellers were used to obtain the lift and lateral stability thanks to the change
in the angle of the blades, two were used to give horizontal propulsion and an additional propeller was
used to steer the vehicle. With this conguration ights of several minutes were made possible and a 1Km
close-circuit controlled ight was achieved.
1
(a) Etienne Oehmichens prototype. (b) Bothezats sketch. (c) Converting Model A.
Figure 1.1: Development of the Quadrotor Concept.
In January 1921, the US Army had Dr. George de Bothezat and Ivan Jerome working on a vertical
ight machine. The result was a quadrotor with six-blade rotors (see Figure 1.1(b)). Two small propellers
with pitch control were used for stability and controlling the craft. This prototype proved that the vertical
ight was theoretically and practically possible despite its poor performance reaching only 5m high, being
underpowered, unresponsive and mechanically complex thus unreliable.
The quadrotor development was then abandoned in favor of research for what have become our tra-
ditional helicopters. Only in 1956 would the concept be refreshed with the Convertawings Model A (see
Figure 1.1(c)); inspired by both Oehmichen and Bothezat, a four rotor aircraft was built. With proper power
and a simplied control mechanism, the vehicle ew successfully.
The quadrotor solution for a VTOL carrying large payloads has been carried on rather discretely in the
past decades, mainly sponsored by military funds betting on alternatives to traditional helicopters. The
2006 Bell Boeing Quad TiltRotor, seen in Figure 1.2(a), was inspired by the Bell X-22 and Curtiss-
Wright X-19 (see Figure 1.2), two experimental VTOL tiltrotor airplanes of the early 1960s. They present
the particularity of tilting the rotor axes to obtain more or less horizontal propulsion and use wing surfaces
to contribute to the lift of the craft. This aircraft was designed for military purposes, for transporting up
to 9100kg at takeoff. However the project is not nished and research is still going on to meet new re-
quirements concerning increased payloads and sizes. The vehicle presents the advantage of landing on
unimproved sites thanks to its VTOL capabilities.
(a) Bell Boeing Quad TiltRotor. (b) Curtiss-Wright X-19. (c) Bell X-22.
Figure 1.2: Further Development of the Quadrotor Concept.
At the same time that military organizations encourage the creation of larger and heavier quadrotors, the
interest in unmanned quadrotors has been growing in the academic milieu, where these vehicles appear
as challenging platforms to develop new solutions for control, estimation, communication problems, 3D
orientation and navigation algorithms. The attention given to these objects has been increasing thanks
to miniaturization and the development of cheaper components. With these platforms the algorithms can
be tested, improved and can then be implemented on larger objects. For example one might think of the
2
problem of having a ying object autonomously landing on a moving platform as developed by the Aalborg
University (see [10]). The idea could be applied to helicopters landing on aircraft carriers.
The private sector has also found applications for quadrotors. As an example one might think of
UAVison
R
, a portuguese company which entered a growing market with its U4 QuadCopter shown in
Figure 1.3(a), thinking of quadrotors not only as military devices but nding some civilian applications.
For instance, the company proposes the usage of these vehicles as a broadcasting tool, accessing dif-
cult angles for image acquisition in sport or outdoors events or even as an aerial surveillance tool for law
enforcement, among others
1
.
(a) UAVision
R
prototype: U4 QuadCopter. (b) Aggressive Maneuvers.
Figure 1.3: Quadrotors in the Private Sector and in Universities.
Quadrotors are vehicles capable of aggressive maneuvers allowing them to t a large set of applica-
tions
2
. As an example, Figure 1.3(b) presents a quadrotor passing through a window, demonstrating the
potential of this vehicle. It becomes clear that the concept of the platform is promising and allows variations.
However it is important to note that each set of sensors, each set of actuators may imply a very different
approach, therefore justifying the continuous improvements and research dedicated to this area.
1.2 Objectives and Contributions
Jorge Domingues, a former Mechanical Engineering student, proposed to control a quadrotor prototype
based on data acquisition from a three-axes accelerometer and a two-axes compass [7]. An estimator was
used to reconstruct the attitude and a controller was developed to stabilize the vehicle. Unfortunately it
was concluded that the used sensors were insufcient due to the noise corrupting the readings and it was
assessed that gyroscopes were needed.
The scope of this work is to continue the work of Jorge Domingues, creating a functional platform where
different control and estimation approaches can be tested and compared. The platform is considered to be
functional if able to sustain an autonomous hovering ight with small drift in position and a stabilized ight
when remotely controlled. This is to be achieved based solely on inexpensive sensors and actuators.
In order to reach the objectives, different steps have to be reached:
A model of the prototype is developed and implemented, with measured and estimated parameters.
A model of the sensors is also developed and tuned using real data from the sensors. For this
purpose an interface program between the prototype and the ground base is adapted.
1
More information is available at http://www.uavision.com
2
The video from http://www.youtube.com/watch?v=MvRTALJp8DM&feature=player_embedded, shows a quadrotor performing
agressive manoeuvers, having it passing through narrow openings
3
The actuators are modeled using a tachometer for greater precision.
Different estimation methods are implemented in simulation and tested with real data from the sen-
sors. A discussion about the performance of the different methods is presented. The chosen method
is also implemented and tested on the prototype.
A control method is presented and tested in simulation allowing the loop to be closed with the esti-
mated attitude.
1.3 Structure of the Dissertation
The work is divided into two parts. The rst part deals with the modeling of the quadrotor, presenting
the conventions used throughout the work, the dynamics and kinematics describing the quadrotor motion
and the models of the sensors and actuators, including experimental tests to evaluate the parameters of
the models. The choice of the sensors is explained and the equations are linearized and assembled to form
the state space model, which is analyzed regarding its open loop stability and implemented in a simulator.
In the second part, a control methods is presented and implemented in Simulink to generate data
useful for estimation purposes. Then different estimation approaches are discussed and compared using
the simulations. Then the control with estimation feedback is presented. The chosen estimator is also
tested on the prototype to verify its behavior.
Finally, the conclusions are drawn and some possible improvements for the stabilization of the quadrotor
attitude are presented and left for future work.
4
Part I
Modeling the Dynamics of a Quadrotor
5
Chapter 2
Denitions
In this part, the model of the quadrotor is introduced; this allows the creation of a model based controller
and estimation algorithms.
In this chapter some general characteristics concerning conventions and frames, in which the model is
dened, are introduced. Then the general concept of the quadrotor and how it maneuvers is explained.
Finally, some simplifying assumptions are considered.
2.1 Conventions and Characteristics of a Quadrotor
2.1.1 Coordinate Systems
The available quadrotor prototype can be seen in Figure 2.1(b); it has a red arm, which makes the visual
tracking of the attitude easier. The four motors are numbered as indicated. To describe the movement of
the quadrotor and its attitude, two right-hand frames are used; a reference frame, called NED frame and
centered on O, is set on the ground and points towards North, East and Down, as shown in Figure 2.1(a).
This frame is considered to be inertial. The second frame is xed on the quadrotor and centered on Oc,
the center of mass of the quadrotor, thus called Body-xed frame or Aircraft-Body-Centered frame, ABC
frame, shown in Figure 2.1(b). Vectors expressed in the inertial frame are marked with the superscript
I
and vectors expressed in the body xed frame have the superscript
B
.
U
N
U
E
U
D
O
(a) Inertial Frame
M
1
M
2 M
3
M
4
u
x
u
y
u
z
Oc

(b) Body Fixed frame
Figure 2.1: Inertial and Body-Fixed Frames.
The position of the quadrotor, denoted P, corresponds to the displacement from O to Oc:
P
I
= [X, Y, Z]
T
(2.1)
7
The velocity of the quadrotor is expressed in the body-xed frame:
V
B
= [U, V, W]
T
(2.2)
The rotation of the ABC frame relatively to the NED frame denes the attitude of the aircraft. According
to Eulers rotation theorem, any rotation can be described using three angles. In aeronautic literature these
three rotations are usually , and :
= [, , ]
T
(2.3)
The angular velocity is measured in the body-xed frame:

B
= [P, Q, R]
T
(2.4)
where: P corresponds to the angular velocity about the u
x
axis; Q corresponds to the angular velocity
about the u
y
axis; R corresponds to the angular velocity about the u
z
axis.
2.1.2 Maneuvering a Quadrotor
Both quadrotors and helicopters are rotor-craft vehicles able to hover. The advantages of the quadrotors
over the helicopters are not obvious. In this section the concept of quadrotors is further analyzed and its
maneuvering capabilities are described.
Standard helicopters have two rotors, the main one, located over the vehicle, produces the lift. The
second rotor is located on the tail and cancels the torque produced by the main one. This allows the
helicopter to yaw by simply changing the velocity of the tail rotor. To pitch or roll, the helicopter is equipped
with a complex system which changes the angle of attack of the blades of the main rotor.
Quadrotors have four identical rotors and the propellers have a xed angle of attack. As shown on
Figure 2.1 the blades are paired and each pair rotates in a different direction. Motors M1 and M3 have a
clockwise rotation when looked from above whilst motors M2 and M4 have a counter-clockwise rotation. To
obtain the maneuvers depicted in Figure 2.1(b) the speed of each motor is adjusted. The angular speeds
of the motors are written = [
1
,
2
,
3
,
4
]
T
. Figure 2.2 presents these adjustments and their respective
effect.
8




(a) (c) (e) (g)
(b) (d) (f) (h)

(a) Positive Roll (e)
Positive Yaw (b) Negative Roll (f)
Negatice Yaw
(c) Positive Pitch (g) Takeoff
(d) Negative Pitch (h) Land
2
1
4
3
Figure 2.2: Quadrotor Rotation System (from [3]).
The arrows in Figure 2.2 characterize the angular speed of the motors.
Roll-:
The rolling motion corresponds to a rotation of the quadrotor about the u
x
axis; it is obtained when
2
and
4
are changed. For a positive rolling,
4
is decreased while
2
is increased. The contrary will produce
a negative rolling action; both are depicted in cases (a) and (b).
Pitch-:
The pitch motion corresponds to a rotation of the quadrotor about the u
y
axis; it is obtained when
1
and
3
are changed. For a positive pitch,
3
is decreased while
1
is increased. The contrary will produce
a negative pitch action; both are depicted in cases (c) and (d).
Yaw-:
The yaw motion corresponds to a rotation of the quadrotor about the u
z
axis; it is produced by the
difference in the torque developed by each pair of propellers. Since the propellers are paired, two create
a clockwise torque and two an anticlockwise one; by varying the angular speed of one pair over the other,
the net torque applied to the aircraft changes which results in the yaw motion as shown in cases (e) and
(f).
Translational motion:
To perform a vertical takeoff and landing, quadrotors must be able to move vertically. As shown in
cases (g) and (h), this movement is obtained by equally augmenting or diminishing the angular speed of all
motors. Positive pitch or roll angles produce respectively negative and positive translational motion along
u
x
and u
y
.
9
This indicates as well that the quadrotor is an underactuated vehicle since there are four controllable
degrees of freedom (the three angles of attitude and the vertical motion) in a six-degrees of freedom space.
The general features of the quadrotors movement have now been explained. The hardware of the
prototype is presented in Appendix A.
2.2 Prototype Identication
In this section some characteristics of the prototype are estimated, namely the mass, the center of
mass and the inertia matrix.
A scale measures the total mass: m = 0.976kg. To estimate the center of mass, it is remembered that
when hung by an extremity a rigid body has its center of mass aligned in the vertical of the hanging point.
A weight attached to a string is used to mark the vertical and pictures are taken and analyzed, as shown
in Appendix B.
Most of the heavier components respect the symmetry of the prototype. By hanging the prototype as
shown in Figure 2.3, Figures B.1(a) and B.1(b) from Appendix B it is conrmed that the center of mass is
located on the intersection of the u
x
and u
y
axes.
vertical
hanging point
M2
M3
M4
M1
u
x
u
y
Figure 2.3: Determination of the Center of Mass.
To dene the location of the center of mass along the u
z
axis, the prototype is hung as depicted in
Figure 2.4. Different hanging points are considered for a greater condence in the result. In Appendix B
the pictures are presented and the procedure used to dene the center of mass is explained.
10
M2
M4
M1
vertical
hanging point
Figure 2.4: Determination of the Center of Mass along u
z
.
The result of the experiment shows that the center of mass is located h
feet
5.9cm above the lowest
tip of the prototype, namely the feet. The result of 5.2cm obtained in a previous work concerning the same
prototype differs from the obtained now possibly because of changes in the prototype.
Regarding the inertia matrix of the prototype, the values obtained at [7] were considered accurate.
According to [23], it is acceptable to consider that the matrix is diagonal because of the symmetry of the
prototype:
I =
_

_
I
x
0 0
0 I
y
0
0 0 I
z
_

_
=
_

_
0.0081 0 0
0 0.0081 0
0 0 0.0162
_

_
kg.m
2
(2.5)
The relevant dimensions of the prototype are shown in Figure 2.5:
d
cm=0.29
O
c
h
f
e
e
t
=
0
.
0
5
9
d
feet=0.20
Figure 2.5: Dimensions of the Prototype.
2.3 General Assumptions
Considering the purpose of stabilizing the quadrotor, several general assumptions can be introduced.
These assumptions simplify the model.
In hover, the accelerations of the quadrotor can be neglected.
The quadrotor is symmetric along u
x
and u
y
.
The quadrotor is a rigid body.
All aerodynamic forces acting on the quadrotor are neglected.
The magnetic inclination is neglected.
The rotors apping effect is neglected.
The ground effect is neglected on takeoff and landing situations.
All the sensors composing the IMU are considered to be at its center and on u
z
.
Nonlinearities of the battery will be neglected.
There is no slippering between the propeller and the rotor of the motor.
11
All motors have the same time constant.
12
Chapter 3
Quadrotor Model
The equations of motion of the system represented by the block diagram in Figure 3.1 can be divided
into two parts, the kinematics equations and the dynamics equations. In this chapter, these equations are
presented and explained. To complete the dynamics model, the ground reaction is presented as well.
Rigid Body Dynamics
and Kinematics
Ground
Reaction
Integrator
V,,,P
F
B
=[F
x
,F
y
,F
z
]
M
B
=[M
x
,M
y
,M
z
]
M
oor
F
oor
+
+
+
+
Figure 3.1: Dynamics and Kinematics Block Diagram.
3.1 Kinematics and Dynamics Equations
3.1.1 Quaternions and Euler Angles
Since the position of the vehicle is expressed in the inertial frame and its velocity is expressed in the
body-xed frame, it becomes necessary to be able to pass from one frame to the other.
The rotations expressed by Euler angles are not commutative and according to [17] they must be
applied as a sequence of rotations. In this work the convention is roll, pitch, yaw: to bring the inertial
frame coincident with the body-xed one, the rst rotation corresponds to yaw (R()
T
), then pitch (R()
T
)
and nally roll (R()
T
) (see equation (3.1)). This corresponds to describe the orientation of the body-xed
frame relative to the inertial one.
13
R()
T
=
_

_
1 0 0
0 cos sin
0 sin cos
_

_
, R()
T
=
_

_
cos 0 sin
0 1 0
sin 0 cos
_

_
, R()
T
=
_

_
cos sin 0
sin cos 0
0 0 1
_

_
(3.1)
Finally combining those three rotations results in applying equation (3.2):
S = R()
T
R()
T
R()
T
=
_

_
cos cos cos sin sin
cos sin sin cos sin sin sin sin + cos cos cos sin
cos cos sin + sin sin cos sin sin cos sin cos cos
_

_
(3.2)
where: S is the rotation matrix which expresses a vector from the inertial frame to the body-xed frame.
When = /2, the rotation matrix becomes equation (3.3) and reveals a singularity:
S =
_

_
0 0 1
cos sin cos sin sin sin + cos cos 0
cos cos + sin sin cos sin cos sin 0
_

_
=
_

_
0 0 1
sin( ) cos( ) 0
cos( ) sin( ) 0
_

_
(3.3)
This phenomenon is called the gimbals lock and corresponds to the loss of a degree of freedom in a
three-dimensional space when two gimbals start spinning in the same plane. As seen in equation (3.3),
a change in or in has the same effect, thus a situation of ambiguity is created: one notation may
represent two orientations. To avoid this ambiguity different methods may be used.
Instead of using the Euler angles (roll,pitch,yaw), a quaternion-based method can be applied. A quater-
nion is an extension of complex numbers with two additional complex dimensions: q = [q
0
, q
1
, q
2
, q
3
]
T
.
Equation (3.4) denes the representation of a rotation in the quaternion form and relates it to the Euler
angles representation:
q = [q
0
, q
1
, q
2
, q
3
]
T
=
_

_
cos(/2) cos(/2) cos(/2) + sin(/2) sin(/2) sin(/2)
sin(/2) cos(/2) cos(/2) cos(/2) sin(/2) sin(/2)
cos(/2) sin(/2) cos(/2) + sin(/2) cos(/2) sin(/2)
cos(/2) cos(/2) sin(/2) sin(/2) sin(/2) cos(/2)
_

_
(3.4)
Both quaternions and Euler angles represent the same attitude and one can convert a quaternion into
Euler angles or vice-versa. As a result, according to [17] and [11], the rotation matrix can be written as in
equation (3.5):
S
q
=
_

_
1 2(q
2
2
+q
2
3
) 2(q
1
q
2
+q
3
q
0
) 2(q
1
q
3
q
2
q
0
)
2(q
1
q
2
q
3
q
0
) 1 2(q
2
1
+q
2
3
) 2(q
2
q
3
+q
1
q
0
)
2(q
1
q
3
+q
2
q
0
) 2(q
2
q
3
q
1
q
0
) 1 2(q
2
1
+q
2
2
)
_

_
(3.5)
The rotation matrix presents the particularity of having its inverse identical to its transpose. This simpli-
es the calculations and can be written as: S
1
= S
T
.
The quaternion formulation is used in this work for computational purposes due to its advantage re-
garding the singularity. The Euler angles are used for their simpler understanding.
3.1.2 Quaternion Formulation
The vector P
I
= [X, Y, Z]
T
denes the position of the quadrotor relative to the inertial frame and V
B
=
[U, V, W]
T
denes the linear velocity of the quadrotor in the body-xed frame. Equation (3.6) expresses the
14
existing relation between the two vectors:
_

Z
_

_
= S
T
q
_

_
U
V
W
_

_
(3.6)
According to [17] and [18], equation (3.7) relates the angular velocities
B
= [P, Q, R]
T
with the quadro-
tor attitude:
_

_
q
0
q
1
q
2
q
3
_

_
=
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
_

_
(3.7)
As referred in section 2.3, the aerodynamic forces applied to the quadrotor are neglected. The only
forces considered are the thrust produced by the propellers and gravity.
Each propeller produces a force along u
z
, written [F
1
, F
2
, F
3
, F
4
]
T
; all have the same direction and one
can write the net force applied to the quadrotor (excluding gravity) as: F
B
= [F
x
, F
y
, F
z
]
T
= [0, 0,

4
i=1
F
i
]
T
.
These forces produce moments around the axes of the quadrotor. The net moments are written M
B
=
[M
x
, M
y
, M
z
]
T
. In Chapter 5, the model of the propellers is introduced and these forces and moments are
properly explained.
According to [18], the dynamics of the quadrotor concerning the rotations are given by: I

=
I+M
B
, which leads to:
_

R
_

_
=
_

_
M
x
I
x
M
y
I
y
M
z
I
z
_

_
(I
z
I
y
)QR
I
x
(I
x
I
z
)PR
I
y
(I
y
I
x
)PQ
I
z
_

_
(3.8)
where: = [P, Q, R]
T
is the angular velocity; I = diag(I
x
, I
y
, I
z
) is the inertia matrix.
Applying the second law of Newton, the acceleration of the quadrotor in the body xed frame results in:
ma
B
= F
B
+mS
q
g
I
mV
B
(3.9)
where: a
B
=

V
B
= [

U,

V ,

W]
T
is the acceleration vector in the body-xed frame; g
I
= [0, 0, g
0
]
T
is the
gravity vector, g
0
= 9.81m/s
2
.
Thus:
_

W
_

_
=
1
m
_

_
F
x
F
y
F
z
_

_
+g
0
_

_
2(q
1
q
3
q
2
q
0
)
2(q
2
q
3
+q
1
q
0
)
1 2(q
2
1
+q
2
2
)
_

_
QW RV
RU PW
PV QU
_

_
(3.10)
3.1.3 Euler Angles Formulation
The angular velocity vector
B
does not correspond to the Euler angles rates

= [

,

,

]
T
but one
can write equation(3.11):
_

_
P
Q
R
_

_
= R()
T
R()
T
R()
T
_

_
0
0

_
+R()
T
R()
T
_

_
0

0
_

_
+R()
T
_

0
0
_

_
(3.11)
15
Developing the equation and solving for the Euler angles rates results in equation(3.12):
_

_
=
_

_
1 tan sin tan cos
0 cos sin
0 sin / cos cos / cos
_

_
_

_
P
Q
R
_

_
(3.12)
The dynamics equation concerning the rotation of the quadrotor is the same as equation (3.8).
3.2 Model of the Ground Reaction
In this section the model used to simulate the ground reaction is developed.
The quadrotor only touches the ground with the four feet located under each arm of the aircraft. Let h
feet
=
0.059m be the vertical distance between the base of the feet and the center of mass of the quadrotor. To
account for the forces of the ground on those feet, a linear spring and damper system is considered for
each foot.
Knowing that the spring makes a force contrary and proportional to its deformation, a maximumdeformation
of 1mm is allowed when the quadrotor hits the oor. Equation (3.13) comes from the equilibrium from the
forces of the four springs and the weight of the quadrotor.
mg
0
= 4K
spring
0.001
K
spring
= 0.001mg
0
/4
(3.13)
where: K
spring
is the spring constant. It results that K
spring
= 2393N/m.
The ground reaction is modeled as a damped mass-spring system; a damping ratio of = 0.9 is
assumed. To calculate the damper coefcient K
damp
equation (3.14) is used:
X
F
=
1/m
s
2
+K
damp
/ms +K
spring
/m
=
1/m
s
2
+ 2
0
s +
2
0
=
K
damp
2m
0

0
=
_
K
spring
m
(3.14)
where: is the damping ratio of the ground reaction model;
0
is the natural frequency of the ground reac-
tion model; F is a force applied to the mass-spring system and X the Laplace transform of its location.
It results from equation (3.14) that K
damp
87N.s/m.
16
The positions of the four feet relative to the inertial frame are given by equation (3.15):
z
1
= Z d
feet
sin +h
feet
cos
z
2
= Z +d
feet
sin +h
feet
cos
z
3
= Z d
feet
sin +h
feet
cos
z
4
= Z +d
feet
sin +h
feet
cos
(3.15)
where: z
i
is the height of the foot under motor M
i
; Z is the height of the center of mass of the quadrotor;
d
feet
is the distance from the center of mass to the feet along u
x
or u
y
and h
feet
corresponds to the length
of the feet along u
z
.
Equation (3.16) denes the linear velocity of the feet:
z
1
= W d
feet
Qcos
z
2
= W +d
feet
Qcos
z
3
= W d
feet
P cos
z
4
= W +d
feet
P cos
(3.16)
where: z
i
is the vertical speed of the foot under motor M
i
.
Since the elastic force is proportional to the compression of the spring and the damper force is propor-
tional to the linear velocity, equation (3.17) results:
F
floor
z,i
= (z
i
K
spring
+ z
i
K
damp
) (3.17)
For the sake of simplicity, the forces are considered as applied in a collinear vector to u
z
passing
through the contact point with the ground. The forces along u
x
and u
y
are opposed to the movement and
correspond to the force of friction; equation (3.18) denes the linear velocity of each foot along u
x
and u
y
:
x
1
= U
x
2
= U
x
3
= d
feet
R +U
x
4
= d
feet
R +U
y
1
= d
feet
R +V
y
2
= V
y
3
= d
feet
R +V
y
4
= V
(3.18)
where: x
i
is the linear velocity along u
x
of the foot under motor M
i
; y
i
is the linear velocity along u
y
of the
foot under motor M
i
.
The forces of friction along u
x
and u
y
are dened by equation (3.19) where the friction coefcient is set
to f
c
= 0.6 according to [12]:
17
F
floor
x,i
= |F
floor
z,i
|f
c
sign( x
i
)
F
floor
y,i
= |F
floor
z,i
|f
c
sign( y
i
)
for i = 1, 2, 3, 4
(3.19)
It results:
F
B
=
__
4

i=1
F
floor
x,i
_
,
_
4

i=1
F
floor
y,i
_
,
_

i=1
F
i
+
4

i=1
F
floor
z,i
__
T
. (3.20)
where: F
i
is the force produced by the propeller of motor M
i
; F
floor
z,i
is the z-component of the ground
reaction on the support under motor M
i
.
Considering that landing is to be performed vertically, the moments M
floor
resulting from the ground
reaction can be written as:
M
floor
=
_

_
(F
floor
z,4
F
floor
z,2
)d
feet
(F
floor
z,3
F
floor
z,1
)d
feet
(F
floor
y,1
+F
floor
x,2
F
floor
y,3
F
floor
x,4
)d
feet
_

_
(3.21)
Finally, the net moments acting on the quadrotor can be expressed by equation (3.22), considering the
ground reaction and the motors:
M
x
= (F
2
F
4
)d
cm
+ (F
floor
z,4
F
floor
z,2
)d
feet
M
y
= (F
1
F
3
)d
cm
+ (F
floor
z,3
F
floor
z,1
)d
feet
M
z
= (F
1
F
2
+F
3
F
4
)K + (F
floor
y,1
+F
floor
x,2
F
floor
y,3
F
floor
x,4
)d
feet
(3.22)
where: K is a constant which relates the thrust produced by the motor with the moment developed. The
constant will be explained in chapter 5.
In this chapter, the dynamics and kinematics of the quadrotor have been presented considering both the
Euler angles and the quaternion form to avoid the singularity inherent to the Euler angles. Then based on
a chosen damping ratio, the characteristics of the ground reaction were derived and dened. The ground
reaction affects the dynamics of the quadrotor introducing extra terms. In the following chapters the model
of the sensors and actuators will be derived explaining the origin of the variable K in equation (3.22).
18
Chapter 4
Sensors
In this chapter, a short description of the choice of the sensors is given and their principle of operation
are explained, allowing a model of the sensors to be created. The noise associated to the sensors is
measured on the prototype for greater accuracy of the simulations.
4.1 Choosing the Sensors
In order to justify the choice of the sensors and explain what the sensors must be sensitive to, a brief
reference to the basic concepts of the estimator must be done.
As explained by Wahbas paper in 1966 [13], the solution for estimating the attitude of a satellite (or
a quadrotor in this case) lies on nding a matrix which brings a rst set of two non-collinear vectors into
the best coincidence with a second set of non-collinear vectors. The important idea is that, for a correct
estimation, the measurements of two non-collinear vectors are needed.
If only one vector is available there will be an ambiguity concerning the position around the measured
vector. For example, a correct estimation for a tethered craft was obtained by means of an accelerometer
and a gyroscope in [9]. However the untethered ight remained unachieved perhaps due to the difculty in
correctly estimating the second vector.
As done by Jorge Domingues [7], the usage of an accelerometer and a compass corresponds to mea-
suring two non collinear vectors, which are the magnetic North vector and the gravity vector. That set of
sensors proved to be insufcient because of noise corruption of the sensor measurements which motivates
the search for additional sources of information regarding the attitude.
According to [11], a classical solution consists of using an AHRS (Attitude Heading Reference System)
in which gyroscopes provide the main information which is updated by a gravity sensor and a magnetic
sensor. This set is also used in [18] and [24] where accelerometers are used for the gravity vector and a
magnetic compass is used for the magnetic vector. Since the prototype had originally a 2-axis compass
and a 3-axis accelerometer, the gyroscopes could be the necessary improvement to achieve a correct
estimation, in addition to a range nder needed to set the hovering height. In consequence the chosen
sensors are:
A 2-axis Compass Honeywell HMC6352.
A 3-axis Accelerometer ADXL330.
A 1-axis Gyroscope LY530ALH for u
z
and a 2-axis Gyroscope LPR530AL for u
x
and u
y
.
19
A Sharp DP2D12 range nder.
The accelerometer and the gyroscope, forming the IMU, come in a single board the Razor 6DOF from
SPARKFUN ELECTRONICS
R
, being easier to install on the prototype. The connections are presented in
Appendix A.
4.2 Model of the Sensors
The sensors are necessary to measure state variables. Naturally, they carry limitations directly linked to
their inherent functioning principle. By understanding it, it becomes possible to reduce the noise affecting
the measurements and guarantee the correct behavior of the quadrotor.
Gyroscope
Thanks to the advance of miniaturization and micro machining techniques, MEMS (Micro Electro Me-
chanical Systems) gyroscopes were created. These small electronic devices are divided into different
categories according to their sensing elements but most use the Coriolis principle to evaluate an angular
rate. According to [27], the simplest MEMS gyroscopes are vibratory rate gyroscopes like the chosen ones.
The gyroscopes are integrated in the IMU and are designed to measure
B
, the angular velocities of
the quadrotor. The sensor measurement vector is written as:

B
= [g
x
, g
y
, g
z
]
T
(4.1)
The IMU board comprises a LPR530AL sensor to measure angular velocities corresponding to P and Q
and a LY530ALH for R. Both sensors are from STMICROELECTRONICS
R
and present the same dynamics
and governing equations; as suggested by [18] and [8] and conrmed from the sensor data sheet, the
gyroscopes are mostly affected in two ways: a stochastic Gaussian noise component and a slowly time-
varying non-stochastic bias corrupting the readings:

B
=
B
+
g
+b
g
(4.2)
where:
B
is the measured angular rate, dened in the body-xed frame;
B
is the real angular rate,
dened in the body-xed frame;
g
is the Gaussian noise of the gyroscope; b
g
is the slowly time varying
bias.
The sensor LPR530AL data sheet presents a value for the noise density and [18] suggests a value for
the bias. These theoretical characteristics of the sensor are presented in Table 4.1 :
Table 4.1: Gyroscopes Characteristics
measured range 300

/s
sensitivity 3.33mV/(

/s)
noise 0.035(

/s)/

Hz
bias 2.8(

/s)/s
bias varying rate 0.1Hz
resolution 0.9678

/s
20
The resolution is calculated from:
GyroRes =
3.3V
1024 3.33mV/(

/s)
= 0.9678

/s (4.3)
Because the analog signal fromthe sensor is converted to a digital signal, there is an imposed resolution
associated to the number of bits of the processor. In this case, the analog signal is converted to a digital
signal with 2
10
= 1024 intervals. As a consequence, to calculate the resolution, the reference voltage of
3.3V is divided in 1024 intervals. Then knowing that the sensitivity is 3.33mV/(

/s), it is known that one

interval corresponds to 0.9678

/s.
In [1] and [27], it is suggested that the conditions of the environment affect the readings of the sensor.
Indeed, the temperature affects the stiffness of the materials composing the sensor; as a consequence
the gyroscopes are used only in a steady-state condition; it is advised to let the IMU run for a few minutes
before requesting its data.
To better understand the nature of the measurement corruption, it is convenient to understand the
working principle of the sensor; the vibratory gyroscope works based on the Coriolis principle. A small
mass is kept vibrating along one axis imposing a linear velocity v to the mass; when an angular velocity
along a second axis is applied to the system, an alternating force or acceleration, a
gyro
is developed along
the third axis due to Coriolis force as written in equation (4.4), considering the axes of the gyroscope and
the axes of the body-xed frame from Figure 4.1:
body-xed frame
gyroscope
frame
r
z
u
y
u
x
u
z
j
i
k
mass

Figure 4.1: Frame of the Gyroscope.

a
gyro
= 2 v (4.4)
where: a
gyro
is the acceleration resulting of the Coriolis effect. is a generic angular velocity applied to
the gyroscope. v is the linear velocity imposed to the mass.
A more accurate schema of a one axis gyroscope is presented in Figure 4.2. The accelerations resulting
of the Coriolis effect correspond to a force acting on the springs; piezoelectric materials can be used to
transform that alternating force into an electric signal.
The gyroscopes are not located on the center of mass of the quadrotor; one might ask if that will not
cause erroneous readings. All the gyroscopes are considered to be located on the center of the IMU; let
r = [0, 0, r
z
]
T
be the location of the center of the IMU relative to the body-xed frame. In this situation
equation (4.4) can be re-written, dismissing the angular acceleration:
a
gyro
= ( r) + 2 v (4.5)
21
Subtract
Spring Suspension
Anchor
Mass
Figure 4.2: Functioning Principle of a MEMS Gyroscope.[27]
For the LY530ALH sensor, measuring the angular velocity R, a mass is kept vibrating along the i axis
(or j axis) and the acceleration appears along the j axis (or i axis). Since the vertical axis of the
gyroscope coincides with u
z
, having the sensor located out of the center of mass has no effect on the
readings because the rst term of equation (4.5) is canceled.
For the LPR530AL sensor, measuring the angular velocities P and Q, the mass is kept vibrating along
the k axis, as a result the acceleration can be written as:
a
gyro
= 2
_

_
P
0
0
_

_
0
0
v
z
_

_
+
_

_
P
0
0
_

_
_
_
_
_

_
P
0
0
_

_
0
0
r
z
_

_
_
_
_
_
=
_

_
0
2Pv
z
P
2
r
z
_

_
(4.6)
Note that the acceleration has a component along the j axis (or i axis) and the kaxis. Remember
that the gyroscope has the mass vibrating in one direction and measures the acceleration on another one.
Remember now that there are two independent gyroscopes. The LPR530AL sensor has two sensing axes
along the i axis and the j axis but not along the k axis. As a result, r
z
has no effect on the measured
value.
Accelerometer
The accelerometer is used to measure the direction of the gravity vector, g
I
, in order to dene the pitch
and roll angles; let it be taken as constant, pointing down along U
D
with an intensity g
0
= 9.81m/s
2
and let
a
B
denote the accelerometer measurement vector.
There are different types of MEMS accelerometers based on different functioning principles; some
use the piezoelectric effect to create an electric signal due to accelerative forces thanks to micro-crystal
structures, others, as the ADXL335, measure changes in the capacitance. According to [21], capacitive
interfaces present several advantages such as the measurement being independent of the base material.
The basic scheme of a capacitive MEMS accelerometer is presented in Figure 4.3; a tethered mass,
presenting movable plates, subjected to an acceleration moves relatively to xed outer plates; the displace-
ment causes a variation in the capacitance which through signal conditioning is translated into a voltage
variation. This signal conditioning feature is usually incorporated into the sensor board.
This means that the accelerometer is not only sensitive to the gravity but also to accelerations due to
its movement, written a
B
. According to [22], for the chosen frame system, one can write: a
B
= g
B
a
B
.
Introducing a bias Gaussian measurement noise as in [18], the general model can be written as:
22
spring
spring
m
o
v
a
b
l
e

m
a
s
s

w
i
t
h

p
l
a
t
e
s
xed outer plate
C
1
C
2
dx
Figure 4.3: Functioning Principle of a Capacitive Accelerometer.[21]
a
B
= Sg
I
a
B
+
a
+b
a
(4.7)
where: a
B
is the sensor output in m/s
2
; S is the rotation matrix; g
I
is the gravity vector dened in the
inertial frame; a
B
is the acceleration of the craft due to its movement dened in the body xed frame;
a
is
a Gaussian noise component; b
a
is the constant bias of the accelerometer.
As proposed by [2], the bias term is neglected: according to the sensor data sheet the bias term is a
function of the temperature; to eliminate the problem, a self-calibration routine, explained in section A.4,
allows the conditions to be constant and the bias term to be compensated for.
Further developing equation (4.7), according to [17] and [22], one can relate the acceleration due to
the movement to the applied forces. Neglecting the Coriolis acceleration term because it is proportional to
linear velocities which will be kept as low as possible in a hovering situation. Neglecting also aerodynamic
forces such as air resistance, the only forces applied to the ying quadrotor are the ones produced by the
four propellers, resulting in:
a
B
=
F
B
+mg
B
m
(4.8)
where: m is the mass of the prototype; F
B
are the forces applied to the Quadrotor (excluding gravity).
which results in:
a
B
=
F
B
m
+
a
(4.9)
Furthermore, since the accelerometer is not positioned at the center of mass, a centripetal and a
tangential term appear:
a
B
=
F
B
m
+

r +(r) +
a
(4.10)
where: r = [0, 0, r
z
]
T
representing the location of the sensor relative to the center of mass; is the angular
rate [P, Q, R]
T
;
a
is a Gaussian noise component.
Expanding the equation, and keeping r full, leads to:
_

_
a
x
a
y
a
z
_

_
=
1
m
_

_
F
x
F
y
F
z
_

_
+
_

Qr
z


Rr
y

Rr
x


Pr
z

Pr
y


Qr
x
_

_
+
_

_
Q(Pr
y
Qr
x
) R(Rr
x
Pr
z
)
R(Qr
z
Rr
y
) P(Pr
y
Qr
x
)
P(Rr
x
Pr
z
) Q(Qr
z
Rr
y
)
_

_
+
_

ax

ay

az
_

_
(4.11)
The model of the accelerometer as presented appears to be dependent on the properties of the proto-
type as the mass, but also on the dynamics model of the actuators providing the force. Although mathemat-
ically accurate and following physical reasonings, it is somehow not satisfying; the measurements of the
23
accelerometer should be independent of the object on which it is strapped on. A simplication is proposed
in [18] for a near-hovering situation:
a
B
= Sg
I
+

r +(r) +
a
(4.12)
where the acceleration due to movement is neglected.
A quick look at equation (4.12) reveals a limitation of the simplied model; if the quadrotor is in free
fall in a horizontal position, the output of the accelerometer should be 0, but with the simplication, the
measurement would remain at Sg
I
. The simplication is only valid for near-hovering situations and can be
written as:
_

_
a
x
a
y
a
z
_

_
= g
0
_

_
sin
cos sin
cos cos
_

_
+
_

Qr
z


Rr
y

Rr
x


Pr
z

Pr
y


Qr
x
_

_
+
_

_
Q(Pr
y
Qr
x
) R(Rr
x
Pr
z
)
R(Qr
z
Rr
y
) P(Pr
y
Qr
x
)
P(Rr
x
Pr
z
) Q(Qr
z
Rr
y
)
_

_
+
_

ax

ay

az
_

_
(4.13)
Both equation (4.11) and equation (4.13) are implemented and their impact on the model and the
simulations will be discussed.
The theoretical values for the sensor, taken from the sensor data sheet, can be seen in Table 4.2:
Table 4.2: Accelerometers Characteristics
measured range 3g
sensitivity 330mV /g
noise Xout, Yout 150g/

Hzrms
noise Zout 300g/

Hzrms
resolution 0.0958m/s
2
The resolution is calculated in the same way as for the gyroscopes:
AccRes =
3.3V 9.81m/s
2
1024 0.33V
= 0.0958m/s
2
(4.14)
Compass
The compass is a sensor designed to detect the magnetic North direction, written N
I
. By the denition
of the reference frame and neglecting the magnetic inclination or magnetic dip, N
I
= [1, 0, 0]
T
.
A Honeywell HMC6352 was chosen; it is a two-axes magneto-resistive sensor; it is made of thin strips of
permalloy, whose electrical resistance varies with a change in the applied magnetic eld [15]. By measuring
the electric resistance, the orientation of the magnetic eld can be estimated. Let N
B
be the sensor
measurement; considering the frame system, a Gaussian measurement noise,
m
, and a bias term, b
m
,
as in [20], the compass measurement corresponds to:
N
B
= SN
I
+
m
+b
m
_

_
N
x
N
y
N
z
_

_
=
_

_
cos cos
cos sin sin cos sin
cos cos sin + sin sin
_

_
+
_

mx

my

mz
_

_
+
_

_
b
mx
b
my
b
mz
_

_
(4.15)
where: N
B
is the measured magnetic vector; N
I
is the magnetic vector;
m
= [
mx
,
my
,
mz
]
T
is the
Gaussian measurement noise; b
m
= [b
mx
, b
my
, b
mz
]
T
is the measurement bias.
24
Because the chosen sensor has only two sensing axes, the compass measurement is a normalization
of the projection of N
I
on the body-xed frame:
N
B
= norm
_
diag([1, 1, 0])SN
I
_
+
m
+b
m
= norm
_
_
_
_
_

_
cos cos 0 0
0 cos sin sin cos sin 0
0 0 0
_

_
_
_
_
_
+
m
+b
m
(4.16)
The measurement of the compass is affected by soft iron and hard iron distortions. According to [6],
hard iron distortions are caused by the presence of permanent magnets and magnetized iron or steel, they
produce a constant additive error regardless of the orientation. Soft iron distortions are similar to hard
iron distortions but the error varies with the orientation. As a consequence, the hard iron distortions are
included in a constant bias term of equation (4.17), but the soft iron effect cannot be easily accounted for
and will be neglected.
When in hover, we can assume that the compass is held horizontally ( = = 0) with neglected tilt
affecting the projection on the u
x
and u
y
axes; considering that the sensor has only two sensing axes, the
model can be further simplied to:
N
B
=
_

_
N
x
N
y
N
z
_

_
=
_

_
cos
sin
0
_

_
+
m
+b
m
(4.17)
The compass outputs the heading angle, based on the two sensing axes. The equations presented
so far correspond to the decoupling of the heading angle into those two axes. The compass also has the
possibility of outputting the measurements along both axes but the delay associated with the transmission
of data imposes a lower sampling frequency. As a consequence, the compass is used with the option of
sending out the heading and any usage of the compass measurement separates the data using equation
(4.17).
The Gaussian noise term is given on the data sheet of the sensor and presented in Table 4.3:
Table 4.3: Compass Characteristics
measured range 360

heading resolution 1

noise 2.5

rms
Range Finder
The purpose of the range nder is to provide the quadrotor with a measurement of the height, Z. The
measurement is written z
B
in meters and corresponds to a distance between the center of mass of the
quadrotor and the oor along u
z
in a hover situation where tilt and roll are small.
A Sharp GP2D12 has been chosen for this function; the infrared sensor is based on a triangulation.
Basically, an emitted infrared beam is reected on an object and retrieved by the sensor; analyzing the
triangle formed by the receiver, emitter and reecting point, the angles are measured and the distance to
the oor is estimated as presented on Figure 4.4:
25
V
c
c
G
N
D
V
0
wall
measured
angle
Figure 4.4: Range Finder working Principle.
The sensor has a nonlinear behavior as presented in Figure 4.5
1
O
u
t
p
u
t

i
n

V
o
l
t
s
(
V
)
Figure 4.5: Nonlinear Behavior of the Range Finder
The output voltage of the sensor is a function of the inverse of the measured distance; as a conse-
quence, the sensor presents more accurate readings for smaller distances. In addition, a Gaussian noise
term,
z
, corrupts the readings. Since the sensor is not located on the center of mass of the quadrotor,
there is an offset in the measurement, or a constant bias b
z
. Equation (4.18) corresponds to the model of
the sensor:
z
B
= Z +
z
+b
z
(4.18)
In Table 4.4 some properties of the sensor are summarized. According to the sensor data sheet, the
resolution is obtained as the fraction of the measurement range by 10, the number of bits .
Table 4.4: Range Finder Characteristics
measured range 10 80cm
maximum sampling frequency 25Hz
noise on analog output < 200mV
resolution 0.0781cm
Note that the range nder allows a maximum sampling frequency of 25Hz while the other sensors are
supposed to work at 50Hz. Because the range nder has a nonlinear behavior the presented resolution
corresponds to a mean value.
1
from http://www.robotroom.com/DistanceSensor3.html
26
Discussion of the Choice of Sensors
Now that the characteristics and models of the sensors are understood, it is important to justify the
acquisition of the new sensors and present their advantages.
The accelerometer reacts to the accelerations corresponding to the movement of the craft, to gravity
and to vibrations of motors; this makes a only-accelerometer estimation difcult. Moreover, these vibrations
are usually considered in the noise term of the model. These vibrations affect the high frequency mea-
surements of the sensor. On the other hand, the gyroscopes measure angular rates which are subjected
to a low-frequency bias. According to [19], this bias destroys any low-frequency information from the gyro
readings. One would think that integrating the gyroscopes output would result in the attitude. That would
be correct if the integration was not applied to the bias and noise terms.
It becomes clear that the accelerometer alone is not efcient in providing information about pitch and roll
because of its inaccuracy on high frequencies. The gyroscope fails at low frequencies. The fusion comes
as a good idea. This explains the difculties experienced by Jorge Domingues and justies the acquisition
of the gyroscopes. For security reasons it is desired to have the quadrotor above a certain threshold but
also not ying too high, which would possibly become a threat to humans. Therefore, the range nder was
acquired.
4.3 Experimental Identication of the Sensors
In this section real measurements are performed to estimate how the sensors are really affected by
noise. The sensors are connected as presented in Appendix A and a calibration routine explained in
section A.4 allows for stationary conditions to be reached.
As explained in section 4.2, the readings of the accelerometer are affected by its location relative to the
center of mass. It is considered that all sensors of the IMU board are located at r = [0, 0, 5.2cm]
T
.
Accelerometers
Thanks to the calibration routine, any offset of the accelerometers can be compensated for and the
constant bias associated to temperature variations can be neglected. Keeping the accelerometers steady
and reading the output conrms the absence of any bias.
The accelerometers are sensitive to accelerations but also to high frequency vibrations such as those
created by the motors. The undesired effects of the motors are considered to be part of the noise
a
. To
test the accelerometers, data is acquired with all motors off; then with two motors on and nally with all of
them on. With this test the inuence of the vibrations of the motors are estimated. It is expected to see an
increase of noise with the increase of the number of motors working.
The spectral power of the sensor measurements is given by:
Pow =
2
.(1/freq) (4.19)
where: Pow is the spectral power. the standard deviation. freq = 50Hz is the sampling frequency.
The values provided by the manufacturer for the accelerometer noise are: 150g/

Hzrms for u
x
and u
y
axes and 300g/

Hzrms for the u

z
axis, corresponding to the spectral power: 150
2
g
2
/Hz and
300
2
g
2
/Hz.
27
Table 4.5: Noise Characteristics of the Accelerometers
Spectral Power (m/s
2
)
2
/Hz
Accelerometers component theoretical no motor motor 1 and 3 all motors
x 2.1653 10
6
3.8133 10
5
4.2480 10
2
9.3250 10
2
y 2.1653 10
6
5.6112 10
5
3.8355 10
2
8.8530 10
2
z 8.6612 10
6
3.1594 10
5
2.5590 10
2
3.0210 10
2
From Table 4.5, the values from the manufacturer and experimental values present large differences;
note also that the units have been converted to m/s
2
instead of g. Even with all motors off, the noise mea-
surements differ from the theoretical values. Note also the similarity between the u
x
and u
y
components
and the difference to the u
z
component when all motors are working. Recalling equation (4.11) and setting
the forces to zero one obtains:
a
B
=

r +(r) +
a
=
_

Qr
z

Pr
z
0
_

_
+
_

_
RPr
z
RQr
z
P
2
r
z
Q
2
r
z
_

_
+
_

ax

ay

az
_

_
(4.20)
Equation (4.20) corresponds to the conditions in which the experiments were undertaken. The vibrations
produced by the motors can be considered to be small angular accelerations and angular velocities. Note
that the quadrotor is positioned on silent blocks, so the vibrations around u
x
and u
y
are possible but not
around u
z
, therefore R = 0. As a consequence one can write equation (4.21) for the experiment:
a
B
=
_

Qr
z

Pr
z
P
2
r
z
Q
2
r
z
_

_
+
_

ax

ay

az
_

_
(4.21)
The difference in the measurement noise between no motors on and 2 or 4 motors on corresponds to
these accelerations which are taken as Gaussian measurement noise. By symmetry P and Q are similar;
thus equation (4.21) also explains why the noise around u
x
and u
y
is affected in the same way by the
vibrations. The retained values for simulation purposes are the ones obtained with all motors turned on.
Gyroscopes
The same procedure was applied for the gyroscopes. Table 4.6 summarizes the results.
Table 4.6: Noise Characteristics of the Gyroscopes
Spectral Power (

/s)
2
/Hz
Gyroscope component theoretical no motor motor 1 and 3 all motors
roll rate 0.0012 0.0062 0.1041 0.2453
pitch rate 0.0012 0.0067 0.1126 0.3012
yaw rate 0.0012 0.0051 0.1205 0.1961
Note again that the theoretical values for the noise are very different from the ones obtained experimen-
tally. The vibrations of the motors cause considerable deterioration of the readings. From the experiments
28
and thanks to the calibration routine, the bias term from equation (4.2) could be neglected. Indeed, the
mean measurement does not present signicant change in 10000 readings at 50Hz. A time varying bias is
considered to affect the gyroscopes: b
g
(t) = [0.40t, 0.40t, 0.93t]
T
in (deg/s)/s.
The retained values for simulation purposes are the ones obtained with all motors turned on.
Compass
The hard iron distortions can be caused by the magnetic eld of the motors or the electric wires. Exper-
iments must be undertaken in order to determine how the components of the quadrotor prototype inuence
the compass. First, all motors were turned off; then successively each motor was turned on for some time.
This procedure determines if there is a direction with greater impact on the compass measurement.
As seen in Table 4.7, the theoretical values seem reasonable since it is close to the ones obtained,
therefore being kept for simulation purposes. Note also that the motors have little or no inuence on the
compass and that it is apparent that the compass has no varying bias term disturbing the sensor. Any
constant bias term can be neglected since the exact attitude to the magnetic north is not relevant.
Table 4.7: Noise Characteristics of the Compass
theoretical all motors off motor 1 motor 2 motor3
Spectral Power (deg
2
/Hz) 0.125 0.162 0.192 0.131 0.116
Note that the test has been done with the prototype still, the location of the quadrotor inside the labora-
tory may inuence the reading due to external magnetic elds caused by material present in the laboratory.
Range Finder
The equation converting the output voltage of the sensor into a distance must be experimentally found.
In order to do that, the prototype is set at dened distances from the oor and the output is read; then
the equation can be determined. Figure 4.6 presents the approximation of the behavior of the sensor to
a rational function dened by equation (4.22). The values up to 50cm are presented because it would be
preferable to have the quadrotor ying at low heights avoiding the danger of passing 80cm:
z
B
= 14882/(Sensor
output
+ 91.8) 0.8 (4.22)
where: z
B
is the estimation of the height of the prototype in cm; Sensor
output
is the sensor output as read
by the Arduino.
The constant termpresent in (4.22) takes into account the location of the sensor relative to the quadrotor
center of mass. From the curve representing the behavior of the sensor, it appears that the readings are
more sensitive to noise for higher distances.
For an autonomous ight, the noise term of the range nder model will be dropped.
29
0 200 400 600 800 1000
0
10
20
30
40
50
60
Sensor signal
d
i
s
t
a
n
c
e
(
c
m
)
Sharp GP2D12 calibration


Sensor curve
Curve Fitting
y=(14882/(x+91.8))-6.7
Figure 4.6: Identication of the Range Finder.
30
Chapter 5
Actuators
The actuators are the components responsible for applying forces on the system to bring it to a desired
state. In this case the actuators are the motors and propellers. In Figure 5.1 the actuator set is schema-
tized. The Arduino board, on which the controller and estimator are implemented, is connected to the
speed controller, which receives the PWM signal and controls the motor.
The motor M
i
produces an angular velocity
i
and the propeller is considered to spin without slippering
at the dened angular velocity. In this chapter, the PWM signal is introduced. Also a simplied model for
the motor with the model of the propellers explaining the generation of thrust is presented.
Arduino
Board
Speed
Controller
Motor Propeller
PWM

Forces
Moments
Figure 5.1: Conguration of the Actuators.
5.1 Model of the Propeller
The aerodynamic forces, as the friction of the air in contact on the propellers, can be neglected. The
apping of the blades and the ground effect are not considered either.
According to [4] and [26], the thrust, F
i
, and the moment, M
z i
, created by each propeller are propor-
tional to the squared angular velocity of the blade, as shown in equation (5.1):
F
i
= K
T

2
i
M
z i
= K
M

2
i
(5.1)
where: K
T
is the constant which relates the thrust to the square of the angular velocity of the propellers;
K
M
is the constant which relates the moment to the square of the angular velocity of the propellers.
The two constants can be written as:
K
T
= c
T
D
4
4
2
K
M
= c
P
D
5
8
2
(5.2)
31
where: D = 25.4mm is the diameter of the propeller; is the density of the air; c
T
= 0.1154 is the thrust
coefcient; c
P
= 0.0743 is the power coefcient;
The two coefcients are dependent on the propeller and were obtained experimentally by
1
; as a result,
K
T
= 1.46 10
5
kg.m.rad
2
and K
M
= 3.8 10
7
kg.m
2
.rad
2
.
Writing K = K
M
/K
T
= 0.026m, the forces and moments presented in chapter 3 can now be under-
stood; they result from:
F
i
= K
T

2
i
M
x
= (
2
2

2
4
)K
T
d
cm
= (F
2
F
4
)d
cm
M
y
= (
2
1

2
3
)K
T
d
cm
= (F
1
F
3
)d
cm
M
z
= (
2
1
+
2
3

2
2

2
4
)K
M
= (F
1
+F
3
F
2
F
4
)K
(5.3)
5.2 Model of the Motor
The PWM signal, a square digital signal, is a common method to provide an analog signal with digital
means
2
; the digital pins of the Arduino use a normalized 50Hz square wave to code the signal as a pulse
width varying from 1 ms to 2 ms. The Arduino project has different methods to create the PWM signal;
in this case the Servo.h library has been chosen and the function writeMicroseconds() has been used to
dene the width of the pulse. Speed controllers are used to turn the PWM signal into a triphasic signal fed
to the brushless DC motors.
The motor i receives the triphasic signal corresponding to an order of angular velocity
i
. The behavior
of the motor is a consequence of both electric and mechanical dynamics. The electrical and mechanical
systems can be represented as Figure 5.2
3
, where E is the counter electromotive force and is the angle
of rotation of the rotor of the motor relative to the stator.
r
e L
V
i
E
(a) Electrical System of the Motor
J
b
.

T
(b) Mechanical System of the Motor
Figure 5.2: Electrical and Mechanical Systems of the Motor.
Applying the second law of Newton, the Laplace transform and solving to eliminate the intensity, a
second-order system is obtained, presented as a transfer function in equation (5.4). Note that instead of
1
Experimental determination of the propellers variables: www.radrotary.com
2
A brief introduction to the PWM signal is presented in the documents supporting the Arduino project: http://www.arduino.cc/
en/Tutorial/PWM
3
The online courses of Canegie-Mellon University present the model of the DC motor at http://www.engin.umich.edu/group/
ctm/examples/motor/motor.html
32
the voltage V , PWM
i
is used:
V = r
e
I +L
dI
dt
+K
e

K
e
I = J

+b

i
PWM
i
=
K
e
(Js +b)(Ls +r
e
) +K
2
e
(5.4)
where:
i
is the angular velocity of the rotor of motor i; PWM
i
is the input PWM signal to motor i; K
e
is
the electromotive constant; J is the moment of inertia of the rotor; b is the damping ratio of the mechanical
system; L is the electric inductance; r
e
is the electric resistance; I is the intensity of the current.
Equation (5.4) presents an accurate model of the motor; however it is difcult to measure all the param-
eters of the motor. The model can be simplied to a rst-order system
4
corresponding to the mechanical
dynamics:

i
PWM
i
=
k
i
s + 1
(5.5)
where: is the time constant of the system; k
i
is the dc gain.
To dene the parameters of equation (5.5) the speed controller and the motor with the propellers at-
tached are considered. With this approach it is considered that the inuence of the electrical dynamics
of the motor is considerably faster than the mechanical dynamics of the motor with the propellers. The
identication of the actuator set is done to determine the dc gain k
i
and the mechanical time constant .
Because the electrical dynamics is much faster, it can be linearized and taken as a multiplicative constant
included in k
i
.
In addition to the presented model, the actuator set contains some nonlinearities. In fact, the writeMi-
croseconds() function accepts only integer values as inputs. Therefore there is a quantization on the PWM
signal.
5.3 Identication of the Motors
The four motors are expected to present different characteristics partly because the speed controllers
used are not identical. Motors M
1
and M
2
have the same electronic components but motors M
3
and M
4
have each a different element.
In section A.2 the motors wiring scheme is presented. The PWMis bounded to [1000; 2000]s. There
is yet another nonlinearity to consider; each motor has a dead zone. To dene that value, corresponding to
the PWM input, a simple experiment was undertaken, in which the PWM value was progressively increased
until the motor starts. Table 5.1 presents the results. The dead zone of motor M
i
can be written PWM
d
i
.
4
Prof. J.Fainguelernt from the university of Tel-Aviv proposes the simplication in a document aiming at DC Motor Control
33
Table 5.1: Dead Zone of the Motors
Motor PWM
d
i
in s
M
1
1179
M
2
1184
M
3
1190
M
4
1260
The multiplicative constant k
i
of equation (5.5) is the static gain of the model, it denes the relation
between the stationary response of the system and its solicitation. To dene the static gain, a tachometer
is needed to measure the angular velocity of the propeller at different PWM inputs. The tachometer is
based on the reection of a laser beam on a reective duct tape attached to the blade; measuring the time
between two reected beams the angular velocity can be estimated. Table 5.2 and Figure 5.3 present the
result; over 1600s the vibration of the blades make it difcult to determine correctly the angular velocity
with a laser based tachometer.
Table 5.2: Static Gain of the Motors
PWM in s 1200 1250 1300 1350 1400 1450 1500 1550 1600

1
in rad/s 251.0 406.2 521.5 623.1 714.2 804.6 885.9 - -

2
in rad/s 261.8 417.8 529.8 636.6 728.8 823.5 905.8 - -

3
in rad/s 128.8 357.1 511.0 651.4 788.5 900.6 1030.4 - -

4
in rad/s 0 0 341.3 485.8 578.0 684.8 793.7 872.3 938.2
1200 1250 1300 1350 1400 1450 1500
200
300
400
500
600
700
800
900
MotorM
1

1
(
r
a
d
/
s
)
PWM
1
(s)


stationary response
polynomial fitting
1200 1250 1300 1350 1400 1450 1500
200
400
600
800
1000
MotorM
2

2
(
r
a
d
/
s
)
PWM
2
(s)


stationary response
polynomial fitting
1200 1250 1300 1350 1400 1450 1500
0
200
400
600
800
1000
1200
MotorM
3

3
(
r
a
d
/
s
)
PWM
3
(s)


stationary response
polynomial fitting
1300 1350 1400 1450 1500 1550 1600
300
400
500
600
700
800
900
1000
MotorM
4

4
(
r
a
d
/
s
)
PWM
4
(s)


stationary response
polynomial fitting
Figure 5.3: Identication of the Actuators.
The curves are approximated by a second-order polynomial using the function polyt and polyval from
Matlab. Table 5.3 presents the polynomial.
34
Table 5.3: Polynomial Approximation; ax
2
+bx +c
Motor a b c
M
1
0.0025 8.6849 6.63 10
3
M
2
0.0023 8.3355 6.40 10
3
M
3
0.0034 12.008 9.41 10
3
M
4
0.0022 8.2903 6.75 10
3
To determine the static gain the polynomial curve must be linearized around the operating point. The
procedure will be explained in the following chapter. The static gain k
i
will correspond to the slope of the
tangent to the curve at the operating point.
It is considered that the time constant of all motors are identical for the sake of simplicity. To determine
it, a square wave was imposed as input PWM signal to a motor. The tachometer was used to read the
angular velocity of the propeller. Figure 5.4 presents the result.
1 2 3 4 5 6 7 8
1200
1250
1300

motor response
reference
P
W
M

(

s
)
time(s)
Time Constant Identication
Figure 5.4: Determination of the Time Constant of the Motor Set.
The square wave varies from 1200s to 1300s; in the following chapter, the linearization point is deter-
mined, using Identication Toolbox from Matlab, the system can be approximated by a rst-order system
with a time constant = 0.162s. The result is very similar to the one obtained by Jorge Domingues.
35
36
Chapter 6
State Space Model
6.1 Assembling the State Space Model
The four vectors V, , P, , with three components each, representing respectively the linear veloc-
ity, the angular velocity, the position and the attitude of the quadrotor, compose the state vector X =
[V, , P, ]
T
.
The quaternion representation is used to simulate the dynamics and kinematics of the quadrotor, but
the control and estimation methods are applied with Euler angles. Therefore to obtain model-based control
techniques, the model is derived with Euler angles.
Modeling the system consists of dening the functions g and h which satisfy the general nonlinear state
space formulation:

X = g(X, U) Dynamic Equation

Y = h(X, U) Output Equation
(6.1)
where: X is the state vector of the system; U is the input vector of the system; Y is the output vector of
the system.
The output vector Y is obtained from the sensors and corresponds to the measurements: Y =
[a, , N]
T
. Excluding the range nder for now.
If equations (3.6)-(3.10) dene the quadrotor dynamics, the input vector is composed by the forces
and moments applied to the quadrotor. This formulation holds the disadvantage of not being intuitive and
not considering the true systems inputs: the actuators. Although many quadrotor projects present a model
based on the forces and moments, this work will take the input vector Uas the signal sent from the Arduino
board, thus U = PWM = [PWM
1
, PWM
2
, PWM
3
, PWM
4
]
T
. Applying equations (5.3), the forces and
moments can be expressed as a function of the angular velocity of the motors. Then, the linearization of
the rst order systems modeling the motors express the angular velocities as a function of PWM; for that
the static gains have to be dened.
The output function can be dened by two sets of equations; as discussed earlier, there are two models
for the accelerometer worth being tested and compared. The rst output set, based on the properties and
dynamics of the prototype such as the mass and the forces applied, is dened by equations (4.2), (4.11)
and (4.17). The second output set, based on the projection of the gravity vector onto the body xed frame,
is dened by equations (4.2), (4.13) and (4.17). Once again, the forces and moments must be written
37
as functions of PWM. To do this, the static gains of the motors must be dened through a linearization
process.
6.2 Linearization Point
The state space formulation described so far is nonlinear; the control method proposed in this work
is based on a linear model, therefore a linearization must be performed; it consists in approximating the
nonlinear state space system by a linear formulation:

X AX+BU
Y CX+DU
(6.2)
The approximation is only valid in the vicinity of an operating point or linearization point X
0
; the primary
target of this work would be to have the quadrotor hovering. This corresponds to:
X
0
= [0, 0, 0, 0, 0, 0, 0, 0, z, 0, 0, 0]
T
. Mathematically, to perform a linearization of a function f(X
1
, X
2
) at
a given operating point (X
10
, X
20
), the rst order approximation of the Taylor series is used as shown in
equation (6.3):
f(X
1
, X
2
) f(X
10
, X
20
) +f
X
1
(X
10
, X
20
)(X
1
X
10
) +f
X
2
(X
10
, X
20
)(X
2
X
20
) (6.3)
where:f(X
1
, X
2
) is the dynamic equation; f
X
1
is the derivative of f in the variable X
1
; f(X
10
, X
20
) is the
trim condition.
The input vector corresponding to the desired operating point is dened solving equation (6.4), which
balances the total thrust with the weight of the prototype.
F
z
= mg
0
4

i=1
F
i
= mg
0
F
i
= mg
0
/4
(
0
i
)
2
=
mg
0
4K
T
(6.4)
where:
0
i
is the angular velocity of the propeller at the linearization condition.
Considering the equilibrium condition and the model of the motor, the linearized equation ca be written
as equation (6.5):

i
=
0
i
+k
i
(PWM
i
PWM
0
i
) (6.5)
where: PWM
i
is the PWMsignal sent to motor i. PWM
0
i
is the PWMvalue corresponding to the trimvalue.
To remove
0
i
, the equation can be written introducing a new term PWM
deq
i
, corresponding to the
intersection of the tangent of the motor identication curve with the abscise axis.

i
= k
i
(PWM
i
PWM
deq
i
) (6.6)
Table 6.1 presents the result for the PWM linearization points and the static gain obtained after lineariz-
ing the polynomial equations described by Table 5.3 and the newly introduced term PWM
deq
i
:
38
Table 6.1: Dead Zone of the Motors
Motor Linearization Point PWM
0
i
(s) Static Gain k
i
((rad/s)/s) Equivalent Dead Zone PWM
deq
i
(s)
M
1
1254.2 2.53 1094.6
M
2
1250.0 2.56 1092.1
M
3
1265.9 3.47 1149.3
M
4
1322.9 2.53 1163.4
From equation (6.4) the linearization point in terms of angular velocities is determined. Then, from the
curves representing the behavior of the model of the motors, the linearization point in terms of PWM
0
i
is
determined. The polynomials are then linearized around that point and the static gain is obtained.
Note the difference between the different motors. Because the speed controllers are different, the
motors present different characteristics. Motors M
1
and M
2
have similar dead zone values, equilibrium
points and static gains whereas motor M
3
is considerably different.
6.3 Linearization of the Model
For this work two alternative methods are used to linearize the dynamic and the output equations.
The rst consists of applying the Taylor expansion from equation (6.3) and the second is computationally
achieved. Both methods lead to the same result presented in equation (6.7); this system considers the
accelerometer based on forces. The input vector is U = PWM = [PWM
1
, PWM
2
, PWM
3
, PWM
4
]
T
,
the state vector is X = [V, , P, ]
T
and the output vector is Y = [a, , N]
T
.
A =
_

_
0 0 0 0 0 0 0 0 0 0 g
0
0
0 0 0 0 0 0 0 0 0 g0 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 0
0 0 0 0 0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0 0 0 0 0
0 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 1 0 0 0 0 0 0
_

_
(6.7a)
39
B =
_

_
0 0 0 0
0 0 0 0
0.0307 0.0310 0.0421 0.0307
0 1.0851 0 1.0743
1.0742 0 1.4713 0
0.0483 0.0488 0.0661 0.0483
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
_

_
(6.7b)
C =
_

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

_
(6.7c)
D =
_

_
0.0559 0 0.0765 0
0 0.0564 0 0.0559
0.0307 0.0310 0.0421 0.0307
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
_

_
(6.7d)
If the accelerometer is modeled with the projection of the gravity vector instead, matrices C and D:
C =
_

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

_
(6.8a)
40
D =
_

_
0.0559 0 0.0765 0
0 0.0564 0 0.0559
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
_

_
(6.8b)
Note that the output vector has in both cases two null rows: N
x
and N
z
. This suggests that neither
the input vector nor the state vector has any inuence on those sensor measurements. The accelerometer
based on the gravity presents also a null row: a
z
. The output vector is reduced to Y = [a
x
, a
y
, g
x
, g
y
, g
z
, N
y
]
The observability and the controllability are two notions common to the state space analysis of a system.
The controllability translates the possibility of moving the quadrotor through its states. The observability
translates the ability to measure the internal states of the system. It is conrmed that both models are fully
controllable.
On the other side, neither model is fully observable. The forces-based one has four observable states
[P, Q, R, ]
T
. The system from equation (6.8) has six observable states: [P, Q, R, , , ]
T
. It is intuitive that
the chosen sensor set is not able to provide information regarding the position or the linear velocities thus
the system cannot be fully observable. Although more accurate, the forces-based model will be dropped
due to its disadvantage for model-based control and estimation strategies.
The poles of the system hold the information of the dynamic response or behavior of the system. The
eigenvalues of matrix A reveal the poles of the chosen continuous system. Using Matlab, it is shown that
the twelve poles are located at the origin; the system is unstable.
The trim condition, corresponding to the rst term of the Taylor expansion, regards the input vector
(U
trim
), the output vector (Y
trim
) and the state vector (X
trim
); it is presented in equation (6.9) and is
calculated using the chosen model:
U
trim
= [PWM
0
1
, PWM
0
2
, PWM
0
3
, PWM
0
4
]
T
Y
trim
= [0, 0, g
0
, 0, 0, 0, 1, 0, 0]
T
X
trim
= [0, 0, 0, 0, 0, 0, 0, 0, z, 0, 0, 0]
T
(6.9)
The chosen model is continuous. The observation and control algorithms will be loaded on the Arduino
board and intended to run at a frequency freq = 50Hz (note that the range nder is still not considered).
This means that only at each interval of T = 0.02s will the sensors output measurements, will the observ-
able states be reconstructed and a control signal sent. If model-based approaches are chosen then the
system must be discretized.
The discrete state space system is written as:
X
k+1
= A
d
X
k
+B
d
U
k
Y
k
= C
d
X
k
+D
d
U
k
(6.10)
where: A
d
, B
d
, C
d
, D
d
are the discrete state space matrices; X
k
, U
k
, Y
k
are respectively the state vector,
input vector and output vector at time k; X
k+1
is the state at time k + 1.
41
Equation (6.11) presents the discretization process using the zero order holder:
A
d
= e
AT
(6.11a)
B
d
= A
1
(A
d
I)B (6.11b)
C
d
= C (6.11c)
D
d
= D (6.11d)
42
Chapter 7
Quadrotor Simulator
In this chapter, the model is implemented in Simulink and the stability of the open loop system is
accessed.
7.1 Implementation of the Model
7.1.1 Dynamic and Output Equations
Both the dynamics and output equations are implemented in the same function FfunctionHfunction.m
in their continuous, nonlinear form. Figure 7.1 presents the block diagram containing the functions and the
inputs and outputs:
Gfunction-Hfunction
MATLAB
Function
Integrator
1
s
[,,,]
X

X
Y
Figure 7.1: Simulink Implementation of the System.
Note that the input is [
1
,
2
,
3
,
4
]
T
; the model of the motors, translates PWMinto to the dynamics
and kinematics model.
Figure 7.2 presents a simulation of a take off, followed by a hovering ight at 2m and then landing at
20s. This simulation was obtained after the implementation of a LQR controller which will be presented in
the following chapters. When the quadrotor reaches a certain height, the motors are turned off, making
the landing smoother and safer. Note that after touching the ground, the quadrotor stops automatically
and remains on the oor. After turning off the motors in a landing situation, the quadrotor can only take off
again by manually restarting the variables, thus avoiding any rebounds.
The ground reaction is only meant to work when in contact with the oor. To ensure this, two ags are
used. If the quadrotor is ying a ag is set and from that time on, if the height of the center of mass goes
under the length of the feet d
feet
, then the engines are turned off by setting the other ag. This corresponds
to the desired safety feature and ensures that the quadrotor can takeoff and remain still after landing. The
height of the center of mass is presented although the height of each foot is computed.
43
0 5 10 15 20 25 30
-2.5
-2
-1.5
-1
-0.5
0
time(s)
Take off and Landing
Z
(
m
)


Quadrotor Tracking
reference
Figure 7.2: takeoff and Landing Simulation.
7.1.2 Sensors
The output equation corresponding to the sensor measurements is included in FfunctionHfunction.m
le. The function only accounts for the dynamics of the sensors but not the bias and noise terms. To
introduce those terms the model presented in Figure 7.3 is implemented:
Out1
1
Theoretical Comp Noise
Theoretical Gyro Noise
Theoretical Acc Noise
Sine Wave
Saturation
Saturation
Saturation
Radians
to Degrees
R2D
Radians
to Degrees
R2D
Quantizer
Quantizer
Quantizer
Measured Gyro Noise
Measured Comp Noise
Measured Acc Noise
-K-
Degrees to
Radians
D2R
Degrees to
Radians
D2R
In1
1
gyroscope
magnetometer
accelerometer
switch
switch
switch
Figure 7.3: Implementation of the Model of the Sensors.
Note that by setting a ag, the user can chose to perform simulations using either theoretical values
for the noise affecting the sensors or use the experimentally dened values. Note also that different seeds
were used to generate the noise, avoiding a correlation between the different sensors.
7.1.3 Motors
The model of the motors is presented in Figure 7.4. The different nonlinearities are applied to PWM
entering the model; rst the saturation block ensures that the signal is within the dened boundaries,
then the dead zone of the motors is simulated and nally the quantizer ensures that the signal is an
integer number. The signal is then converted to obtain the angular velocities of the propellers. These
conversions are implemented using rst order state space blocks corresponding to the transfer functions
dened previously; this block has the advantage of setting the initial condition.
44
Out1
1
State-Space3
x' = Ax+Bu
y = Cx+Du
State-Space2
x' = Ax+Bu
y = Cx+Du
State-Space1
x' = Ax+Bu
y = Cx+Du
State-Space
x' = Ax+Bu
y = Cx+Du
Saturation3
Saturation2
Saturation1
Saturation
Quantizer4
Quantizer3
Quantizer2
Quantizer1
Dead Zone3
Dead Zone2
Dead Zone1
Dead Zone
In1
1
4 4
PWM
[,,,]
Figure 7.4: Implementation of the Model of the Motors.
7.2 Simulation Model
The overall model is shown in Figure 7.5. It contains in child-les the model of the motors and the
models of the sensors. It also contains the m-le performing the dynamics and the kinematics calculations.
PWM0
Signal Builder
V
U
W
P
Q
R
X
Y
Z
phi
tetha
psi
Sensors
In1 Out1
Quadrotor
dynamics
MATLAB
Function
PWM

LQR
In1 Out1
Integrator
1
s
X
Y
ideal
Figure 7.5: Implementation of the Complete Model.
As presented in Figure 7.6, the open-loop system is unstable; when left alone from the initial conditions
set as the linearization point, thus corresponding to the hovering situation, the quadrotor quickly diverges
from the hovering state, suggesting that the system is unstable.
In this part, the conventions and nomenclature regarding the quadrotor concept have been presented,
then the kinematics and dynamics governing the quadrotor have been presented. Then the models of
sensors and the actuators were derived with a discussion about the choice of the sensors. Experiments
on the prototype allowed more accuracy for the model parameters. Finally the equations were assembled
to form the state space model and linearized around a chosen equilibrium point. This revealed that the
system was unstable as conrmed by the open loop response.
45
0 5 10 15 20 25
-500
0
500
v
e
l
o
c
i
t
y
(
m
/
s
)
[U, V, W]
T


U
V
W
0 5 10 15 20 25
-2
-1
0
1
a
n
g
.
v
e
l
(
r
a
d
/
s
)
[P, Q, R]
T


P
Q
R
0 5 10 15 20 25
-2000
0
2000
4000
p
o
s
i
t
i
o
n
(
m
)
[X, Y, Z]
T


X
Y
Z
0 5 10 15 20 25
-200
0
200
a
t
t
i
t
u
d
e
(
d
e
g
)
[, , ]
T
time(s)

Figure 7.6: Open-Loop Response of the System

46
Part II
Flight Control of a Quadrotor
47
Chapter 8
Control Design
In this chapter a 12 states LQR is needed to control the quadrotor with ideal data; it is used with
a reference set, designed to excite all the states of the system, to generate data in order to tune the
estimators presented in Chapter 9. Then in Chapter 10, an LQR based on the observable states of the
model is presented to close the loop with the estimated states.
8.1 Linear Quadratic Regulator
8.1.1 Control Methods
The quadrotor has become a popular object and different approaches have been considered to tackle
the control problem. A possible method is based on the use of a PID controller, as done at [28] and [9],
where a PID controllers are used to stabilize the quadrotor and have it hover.
A more complex approach including the attitude and the position of the quadrotor is implemented in
[5], where a backstepping method ensures the asymptotic convergence of the controlled states. By means
of Lyapunov functions the control law is dened. The backstepping approach presents the advantage of
being non linear, thus avoiding the inaccuracies induced by the linearization. It also decouples the position
from the attitude thus allowing for independent work on both systems.
In [10], a PID controller and an LQR controller are developed and implemented for stabilization and
positioning purposes. It is defended that the LQR is faster to tune than the PID. Because the LQR is an
optimal controller presented in the form of a matrix, its implementation is easier and less computationally
demanding than the PID method. Therefore the LQR is the chosen control method.
8.1.2 Introduction to the LQR
Consider the generic state space model:
x = Ax +Bu
y = Cx +Du
(8.1)
The optimal control problem consists of nding the optimal control action u = K
lqr
x which will control
the state vector minimizing a cost function, with K being the optimal gain matrix:
J
LQR
=
_

0
(x
T
Q
lqr
x +u
T
R
lqr
u)dt (8.2)
49
where: x is the state vector with dimension [n 1]; Q
lqr
and R
lqr
are semi-positive denite weighting ma-
trices.
The weighting matrices are responsible for the trade off performed between the importance given to the
control action and the state vector. The matrices are initially determined by the Brysons rule, which states
that Q
lqr
and R
lqr
can be written as in equation (8.3) as diagonal matrices:
Q
lqr
=
1
x
2
max
R
lqr
=
1
u
2
max
(8.3)
where: x
max
and u
max
are the maximum desired variations.
The dynamic of the closed loop is given by:
x = (ABK
lqr
)x (8.4)
The LQR is used for two purposes in this work. In a rst step, a 12 states LQR is needed to control the
quadrotor with ideal data; this will allow a set of movements to be performed in simulation and the resulting
attitude can be used to design the estimators. Since the model is actually not fully observable, a reduced
state LQR is then used to take care of the stabilization problem.
8.2 Ideal Full State Control via LQR
To compare the estimators, it is wished to have variations in the three estimated angles. To obtain that,
a reference set presented in Figure 8.1 is designed. A twelve-states LQR is used to close the loop and
guarantee that the quadrotor follows the reference using ideal sensors as input data. The idea is to later
use the data from that simulation to test the different estimators against the ideal data.
It has been assessed that the system presented by equation (6.7) is fully controllable. This means that
the twelve states can be controlled by the four components of the input vector.
Figure 8.2(a) presents the closed loop implementation. Note that the LQR-block receives ideal mea-
surements. This is very useful to compare the estimators with the ideal situation.
Figure 8.2(b), presents the inside of the LQR-block. The state references are designed to excite all the
states and dynamics of the system, it comprises X, Y, Z translations but also rotations; some translations
are preformed while the quadrotor is rotated. Because the system is linearized around = 0 it is necessary
to rotate the error vector E = X
ref
X. The errors E
X
and E
Y
are dened in the inertial frame but the
LQR considers it to be written in the body-xed frame. To correct that when = 0, E
X
and E
Y
are rotated
into the body frame using the ideal measurement of .
The weighting matrices are dened as equation (8.5). To use the Brysons rule, the following values
are considered as maximum values for the error vector: [1, 1, 1, 0.1, 0.1, 1, 0.5, 0.5, 0.1, 0.08, 0.08, 0.08]. The
angles are in rad and the angular velocities in rad/s. Then an iterative process, in which the values are
rened in order to eliminate any overshoot, is considered. The attitude can vary by up to 5

, the linear
velocities by 1m/s, the position error is kept lower for Z than for X and Y .
Q
lqr
= diag([5, 5, 2, 100, 100, 50, 4, 4, 10, 150, 150, 60])
R
lqr
= diag([0.01, 0.01, 0.01, 0.01])
(8.5)
50
0 10 20 30 40 50
-1
0
1
v
e
l
o
c
i
t
y
(
m
/
s
)
[U, V, W]
T


U
V
W
0 10 20 30 40 50
0
1
a
n
g
v
e
l
(
r
a
d
/
s
)
[P, Q, R]
T


P
Q
R
0 10 20 30 40 50
-2
0
2
p
o
s
i
t
i
o
n
(
m
)
[X, Y, Z]
T


X
Y
-Z
0 10 20 30 40 50
0
20
40
60
80
a
t
t
i
t
u
d
e
(
d
e
g
)
[, , ]
T
time(s)

Figure 8.1: Reference Set.

State References
Quadrotor
dynamics
MATLAB
Function
PWM reference
PWM0
Motors
In1 Out1
LQR
Error
ideal sensor
Out
Integrator
1
s
Ideal State Vector
Sensor Measurements
Error
Signal Builder
(a) Full-state control with ideal sensor.
Out
1
LQR
K*uvec U Y Rotation
ideal sensor
2
Error
1
(b) Subsystem of the controller.
Figure 8.2: Full-State Control.
51
The closed-loop response to the state-references X
ref
is presented in Figure 8.3:
0 10 20 30 40 50
-1
0
1
v
e
l
o
c
i
t
y
(
m
/
s
)
[U, V, W]
T


U
V
W
0 10 20 30 40 50
-2
0
2
a
n
g
v
e
l
(
r
a
d
/
s
)
[P, Q, R]
T


P
Q
R
0 10 20 30 40 50
-2
0
2
4
p
o
s
i
t
i
o
n
(
m
)
[X, Y, Z]
T


X
Y
-Z
X
ref
Y
ref
-Z
ref
0 10 20 30 40 50
-50
0
50
100
a
t
t
i
t
u
d
e
(
d
e
g
)
[, , ]
T
time(s)

ref

ref

ref
Figure 8.3: Full-state Control Response.
The controller stabilizes the system. To analyze the closed loop dynamic, equation (8.4) is applied.
Table 8.1 presents the location of the poles and Figure 8.4 presents a detail of the plotted poles in the
Argand plane.
Table 8.1: Closed Loop Poles
Real (rad/s) -182.18 -152.69 -7.26 -1.06 -1.06 -0.75 -0.75 -0.75 -0.75 -1.10 -1.41 -1.41
Imaginary 0 0 0 0.96 -0.96 0.90 -0.90 0.90 -0.90 0 0 0
All the poles are now located on the left half plane of the Argand plane, they all have a negative real
part. This means that the system is stable. In Figure 8.5 the response of the system to step references is
shown. Unit steps are used to excite the position variables and the yaw angle. The absence of overshoot
and oscillations conrms that the controller has been well tuned. The rising time corresponds to necessary
time for the system to reach 90% of its nal value. The settling time is the amount of time taken for the
system to reach stationary state, within 2% of the nal value. Analyzing the the curves, the rising time and
the settling time are measured and presented in Table 8.2.
The weighting matrix holds a stronger weight regarding the height than the other position variables; as
a result, the response to a step in the height is faster than the response to a step in X or Y . There is also
a strong resemblance between the X and Y values, consequence of the symmetry of the quadrotor. To
obtain better response times, the weights can be further modied.
The data obtained with the 12 states controller is now available to tune the estimator and to test different
52
-20 -15 -10 -5 0 5
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
Real Axis
I
m
a
g
i
n
a
r
y
A
x
i
s
Close Loop Poles
Figure 8.4: Closed Loop Poles.
16 18 20 22 24 26
0
0.5
1
X
(
m
)
X step


Response
Reference
26 28 30 32 34 36
0
0.5
1
Y
(
m
)
Y step


Response
Reference
0 2 4 6 8 10
1
1.5
2

Z
(
m
)
Z step


Response
Reference
6 8 10 12 14 16
0
20
40
60

(
d
e
g
)
step
time(s)


Response
Reference
Figure 8.5: Closed Loop step Response.
Table 8.2: Rising and Settling Time
X Y Z
Rising Time(s) 3.64 3.56 1.58 2.24
Settling Time(s) 5.22 4.72 2.70 3.82
53
estimation strategies. Then these estimators can be used with a control strategy based on observable
states to fully simulate the quadrotor ight.
54
Chapter 9
Estimation
The control methods presented so far use ideal sensors; however, as explained previously, the sensor
set has inaccuracies as noise and bias and measures only indirectly the internal states of the quadrotor.
The objective of the estimator is to extract the information from the sensors, separating it from those
perturbations.
In this chapter complementary lters and Kalman lters are implemented to estimate the attitude. Then
the height estimator is exposed.
Figure 9.1 presents the closed loop model of the system with the estimator. This conguration is
implemented with the reference set presented in the previous chapter and the full-state LQR controller to
close the loop. The ideal data generated with the full-state LQR is used to evaluate the estimators and
compare them.
estimated state
State Reference
V
U
W
P
Q
R
X
Y
Z
phi
tetha
psi
Sensor Model
In1 Out1
Scope
Estimator
Estimator
Quadrotor
dynamics
MATLAB
Function
PWM reference
PWM0
Motor Dynamics
In1 Out1
LQR Controller
In1
In2
Out1
Integrator
1
s
ideal state
ideal state
ideal state
modeled sensors
Figure 9.1: Block Diagram of the Simulation.
9.1 Complementary Filter
As explained previously, the different sensors are affected in different ways by noise. It has been shown
that accelerometers are corrupted by high frequency noise. On the other side gyroscopes are affected by
low frequency bias. The whole idea behind the complementary lter is the existence of this complementary
55
behavior of the sensors. A low-pass lter applied to the accelerometers and a high-pass lter applied to
the gyroscopes would retrieve the information concerning the attitude over the whole spectrum.
As explained in [19], the complementary lter allows the fusion of low bandwidth position measurements
with high bandwidth rate measurements. Equation (9.1) presents the general idea behind the complemen-
tary lter:

X =
C(s)
s +C(s)

X
acc
(s) +
s
s +C(s)

X
gyro
s
= T(s)

X
acc
(s) +S(s)

X
gyro
s
(9.1)
where:

X is the estimated state vector;

X
acc
is the estimation based on accelerometers;

X
gyro
/s is the
integration of the estimation based gyroscopes; T(s) is a low pass lter; S(s) is a high pass lter.
The requirement T(s) + S(s) = 1 ensures that the full bandwidth is considered. The simplest lter
considers C(s) as a constant. If a constant bias was to be removed, an integrator would be included too,
along with the constant in C(s). But thanks to the initialization routine developed and explained in Appendix
A, the constant bias has already been removed, leaving only a time varying bias. The complementary lter
is able to retrieve the information concerning the attitude but does not address the problem of estimating
the angle rates P, Q and R.
9.1.1 First Complementary Filter
The rst complementary lter is fairly simple; it is based on the algorithm developed in [25] to obtain the
estimations from the gyroscopes and the accelerometers; explicit high pass and low pass lters are used
for the fusion.
Let us recall some useful properties of the direction cosine matrix dened in section 3.1.1:
the matrix is orthogonal.
the norm of the column vectors is 1.
the norm of the row vectors is 1.
These properties are very important. The accumulation of numerical errors will modify the length of the
vectors and distort them, resulting in the loss of the orthogonality property. Only two orthogonal vectors
have to be known to construct the matrix and obtain an estimation of the attitude; the compass and the
accelerometer measure orthogonal vectors: the North and the gravity.
Recalling equation (4.17) and that N
I
= [1, 0, 0]
I
then the rst column of the direct cosine matrix, S,
corresponds to the projection of the North vector onto the body xed frame, which is approximated to
the compass measurement vector. The same idea denes the third column of the rotation matrix to the
accelerometers measurement vector, in a hover ight. By the orthogonality property, the second column is
obtained as the cross product of the two previous vectors. A renormalization procedure must be applied to
correct numerical errors. Finally using the relation between the matrix and the Euler angles, the latter are
estimated. The algorithm 9.1 is set to run at a frequency freq = 50Hz.
Note the consequence of not having a three-axes compass. The present compass has only two sensi-
tive axes, as consequence

is affected. The use of a two-axes compass is limited to hovering situations;
independently of the attitude of the quadrotor the compass will always consider the measured vector in the
56
Algorithm 9.1 Estimation from the accelerometer and compass.
S = [N
B
, a
B
N
B
, a
B
]
S(:, 1) = S(:, 1)/norm(S(:, 1))
S(:, 2) = S(:, 2)/norm(S(:, 2))
S(:, 3) = S(:, 3)/norm(S(:, 3))

1
= atan2(S(2, 3), S(3, 3))

1
= asin(S(1, 3))

1
= atan2(S(1, 2), S(1, 1))
horizontal plane of the craft. In non-hovering situations, the accelerometer vector will not be orthogonal
to the compass vector, violating the properties of the matrix. This error is passed onto the second vector
through the cross product and the yaw estimation is compromised.
To guarantee the orthogonality of the vectors before the cross product a simple procedure can be
applied as done in [25]:
error = dot(N
B
, a
B
)
N
B
= N
B
a
B
error/2
a
B
= a
B
N
B
error/2
(9.2)
where: dot() is the dot product.
Equation (9.2) considers the orthogonality error to be both on the accelerometer and the compass
and attempts to split the error on both sensors. Simulations proved this method to propagate the error
to the estimation of and rather than to solve the problem. A correction could be fully applied to the
compass, but due to the noise affecting the accelerometer, this would result in a correlated error applied to
the estimation of the attitude.
Because the estimation of the attitude based on the accelerometer and the compass is not dependent
on previous estimations, the error does not accumulate through time.
The numerical integration of the gyroscopes to obtain an angular estimation is explained in algo-
rithm 9.2, where t is the computing time and T is the sampling interval.
Because the estimation is based on previous states, the renormalization plays an important role in the
estimation performance because of the cumulative error.
The fusion is obtained after the application of the lters. Both the low pass and the high pass lters
are rst order systems. Equations (9.3) shows the state space representation of the lters for a cut-off
frequency 1/T
c
rad/s:
x
hp
=u
hp
/T
c
x
hp
/T
c
HighPassFilter
y
hp
=u
hp
x
hp
x
lp
=u
lp
/T
c
x
lp
/T
c
LowPassFilter
y
lp
=x
lp
(9.3)
where: x
hp
and x
lp
are the state variables of the high pass and low pass lters. u
hp
= [

2
,

2
,

2
]
T
and
u
lp
= [

1
,

1
,

1
]
T
are the input variables of the high pass and low pass lters. y
hp
= [

hp
,

hp
,

hp
]
T
and
y
lp
= [

lp
,

lp
,

lp
]
T
are the output variables of the high pass and low pass lters. T
c
is the cut-off frequency
57
Algorithm 9.2 Estimation from the gyroscopes.

= [0, g
z
, g
y
; g
z
, 0, g
x
; g
y
, g
x
, 0]
if t = 0, rst iteration then
S
0
= I
3
else {t = 0, not the rst iteration}
S
t
= (I
3
+T
x
)S
tT
error = dot(S
t
(:, 1), S
t
(: 3))
S
t
(:, 1) = S
t
(:, 1) S
t
(: 3)error
g
/2
S
t
(: 3) = S
t
(: 3) S
t
(:, 1)error
g
/2
S
t
(:, 2) = cross(S
t
(: 3), S
t
(:, 1))
end if

2
= atan2(S
t
(2, 3), S(3, 3))

2
= asin(S
t
(1, 3))

2
= atan2(S
t
(1, 2), S(1, 1))
of the lters in rad/s.
The low pass lter is applied to the three angles estimated from the accelerometer and compass and
the high pass lter to the three angles estimated from on the gyroscopes. The resulting estimation is
presented in equation (9.4):

hp
+

lp

hp
+

lp

hp
+

lp
(9.4)
This procedure contains only one variable: the cutoff frequency. Through an iterative process, to mini-
mize the MSE (Mean Squared Error), a cutoff frequency 1/T = 1/0.8rad/s was chosen. The initial condition
set to the estimation of the gyroscopes has an impact on the initial behavior of the overall estimation. This
can be avoided by letting the algorithm work a certain time before take off or take as rst guess the angles
from the accelerometer and compass estimation.
9.1.2 Passive Complementary Filter
The passive complementary lter presented in [18], [19] and [20] is based on the same logic as the
previously described lter, but the formulation is slightly different. Instead of explicitly dening the high and
low pass lters with the cut-off frequency, the lters are implemented as in Figure 9.2, which represents
equation (9.1).
C(s)
Xacc
^
X
g
y
r
o
^
+
+
+
-
X
^
Figure 9.2: Complementary Filter.
58
Let R
s
= S
T
denote the transpose of the rotation matrix. The complementary lter can be written as:

R
s
= (R
s

B
+k
p

R
s

corr
)

R
s
()

=
_

_
0
3

2

3
0
1

2

1
0
_

corr
= k
est
/2vex(

R
s
T
R
s
R
s
T

R
s
)
(9.5)
where:

R
s
is the estimation of the direct cosine matrix;
B
is the gyroscope measurement vector; k
p
is a
proportional control variable, it corresponds to the cut-off frequency of the lters;
corr
is a correction term;
vex(

) = is a dened algebraic operator, corresponding to the inverse of ()

.
The general form of this complementary lter is based on the true value of R
s
to calculate the error

R
s
T
R
s
. However that term is not available and two options can be used to solve the problem. The
rst option is to consider the accelerometers and the compass as the best estimation and use the matrix
resulting from those sensors instead of R
s
. Let R
y
denote that matrix; equation (9.6) denes the matrix
according to [18]:

R
s
= (R
y

B
+k
p

R
s

corr
)

R
s
R
y
= arg(min(
1
|N
I

R
s
N
B
|
2
+
2
|g
I

R
s
a
B
|
2
))

corr
= k
est
/2vex(

R
s
T
R
y
R
y
T

R
s
)
(9.6)
where: arg(min()) is an optimization operation which nds the variable

R
s
that minimizes the function
The direct complementary lter introduces the idea of weighting an error measurement through
1
and

2
, allowing a higher importance to be given to the accelerometer or to the compass. This weighted error
is later passed onto
corr
. However, because optimization algorithms require a great computational power
which is incompatible with this real-time application, the direct complementary lter is dropped.
To simplify the process of calculating the error, equation (9.7) is proposed:

corr
=
1
(N
B

R
s
T
N
I
) +
2
(a
B

R
s
T
g
I
/g
0
) (9.7)
Instead of using R
y
, the passive complementary lter considers the estimated matrix

R
s
instead of R
s
:

R
s
=

R
s
(
B
+
corr
)

(9.8)
In addition, a term can be included to compensate for the bias. According to [18], the passive comple-
mentary lter can be written as:

R
s
=

R
s
(
B
+
corr

bias
)

corr
=
1
(N
B

R
s
T
N
I
) +
2
(a
B

R
s
T
g
I
/g
0
)

bias
=
i

corr
(9.9)
59
9.1.3 Discussion of the Complementary Filters
To evaluate the different lters, the full-state continuous LQR controller is used to close the loop using
ideal sensors. By implementing the model displayed in Figure 9.1 the estimations can be compared to the
real states and the MSE is computed to evaluate the performance of the different estimators. Note that
although the closed-loop is continuous, the lters are implemented in S-functions in a discrete mode. In
Figure 9.3, the behavior of the estimators are compared, and the passive complementary lter is imple-
mented with the bias compensation term in Figure 9.4.
0 10 20 30 40 50
-20
-10
0
10
20

(
d
e
g
)
Attitude Estimation with Complementary Filters


0 10 20 30 40 50
-20
-10
0
10
20

(
d
e
g
)


0 10 20 30 40 50
0
20
40
60
80

(
d
e
g
)
time(s)


Ideal sensor
First Filter
Passive Filter
Figure 9.3: Attitude Estimation with Complementary Filters.
Before comparing the lters, it is important to note that the rst complementary lter has only one
variable to tune which will weight the estimation bandwidth from the gyroscopes or both the accelerometer
and the compass. The cut-off frequency was set to 1/T
c
= 1/0.8rad/s. This value was obtained by a
trial and error process led by the trade-off between noise or bias ltering comparing the mean, standard
variation and mean squared error of the estimation.
The passive complementary lter has two parameters to independently control the importance given
to the accelerometer and the compass. This information is later used to correct the estimation from the
gyroscope. This procedure means that the two parameters allow the whole set of sensors to be weighted.
The chosen weights are:
1
= 0.5,
2
= 0.6 and
i
= 5.
The passive complementary lter appears to be a ner method than the rst lter; it allows a discrimi-
nate weighting of the individual sensors and is computationally more efcient to implement due to its lack
of renormalization. On the other hand it is less intuitive.
Both lters depend on inverse trigonometric functions. As seen from the rst complementary lter,
which has no bias compensation, the non continuity of the atan() function used to calculate the angle
60
0 10 20 30 40 50
-20
-10
0
10
20

(
d
e
g
)
Attitude Estimation withBias Compensation
0 10 20 30 40 50
-20
-10
0
10
20

(
d
e
g
)
0 10 20 30 40 50
0
20
40
60
80

(
d
e
g
)
time(s)


Ideal sensor
Passive Filter
Figure 9.4: Passive Filter with Bias Compensation.
Table 9.1: Comparison of Complementary Filters
Estimated Angles
Estimator
mse std mean mse std mean mse std mean
(deg)
2
(deg) (deg) (deg)
2
(deg) (deg) (deg)
2
(deg) (deg)
First Filter 76.69 8.25 3.03 42.30 6.50 3.08 665.34 25.34 2.67
Passive Filter 35.10 2.14 5.32 12.61 1.83 3.04 161.06 3.06 12.31
Filter w Bias Compensation 6.73 2.36 1.07 7.12 2.66 0.19 3.57 0.80 1.82
61
is evident; because of the bias, the estimate based on the gyroscopes reaches the limit of denition of
the trigonometric function, triggering the non-continuity. To correct that, the attitude should be computed
through a supplementary algorithm, which would make it even less computationally efcient. A comparison
of different parameters are presented in Table 9.1.
Because of that non-continuity, the rst complementary lter presents a worse estimation than the
passive lter for the three angles. As a consequence only the passive lter was further investigated and
implemented with bias compensation.
As seen again from Figure 9.4, the bias compensation term appears to reduce greatly the error; from
Table 9.1, it is seen that the mean error is reduced for every angle. However, the noise affecting and
increases; indeed the standard variation appears to be greater with bias compensation than without. Al-
though not represented, this lter also fails because of the trigonometric functions; it has been experienced
that too strong correction factors cause the asin() function to provide imaginary numbers.
As a conclusion, it appears that the passive complementary lter is better than the rst complementary
lter (with the chosen set of parameters) but is still too dependent on trigonometric functions and struggles
with the noise ltering.
9.2 Kalman Filter
An alternative to the complementary lters is to use the Kalman lter as done in [16], [29] and [14].
According to [31] and [30], the Kalman lter is an optimal linear model-based observer-corrector estimator;
it is optimal in the sense that it minimizes the estimated covariance error.
Consider the discrete linear system described by equation (9.10):
X
k+1
= A
d
X
k
+B
d
U
k
+w
k
Y
k
= C
d
X
k
+D
d
U
k
+v
k
(9.10)
where: A
d
, B
d
, C
d
, D
d
are the discrete state space matrices. X
k
is the state vector at time k. w
k
and v
k
are respectively the process and measurement noises.
Let Q
k
and R
k
denote the process and measurement noise covariances of the stochastic variables w
k
and v
k
. The Kalman lter can be written as a set of mathematical equations (described by the diagram of
Figure 9.5). First the lter acts as a predictor: it uses the model of the system, the current state and the
input vector to predict the future state considering the covariance error; this is the time update phase. Then
the measurement update phase corrects the predicted state and the estimated covariance error according
to the measurements and the noise covariance. The Kalman gain K
k
, weights the time prediction and the
measurement correction.
The following approaches to the Kalman lter are all implemented in the discrete mode using S-
functions. The discretization is performed computationally using equations (6.11). The matrices R
k
and
Q
k
are diagonal matrices with values corresponding to the noise characteristics of the sensors. To obtain
the discrete weighting matrices, equation (9.11) is used:
R
d
= R
k
/T
Q
d
=
_
T
=0
e
A
Q
k
e
A
d
(9.11)
Figure 9.6 presents the position of the Kalman lter in the general bloc diagram.
62
Time Update (Prediction phase) Measurement Update (Correctionphase)
(1) Compute the Kalman Gain
(2) Update estimate with measurement Y
k
(3) Update the Error Covariance
(1) Project the State ahead
(2) Project the Error Covariance ahead
K
k
= P
-
k

C
d
T
( C
d

P
-
k

C
d
T
+ R )
-1
X
k
= X
-
k
+ K
k


( Y
k
- C
d

X
-
k
- D
d

U
k
)

P
k
= ( I - K
k
C
k
)

P
-
k

X
-
k
= A
d
X
k-1
+ B
k
U
k

P
-
k

= A
d
P
k-1
A
d
T
+ Q

Initial Estimates X
k-1
and

P
k-1
^ ^ ^
^ ^
^
Figure 9.5: Mathematical Formulation of the Kalman Filter[31].
X
k
=A
d
X
k
+ B
d
U
k

Y
k
=C
d
X
k
+ D
d
U
k

Kalman Filter
U
k
X
k
Y
k
Y
0
U
0
X
0
X
k
^
+
+
+
+
-
-
Figure 9.6: Block Diagram of the Kalman Filter.
63
9.2.1 Simple Linear Kalman Filter
A rst approach suggests using the full observable system with Y = [a
x
, a
y
, g
x
, g
y
, g
z
, N
y
]
T
, X =
[P, Q, R, , , ]
T
and the PWM signal as input variable.
The covariance matrices and initial covariance error matrix were taken using the noise characteristics
as:
Q
k
= diag([.001, .001, 1, .001, .001, 10])
R
k
= diag([0.09325, 0.08853, 0.245, 0.3012, 0.1961, 0.295])
P
0
= eye(6)
(9.12)
The values used for Q
k
were obtained by trial and error method with a trade off between the MSE, the
variance of the error and the mean error. The initial state vector condition is set as the linearization point,

X
0
= [0, 0, 0, 0, 0, 0]
T
. The sensors are affected by noise and bias; the matrices Q
k
and R
k
are used to
weight the condence in the measurements or in the model.
The performance of this lter is presented and compared with other options of lters in further sections.
This conguration of the Kalman lter is computationally heavy but provides the system with an estimation
of the attitude and the angle rates.
9.2.2 Improved Linear Kalman Filter
According to [29] there is a possible improvement. Reminding the concept of the complementary lters,
in which the gyroscope and the other sensors are treated separately, the idea of writing the model in a
more compact form comes to mind. The complementary lters integrate the gyroscopes to predict the
evolution of the the attitude and correct that prediction with the information from the accelerometer and
the compass. Instead of using the PWM signal as the input vector, one can write the system so that the
gyroscopes are taken as input vector as:

X =
_

_
0 0 0
0 0 0
0 0 0
_

_
X+
_

_
1 0 0
0 1 0
0 0 1
_

_
U (9.13)
where: X = [, , ]
T
; U = [P, Q, R]
T
.
Naturally matrices C and D are also adapted to the new system. This approach could be seen as
clumsy but the loss in input information and disregard for part of the model benets the robustness of the
estimation. The covariance weights are set using the noise characteristics as:
Q
k
= diag([.02, .02, 2])
R
k
= diag([0.09325, 0.08853, 0.245, 0.3012, 0.1961, 0.295])
P
0
= zeros(3)
(9.14)
The entries of matrix Q
k
represent the trust given to the gyroscopes. Since it is known that the bias
affects the third reading of the gyroscopes more than the others, a different coefcient is given to this one.
The algorithm is implemented as the previous version of the Kalman lter.
The improved Kalman lter is computationally more efcient than the rst approach; on the other side,
only the estimation of the attitude is obtained, a control strategy based on this conguration of the Kalman
lter requires using the output of the gyroscopes as direct measurement of the angular velocities.
64
9.2.3 Extended Kalman Filter
The Kalman lter is a very powerful tool, which has been greatly studied and used over the past
decades. One of the successful modication of the basic Kalman lter is the Extended Kalman lter (EKF).
The EKF addresses the problem of estimating a state based on a non-linear model. The EKF linearizes
the model about the current estimation. Figure 9.7 presents the algorithm. Pay special attention to the fact
that instead of using the matrices A
d
, B
d
, C
d
, D
d
, the nonlinear output and dynamic functions are used to
linearize around the current state, resulting in the use of A
k
and C
k
, obtained at each iteration.
Time Update (Prediction phase) Measurement Update (Correctionphase)
(1) Compute the Kalman Gain
(2) Update estimate with measurement Y
k
(3) Update the Error Covariance
(1) Project the State ahead
(2) Project the Error Covariance ahead
K
k
= P
-
k

C
k
T
( C
k

P
-
k

C
k
T
+ R )
-1
X
k
= X
-
k
+ K
k


( Y
k
- h(X
k-1
,U
k
, 0)
P
k
= ( I - K
k
C
k
)

P
-
k

X
-
k
= g(

X
k-1
,U
k
, 0)
P
-
k

= A
k
P
k-1
A
k
T
+ Q

Initial Estimates X
k-1
and

P
k-1
^ ^ ^
^ ^
^
Figure 9.7: Bloc Diagram of the EKF [31].
This application of EKF is based on the same logic as the improved Kalman lter, considering the same
system, where the dynamic equation is already linear by nature, simplifying greatly the computations. As
a result, the time update can be performed as previously, with A
k
= A
d
and B
k
= B
d
. The same logic can
be applied to D
k
= D
d
.
However C
k
will have to be dened at each step. The algorithm 9.3 is the one used.
The algorithmis simple but requires the use of trigonometric functions for each iteration. The covariance
matrices are presented by equation (9.15):
Q
k
= diag([.02, .02, 2])
R
k
= diag(([0.09325, 0.08853, 0.245, 0.3012, 0.1961, 0.295]))
P
0
= zeros(3)
(9.15)
9.2.4 Discussion of the Kalman Filters
The estimations obtained with the described methods are presented in Figure 9.8. The MSE, standard
deviation and mean estimation error are presented in Table 9.2.
The linear lter and the improved lter are both linear methods. They both present the same impos-
sibility of reaching an estimated angle over 57deg. Indeed, the approximation sin = is correct only
for small angles around the linearization point. However since the trigonometric function is bounded, the
approximation will also be bounded by < 1rad, which explains the 57

bound.
65
Algorithm 9.3 Estimation using the EKF.
Y
k
= [a
x
, a
y
, g
x
, g
y
, g
z
, N
y
]
u
k
= g
B
dening the state trim condition for iteration k X
0
k
= [0, 0,

k1
]
T
dening the output trim condition for iteration k Y
0
k
= [0, 0, 000, sin(
k1
)]
T
Time Update:
X

k
= X
0
k
+A
d
(

X
k1
X
0
k
) +B
d
u
k
P

k
= A
d
P
k1
A
T
d
+Q
k
Linearization:
C
k
=
_

_
0 g
0
0
g
0
0 0
0 0 0
0 0 0
0 0 0
0 0 cos(X

k
(3))
_

_
Measurement Update:
K
k
= P

k
C
T
k
(C
k
P

k
C
T
k
+R
k
)

1
X
k
= X

k
+K
k
(Y
k
(C
k
(X

k
X
0
k
) +D
d
u
k
) Y
0
k
)
P
k
= (I(3) K
k
C
k
)P

k
0 10 20 30 40 50
-20
-10
0
10
20

(
d
e
g
)
Attitude Estimation via Kalman Filter
0 10 20 30 40 50
-20
-10
0
10
20

(
d
e
g
)
0 10 20 30 40 50
0
20
40
60
80

(
d
e
g
)
time(s)


Ideal sensor
Linear Filter
Improved Filter
Extended Filter
Figure 9.8: Attitude Estimation with Kalman Filters.
66
The linear lter has the advantage of estimating all necessary states whereas the improved lter has no
processing on the angle rates. On the other side, the improved lter is computationally more efcient and
presents a better estimate; the mean squared error, the variance and the mean error are smaller using the
improved lter. It is interesting to notice that although part of the model was abandoned, the lter is able to
reach better estimated values.
The advantage of the EKF is to overcome the = 1rad limitation; therefore considering that for the
hovering situation, the angles and do not change considerably, the system is only linearized around

k
. It is possible to observe that the estimation of the yaw angle is no longer limited by the trigonometric
function. It is interesting to notice that the EKF has a slightly bigger error variance than the improved lter
although the mean error and the mean squared error are smaller. It suggests that the EKF lters out slightly
less noise but reaches a better stationary estimation. A trade off seems to exist between ltering the bias
and ltering the noise.
Table 9.2: Comparison of Kalman Filters
Estimated Angles
Estimator
mse std mean mse std mean mse std mean
(deg)
2
(deg) (deg) (deg)
2
(deg) (deg) (deg)
2
(deg) (deg)
Linear Filter 29.91 4.80 2.62 9.70 3.09 0.40 104.47 7.38 7.06
Improved Filter 5.08 1.85 1.28 4.22 1.59 1.32 99.22 7.31 6.75
Extended Filter 4.20 2.02 0.33 3.19 1.78 0.12 1.07 1.02 0.13
As a consequence, to lter even more the noise, an EKF with bias estimation is implemented. In this
strategy, a greater importance is given to the noise ltering while the bias estimator cancels the bias.
The idea behind it, is to write the model with three supplementary states corresponding to the bias as in
equation (9.16), with X = [, , , b
g1
, b
g2
, b
g3
] and U = [P, Q, R]
T
:

X =
_

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

_
X+
_

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

_
U (9.16)
The improvement obtained with the bias estimation is not signicant. With the considered values for
the bias affecting the gyroscopes, this new strategy presents no signicant improvements when compared
to the standard EKF.
From the different variations of the Kalman lter, one can conclude that the standard EKF is the best
estimator. It is computationally more efcient than the complementary lter with bias estimation and it is
able to overcome the linearization limits.
67
9.3 Height Estimation
So far the considered estimators regarded the attitude. They either estimated the six states or took
directly the gyroscope measurements as the angle rates states. The height and vertical velocity are con-
sidered in this section.
To control the height, the range nder is used as a direct estimate. However that is not sufcient, the
LQR controller needs also an estimation of the vertical velocity W. Different options can be used. A rst
hypothesis is to derive the range nder measurement. The second is to integrate the accelerometer.
Since the height estimation is not the main concern of this work, it is rather a security feature to keep
the quadrotor from crashing if necessary, a Kalman lter will not be implemented to dene with precision
the height. The implemented approach consists in considering that a high pass lter works as a derivation.
In fact the high pass lter basically lters out the low frequency dynamics, not affecting the high frequency
variations. Those variations can be approximated to the differentiation.
Therefore, setting a time constant to 0.005s, a high pass lter is used to estimate the vertical veloc-
ity and the range nder is used to estimate directly the height. It would however be possible to use a
complementary lter or Kalman lter could fuse the accelerometer measurement and the range nder.
9.4 Conclusion about the Estimation
In this section different complementary lters and Kalman lters have been presented. To chose the
estimator, the computational requirements and the behavior of the estimator were taken into account.
Because the complementary lter is too dependent on inverted trigonometric functions, the standard EKF is
chosen to be implemented on the prototype. Moreover, the EKF presents better results. The big advantage
of the Kalman lter over the complementary lter is the ability to weight independently the different output
variables and the state variables, while the complementary lter mostly sets the cut-off frequency and
correcting coefcient.
In the next chapter, the height estimator and EKF will be implemented on the Arduino board and tested.
68
Chapter 10
Practical Implementation
The controller developed so far is based on the 12 states model and considers the ideal sensors; it was
used to develop the estimation strategies and to tune the estimators. However, because the 12 states are
not available since the model is not fully observable, in this chapter, a controller based on the 8 states model
(6 states from the attitude and 2 states from the height estimator) is designed to stabilize the quadrotor,
rst using ideal data and then using the estimated values. Then the communication procedure necessary
to remote control the quadrotor is presented. Finally, the estimator is tested on the prototype to validate
the implementation and the communication procedure.
10.1 Attitude Stabilization Via LQR
10.1.1 Continuous LQR
To stabilize the quadrotor in a hovering ight 8 states are necessary: X = [W, P, Q, R, Z, , , ]
T
and
U = PWM. The stabilization problem corresponds to keeping the quadrotor hovering at a certain height.
The model considered to obtain this controller is obtained from the initial 12 states model, eliminating the
rows and columns corresponding to unnecessary states:

X =
_

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

_
X+
_

_
0.0307 0.0310 0.0421 0.0307
0 1.0851 0 1.0743
1.0742 0 1.4713 0
0.0483 0.0488 0.0661 0.0483
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
_

_
U (10.1)
The weighting matrices of the 8 states controller are obtained applying the Brysons rule, followed by a
ne tunning as for the 12 states controller. They are written as:
R
lqr
= diag([0.01, 0.01, 0.01, 0.01])
Q
lqr
= diag([10, 100, 100, 50, 10, 150, 150, 60])
(10.2)
To test the controller, rst a reference is set to X
ref
= [0, 0, 0, 0, 0, 1, 0, 0, 0]
T
and the initial conditions
of the states are set to that same reference. Then a second test considers the same reference with initial
69
conditions of 0.2rad for the attitude and 2m for the height. The rst test gives a rst idea about the stability
of the controller and generates data to be compared with the discrete controller. The second test conrms
the ability of the controller in effectively stabilizing the quadrotor.
Figure 10.1 presents the response of the controller to the stabilization problem corresponding to the
rst test. Figure 10.2 presents the closed loop response of the system with different initial conditions. Note
that the initial conditions were tested one at the time; the presented gure corresponds to 4 simulations
each setting a different state with its initial condition at the time.
0 10 20 30 40 50
-2
0
2
4
[
U
,
V
,
W
]
T


U
V
W
0 10 20 30 40 50
-1
0
1
[
P
,
Q
,
R
]
T


P
Q
R
0 10 20 30 40 50
-10
0
10
[
X
,
Y
,

Z
]
T


X
Y
-Z
0 10 20 30 40 50
-0.5
0
0.5
[

]
T
time(s)

Figure 10.1: Stabilization via Continuous LQR.

The controller appears to correctly stabilize the quadrotor, keeping it at the desired height and keeping
the attitude near the desired values. Due to the nonlinearities affecting the model, as the quantizer of the
motors and the different static gains of each motor, a small static error appears to affect the attitude. As a
result, because this controller has no action nor measurement of the position, the phenomenon of random
walk is visible; the error in the attitude causes the quadrotor to randomly move along X and Y .
The test with different initial conditions presents the same error in the attitude but it is covered by
the scale of the graphic. To understand the effect of the nonlinearities of the model of the motor on the
stabilization performance, the rst test is presented in Figure 10.3 removing the quantizer rst and then
also considering all motors to be identical.
The simple removal of the quantizer reduces the static gain by over ten times. As a consequence, the
random walk affecting the quadrotor is less signicant with a drift about ten times smaller at the 25s mark.
Considering in addition that the motors are all identical, removes completely the static gain and the random
walk. This proves that not only the random walk is caused by the static gain affecting the attitude, but also
that this static gain is caused by the nonlinearities of the model of the motors. This suggests also that
having identical speed controllers would greatly increase the performance of the controller.
70
0 1 2 3 4 5
-10
0
10
20

(
d
e
g
)


0 1 2 3 4 5
-20
0
20

(
d
e
g
)


Response
Reference
0 1 2 3 4 5
-10
0
10
20

(
d
e
g
)


Response
Reference
Response
Reference
0 1 2 3 4 5
1
1.5
2
-
Z
(
m
)


Response
Reference
Figure 10.2: Continuous Closed Loop Response to Initial Conditions.
0 5 10 15 20 25
-1
0
1
[
X
,
Y
,

Z
]
T
Stabilization Via LQR without any Nonlinearities


X
Y
-Z
0 5 10 15 20 25
-0.1
0
0.1
[

]
T
time(s)

0 5 10 15 20 25
-1
0
1
[
X
,
Y
,

Z
]
T
Stabilization Via LQR without Quantizer


X
Y
-Z
0 5 10 15 20 25
-0.1
0
0.1
[

]
T

Figure 10.3: Closed Loop Response.

71
Moreover, the 12 states controller fromChapter 8 does not present any static error for the attitude. When
is positive V is positive. The 12 states controller not only corrects the attitude but also V ; correcting V
results in increasing M
4
and reducing M
2
, which results also in correcting . The same thing happens with
and U. A very important conclusion can be drawn from here: the position control method not only corrects
the random walk phenomenon, but also improves the attitude stabilization. This justies the remote control
as an interesting stabilization tool.
From Figure 10.2, it is concluded that, with settling times (within 2% of the nal value) of about 1.5s for
and , 2s for and 3s for Z, the controller is stable and well tuned. The poles of the closed loop system
are presented in Table 10.1.1, proving the stability of the system.
Table 10.1: Stabilization Closed Loop Poles
Real (rad/s) -182.18 -152.69 -7.26 -1.64 -1.10 -1.25 -1.22 -1.22
10.1.2 Discrete LQR
The controller is dened in the continuous space so far but the processor is to operate at a 50Hz fre-
quency. In this section the 8 states discrete controller is presented. To obtain a discrete LQR, one can use
the lqrd() function in Matlab. The function receives the weighting matrices and the state equation matrices
A and B as well as the sampling time. The function performs the discretization from equation (6.11) to
obtain A
d
and B
d
and from equation (9.11) to obtain the discrete weighting matrices.
The closed loop response to the stabilization problem with a reduced state controller is presented in
Figure 10.4. The reference signal is: X
ref
= [0, 0, 0, 0, 0, 1, 0, 0, 0]
T
and the initial condition is set to the
reference condition. The weighting matrices used for the discrete controller are the ones used for the
continuous controller after discretization.
Figure 10.5 presents the stabilization via discrete LQR with initial conditions different from the reference.
Is shows that the LQR operates correctly and is able to stabilize the quadrotor in the stated conditions. The
initial conditions for the attitude are 0.2rad and 2m for the height. The random walk phenomenon appears
to affect the longitudinal position. However note that the effect is weaker with the discrete controller than
with the continuous one; only after 30s of simulation is the quadrotor crossing the 10m barrier whereas in
the continuous case, that happens right after 20s. Table 10.2 presents a comparison of the continuous and
discrete LQR. The random walk is justied by the mean error of the angles causing the quadrotor to drift
randomly. As suggested by the Table, the mean error of the angles is smaller in the discrete case than in
the continuous one.
Table 10.2: Comparison of Continuous and Discrete Controllers
Estimated Angles
Estimator
mse std mean mse std mean mse std mean
(deg)
2
(deg) (deg) (deg)
2
(deg) (deg) (deg)
2
(deg) (deg)
Continuous 0.016 0.017 0.126 1E-4 8E-4 0.010 0.003 0.012 0.059
Discrete 0.015 0.049 0.111 0.003 0.052 0.018 9E-4 0.020 0.022
72
0 10 20 30 40 50
-2
0
2
4
v
e
l
o
c
i
t
y
(
m
/
s
)
[U, V, W]
T


U
V
W
0 10 20 30 40 50
-1
0
1
a
n
g
v
e
l
(
r
a
d
/
s
)
[P, Q, R]
T


P
Q
R
0 10 20 30 40 50
-10
0
10
p
o
s
i
t
i
o
n
(
m
)
[X, Y, Z]
T


X
Y
-Z
0 10 20 30 40 50
-0.5
0
0.5
a
t
t
i
t
u
d
e
(
d
e
g
)
[, , ]
T
time(s)

Figure 10.4: Stabilization via Discrete LQR.

0 2 4 6 8 10
0
5
10
15

(
d
e
g
)


Response
Reference
0 2 4 6 8 10
-10
0
10
20

(
d
e
g
)


Response
Reference
0 2 4 6 8 10
-10
0
10
20

(
d
e
g
)


Response
Reference
0 2 4 6 8 10
0.5
1
1.5
2

Z
(
m
)
time(s)


Response
Reference
Figure 10.5: Discrete Closed Loop Response to Initial Conditions.
73
Unsurprisingly, the discrete controller presents bigger standard deviation values of the reference track-
ing error. On the other side the mean error is smaller for and in the discrete case while presents a
similar value. This improvement in the static error results in a reduction in the random walk phenomenon.
The closed loop poles are plotted in Figure 10.6; all the poles are within the unit circle, conrming that
the controller stabilizes the system.
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
real axis
i
m
a
g
i
n
a
r
y

a
x
i
s
Discrete Close Loop Poles
Figure 10.6: Discrete Closed Loop Poles.
A continuous and a discrete 8 states LQR controllers have been developed and compared in this sec-
tion. It appears that the discrete controller has an advantage over the continuous controller concerning
the random walk but on the other side it presents bigger variations. To test this controller, the estimation
feedback is added to the simulations.
10.2 Discrete LQR with Estimation Feedback
The behavior of the 8 states discrete controller using ideal sensors has been proven to be stable and
effective at stabilizing the attitude and height of the quadrotor. The EKF has also proven to provide accurate
estimations of the attitude. It is now of interest to analyze the combination of both.
From simulations, it is found that the controller cannot stabilize the quadrotor due to excessive cor-
ruption of the gyroscopes measurements. Two options are available: to augment the order of the system
considering the bias estimation terms or use a high pass lter. Applying a high pass lter, the bias can
be eliminated without augmenting the order of the system. Figure 10.7 presents the simulation, using
X
ref
= [0, 0, 0, 0, 0, 1, 0, 0, 0]
T
as a reference and initial condition.
For this simulation the matrices of the controller and estimator are re-tuned resulting in (10.3) for the
LQR and (10.4) for the EKF:
Q
lqr
= diag([60, 5, 5, 20, 100, 10, 10, 30])
R
lqr
= diag([0.01, 0.01, 0.01, 0.01])
(10.3)
Q
k
= diag([.05, .05, .1])
R
k
= diag([.2, .2, .05, .05, .1, .01])
(10.4)
Because the angular velocities remain mostly affected by high frequency noise, the weighting values of
the controller are lowered (when compared to the ideal sensor case) to allow bigger variations. Stronger
74
0 10 20 30 40 50
-0.5
0
0.5
W
(
m
/
s
)
0 10 20 30 40 50
-0.5
0
0.5
a
n
g
v
e
l
(
r
a
d
/
s
)


P
Q
R
0 10 20 30 40 50
0.9
1
1.1

Z
(
m
)
Z step
0 10 20 30 40 50
-10
0
10
a
t
t
i
t
u
d
e
(
d
e
g
)
time(s)

Figure 10.7: Attitude Stabilization.

values cause the controller to ght the high frequency variations and unstablizes the quadrotor. Table 10.3
presents some characteristics of the attitude error.
Table 10.3: Attitude Error

mse std mean mse std mean mse std mean
(deg)
2
(deg) (deg) (deg)
2
(deg) (deg) (deg)
2
(deg) (deg)
5.18 2.26 0.25 3.08 1.75 0.10 0.50 0.70 0.11
All sensors are considered to work at freq = 50Hz except the range nder which is set at 25Hz. The
high pass lter used to obtain the vertical velocity uses a cut-off frequency 1/0.005rad/s and the high pass
lter used for the gyroscopes uses a cut-off frequency 1/1rad/s.
This simulation suggests that the quadrotor could be stabilized for a hover ight using the chosen
sensors and the implemented control and estimation methods. Unsurprisingly, the standard variation and
the mean error are greater in the simulation using the estimation feedback. This suggests that the random
walk phenomenon is stronger. However, since the controller is not the same, a full comparison cannot be
made. To further analyze the stabilization of the attitude and altitude with estimation feedback, another test
is proposed: setting the initial conditions to 0.2rad for the angles and 2m for the height keeping the same
reference vector. The result is shown in Figure 10.8.
Because the noise affecting the measurements keeps the quadrotor moving around the reference val-
ues, it is difcult to correctly measure a rising time or settling time for or . However the test conrms that
75
0 5 10 15 20
0
1
2
time(s)
-
Z


Response
Reference
0 5 10 15 20
-10
0
10

0 5 10 15 20
-10
0
10

0 5 10 15 20
-10
0
10
p
s
i
Figure 10.8: Attitude Stabilization with Initial Conditions.
the LQR and EKF solution for attitude and height estimation is an efcient method.
10.3 Remote Control
It has already been seen that the control of the position has a positive inuence on the stabilization of
the attitude. It is proposed to use remote control telemetry to control the position of the quadrotor. Instead
of setting a reference in the position directly, the user changes the attitude reference and therefore indirectly
control the position of the quadrotor. A joystick is used to set the and references and an analog cursor
is used for Z. Because the joystick only has three analog inputs, the yaw angle is not considered.
The Arduino board may communicate via wireless using a shield, with an identical set plugged to a
computer, a user can send through a USB port information onboard Arduino. In practical terms, two
small programs are used to communicate with the board. The main program, modied from
1
and written
in Python, connects the joystick and the wireless device, both plugged to USB ports. This program is
responsible for data transfer between both devices. Since the original program was intended to control
the position of servos, little modications were required. A second program is used as a satellite program,
allowing the user to visualize the data plotted in real time. This satellite program is connected to the main
one through a virtual USB port. Figure 10.9 depicts the communication scheme.
The joystick has three analog outputs with range [1, 1]. This interval is converted to [0; 255] by multi-
plying and adding 127. The idea is that both the processor and the wireless device read ASCII characters
as one byte data. Therefore by only sending one ASCII number for each control action, the amount of data
1
url = http://principialabs.com/arduino-python-4-axis-servo-control/
76
Arduino
Board
XBee
Shield
XBee
Shield
Joystick
Python Main
Program
C++ Visualization
Satellite Program
virtual USB
connector
wireless
COM 5
COM 17 COM 7
COM 3
Quadrotor Ground Station
Figure 10.9: Communication Set.
sent is reduced. A security feature is also present: by pressing a chosen digital button, the motors can be
turned off instantly. This is a great advantage to avoid accidents.
The Python program only sends data when the user manipulates the joystick, or keeps it away from its
idle position. It is capable of sending data to the onboard Arduino but also to retrieve data fro it. In the next
section the communication set is used to retrieve data computed by the estimation onboard the Arduino,
conrming that the protocol is efcient.
10.4 Practical Validation of the Estimator
A main concern of the practical implementation of the algorithms on the Arduino board is to respect the
desired frequency. Indeed, the board has several tasks to perform each cycle of 0.02s:
Data acquisition from the sensors
Data conditioning
States Estimation
Communication with the ground station
Compute control signal and send it to the motors
In section A.4, the program and its modules are presented. To assure that the chosen frequency
is respected, the EKF algorithm is optimized: to avoid multiplications involving sparse matrices, which
generate mostly sparse matrices and vectors, the algorithm is broken down to operations not involving null
multiplicator. This choice makes it more difcult to change the algorithm but guarantees that the 0.02s cycle
is respected.
To test the EKF on the prototype, using the gyroscopes, two tests are undertaken: in the rst, the
prototype is held still, thus having = = 0 and (t) = 0, with all motors on, and the behavior of the
estimator is compared to the values presented in Table 9.2. The estimated data is acquired and plotted in
Figure 10.10. Table 10.4 presents the mean estimated value for the attitude,

, and the standard deviation
of the error. The objective is to test whether the implemented EKF is affected by any bias or not. Note
that because the prototype is not aligned with the magnetic North, the values of are not expected to be
= 0: the variations of the estimation is the subject of interest in this test.
Then in a second test, the EKF is tested using different attitude references, to verify whether the im-
plemented EKF is able to properly track the estimated attitude. The quadrotor is set horizontally, then it is
moved to = 10

and = 0

, and nally to = 10

and = 0

. The results are presented in Table 10.5,

77
showing the standard deviation of the error and the mean measurement written

for simplicity. Only the
stationary estimations, corresponding to the interval between movements, are characterized.
Finally, the yaw motion is tested separately because it requires additional caution due to the difculty in
dening the magnetic North in the laboratory. First the prototype is turned on and aligned with the magnetic
North, then successively it is rotated to = 10

, 90

and 180

. Because it is difcult to manually determine

the angles with precision to rotate the prototype. The results are presented in Table 10.6, showing the
mean measurement and the standard deviation of the measurement.
10 11 12 13 14 15 16 17 18
-20
-15
-10
-5
0
5
time(s)
A
t
t
i
t
u
d
e
(
d
e
g
)

Figure 10.10: Implementation of the Estimator.

Table 10.4: Attitude Estimation Error with null Reference

std mean std mean std mean
(deg) (deg) (deg) (deg) (deg) (deg) (deg) (deg) (deg)
0 0.547 -0.452 0 0.715 0.099 0 0.270 -15.360
Recalling from Table 9.2 the results obtained in simulation using the EKF, it is shown that the mean
error of the estimation of , and in simulation and in the implementation are similar: 0.30

for and
0.12

for in simulation, and 0.45

for and 0.1

for in the implementation. These mean values conrm

that the initialization routine, which compensates for any constant bias, is efcient.
The standard deviation of the error are better in the implementation than in simulation. Indeed, for the
simulation indicates a 2.02

standard deviation, which is greater than the 0.547

measured. The same result

is seen regarding , where the simulation indicating 1.78

contrasts with the 0.715

measured , and , where

1.02

of the simulation is greater than 0.27

of the implementation. The difference between the simulation

and the implementation can be explained by a conservative approach of the noise measurements and
improvements of the wire connections of the sensors. Nevertheless, the test shows that the EKF is not
affected by any time varying bias; the EKF successfully lters out the noise affecting the sensors.
The values obtained in both tests are consistent. Indeed, the mean values and the standard deviation
of the error concerning and are similar for the test where the prototype was kept horizontally, Table
10.4, and the test where the attitude is changed, Table 10.5. Therefore, the EKF proves to be efcient at
78
Table 10.5: Attitude Estimation Error with different References

std mean std mean
(deg) (deg) (deg) (deg) (deg) (deg)
0 0.482 0.249 0 0.649 -0.103
10 0.514 10.32 0 0.729 -0.063
0 0.524 0.322 10 0.682 9.973
updating the attitude during the motion of the prototype.
Table 10.6: Heading Estimation

std mean
(deg) (deg) (deg)
0 0.297 1.629
10 0.278 10.550
90 0.287 92.572
180 0.331 186.302
From Table 10.6 the mean values obtained in simulation are better than the mean values obtained with
the implementation. This is due to the difculty of manually turning the prototype. The values obtained for
the standard deviation of the error are in concordance with the values presented in Table 10.4. This test
conrms the ability of the EKF to estimate the attitude, namely .
These tests suggest that EKF is a suitable method for attitude estimation purposes. Moreover, because
the EKF acts as predicted by the simulations, it suggests that the noise identication of the sensors models
is correct.
To validate the estimation of the height, the prototype is taken to 10cm, 20cm and nally 30cm. The
mean estimated value for each height is then compared to the nominal value. This test is not only intended
to validate the estimation of the height but also the model of the range nder. It was considered that the
noise affecting the sensor was neglectable. The standard variation of the measurement error is then a good
indicator of the accuracy of the model. Unfortunately, because there are no means of correctly measuring
the real vertical velocity of the prototype, the speed estimator can not be completely tested, however it is
expected to show that the vertical velocity is null. Table 10.7 presents the mean values of the height and
the standard deviation of the measurement.
It is shown in Table 10.7 that the mean value is not identical to the nominal value. This error comes from
the difculty of correctly position the prototype at the dened height. Since the range nder is intended to
be used to keep the quadrotor hovering at a dened height, any constant bias can be corrected by remote
control as already explained. As expected, because the behavior of the range nder is not linear, the
standard deviation increases with the distance to the oor, reaching 0.73cm for a 30cm nominal height.
79
Table 10.7: Height Estimation
W
Z std mean W std mean
(cm) (cm) (cm) (m/2) (m/2) (m/2)
10 0.203 10.665 0 0.070 0.0013
20 0.560 21.200 0 0.249 0.0044
30 0.731 28.651 0 0.364 0.0049
This suggests that the model of the range nder may not be accurate and the height estimation should
include a noise ltering operation.
The values obtained concerning the vertical velocity correspond to the expected values. The mean of
the estimation correspond to the expected values although they are not a good indicator of the behavior
of the estimator because no velocity references are tested. Unsurprisingly, the standard deviation of W
increases with the increase in the standard deviation of Z. This is expected because the vertical velocity
is obtained by high pass ltering the height estimation. This suggests that improvements in the height
estimation would result in improvements in the W estimation. A possibility is to use a barometric sensor
for the height estimation. Not only is the sensor less affected by external factors as the material of the oor
but it is unaffected by tilt and roll motions.
In this chapter the loop was closed using an 8 states LQR controller with estimation feedback sug-
gesting that the chosen estimation and control methods are effective at stabilizing the quadrotor. The
practical implementation of the estimator was explained and tested. The results of the attitude estimation
corresponded to the results expected from the simulations, conrming not only the validity of the estimator
but also the validity of the model of the sensors. The practical implementation of the height estimation
showed that the model of the range nder may be inaccurate and that a barometric sensor would reach
better results. Finally, the remote control and communication procedure was explained to allow remote
control of the prototype. Since the acquisition of the practical estimation data was conducted through the
communication procedure, the latest is validated also.
80
Chapter 11
Conclusions and Future Work
11.1 Conclusions
With the aim of creating a functional platform, able to sustain an autonomous hovering ight with small
drift in position and a stabilized ight when remotely controlled, different steps were reached:
The model of prototype was obtained dening experimentally parameters as the mass, the inertia
matrix and the center of mass. The method used to determine the center of mass via pictures of the
prototype was positively conclusive, providing a location consistent with previous works on the same
prototype.
The model of the sensors was obtained considering different approaches for the accelerometer: the
forces and the gravity vector. It was concluded that using the forces in the model compromised the
observability of the model and therefore the estimation of the attitude. For greater realism in the
simulations, the sensors were modeled using real noise measurements corresponding to the ight
conditions, therefore with all motors turned on. This implied doing the wiring of all components of the
prototype as well as the development of Arduino based code to read the sensor measurements and
interpret them. An initialization routine was created ensuring the elimination of any constant bias in
the IMU. It is important to note that the welding and wiring of the components requires great work
and patience.
The model of the actuators was obtained considering the speed controllers, the propellers and the
motors. The motors were approximated to a rst order system; all motors were considered to have
the same time constant which was obtained experimentally using a laser based tachometer to mea-
sure the angular speed of the propellers. Because the four speed controllers are not identical, the
stationary response and the dead zone of the motors varies. The tachometer allowed an estimation
of the stationary response of the different motors, which was used to compute the static gain of the
model of the motors. The laser based tachometer technique proved to be limited to low angular
speeds; over 950rad/s the apping of the blades made it difcult.
The models of the motors, sensors and quadrotor were linearized around a determined operating
point corresponding to the hover situation. The linearization was performed analytically and compu-
tationally, both obtaining the same result. The analysis of the model conrmed that the model of the
accelerometers based on the forces implied a loss of information compared to the approximation with
81
the gravity vector. A simulator was also developed and used to show that the open loop response
of the quadrotor was unstable, therefore a 12 states LQR was used to control the quadrotor and
generate data which was then used to test the estimators.
Different estimation approaches were considered, namely complementary lters including the pas-
sive complementary lter and different Kalman lters as the linear, the improved and the extended
Kalman lters. It was concluded that the EKF was the best estimator being insensitive to the bias of
the gyroscopes contrarily to the other Kalman lters.
An LQR controller based solely on the 8 observable states was then obtained and tuned in simulation
using ideal sensor data. Then the EKF and the LQR were combined to stabilize the quadrotor. It was
concluded that the estimation method and the control method were both efcient in simulation.
Finally the EKF was implemented on the prototype. Through a determined communication procedure,
the estimated values were plotted in real time and saved in a le for analysis. It was concluded that the
communication process between the ground station and the prototype was functional. The method-
ology used to send and receive data from the joystick to the prototype via wireless communication
was efcient with different programs in different languages combined to allow a fast conditioning of
the sent data and also allow real time visualization of the data sent from the prototype. This also
validates the model of the sensors with the exception of the range nder which proved to be noisy.
11.2 Future Work
Because the sensor measurements and in particular the characteristics of their noise are dependent
on the wiring, it is considered as a priority for future work to substitute the wiring for more trustworthy
elements. This may result in a considerable improvement of the estimation.
The angular speed of the propeller varies over time with the charge of the battery even though the
input signal to the speed controller remains unchanged. This suggests that the non linear behavior
between the battery voltage and the angular velocity should be determined and used to improve the
stabilization of the quadrotor.
It was shown that the difference between the motors caused the random walk phenomenon. There-
fore the speed controllers should be identical. This would greatly improve the performance of the
controller.
Different control techniques such as robust control and nonlinear control should be considered for
future works.
The estimation of the height should be done using a barometric sensor instead of an infra red sensor.
The improvement would result in a better estimation of the height and a possible better estimation of
the vertical velocity.
It is left for future work the implementation of the controller and the actual ight. It is expected to be
necessary some ne tuning of the controller to achieve a hover ight.
Finally, it was seen that the inclusion of a position controller results in a better control of the attitude.
The development of that controller is left as future work.
82
Bibliography
[1] Acar, C., A. Shkel, and C. Painter (2005, October). Two types of michromachined vibratory gyroscopes.
Sensors, 2005 IEEE.
[2] Ang, W. T., S. Y. Khoo, P. K.Khosla, and C. N.Riviere (2004). Physical model of a mems accelerometer
for low-g motion tracking applications. Volume 2, pp. 13451351. IEEE International Conference on
Robotics and Automation.
[3] Bouabdallah, S., M. Becker, V. de Perrot, and R. Siegwart (2007). Towards obstacle avoidance on
quadrotor. XII International Symposium on Dynamic Problems of Mechanics.
[4] Bouabdallah, S., P. Murrieri, and R. Siegwart (2004). Design and control of an indoor micro quadrotor.
Volume 5, pp. 43934398. IEEE International Conference on Robotics and Automation.
[5] Bouabdallah, S. and R. Siegwart (2005). Backstepping and sliding-mode techniques applied to an
indoor micro quadrotor. pp. 22472252. IEEE International Conference on Robotics and Automation.
[6] C.Konvalin (2008). Compensating for Tilt, Hard Iron and Soft Iron Effect.
[7] Domingues, J. (2009). Quadrotor prototype. Msc thesis, Instituto Superior Tecnico, UT Lisbon.
[8] Euston, M., P. Coote, R. Mahony, J. Kim, and T. Hamel (2008). A complementary lter for attitude
estimation of a xed-wing uav. pp. 340345. International Conference on Intelligent Robots and Systems,
2008. IROS 2008. IEEE/RSJ.
[9] Fisher, M. (2007). Attitude stabilisation of a quadrotor aircraft. http://www.google.com/url?
sa=t&source=web&cd=1&ved=0CBgQFjAA&url=https%3A%2F%2Fptop.only.wip.la%3A443%2Fhttp%2Fwww.fseh.cqu.edu.au%2FFCWViewer%
2FgetFile.do%3Fid%3D18029&ei=K77wTZfUJYqFhQeN-owY&usg=AFQjCNEQ5uV6_Qws_-wX22Kr4_
vxGSWz5w&sig2=iw627CULdOqyjTDEjPn9Rg.
[10] Friis, J. and E. Nielsen (2009). Autonomous landing on a moving platform. http://www.control.
aau.dk/uav/reports/09gr830/09gr830_student_report.pdf.
[11] Gebre-Egziabher, D., G. Elkaim, J. Powell, and B. Parkinson (2000). A gyro-free quaternion-based
attitude determination system suitable for implementation using low cost sensors. pp. 185192. IEEE
2000 Position Location and Navigation Symposium.
[12] Green, J. (2008). Slip resistance: What speciers should know. Stone Source.
[13] G.Wahba (1966). A least squares estimate of satellite attitude. SIAM Review Vol n3, 384386.
[14] Haessig, D. and B. Friedland (1998). Separate-bias estimation with reduced-order Kalman lters.
Volume 43, pp. 983987. IEEE Transactions on Automatic Control.
83
[15] J.Caruso. Applications of Magnetoresistive Sensors in Navigation Systems. Honeywell.
[16] Kumar, N. and T.Jann (2004). Estimation of attitudes from a low-cost miniaturized inertial platform
using Kalman lter-based sensor fusion algorithm. 29, 217235.
[17] L.Lewis, F. and B. L.Stevens (1992). Aircraft Control and Simulation. Wiley-Interscience.
[18] Mahony, R., T. Hamel, and S.-H. Cha. A coupled estimation and control analysis for attitude stabilisa-
tion of mini aerial vehicles. Proceedings of the Australasian Conference on Robotics and Automation.
[19] Mahony, R., T. Hamel, and J. M. Pimlin (2005). Complementary lter design on the special orthogonal
group so(3). pp. 14771484. IEEE Conference on Decision and Control.
[20] Mahony, R., T. Hamel, and J.-M. Pimlin (2008). Nonlinear complementary lters on the special
orthogonal group. Volume 53 n-5, pp. 12031218. IEEE Transactions on Automatic Control.
[21] M.Andrejasic (2008). Mems accelerometers seminar. http://mafija.fmf.uni-lj.si/seminar/
files/2007_2008/MEMS_accelerometers-koncna.pdf.
[22] Martin, P. and E. Salauen (2010). The true role of accelerometer feedback in quadrotor control. pp.
16231629. IEEE International Conference on Robotics and Automation (ICRA).
[23] Mian, A. A. and W. Daobo (2008). Nonlinear ight control strategy for an underactuated quadrotor
aerial robot. pp. 938942. IEEE International Conference on Networking, Sensing and Control.
[24] Phuong, N. H. Q., H.-J. Kang, Y. soo Suh, and Y.-S. Ro (2009). A dcm based orientation estimation
algorithm with an inertial measurement unit and a magnetic compass. Volume 15, pp. 859876.
[25] Premerlani, W. and P. Bizard (2009). Direction cosine matrix imu: Theory. http://www.scribd.com/
doc/47609876/Attitude-Determination-and-Bias-Estimation-Using-Kalman-Filtering-Yadlin.
[26] Raimundez, C. and A. F. Villaverde (2008). Adaptive tracking control for a quadrotor. 6th EUROMECH
Nonlinear Dynamics Conference ENOC.
[27] Rashed, R. and H. Momeni (2007). System modeling of mems gyroscopes. pp. 16. Mediterranean
Conference on Control and Automation.
[28] Roberts, J., T. Stirling, J.-C. Zufferey, and D. Floreano (2007). Quadrotor using minimal sensing for
autonomous indoor ight. MAV 07.
[29] R.Yadlin. Attitude determination and bias estimation using Kalman ltering. Department of
Astronautics Publications of the U.S Airforce Academy, http://www.scribd.com/doc/47609876/
Attitude-Determination-and-Bias-Estimation-Using-Kalman-Filtering-Yadlin.
[30] S.Maybeck, P. (1979). Stochastic models, estimation and control. Ohio, USA: Manning Publications
Co.
[31] Welch, G. and G. Bishop (2001). An introduction to the Kalman lter. http://www.cs.unc.edu/
~
welch/kalman/kalmanIntro.html.
84
Appendices
85
Appendix A
Practical Usage of the Prototype
A.1 Components of the Prototype
This work comes in the sequence of a previous work concluded by Jorge Domingues for his master
degree at Instituto Superior T ecnico in 2009, [7]. The platform conceived by Jorge Domingues will be
described in this section and the main characteristics will be presented.
The prototype was designed, following certain specications which follow:
1. Overall weight not greater than one kilogram.
2. On-board controller for stabilization purposes. This implies the presence of a processing unit on-
board where the controller will be loaded.
3. The prototype must be able to communicate with a ground station, such as a personal computer.
The main structure of the quadrotor is a Vario-43 from VARIO
R
. The berglass structure offers a
protecting cocoon to the electronics in the center of the cross-shaped frame as seen in Figure A.1.
The chosen frame weights 258g corresponding to a little more than the quarter of the total weight
allowed for the work.
Figure A.1: Frame of the Prototype.
The thrust is developed using two counter rotating matched sets of EPP1045 propellers weighing
each 23g. The diameter is ten inches (25.4mm) and the pitch is 4.5 inches per revolution.
Figure A.2: Propellers.
87
The propellers are set on four identical motors. For this application, a brushless outrunner Direct Cur-
rent (DC) motor is suited; the choice came to the BL-Outrunner 2824-34 from Robbe ROXXY
R
. The
motor can be seen in Figure A.3(a). Brushless motors need an electronic component called speed
controller. The four motors have different speed controllers. Motors M
1
and M
2
have FlightTech
10amp, motor M
3
has a Maxpower 10amp and motor M
4
has a Xpower 10amp. These speed con-
trollers receive power from the battery and are connected to the processor from which they receive
the signal. These speed controllers have the ability of driving the energy back to the processor from
the battery if needed. This allows the processor to be safely connected to the energy supply and in
case of emergency to land the craft. The speed controller can be seen in Figure A.3(b). Additionally,
the speed controllers have the ability to cut the power supply to the engines if the battery runs low,
this helps avoiding damages to the battery; they can also start the motors smoothly for greater safety.
Different settings can be used to optimize the behavior of the motors. The following settings were
chosen:
soft cutoff mode by reducing the power progressively
high cutoff threshold to ensure the conservation of the batteries
super soft startup mode for a smooth take off
(a) Robbe BL-
Outrunner 2824-34.
(b) FlightTech Speed Con-
troller.
Figure A.3: Motor and Speed Controller.
The processor must be able to communicate to the speed controllers and to receive and transmit
information to the telemetry device as well as receive information from the different sensors.
The chosen processor device was the Arduino Duemilenove
R
platform with ATMega320 micro con-
troller. The board can send PWM (Pulse Width Modulation) signals to the speed controllers. The
nature of these signals will be explained in a subsequent section. The board can be programmed
using a C-based language. The chosen component can be seen in Figure A.4. It has 6 analog input
pins, 14 digital I/O pins (of which 6 can provide PWM signals). It has also a 3.3V and a 5V supply
and I2C (a specic communication protocol) connections.
Figure A.4: Arduino Diecimila.
88
The communication between the Arduino and the ground station is necessary and done through a
wireless module, the XBee (see Figure A.5(a)). Two modules are used, one is placed on the Arduino
with a shield (see Figure A.5(b)) and the other one is connected to the ground station.
(a) Xbee unit. (b) Xbee shield.
Figure A.5: Xbee and Shield.
The sensors are necessary to achieve stability, they are responsible for closing the loop and feed
information back to the controller. Initially it was estimated that two sensors would be necessary for
the hovering operation. The 3 axis accelerometer would measure accelerations. The chosen sensor
was a ADXL330 accelerometer which has analog outputs and therefore is plugged to the analog
input pins of the processor. In addition to the accelerometer it was determined that a compass
would be necessary to dene the direction of the magnetic North. The chosen compass is the 2-
axis compass HMC6352. This sensor requires an I2C communication protocol. Luckily the chosen
processor shares, on analog pins 4 and 5, the I2C protocol. A discussion of the sensors is presented
in section 4.2.
(a) SparkFun IMU 6DOF Razor . (b) HONEYWELL R compass.
Figure A.6: Sensors.
Since the motors are DC motors the battery must t them. A 12 Volt LiPo (Lithium-Polymer) battery
was chosen (see Figure A.7(a)).
89
(a) The battery unit. (b) Arduino Mega.
Figure A.7: Board and Battery.
The necessary modications, justied in Chapter 4, correspond to the installation of the IMU board, the
range nder and the substitution of the Adruino Duemilanove board for the Arduino Mega.
As seen in Figure A.6(a), the IMU has six analog outputs and the Arduino Duemilanove board has six
analog inputs. However, the compass communicates through I2C protocol which stands on analog input
pins 4 and 5. As a consequence there were not enough pins to accommodate all sensors.
The solution was the acquisition of a new board. The Arduino Duemilanove was updated into the
Arduino Mega (see Figure A.7(b)) which provides 54 digital I/O pins, 16 analog input pins and the I2C
communication located on digital pins. This board has also the advantage of a higher processing power
with its ATMega1280 microcontroller.
The chosen range nder is a Sharp GP2d12. The justied improvements of the processing board
allows the allocation of the extra-sensor used to dene the height of the quadrotor during hovering ight. It
is connected through an analog pin on the Arduino and works with a 5V supply tension.
A.2 Wiring
After correctly assembling the prototype, only the electric connections are left to do. These connections
regard the sensors , the processor and the battery.
All connections are numbered and indicated on the prototype. In Figure A.8 the scheme is presented.
The IMU (accelerometer+gyroscope from SPARKFUN
R
) is connected to the 3.3V and to the ground of the
Arduino, this will provide the energy to run the sensor. The 3.3V line is also pulled to the reference tension
entry. This reference tension is used to dene the 1024 intervals that the Arduino is capable of reading.
The outputs of the IMU, corresponding to the accelerometers and gyroscopes information are connected
to the Analog input pins. The compass receives a 5V tension and outputs its information through I2C
protocol. The Arduino board is compatible with this protocol through digital pins (20 and 21). The range
nder is connected to the 5V supply tension and to pin 8 of the Arduino board.
The compass, the range nder and the speed controllers require the 5V input tension. In that case a
silicon board allows to easily plug all elements to that supply. The same happens for the ground wire
connected to the Arduino.
Extreme caution is to be apply to the welds. Indeed, excessive heat may affect the sensors. Moreover,
short circuits on the silicon boards must be avoided by use of a multimeter to test the continuity between
different points. The presence of the shield has also caused great problems because of a pin carved in the
5V wire and piercing its plastic protection.
The battery is located under the prototype; its function is to supply energy to the motors but also to the
Arduino board and the sensors. In Figure A.9, the connections of the speed controllers and the battery
90
A Gyro
x x y y z z G
N
D
3
.3
V
sparkfun.com
N
HMC6352
SharpGp2d12
G
N
D
V
c
c
S
D
A
S
C
L
R
E
S
E
T
3
V
3
5V GND vin
POWER
ANALOG IN
COMMUNICATION
PWM
D
IG
IT
A
L
0 1 2 3 4 5 6 7 8 9 1 1 1 1 1 1
0 1 2 3 4 5
1 1 1 1
3 2 1 0 9 8 7 6 5 4 3 2 1 0
G
N
D
A
R
E
F
GND
T
X
T
X
3
T
X
2
T
X
1
R
X
R
X
3
R
X
2
R
X
1
S
D
A
S
C
L
4 5 6 7 8 9 0 1
1 1 1 1 1 1 2 2
2
2
2
2
3
3
3
3
3
4
4
4
4
4
5
5
2
4
6
8
0
2
4
6
8
0
2
4
6
8
0
2
3
3
3
3
3
4
4
4
4
4
5
5
1
3
5
7
9
1
3
5
7
9
1
3
ISCP V
c
c
G
N
D
V
0
Figure A.8: Sensors Connections
are shown. The motor is connected to the speed controller by three cables. The order in which these are
plugged is not critic. If the motor spins in the wrong direction just interchange two of the three cables. Note
+
-
12V DC Battery
5V
GND
Signal
+
_
PWM
5V
GND
Signal
+
_
PWM
5V
GND
Signal
+
_
PWM
5V
GND
Signal
+
_
PWM
PWM
motor1
PWM
motor2
PWM
motor3
PWM
motor4
R
E
S
E
T
3
V
3
5V GND vin
POWER
ANALOG IN
COMMUNICATION
PWM
D
IG
IT
A
L
0 1 2 3 4 5 6 7 8 9 1 1 1 1 1 1
0 1 2 3 4 5
1 1 1 1
3 2 1 0 9 8 7 6 5 4 3 2 1 0
G
N
D
A
R
E
F
GND
T
X
T
X
3
T
X
2
T
X
1
R
X
R
X
3
R
X
2
R
X
1
S
D
A
S
C
L
4 5 6 7 8 9 0 1
1 1 1 1 1 1 2 2
2
2
2
2
3
3
3
3
3
4
4
4
4
4
5
5
2
4
6
8
0
2
4
6
8
0
2
4
6
8
0
2
3
3
3
3
3
4
4
4
4
4
5
5
1
3
5
7
9
1
3
5
7
9
1
3
ISCP
Figure A.9: Actuators Connections
that there is no direct supply of energy to the Arduino board. The speed controllers, as already referred
have the particularity of driving the energy backwards if necessary. In fact, the speed controllers have
some preprogrammed commands which assure that should the battery run dry, the priority would be set to
the Arduino, reducing the energy sent to the motors.
A.3 Maintenance
The speed controllers have the ability of cut off the battery supply if its charge drops bellow a dened
limit. This protects the battery. To charge the battery a Lithium-Polymer Battery Charger for RC Models
91
from TOPMODEL
R
is used. This charger allows parallel and independent charging of the different cells,
as well as a balancing and a cut-off action. This means that if one cell is more charged than another, they
will rst be balanced and then charged at the same pace; once the charging is completed, the charger cuts
the supply off, protecting the cells.
The charger works at 1.5A and 10V , being connected to a variable tension transformer available in the
laboratory. Special care must be given to the connections since a short-circuit could mean the death of the
batterys cells.
The prototype must also be protected from humidity and dust which could affect the bearings in the
outrunner motor. A simple rag would work just ne.
It is also convenient to make sure the screws are not missing before each ight test. This can prevent
severe damage on the prototype. The most affected by the vibrations seems to be the ones holding the
sensors. A simple check can easily and quickly be done.
A.4 Arduino Program
The role of the Arduino board is to assure the communication between the sensors, the motors and the
ground station while running the algorithms to keep the quadrotor hovering.
The axes dened on the sensors do not necessarily coincide with the dened body-xed frame. Indeed
a simple test was undertaken to evaluate the orientation of the dened axes. Using the communication set
described in chapter 10, the output of the Arduino could be visualized in real time. By moving the prototype,
the sensing axes of the sensors could be easily identied.
The software uploaded on the board is written in the c-like language. It is divided in four main sections.
First the header section contains the libraries. Second a Setup() section containing the calculations of
the offsets and the motors start-up is used. Then the loop() section is used to compute the necessary
calculations. The last section corresponds to the user dened modules in which different useful functions
can be written and called by the two previous sections when necessary.
The header section calls up libraries to run mathematical functions math.h, the I2C protocol Wire.h and
to use PWM signals Servo.h. It also sets the initial values of the matrices used in the Kalman Filter and the
pins connections:
#include <MsTimer2.h>
#include <Wire.h>
#include <math.h>
#include <Servo.h>
// Compass initialization
int magReading = 0;
int Azimuth = 0;
float lastread=0;
int flag=0;
// The pins correspond to the IMU labeled and not to the oreintation of the system.
const int xpin = 10; // x-axis of the accelerometer
const int ypin = 11; // y-axis
92
const int zpin = 12; // z-axis (only on 3-axis models)
const int Zpin = 13; // z-axis of the gyroscope
const int Ypin = 14; // y-axis of the gyroscope
const int Xpin = 15; // x-axis of the gyroscope
const int Height=8;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
//int minPulse = 600; // minimum servo position, us (microseconds)
//int maxPulse = 2400; // maximum servo position, us
// User input for servo and position
int userInput[3]; // raw input from serial buffer, 3 bytes
int startbyte; // start byte, begin reading input
int Dvariable; // which servo to pulse?
int Dvalue; // servo angle 0-180
float D_value_float;
int i; // iterator
// X-Y-Z errors from Joystick
float D_Fi=0;
float D_Teta=0;
float Throttle=0;
// PWM corrections
float PWM1;
float PWM2;
float PWM3;
float PWM4;
// System Variables
int offset_value[8] = {0,0,0,0,0,0,0,0}; // array of offset data
double sensor_value[10] = {0,0,0,0,0,0,0,0,0,0}; // array of sensor data
// HighPass Filter for W (vertical speed) estimation
float W=0;
float Wx=1;
float con;
//Security Feature
93
int StopEngine=1;
int anotherflag=0;
//System Matrices considering the discrete
//system: X(n+1)=A(n)X(n)+B(n)U(n); Y=C(n)X(n)+D(n)U(n);
double Tin=0.02; //This is the sampling time.
double A[3][3]={
{1,0,0},
{0,1,0},
{0,0,1}};
double B[3][3]={
{Tin,0,0},
{0,Tin,0},
{0,0,Tin}};
double C[9][3]={
{0,-9.81,0},
{9.81,0,0},
{0,0,0},
{0,0,0},
{0,0,0},
{0,0,0},
{0,0,0},
{0,0,-1},
{0,0,0}};
double D[9][3]={
{0,0,0},
{0,0,0},
{0,0,0},
{1,0,0},
{0,1,0},
{0,0,1},
{0,0,0},
{0,0,0},
{0,0,0}};
// Kalman Matrices representing in order the measurement
//covariance noise and the process covariance noise.
double R[9][9]={
{2,0,0,0,0,0,0,0,0},
{0,2,0,0,0,0,0,0,0},
{0,0,1,0,0,0,0,0,0},
{0,0,0,.1250,0,0,0,0,0},
{0,0,0,0,.1250,0,0,0,0},
{0,0,0,0,0,.5,0,0,0},
94
{0,0,0,0,0,0,.005,0,0},
{0,0,0,0,0,0,0,.005,0},
{0,0,0,0,0,0,0,0,.005}};
double Q[3][3]={
{0.00005,0,0},
{0,0.00005,0},
{0,0,0.0002}};
// State Vector:
double X[3]={0,0,0};
double P[3][3]={
{0,0,0},
{0,0,0},
{0,0,0}};
double P2[3][3]={// This matrix corresponds to P matrix sample updated
{0,0,0},
{0,0,0},
{0,0,0}};
// Kalman Filter Matrices for non-linear filter:
// We consider the symetry to take care of the cos and sin of the
// accelerometer. Tehrefore the equation is simplified to this
double Y_0[9]={0,0,9.81,0,0,0,0,0,0};
double X_0[3]={0,0,0};
double X_update[3]={0,0,0};
double Kk[3][9]={// This matrix corresponds to P matrix sample updated
{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}};
double Temp[9][9]={// This matrix corresponds to P matrix sample updated
{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,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,0,0,0,0,0,0,0}};
float Determinant, temp1,temp2,temp3,temp4;
The offset terms are dened by the initialization routine. As stated by the manufacturer, the IMU board
has temperature dependent biases. To avoid the the introduction of these initial biases, the sensors are
turned on and readings are taken for a few minutes to warm up the sensors. Then the initialization routine
takes 5000 measurements to estimate the initial offset:
95
void setup() {
analogReference(EXTERNAL);
Serial.begin(9600);
Wire.begin();// Compass I2C communication
// Initialization Routine
Serial.println("Calculating offset. Please wait ");
for (int a=0; a<5000; a++)
{
read_ADC(); // Bias cancelation routine
}
//Initialization of the motors
serv1.attach(6); //, minPulse, maxPulse);
servo1.writeMicroseconds(00);
delay(5000);
servo1.writeMicroseconds(1000);//1000
delay(5000);
servo1.writeMicroseconds(1200);
}
// Bias Cancelation
void read_ADC(){ //read sensor data
offset_value[0]=((analogRead(xpin)*0.9)+(offset_value[0]*0.1)) ;
offset_value[1]=((analogRead(ypin)*0.9)+(offset_value[1]*0.1)) ;
offset_value[2]=((analogRead(zpin)*0.9)+(offset_value[2]*0.1)) ;
offset_value[3]=((analogRead(Xpin)*0.9)+(offset_value[3]*0.1)) ;
offset_value[4]=((analogRead(Ypin)*0.9)+(offset_value[4]*0.1)) ;
offset_value[5]=((analogRead(Zpin)*0.9)+(offset_value[5]*0.1)) ;
magRead(0);
offset_value[6]=(cos(int(magReading/10)*PI/180)*0.9)+(offset_value[6]*0.1) ;
offset_value[7]=(sin(int(magReading/10)*PI/180)*0.9)+(offset_value[7]*0.1) ;
}
The loop() section contains the Kalman lter and the LQR. Since it is set to run at a frequency of
freq = 50Hz, two options were available. The rst consisted in using an interrupt. However it was noticed
that the interrupt routine is incompatible with the I2C protocol. Therefore it was decided to use a timer and
ensure the cycle to be faster than 0.02s.
void loop() {
if((millis()-lastread) >= 20) { // sample every ms -> 0.02s
lastread = millis();
96
MainRoutine(); // Estimator Routine
}
}
The loop() calls the MainRoutine() function which holds the Kalman lter and the LQR:
void MainRoutine() {
// Accelerometer X axis- m/s^2
sensor_value[0]=(analogRead(xpin)-offset_value[0])*3.3/1023/0.33*9.81;
// Accelerometer Y axis- m/s^2
sensor_value[1]=(-analogRead(ypin)+offset_value[1])*3.3/1023/0.33*9.81;
// Accelerometer Z axis- m/s^2
sensor_value[2]=(analogRead(zpin)-offset_value[2]+101)*3.3/1023/0.33*9.81;
// Gyroscope Y axis- rad/s
sensor_value[3]=(analogRead(Xpin)-offset_value[3])*3.3/1023/0.0033*PI/180;
// Gyroscope X axis- rad/s
sensor_value[4]=(analogRead(Ypin)-offset_value[4])*3.3/1023/0.0033*PI/180;
// Gyroscope Z axis- rad/s
sensor_value[5]=(-analogRead(Zpin)+offset_value[5])*3.3/1023/0.0033*PI/180;
magRead(0);
sensor_value[6]=cos(int(magReading/10)*PI/180); // Magnetometer X axis- rad
sensor_value[7]=sin(int(magReading/10)*PI/180); // Magnetometer Y axis- rad
sensor_value[8]=0;
if (flag==0)
{
sensor_value[9]=((14883.6/(analogRead(Height) + 92.8))-6.7)/100;// resultado em metros.
// High Pass filter to estimate W.
con=exp(-0.02/0.005);
W=(Wx-sensor_value[9])/0.02;
Wx=con*Wx+(1-con)*sensor_value[9];
}
flag=!flag;
if (sensor_value[9]>.2){
anotherflag=1;
}
if (anotherflag==1 && sensor_value[9]<0.15){
StopEngine=0;
}
//input vector supposedly coming from the sensors. Notice that only the nonlinear terms are recalculated;
// We consider the symetry to take care of the cos and sin of the accelerometer. Therefore the equation is simplified to this
97
Y_0[6]=cos(X[2]);
Y_0[7]=-sin(X[2]);
X_0[2]=X[2];
//Sample Update: first step of the Kalman Filter
X_update[0]=X[0]+Tin*sensor_value[3];
X_update[1]=X[1]+Tin*sensor_value[4];
X_update[2]=X[2]+Tin*sensor_value[5];
Matrix_Add(P,Q,P2);
// Redefine the C matrix.
C[6][2]=-sin(X_update[2]);
C[7][2]=-cos(X_update[2]);
// Kalman Gain.
Temp[0][0]=1/(P2[1][1]*9.81*9.81+R[0][0]);
Temp[1][1]=1/(P2[0][0]*9.81*9.81+R[1][1]);
temp1=P2[2][2]*C[6][2]*C[6][2]+R[6][6];
temp2=P2[2][2]*C[6][2]*C[7][2];
temp3=P2[2][2]*C[6][2]*C[7][2];
temp4=P2[2][2]*C[7][2]*C[7][2]+R[7][7];
Determinant=temp1*temp4-temp3*temp2;
Temp[6][6]=(temp4)/Determinant;
Temp[6][7]=-(temp3)/Determinant;
Temp[7][6]=-(temp2)/Determinant;
Temp[7][7]=(temp1)/Determinant;
Kk[1][0]=C[0][1]*Temp[0][0]*P2[1][1];
Kk[0][1]=C[1][0]*Temp[1][1]*P2[0][0];
Kk[2][6]=(C[6][2]*Temp[6][6]+C[7][2]*Temp[7][6])*P2[2][2];
Kk[2][7]=(C[6][2]*Temp[6][7]+C[7][2]*Temp[7][7])*P2[2][2];
// State Estimation.
X[0]=X_update[0]+Kk[0][1]*(sensor_value[1]-X_update[0]*C[1][0]);
X[1]=X_update[1]+Kk[1][0]*(sensor_value[0]-X_update[1]*C[0][1]);
X[2]=X_update[2]+Kk[2][6]*(sensor_value[6]-(X_update[2]-X_0[2])*C[6][2]-Y_0[6]) +Kk[2][7]*(sensor_value[7]-(X_update[2]-X_0[2])*C[7][2]-Y_0[7]);
// P-matrix update.
P[0][0]=(1-Kk[0][1]*C[1][0])*P2[0][0];
P[1][1]=(1-Kk[1][0]*C[0][1])*P2[1][1];
P[2][2]=(1-C[6][2]*Kk[2][6]-C[7][2]*Kk[2][7])*P[2][2];
98
//Receive the data D_Fi, D_Teta, Throttle from the Joystick
ReceiveData();
//Compute the input correction
PWM1=-50.2411*W-0.0010*(sensor_value[3]-0)+10.3008*(sensor_value[4]-0) +29.2160*(sensor_value[5]-0)-52.6785*(-sensor_value[9]+0.5+Throttle)
-0.0011*(X[0]-D_Fi)+13.6871*(X[1]-D_Teta)+28.8420*(X[2]-0);
PWM2=-45.4481*W+12.1344*(sensor_value[3]-0)+0.0260*(sensor_value[4]-0)
-26.7392*(sensor_value[5]-0)-46.7762*(-sensor_value[9]+0.5+Throttle)
+16.2633*(X[0]-D_Fi)+0.0357*(X[1]-D_Teta)-25.9454*(X[2]-0);
PWM3=-38.3183*W-0.0008*(sensor_value[3]-0)-12.4337*(sensor_value[4]-0)
+22.1882*(sensor_value[5]-0)-40.5667*(-sensor_value[9]+0.5+Throttle)
-0.0008*(X[0]-D_Fi)-16.9002*(X[1]-D_Teta)+22.1049*(X[2]-0);
PWM4=-45.8393*W-12.0604*(sensor_value[3]-0)+0.0262*(sensor_value[4]-0)
-26.9735*(sensor_value[5]-0)-47.1624*(-sensor_value[9]+0.5+Throttle)
-16.1509*(X[0]-D_Fi)+0.0360*(X[1]-D_Teta)-26.1643*(X[2]-0);
// Define Engine Speed
servo1.writeMicroseconds((1254-PWM1)*StopEngine);
servo2.writeMicroseconds((1250-PWM2)*StopEngine);
servo3.writeMicroseconds((1265-PWM3)*StopEngine);
servo4.writeMicroseconds((1322-PWM4)*StopEngine);
// Output Data to the Ground Station
Serial.print(sensor_value[9]);
Serial.print(",");
Serial.println(W);}
Finally, the user functions used to receive data from the ground station and to acquire data from the
compass is presented:
// Receive Data from the Ground Station
void ReceiveData(){
// Wait for serial input (min 3 bytes in buffer)
if (Serial.available() > 2) {
// Read the first byte
startbyte = Serial.read();
// If its really the startbyte (255) ...
if (startbyte == 255) {
// ... then get the next two bytes
for (i=0;i<2;i++) {
userInput[i] = Serial.read();
99
}
// First byte = servo to move?
Dvariable = userInput[0];
// Second byte = which position?
Dvalue = userInput[1];
D_value_float=(float(Dvalue)-127)/12.7;
// Packet error checking and recovery
//if (Dvalue == 255) { Dvariable = 255; }
// Interpret Receive Data
switch (Dvariable) {
case 1:
D_Fi=D_value_float*PI/180; // FI angle error in rad
case 2:
D_Teta=-D_value_float*PI/180; // Teta angle error in rad
break;
case 3:
Throttle=-D_value_float/100*5;// Error in meters
break;
}}}}
// Function to read Compass ouput
void magRead(int outputMode)
{
// int HMC6352Address = 0x42;
// Shift the devices documented slave address (0x42) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)
// slaveAddress = HMC6352Address >> 1; // This results in 0x21 as the address to pass to TWI
int slaveAddress = 0x21; // This is calculated from HMC6352s address, see comments above
int ramDelay = 100; // us, delay between a RAM write command and its effect, at least 70us
int getDelay = 10; // ms, delay between a get data command and its effect, at least 6ms
byte magData[2];
int i;
switch (outputMode)
{
Wire.beginTransmission(slaveAddress);
Wire.send(0x47); // Write to RAM command
Wire.send(0x4E); // Output Mode control byte address
100
Wire.send(0x00); // default to Heading mode
Wire.endTransmission();
}
delayMicroseconds(ramDelay); // RAM write needs 70 microseconds to respond
Wire.beginTransmission(slaveAddress);
Wire.send("A"); // The "Get Data" command
Wire.endTransmission();
delay(getDelay); // Get Data needs 6 milliseconds to respond
Wire.requestFrom(slaveAddress, 2); // Request the 2 byte data (MSB comes first)
//i = 0;
//while(Wire.available() && i < 2)
//{
magData[0] = Wire.receive();
magData[1] = Wire.receive();
//i++;
//}
magReading = magData[0]*256 + magData[1];
}
101
102
Appendix B
Determination of the Center of Mass
The actual pictures taken to dene the center of mass of the prototype will be presented in this appendix.
Figures B.1(a) and B.1(b) picture the procedure; the prototype is hanged as shown and a string to which is
attached a dead weight is used to dene the vertical. Through visual inspection of the pictures taken with
a camera, it is possible to estimate the location of the center of mass.
In both pictures, the string follows precisely the axis of the prototype, conrming that the center of mass
is located on their intersection.
M1
M2
M3
M4
(a) Center of Mass location relative to u
x
axis
M1
M2
M3
M4
(b) Center of Mass location relative to u
y
axis
Figure B.1: Determination of the Center of Mass.
To dene the location along the u
z
axis, the prototype was hanged again, but this time sideways. As
a result the quadrotor was leaning having its center of mass directly located in the vertical of the hanging
point. Figures B.2(a) and B.2(b) present two experiments with two different hanging points. To estimate
the location of the center of mass Illustrator
R
was used. The lines created to evince the point can be seen
on the pictures.
103
(a) Center of Mass location along u
z
axis (b) Center of Mass location along u
z
axis
Figure B.2: Determination of the Center of Mass along u
z
axis.
To determine the location, the picture must be analyzed. It has been conrmed that the center of
mass is located along the u
z
axis. First, a line is drawn between the base of the two opposed supports
of the prototype. As seen on both pictures the quadrotor presents almost perfectly either its u
x
u
z
or its
u
y
u
z
plane. As a consequence, the lines are not too biased if taken along that plane. The vertical is
dened by the hanging string, and using Illustrator
R
that line can be copied and made to pass exactly by
the hanging point. Then, the line dening u
z
axis must be found since the center of mass will be dened
as the intersection between the previously drawn line and this axis. The line which runs along the arms of
the quadrotor can be drawn and rotated by 90

and then translated to the correct position. At this stage the

point has been found. To determine the coordinate, a small segment dening the width of one arm (exactly
1cm) is copied several times to join the rst drawn line and the center of mass.
This method is dependent on the precision of the program Illustrator and on the quality of the camera.
The results seem satisfactory but rough and not totally reliable. It is therefore considered that the Control
Strategy must be robust to take into account some imprecision.
To correctly dene the center of mass the same principle could be used but with a better camera, say a
high precision camera set like the one used by the GRASP Lab at the University of Pennsylvania seen on
Figure 1.3(b) from Chapter 1.
104