Estimation and Control of A Quadrotor Attitude

Estimation and Control of a Quadrotor Attitude
Bernardo Sousa Machado Henriques
Dissertação para obtenção do Grau de Mestre em

Engenharia Mecânica
Júri
Presidente: Professor José Rogério Caldas Pinto
Orientador: Professora Alexandra Bento Moutinho
Co-Orientador: Professor José Raúl Carreira Azinheira
Vogal: Professor Paulo Oliveira
Junho 2011
”Ce qui s’apprend sans peine ne vaut rien et ne demeure pas.”
René 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
finder.
We first 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 first 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
filter and the different Kalman filters, as the linear and expended Kalman filters, 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 filter, Complementary filter.
v
vi
Resumo
O objectivo deste trabalho é estimar e controlar a atitude de um quadrirotor usando apenas sensors de
baixo custo: acelerómetro e giroscópio de três eixos, bússola de dois eixos e telémetro.
Inicialmente, o conceito de quadrirotor é introduzido, explicando os movimentos do veı́culo com os sis-
temas de eixos necessários para definir esses movimentos. As caracterı́sticas do protótipo são estimadas
e os modelos dinâmicos desenvolvidos. Para completar o modelo, os actuadores são aproximados por
sistemas de primeira ordem com parâmetros obtidos experimentalmente. Os sensores são modelados
usando medições reais para estimar correctamente o ruı́do dos sensores. Isto resulta na implementação
do modelo em Simulink.
Depois de linearizar o modelo em torno da condição do ponto de funcionamento, um sistema linear
em espaço 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 são
obtidos e usados para afinar os estimadores considerados. Filtros complementares e diferentes filtros de
Kalman, como o filtro linear e filtro extendido ou não linear, são testados e comparados.
Um controlador LQR de 8 estados, utilizando sensores ideais, é afinado para controlar a atitude e
altura do quadrirotor no espaço contı́nuo e discreto. O controlador é também testado com realimentação
do estimador escolhido, o EKF. Em seguida, o estimador é implementado no protótipo provando estar bem
afinado para as condições de funcionamento: com todos os motores a funcionar.
Finalmente, os resultados sugerem que o LQR discreto juntamente com o EKF formam uma boa es-
tratégia 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 Definitions 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 Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 Identification of the Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
5 Actuators 31
5.1 Model of the Propeller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
xi
5.2 Model of the Motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
5.3 Identification 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 uz . . . . . . . . . . . . . . . . . . . . . . . . . . 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 Identification of the Range Finder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.1 Configuration of the Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.2 Electrical and Mechanical Systems of the Motor . . . . . . . . . . . . . . . . . . . . . . . . . 32
5.3 Identification 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 uz 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 fixed frame
PWM - Pulse Width Modulation
MEMS - Micro Electro Mechanical Systems
IMU - Inertial Measurement Unit
AD - Analog-Digital Converter
List of Variables
(UN , UE , UD ) North, East and Down axes of the NED frame

O Origin of the NED frame
(ux , uy , uz ) x, y and z axes of the ABC frame
Oc Origin of the ABC frame
I
P Position vector of Oc in the NED frame, P = [X, Y, Z]T
VB 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: (Ix , Iy , Iz )
dcm Distance from the center of mass to the motor, in the ux uy plane (m)
hf eet Distance between the base of the feet to the center of mass of the Quadrotor along uz
df eet Distance between the base of the feet to the center of mass of the Quadrotor along ux or uy
xix
R(φ)T Roll rotation
T
R(θ) Pitch rotation
T
R(ψ) Yaw rotation
S Transformation matrix from NED frame to ABC frame
Sq Transformation matrix in the quaternion form
q Quaternion element ; q = [q0 , q1 , q2 , q3 ]T
FB Forces applied to the quadrotor (excluding gravity); FB = [Fx , Fy , Fz ]T

Fi Thrust produced by motor number Mi
MB Moments applied on the quadrotor in the ABC frame; MB = [Mx , My , Mz ]T
aB Acceleration vector, in the body-fixed frame: aB = [U̇ , V̇ , Ẇ ]T
gI Gravity vector defined in the inertial frame;
B
g Gravity vector defined in the body-fixed frame
g0 Value of the gravitational constant
Kspring Spring constant of the undercarriage model

Kdamp Damping constant of the undercarriage model
ξ Damping Ratio of the undercarriage model
ω0 Natural frequency of the undercarriage model
zi Coordinate along UD of the foot under motor Mi
żi Vertical speed of the foot under motor Mi
ẋi Linear velocity along ux of the foot under motor Mi
ẏi Linear velocity along uy of the foot under motor Mi
fc Friction coefficient between the ground and the quadrotor
Ff loor Ground reaction on the quadrotor; Ff loor = [Fxf loor , Fyf loor , Fzf loor ]T
Mf loor Moments applied on the quadrotor in the ABC frame; Mf loor = [Mxf loor , Myf loor , Mzf loor ]T
K Constant which relates thrust and torque produced by a propeller
B B
Ω Measured angular rates, in the body-fixed frame; Ω = [g x , g y , g z ]T
µg Gaussian measurement noise of the gyroscope; µg = [µg−x , µg−y , µg−z ]T
bg Slowly time-varying bias of the gyroscope; bg = [bg−x , bg−y , bg−z ]T
r Location of the IMU relative to the center of mass of the prototype; r = [rx , ry , rz ]T
aB Measured acceleration vector, in the body-fixed frame; aB = [ax , ay , az ]T
µa Gaussian measurement noise of the accelerometer; µa = [µa−x , µa−y , µa−z ]T
ba Constant bias of the accelerometer
B B
N Measured magnetic vector, defined in the body-fixed frame; N = [N x , N y , N z ]
NI Magnetic vector, defined in the inertial frame
µm Gaussian measurement noise of the compass; µm = [µm−x , µm−y , µm−z ]T
bm Bias of the compass; bm = [bm−x , bm−y , bm−z ]T
zB Measured height of the quadrotor
bz Bias of the range finder
µz Gaussian noise of the range finder
AccRes Resolution of the accelerometer
xx
GyroRes Resolution of the gyroscope
M agRes Resolution of the compass
RangeF inderRes Resolution of the range finder
f req Considered sampling frequency of the sensors
KT Constant which relates thrust and angular velocity of the propeller

KM Constant which relates torque and the angular velocity of the propeller
Ti Thrust produced by the propeller on motor Mi
Mz−i Torque around uz produced by the propeller on motor Mi
D Diameter of the propeller
ρ Density of the air
cT Thrust coefficient
cP Power coefficient
PWM PWM signal sent; PWM = [P W M1 , P W M2 , P W M3 , P W M4 ]
ki 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
Xk Discrete state vector of the quadrotor
Uk Discrete input vector of the quadrotor system
Yk Discrete output vector of the quadrotor system
A, B, C, D Matrices of the linear state space system
Ad , Bd , Cd , Dd Matrices of the discrete linear state space system
X0 Linearization point
ωi0 Angular velocity of the propeller at the linearization condition.
P W Mid PWM dead zone of motor Mi
P W Mi0 Input linearization point
P W Mid−eq Equivalent PWM dead zone
Utrim Trim condition regarding the input vector
Ytrim Trim condition regarding the output vector
Xtrim Trim condition regarding the state vector
T = 1/f req Time interval
JLQR Cost function of the LQR controller

Xref Reference State
xxi
E Error vector; E = Xref − X
Klqr Optimal gain matrix
Qlqr and Rlqr Weighting matrices
X
b b = [W
Estimated state vector; X c , Pb, Q,
b R,
b Z,
b φ,
b θ, bT
b ψ]
X
bacc Estimated state vector based on accelerometers; X bacc = [φ,
b θ, bT
b ψ]
X
bgyro Estimated state vector based on the integration of the gyroscopes; X
bgyro = [φ,
b θ, bT
b ψ]
T (s) General low pass filter of the complementary filter
S(s) General high pass filter of the complementary filter
Tc Cut-off time of the generic high or low pass filter
Kk Kalman gain of the Kalman filter
Qk and Rk Weighting matrices of the Kalman filter
Superscripts
B Defined in the Body-Fixed frame (ABC frame)

I Defined in the Inertial frame (NED frame)
xxii
Chapter 1
Introduction
Ever since their appearance in the 19800 s, Unmanned Aerial Vehicles (UAV) have been the subject of
interest for the military. The Israeli and US military were amongst the first 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 fixed-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 first sketches of rotorcrafts, appeared the
first 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 flight was
not attained and the short 60cm flight was only made possible thanks to the four arms supporting and
stabilizing the craft. Later that year, a free flight 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 1920’s, 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 configuration flights of several minutes were made possible and a 1Km
close-circuit controlled flight was achieved.
1
(a) Etienne Oehmichen’s prototype. (b) Bothezat’s 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
flight 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
flight 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 simplified control mechanism, the vehicle flew 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 1960’s. 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 finished 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 flying 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 finding some civilian applications.
For instance, the company proposes the usage of these vehicles as a broadcasting tool, accessing diffi-
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 fit a large set of applica-
tions2 . 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 insufficient 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 flight with small drift in position and a stabilized flight
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 first 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
Definitions
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
defined, 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 fixed on the quadrotor and centered on Oc,
the center of mass of the quadrotor, thus called Body-fixed frame or Aircraft-Body-Centered frame, ABC
I
frame, shown in Figure 2.1(b). Vectors expressed in the inertial frame are marked with the superscript
and vectors expressed in the body fixed frame have the superscriptB .
UE UN M3 M2
Oc
M4
φ M1
θ
ψ
UD uy
ux
uz
(a) Inertial Frame (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:
PI = [X, Y, Z]T (2.1)
7
The velocity of the quadrotor is expressed in the body-fixed frame:
VB = [U, V, W ]T (2.2)
The rotation of the ABC frame relatively to the NED frame defines the attitude of the aircraft. According
to Euler’s 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-fixed frame:
ΩB = [P, Q, R]T (2.4)
where: P corresponds to the angular velocity about the ux axis; Q corresponds to the angular velocity
about the uy axis; R corresponds to the angular velocity about the uz 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 fixed 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
2
3
1
4
(a) (c) (e) (g)
(b) (d) (f) (h)
(a) Positive Roll (e) Negatice Yaw

(b) Negative Roll (f) Positive Yaw
(c) Positive Pitch (g) Takeoff
(d) Negative Pitch (h) Land
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 ux 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 uy 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 uz 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
ux and uy .
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 Identification
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 confirmed that the center of mass is
located on the intersection of the ux and uy axes.
hanging point
M2
M1
M3
ux
M4
uy
vertical
Figure 2.3: Determination of the Center of Mass.
To define the location of the center of mass along the uz axis, the prototype is hung as depicted in
Figure 2.4. Different hanging points are considered for a greater confidence in the result. In Appendix B
the pictures are presented and the procedure used to define the center of mass is explained.
10
M2
hanging point
M1
vertical M4
Figure 2.4: Determination of the Center of Mass along uz .
The result of the experiment shows that the center of mass is located hf eet ≈ 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:  
 
Ix 0 0 0.0081 0 0
 kg.m2
   
I=
0 Iy 0 = 0
 0.0081 0  (2.5)
0 0 Iz 0 0 0.0162
The relevant dimensions of the prototype are shown in Figure 2.5:
d
cm=0.29
Oc
feet=0.059
d
h
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 ux and uy .
• The quadrotor is a rigid body.
• All aerodynamic forces acting on the quadrotor are neglected.
• The magnetic inclination is neglected.
• The rotors flapping 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 uz .
• 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.
B
F =[Fx,Fy,Fz]
Rigid Body Dynamics Integrator
+ V,Ω,Ψ,P
+
+
and Kinematics
B +
M =[Mx,My,Mz]
floor
F Ground
Reaction
floor
M
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-fixed 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-fixed one, the first rotation corresponds to yaw (R(ψ)T ), then pitch (R(θ)T )
and finally roll (R(φ)T ) (see equation (3.1)). This corresponds to describe the orientation of the body-fixed
frame relative to the inertial one.
13
     
1 0 0 cos θ 0 − sin θ cos ψ sin ψ 0
T T  , R(ψ)T = − sin ψ
     
R(φ) = 0
 cos φ sin φ  , R(θ) =  0
  1 0   cos ψ 0
 (3.1)
0 − sin φ cos φ sin θ 0 cos θ 0 0 1
Finally combining those three rotations results in applying equation (3.2):

 
cos θ cos ψ cos θ sin ψ − sin θ
T T T
 
S = R(φ) R(θ) R(ψ) = cos ψ sin φ sin θ − cos φ sin ψ sin ψ sin θ sin φ + cos φ cos ψ

 (3.2)
cos θ sin φ 
cos ψ cos φ sin θ + sin φ sin ψ cos φ sin θ sin ψ − cos ψ sin φ cos θ cos φ
where: S is the rotation matrix which expresses a vector from the inertial frame to the body-fixed frame.
When θ = π/2, the rotation matrix becomes equation (3.3) and reveals a singularity:
   
0 0 −1 0 0 −1
   
cos ψ sin φ − cos φ sin ψ sin ψ sin φ + cos φ cos ψ 0  =  sin(φ − ψ) cos(φ − ψ)
S=   0
 (3.3)
cos ψ cos φ + sin φ sin ψ cos φ sin ψ − cos ψ sin φ 0 cos(φ − ψ) − sin(φ − ψ) 0
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 = [q0 , q1 , q2 , q3 ]T .
Equation (3.4) defines the representation of a rotation in the quaternion form and relates it to the Euler
angles representation:
 
cos(φ/2) cos(θ/2) cos(ψ/2) + sin(φ/2) sin(θ/2) sin(ψ/2)
 
sin(φ/2) cos(θ/2) cos(ψ/2) − cos(φ/2) sin(θ/2) sin(ψ/2)
T
q = [q0 , q1 , q2 , q3 ] =  (3.4)
 

cos(φ/2) sin(θ/2) cos(ψ/2) + sin(φ/2) cos(θ/2) sin(ψ/2)
 
cos(φ/2) cos(θ/2) sin(ψ/2) − sin(φ/2) sin(θ/2) cos(ψ/2)
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):  
1 − 2(q22 + q32 ) 2(q1 q2 + q3 q0 ) 2(q1 q3 − q2 q0 )
 
Sq = 
2(q1 q2 − q3 q0 ) 1 − 2(q12 + q32 ) 2(q2 q3 + q1 q0 )
 (3.5)
2(q1 q3 + q2 q0 ) 2(q2 q3 − q1 q0 ) 1 − 2(q12 + q22 )
The rotation matrix presents the particularity of having its inverse identical to its transpose. This simpli-
fies the calculations and can be written as: S−1 = ST .
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 PI = [X, Y, Z]T defines the position of the quadrotor relative to the inertial frame and VB =
[U, V, W ]T defines the linear velocity of the quadrotor in the body-fixed frame. Equation (3.6) expresses the
14
existing relation between the two vectors:
   
Ẋ U
 Ẏ  = STq
   
 
V 
  (3.6)
Ż W
According to [17] and [18], equation (3.7) relates the angular velocities ΩB = [P, Q, R]T with the quadro-
tor attitude:     
q˙0 0 −P −Q −Rq0
    
q˙  1 P
 1 0 R −Q q1 
 
 =  (3.7)
 
q˙2  2 Q −R
 
   0 P  q2 
 
q˙3 R Q −P 0 q3
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 uz , written [F1 , F2 , F3 , F4 ]T ; all have the same direction and one
P4
can write the net force applied to the quadrotor (excluding gravity) as: FB = [Fx , Fy , Fz ]T = [0, 0, − i=1 Fi ]T .
These forces produce moments around the axes of the quadrotor. The net moments are written MB =
[Mx , My , Mz ]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Ω + MB , which leads to:
     (I −I )QR 
Mx z y
Ṗ Ix
  M   (I −IIx )P R 
Q̇ =  y  −  x z  (3.8)
   Iy   Iy 
Mz (Iy −Ix )P Q
Ṙ Iz Iz
where: Ω = [P, Q, R]T is the angular velocity; I = diag(Ix , Iy , Iz ) is the inertia matrix.
Applying the second law of Newton, the acceleration of the quadrotor in the body fixed frame results in:
maB = FB + mSq gI − Ω × mVB (3.9)
where: aB = V̇B = [U̇ , V̇ , Ẇ ]T is the acceleration vector in the body-fixed frame; g I = [0, 0, g0 ]T is the
gravity vector, g0 = 9.81m/s2 .
Thus:        
U̇ Fx 2(q1 q3 − q2 q0 ) QW − RV
 V̇  = 1 Fy  + g0 2(q2 q3 + q1 q0 ) − RU − P W 
       
  m      (3.10)
2 2
Ẇ Fz 1 − 2(q1 + q2 ) P V − QU
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 0 0 φ̇
Q = R(φ)T R(θ)T R(ψ)T  0  + R(φ)T R(θ)T θ̇ + R(φ)T
       
     
0
  (3.11)
R ψ̇ 0 0
15
Developing the equation and solving for the Euler angles rates results in equation(3.12):
    
φ̇ 1 tan θ sin φ tan θ cos φ P
    
 θ̇  = 0 cos φ − sin φ  Q
  (3.12)
   
ψ̇ 0 sin φ/ cos θ cos φ/ cos θ R
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 hf eet =
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 maximum deformation
of 1mm is allowed when the quadrotor hits the floor. Equation (3.13) comes from the equilibrium from the
forces of the four springs and the weight of the quadrotor.
mg0 = 4Kspring 0.001

(3.13)
Kspring = 0.001mg0 /4
where: Kspring is the spring constant. It results that Kspring = 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 coefficient Kdamp equation (3.14) is used:
X 1/m
= 2
F s + Kdamp /ms + Kspring /m
1/m
=
s2 + 2ξω0 s + ω02
(3.14)
Kdamp
ξ=
2mω0
r
Kspring
ω0 =
m
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 Kdamp ≈ 87N.s/m.
16
The positions of the four feet relative to the inertial frame are given by equation (3.15):
z1 = Z − df eet sin θ + hf eet cos θ

z2 = Z + df eet sin θ + hf eet cos θ
(3.15)
z3 = Z − df eet sin φ + hf eet cos φ
z4 = Z + df eet sin φ + hf eet cos φ
where: zi is the height of the foot under motor Mi ; Z is the height of the center of mass of the quadrotor;
df eet is the distance from the center of mass to the feet along ux or uy and hf eet corresponds to the length
of the feet along uz .
Equation (3.16) defines the linear velocity of the feet:
ż1 = W − df eet Q cos θ

ż2 = W + df eet Q cos θ
(3.16)
ż3 = W − df eet P cos φ
ż4 = W + df eet P cos φ
where: żi is the vertical speed of the foot under motor Mi .
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 loor
Fz,i = −(zi Kspring + żi Kdamp ) (3.17)
For the sake of simplicity, the forces are considered as applied in a collinear vector to uz passing
through the contact point with the ground. The forces along ux and uy are opposed to the movement and
correspond to the force of friction; equation (3.18) defines the linear velocity of each foot along ux and uy :
ẋ1 = U
ẋ2 = U
ẋ3 = df eet R + U
ẋ4 = −df eet R + U
(3.18)
ẏ1 = df eet R + V
ẏ2 = V
ẏ3 = −df eet R + V
ẏ4 = V
where: ẋi is the linear velocity along ux of the foot under motor Mi ; ẏi is the linear velocity along uy of the
foot under motor Mi .
The forces of friction along ux and uy are defined by equation (3.19) where the friction coefficient is set
to fc = 0.6 according to [12]:
17
f loor f loor
Fx,i = −|Fz,i |fc sign(ẋi )
f loor f loor
Fy,i = −|Fz,i |fc sign(ẏi ) (3.19)
f or i = 1, 2, 3, 4
It results: !#T
4 4 4 4
" ! !
f loor f loor f loor
X X X X
B
F = Fx,i , Fy,i , − Fi + Fz,i . (3.20)
i=1 i=1 i=1 i=1
f loor
where: Fi is the force produced by the propeller of motor Mi ; Fz,i is the z-component of the ground
reaction on the support under motor Mi .
Considering that landing is to be performed vertically, the moments Mf loor resulting from the ground
reaction can be written as:
 
f loor f loor
(Fz,4 − Fz,2 )df eet
Mf loor =  f loor f loor
 
 (Fz,3 − Fz,1 )df eet 
 (3.21)
f loor f loor f loor f loor
(Fy,1 + Fx,2 − Fy,3 − Fx,4 )df eet
Finally, the net moments acting on the quadrotor can be expressed by equation (3.22), considering the
ground reaction and the motors:
f loor f loor
Mx = (F2 − F4 )dcm + (Fz,4 − Fz,2 )df eet
f loor f loor
My = (F1 − F3 )dcm + (Fz,3 − Fz,1 )df eet (3.22)
f loor f loor f loor f loor
Mz = −(F1 − F2 + F3 − F4 )K + (Fy,1 + Fx,2 − Fy,3 − Fx,4 )df eet
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 defined. 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 Wahba’s paper in 1966 [13], the solution for estimating the attitude of a satellite (or
a quadrotor in this case) lies on finding a matrix which brings a first 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 flight remained unachieved perhaps due to the difficulty 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 insufficient 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 finder 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 uz and a 2-axis Gyroscope LPR530AL for ux and uy .
19
• A Sharp DP2D12 range finder.
The accelerometer and the gyroscope, forming the IMU, come in a single board the Razor 6DOF from
S PARKFUN E LECTRONICS
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 STM ICROELECTRONICS
R
and present the same dynamics
and governing equations; as suggested by [18] and [8] and confirmed 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 + bg (4.2)
B
where: Ω is the measured angular rate, defined in the body-fixed frame; ΩB is the real angular rate,
defined in the body-fixed frame; µg is the Gaussian noise of the gyroscope; bg 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:
3.3V
GyroRes = = 0.9678◦ /s (4.3)
1024 · 3.33mV /(◦ /s)
Because the analog signal from the 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 210 = 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, agyro 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-fixed frame from Figure 4.1:
gyroscope
j
frame ω
rz mass ν
ux
body-fixed frame
k uy
uz
Figure 4.1: Frame of the Gyroscope.
agyro = 2ω × v (4.4)
where: agyro 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, rz ]T be the location of the center of the IMU relative to the body-fixed frame. In this situation
equation (4.4) can be re-written, dismissing the angular acceleration:
agyro = ω × (ω × r) + 2ω × v (4.5)
21
Spring Suspension Subtract
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 uz , having the sensor located out of the center of mass has no effect on the
readings because the first 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:
           
P 0 P P 0 0
agyro
           
 0  ×  0  +  0  ×  0  ×  0  = 2P vz 
= 2 (4.6)
          
0 vz 0 0 rz P 2 rz
Note that the acceleration has a component along the j − axis (or i − axis) and the k − axis. 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, rz has no effect on the measured
value.
Accelerometer
The accelerometer is used to measure the direction of the gravity vector, gI , in order to define the pitch
and roll angles; let it be taken as constant, pointing down along UD with an intensity g0 = 9.81m/s2 and let
aB 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 fixed 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 aB . According to [22], for the chosen frame system, one can write: aB = gB − aB .
Introducing a bias Gaussian measurement noise as in [18], the general model can be written as:
22
spring
fixed outer plate dx
movable mass
C
1
with plates
C
2
spring
Figure 4.3: Functioning Principle of a Capacitive Accelerometer.[21]
aB = SgI − aB + µa + ba (4.7)
where: aB is the sensor output in m/s2 ; S is the rotation matrix; gI is the gravity vector defined in the
inertial frame; aB is the acceleration of the craft due to its movement defined in the body fixed frame; µa is
a Gaussian noise component; ba 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 flying quadrotor are the ones produced by the
four propellers, resulting in:
FB + mgB
aB = (4.8)
m
where: m is the mass of the prototype; FB are the forces applied to the Quadrotor (excluding gravity).
which results in:

FB
aB = − + µa (4.9)
m
Furthermore, since the accelerometer is not positioned at the center of mass, a centripetal and a
tangential term appear:
FB
aB = − + Ω̇ × r + Ω × (Ω × r) + µa (4.10)
m
where: r = [0, 0, rz ]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:

         
ax Fx Q̇rz − Ṙry Q(P ry − Qrx ) − R(Rrx − P rz ) µa−x
ay  = 1 Fy  + Ṙrx − Ṗ rz  + R(Qrz − Rry ) − P (P ry − Qrx ) + µa−y 
         
  m        (4.11)
az Fz Ṗ ry − Q̇rx P (Rrx − P rz ) − Q(Qrz − Rry ) µa−z
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 simplification is proposed
in [18] for a near-hovering situation:
aB = SgI + Ω̇ × 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 simplified model; if the quadrotor is in free
fall in a horizontal position, the output of the accelerometer should be 0, but with the simplification, the
measurement would remain at SgI . The simplification is only valid for near-hovering situations and can be
written as:
         
ax − sin θ Q̇rz − Ṙry Q(P ry − Qrx ) − R(Rrx − P rz ) µa−x
         
ay  = g0  cos θ sin φ  + Ṙrx − Ṗ rz  + R(Qrz − Rry ) − P (P ry − Qrx ) + µa−y  (4.13)
         
az cos θ cos φ Ṗ ry − Q̇rx P (Rrx − P rz ) − Q(Qrz − Rry ) µa−z
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 150µg/ Hzrms
√
noise Zout 300µg/ Hzrms
resolution 0.0958m/s2
The resolution is calculated in the same way as for the gyroscopes:

3.3V · 9.81m/s2
AccRes = = 0.0958m/s2 (4.14)
1024 · 0.33V
Compass
The compass is a sensor designed to detect the magnetic North direction, written NI . By the definition
of the reference frame and neglecting the magnetic inclination or magnetic dip, NI = [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 field [15]. By measuring
B
the electric resistance, the orientation of the magnetic field can be estimated. Let N be the sensor
measurement; considering the frame system, a Gaussian measurement noise, µm , and a bias term, bm ,
as in [20], the compass measurement corresponds to:
B
N = SNI + µm + bm
       
Nx cos θ cos ψ µm−x bm−x
        (4.15)
N y  = cos ψ sin φ sin θ − cos φ sin ψ  + µm−y  + bm−y 
       
Nz cos ψ cos φ sin θ + sin φ sin ψ µm−z bm−z
B
where: N is the measured magnetic vector; NI is the magnetic vector; µm = [µm−x , µm−y , µm−z ]T is the
Gaussian measurement noise; bm = [bm−x , bm−y , bm−z ]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 NI on the body-fixed frame:
B
N = norm diag([1, 1, 0])SNI + µm + bm

 
cos θ cos ψ 0 0
  (4.16)
= norm 

 0 cos ψ sin φ sin θ − cos φ sin ψ 0 + µm + bm

0 0 0
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 ux and uy axes; considering that the sensor has only two sensing axes, the
model can be further simplified to:
   
Nx cos ψ
B    
N =
N y  = − sin ψ  + µm + bm
   (4.17)
Nz 0
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 finder 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 floor along uz 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 reflected on an object and retrieved by the sensor; analyzing the
triangle formed by the receiver, emitter and reflecting point, the angles are measured and the distance to
the floor is estimated as presented on Figure 4.4:
25
wall
measured
angle
GND
Vcc
V0
Figure 4.4: Range Finder working Principle.
The sensor has a nonlinear behavior as presented in Figure 4.51

Output in Volts(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 bz . Equation (4.18) corresponds to the model of
the sensor:
z B = Z + µz + bz (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 finder allows a maximum sampling frequency of 25Hz while the other sensors are
supposed to work at 50Hz. Because the range finder 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 difficult. 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 efficient 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 difficulties experienced by Jorge Domingues and justifies the acquisition
of the gyroscopes. For security reasons it is desired to have the quadrotor above a certain threshold but
also not flying too high, which would possibly become a threat to humans. Therefore, the range finder was
acquired.
4.3 Experimental Identification 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 confirms 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 finally with all of
them on. With this test the influence 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:
P ow = σ 2 .(1/f req) (4.19)
where: P ow is the spectral power. σ the standard deviation. f req = 50Hz is the sampling frequency.
√
The values provided by the manufacturer for the accelerometer noise are: 150µg/ Hzrms for ux
√
and uy axes and 300µg/ Hzrms for the uz axis, corresponding to the spectral power: 1502 µg 2 /Hz and
3002 µg 2 /Hz.
27
Table 4.5: Noise Characteristics of the Accelerometers
Spectral Power (m/s2 )2 /Hz

Accelerometer’s component theoretical no motor motor 1 and 3 all motors
−6 −5 −2
x 2.1653 × 10 3.8133 × 10 4.2480 × 10 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/s2 instead of g. Even with all motors off, the noise mea-
surements differ from the theoretical values. Note also the similarity between the ux and uy components
and the difference to the uz component when all motors are working. Recalling equation (4.11) and setting
the forces to zero one obtains:
     
Q̇rz µa−x RP rz
aB = Ω̇ × r + Ω × (Ω × r) + µa = 
     
−Ṗ rz  + 
  RQrz  + µa−y  (4.20)
  
2 2
0 −P rz − Q rz µa−z
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 ux and uy are possible but not
around uz , therefore R = 0. As a consequence one can write equation (4.21) for the experiment:
   
Q̇rz µa−x
aB = 
   
−Ṗ rz  + µa−y  (4.21)
   
2 2
−P rz − Q rz µa−z
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 ux and uy 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 significant change in 10000 readings at 50Hz. A time varying bias is
considered to affect the gyroscopes: bg (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 field of the motors or the electric wires. Exper-
iments must be undertaken in order to determine how the components of the quadrotor prototype influence
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 influence 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

2
Spectral Power (deg /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 influence the reading due to external magnetic fields 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 defined distances from the floor 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 defined by equation (4.22). The values up to 50cm are presented because it would be
preferable to have the quadrotor flying at low heights avoiding the danger of passing 80cm:
z B = 14882/(Sensoroutput + 91.8) − 0.8 (4.22)
where: z B is the estimation of the height of the prototype in cm; Sensoroutput is the sensor output as read
by the Arduino.
The constant term present 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 flight, the noise term of the range finder model will be dropped.
29
Sharp GP2D12 calibration
60
Sensor curve
Curve Fitting
50
40
distance (cm)
y=(14882/(x+91.8))-6.7
30
20
10
0
0 200 400 600 800 1000
Sensor signal
Figure 4.6: Identification 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 Mi produces an angular velocity ωi and the propeller is considered to spin without slippering
at the defined angular velocity. In this chapter, the PWM signal is introduced. Also a simplified model for
the motor with the model of the propellers explaining the generation of thrust is presented.
Moments
Arduino Speed
Motor Propeller
Board Controller
PWM ω
Forces
Figure 5.1: Configuration 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
flapping of the blades and the ground effect are not considered either.
According to [4] and [26], the thrust, Fi , and the moment, Mz i , created by each propeller are propor-
tional to the squared angular velocity of the blade, as shown in equation (5.1):
Fi = KT ωi2
(5.1)
Mz i = KM ωi2
where: KT is the constant which relates the thrust to the square of the angular velocity of the propellers;
KM is the constant which relates the moment to the square of the angular velocity of the propellers.
The two constants can be written as:

ρD4
KT = cT
4π 2 (5.2)
ρD5
KM = cP
8π 2
31
where: D = 25.4mm is the diameter of the propeller; ρ is the density of the air; cT = 0.1154 is the thrust
coefficient; cP = 0.0743 is the power coefficient;
The two coefficients are dependent on the propeller and were obtained experimentally by 1 ; as a result,
KT = 1.46 × 10−5 kg.m.rad−2 and KM = 3.8 × 10−7 kg.m2 .rad−2 .
Writing K = KM /KT = 0.026m, the forces and moments presented in chapter 3 can now be under-
stood; they result from:
Fi = KT ωi2
Mx = (ω22 − ω42 )KT dcm = (F2 − F4 )dcm
(5.3)
My = (ω12 − ω32 )KT dcm = (F1 − F3 )dcm
Mz = (ω12 + ω32 − ω22 − ω42 )KM = (F1 + F3 − F2 − F4 )K
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
define 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.
re L T θ
V E
J
i .
bθ
(a) Electrical System of the Motor (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

2A 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 , P W Mi is used:
dI
V = re I + L + Ke θ̇
dt
Ke I = J θ̈ + bθ̇ (5.4)
ωi Ke
=
P W Mi (Js + b)(Ls + re ) + Ke2
where: ωi is the angular velocity of the rotor of motor i; P W Mi is the input PWM signal to motor i; Ke 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; re is the electric resistance; I is the intensity of the current.
Equation (5.4) presents an accurate model of the motor; however it is difficult to measure all the param-
eters of the motor. The model can be simplified to a first-order system 4 corresponding to the mechanical
dynamics:
ωi ki
= (5.5)
P W Mi τs + 1
where: τ is the time constant of the system; ki is the dc gain.
To define 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 influence of the electrical dynamics
of the motor is considerably faster than the mechanical dynamics of the motor with the propellers. The
identification of the actuator set is done to determine the dc gain ki and the mechanical time constant τ .
Because the electrical dynamics is much faster, it can be linearized and taken as a multiplicative constant
included in ki .
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 Identification of the Motors
The four motors are expected to present different characteristics partly because the speed controllers
used are not identical. Motors M1 and M2 have the same electronic components but motors M3 and M4
have each a different element.
In section A.2 the motors wiring scheme is presented. The PWM is bounded to [1000; 2000]µs. There
is yet another nonlinearity to consider; each motor has a dead zone. To define 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 Mi can be written P W Mid .
4 Prof. J.Fainguelernt from the university of Tel-Aviv proposes the simplification in a document aiming at DC Motor Control
33
Table 5.1: Dead Zone of the Motors
Motor P W Mid in µs
M1 1179
M2 1184
M3 1190
M4 1260
The multiplicative constant ki of equation (5.5) is the static gain of the model, it defines the relation
between the stationary response of the system and its solicitation. To define 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 reflection of a laser beam on a reflective duct tape attached to the blade; measuring the time
between two reflected beams the angular velocity can be estimated. Table 5.2 and Figure 5.3 present the
result; over 1600µs the vibration of the blades make it difficult 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
MotorM1 MotorM2
900 1000
stationary response stationary response
800
polynomial fitting polynomial fitting
800
700
ω1 (rad/s)
ω2 (rad/s)
600
600
500
400
400
300
200 200
1200 1250 1300 1350 1400 1450 1500 1200 1250 1300 1350 1400 1450 1500
P W M1 (µs) P W M2 (µs)
MotorM3 MotorM4
1200 1000
stationary response stationary response
1000 900
polynomial fitting polynomial fitting
800
ω3 (rad/s)
ω4 (rad/s)
800
700
600
600
400
500
200 400
0 300
1200 1250 1300 1350 1400 1450 1500 1300 1350 1400 1450 1500 1550 1600
P W M3 (µs) P W M4 (µs)
Figure 5.3: Identification of the Actuators.
The curves are approximated by a second-order polynomial using the function polyfit and polyval from
Matlab. Table 5.3 presents the polynomial.
34
Table 5.3: Polynomial Approximation; ax2 + bx + c
Motor a b c
M1 −0.0025 8.6849 −6.63 × 103
M2 −0.0023 8.3355 −6.40 × 103
M3 −0.0034 12.008 −9.41 × 103
M4 −0.0022 8.2903 −6.75 × 103
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 ki 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.
Time Constant Identification

1300 motor response
reference
1250
PWM (μs)
1200
1 2 3 4 5 6 7 8
time(s)
Figure 5.4: Determination of the Time Constant of the Motor Set.
The square wave varies from 1200µs to 1300µs; in the following chapter, the linearization point is deter-
mined, using Identification Toolbox from Matlab, the system can be approximated by a first-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 defining the functions g and h which satisfy the general nonlinear state
space formulation:
Ẋ = g(X, U) Dynamic Equation
(6.1)
Y = h(X, U) Output Equation
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 finder for now.
If equations (3.6)-(3.10) define 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 U as the signal sent from the Arduino
board, thus U = PWM = [P W M1 , P W M2 , P W M3 , P W M4 ]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 first order systems modeling the motors express the angular velocities as a function of PWM; for that
the static gains have to be defined.
The output function can be defined by two sets of equations; as discussed earlier, there are two models
for the accelerometer worth being tested and compared. The first output set, based on the properties and
dynamics of the prototype such as the mass and the forces applied, is defined by equations (4.2), (4.11)
and (4.17). The second output set, based on the projection of the gravity vector onto the body fixed frame,
is defined 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 defined 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
(6.2)
Y ≈ CX + DU
The approximation is only valid in the vicinity of an operating point or linearization point X0 ; the primary
target of this work would be to have the quadrotor hovering. This corresponds to:
X0 = [0, 0, 0, 0, 0, 0, 0, 0, −z, 0, 0, 0]T . Mathematically, to perform a linearization of a function f (X1 , X2 ) at
a given operating point (X10 , X20 ), the first order approximation of the Taylor series is used as shown in
equation (6.3):
f (X1 , X2 ) ≈ f (X10 , X20 ) + fX1 (X10 , X20 )(X1 − X10 ) + fX2 (X10 , X20 )(X2 − X20 ) (6.3)
where:f (X1 , X2 ) is the dynamic equation; fX1 is the derivative of f in the variable X1 ; f (X10 , X20 ) is the
trim condition.
The input vector corresponding to the desired operating point is defined solving equation (6.4), which
balances the total thrust with the weight of the prototype.
Fz = mg0
4
X
Fi = mg0
i=1 (6.4)
Fi = mg0 /4
mg0
(ωi0 )2 =
4KT
where: ωi0 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 = ωi0 + ki (P W Mi − P W Mi0 ) (6.5)
where: P W Mi is the PWM signal sent to motor i. P W Mi0 is the PWM value corresponding to the trim value.
To remove ωi0 , the equation can be written introducing a new term P W Mid−eq , corresponding to the
intersection of the tangent of the motor identification curve with the abscise axis.
ωi = ki (P W Mi − P W Mid−eq ) (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 P W Mid−eq :
38
Table 6.1: Dead Zone of the Motors
Motor Linearization Point P W Mi0 (µs) Static Gain ki ((rad/s)/µs) Equivalent Dead Zone P W Mid−eq (µs)
M1 1254.2 2.53 1094.6
M2 1250.0 2.56 1092.1
M3 1265.9 3.47 1149.3
M4 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 P W Mi0 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 M1 and M2 have similar dead zone values, equilibrium
points and static gains whereas motor M3 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 first 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 = [P W M1 , P W M2 , P W M3 , P W M4 ]T ,
the state vector is X = [V, Ω, P, Ψ]T and the output vector is Y = [a, Ω, N]T .
 
0 0 0 0 0 0 0 0 0 0 −g0 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 
A= (6.7a)
 


 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
39
 
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 
B= (6.7b)
 


 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 1 0 0 0 0 0 0 0 0 

C= 0 0 0 0 1 0 0 0 0 0 0 0  (6.7c)
 
 
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
 
−0.0559 0 0.0765 0
 

 0 0.0564 0 −0.0559 

 
 0.0307 0.0310 0.0421 0.0307 
 
 

 0 0 0 0 

D= 0 0 0 0 (6.7d)
 

 
0 0 0 0
 
 
 

 0 0 0 0 

 

 0 0 0 0 

0 0 0 0
If the accelerometer is modeled with the projection of the gravity vector instead, matrices C and D:
 
0 0 0 0 0 0 0 0 0 0 −g0 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 1 0 0 0 0 0 0 0 0 

C= 0 0 0 0 1 0 0 0 0 0 0 0  (6.8a)
 
 
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
40
 
−0.0559 0 0.0765 0
 

 0 0.0564 0 −0.0559 

 

 0 0 0 0 

 

 0 0 0 0 

D= 0 0 0 0 (6.8b)
 

 
0 0 0 0
 
 
 

 0 0 0 0 

 

 0 0 0 0 

0 0 0 0
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 influence on those sensor measurements. The accelerometer
based on the gravity presents also a null row: az . The output vector is reduced to Y = [ax , ay , 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 confirmed 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 first term of the Taylor expansion, regards the input vector
(Utrim ), the output vector (Ytrim ) and the state vector (Xtrim ); it is presented in equation (6.9) and is
calculated using the chosen model:
Utrim = [P W M10 , P W M20 , P W M30 , P W M40 ]T

Ytrim = [0, 0, g0 , 0, 0, 0, 1, 0, 0]T (6.9)
Xtrim = [0, 0, 0, 0, 0, 0, 0, 0, −z, 0, 0, 0]T
The chosen model is continuous. The observation and control algorithms will be loaded on the Arduino
board and intended to run at a frequency f req = 50Hz (note that the range finder 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:
Xk+1 = Ad Xk + Bd Uk
(6.10)
Yk = Cd Xk + Dd Uk
where: Ad , Bd , Cd , Dd are the discrete state space matrices; Xk , Uk , Yk are respectively the state vector,
input vector and output vector at time k; Xk+1 is the state at time k + 1.
41
Equation (6.11) presents the discretization process using the zero order holder:
Ad = eAT (6.11a)
Bd = A−1 (Ad − I)B (6.11b)
Cd = C (6.11c)
Dd = 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:
X
MATLAB
X. 1
s
Function
[ω1,ω2,ω3,ω4] Integrator
Gfunction-Hfunction 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 PWM into ω to the dynamics
and kinematics model.
Figure 7.2 presents a simulation of a take off, followed by a hovering flight 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 floor. 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 floor. To ensure this, two flags are
used. If the quadrotor is flying a flag is set and from that time on, if the height of the center of mass goes
under the length of the feet df eet , then the engines are turned off by setting the other flag. 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
T ake off and Landing
0
-0.5
-1
Z(m)
-1.5
-2
Quadrotor Tracking
reference
-2.5
0 5 10 15 20 25 30
time(s)
Figure 7.2: takeoff and Landing Simulation.
7.1.2 Sensors
The output equation corresponding to the sensor measurements is included in FfunctionHfunction.m

file. 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:
Theoretical Acc Noise switch

gyroscope
Sine Wave
accelerometer
-K-
Measured Acc Noise magnetometer
Saturation Quantizer
R2D D2R 1
1
Out1
In1 Radians Saturation Quantizer Degrees to
to Degrees Radians
R2D D2R
Radians Saturation Quantizer Degrees to

to Degrees Radians
Theoretical Gyro Noise

switch switch Theoretical Comp Noise
Measured Gyro Noise

Measured Comp Noise
Figure 7.3: Implementation of the Model of the Sensors.
Note that by setting a flag, the user can chose to perform simulations using either theoretical values
for the noise affecting the sensors or use the experimentally defined 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; first the saturation block ensures that the signal is within the defined boundaries,
then the dead zone of the motors is simulated and finally 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 first order state space blocks corresponding to the transfer functions
defined previously; this block has the advantage of setting the initial condition.
44
x' = Ax+Bu
y = Cx+Du
Saturation1 Dead Zone Quantizer1 State-Space
x' = Ax+Bu
y = Cx+Du
4 4
1 Saturation Dead Zone1 Quantizer2 State-Space1 1
In1 PWM x' = Ax+Bu [ω1,ω2,ω3,ω4] Out1
y = Cx+Du
Saturation2 Dead Zone2 Quantizer3 State-Space2
x' = Ax+Bu
y = Cx+Du
Saturation3 Dead Zone3 Quantizer4 State-Space3
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-files the model of the motors and the
models of the sensors. It also contains the m-file performing the dynamics and the kinematics calculations.
X 1
MATLAB s
Function Integrator
PWM0 PWM Ω Yideal
Quadrotor
dynamics
Out1 In1
V
LQR U
W
P
Q
R
X
Y
Z
phi In1 Out1
tetha
psi
Signal Builder Sensors
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 confirmed by the open loop response.
45
[U, V, W ]T
500
ang. vel(rad/s) velocity(m/s)
U
V
0 W
-500
0 5 10 15 20 25
[P, Q, R]T
1
P
0 Q
R
-1
-2
0 5 10 15 20 25
[X, Y, Z]T
4000
attitude(deg) position(m)
X
2000 Y
Z
0
-2000
0 5 10 15 20 25
[φ, θ, ψ]T
200
φ
θ
0
ψ
-200
0 5 10 15 20 25
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 defined. 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
(8.1)
y = Cx + Du
The optimal control problem consists of finding the optimal control action u = −Klqr x which will control
the state vector minimizing a cost function, with K being the optimal gain matrix:
Z ∞
JLQR = (xT Qlqr x + uT Rlqr u)dt (8.2)
0
49
where: x is the state vector with dimension [n × 1]; Qlqr and Rlqr are semi-positive definite 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 Bryson’s rule, which states
that Qlqr and Rlqr can be written as in equation (8.3) as diagonal matrices:
1
Qlqr =
x2max
(8.3)
1
Rlqr = 2
umax
where: xmax and umax are the maximum desired variations.
The dynamic of the closed loop is given by:
ẋ = (A − BKlqr )x (8.4)
The LQR is used for two purposes in this work. In a first 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 = Xref − X. The errors EX and EY are defined in the inertial frame but the
LQR considers it to be written in the body-fixed frame. To correct that when ψ 6= 0, EX and EY are rotated
into the body frame using the ideal measurement of ψ.
The weighting matrices are defined as equation (8.5). To use the Bryson’s 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
refined 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 .
Qlqr = diag([5, 5, 2, 100, 100, 50, 4, 4, 10, 150, 150, 60])

(8.5)
Rlqr = diag([0.01, 0.01, 0.01, 0.01])
50
[U, V, W]T
1
velocity(m/s)
U
V
0 W
-1
0 10 20 30 40 50
[P, Q, R]T
1
ang vel (rad/s)
P
Q
0 R
0 10 20 30 40 50
[X, Y, −Z]T
2 X
position(m)
Y
0 -Z
-2
0 10 20 30 40 50
[φ, θ, ψ]T
80
attitude(deg)
φ
60
θ
40
ψ
20
0
0 10 20 30 40 50
time(s)
Figure 8.1: Reference Set.
Integrator
1
MATLAB s
Function
PWM0 In1 Out1
Quadrotor
PWM reference Motors dynamics Sensor Measurements
Error
Error
Out Ideal State Vector
ideal sensor
LQR Signal Builder
State References
(a) Full-state control with ideal sensor.
1
Error
U Rotation Y K*uvec 1
Out
2 LQR
ideal sensor
(b) Subsystem of the controller.
Figure 8.2: Full-State Control.
51
The closed-loop response to the state-references Xref is presented in Figure 8.3:
[U, V, W ]T
ang vel(rad/s) velocity(m/s)

1
U
V
0 W
-1
0 10 20 30 40 50
[P, Q, R]T
2
P
Q
0 R
-2
0 10 20 30 40 50
[X, Y, −Z]T X
4
position(m)
Y
2 -Z
X ref
0
Y ref
-2
0 10 20 30 40 -Z ref
50
T
[φ, θ, ψ]
attitude(deg)
φ
100
θ
50 ψ
0 φref
θref
-50
0 10 20 30 40 ψref
50
time(s)
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 confirms that the controller has been well tuned. The rising time corresponds to necessary
time for the system to reach 90% of its final value. The settling time is the amount of time taken for the
system to reach stationary state, within 2% of the final 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 modified.
The data obtained with the 12 states controller is now available to tune the estimator and to test different
52
Close Loop Poles
0.8
0.6
0.4
Imaginary Axis
0.2
-0.2
-0.4
-0.6
-0.8
-1
-20 -15 -10 -5 0 5
Real Axis
Figure 8.4: Closed Loop Poles.
X − step
1 Response
X (m)
Reference
0.5
0
16 18 20 22 24 26
Y − step
1 Response
Y (m)
Reference
0.5
0
26 28 30 32 34 36
Z − step
2 Response
−Z(m)
Reference
1.5
1
0 2 4 6 8 10
ψ − step
60 Response
ψ(deg)
40 Reference
20
0
6 8 10 12 14 16
time(s)
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 flight.
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 filters and Kalman filters 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 configuration 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.
1
s ideal state
MATLAB
Function Integrator
PWM0 In1 Out1 Quadrotor
dynamics
PWM reference Motor Dynamics
V
U
W
P
Q
R
X
Y
Z
phi
tetha
psi
State Reference
In1
Out1
ideal state
In2
LQR Controller
ideal state
estimated state modeled sensors

Estimator Out1 In1
Scope
Estimator Sensor Model
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 filter is the existence of this complementary
55
behavior of the sensors. A low-pass filter applied to the accelerometers and a high-pass filter applied to
the gyroscopes would retrieve the information concerning the attitude over the whole spectrum.
As explained in [19], the complementary filter allows the fusion of low bandwidth position measurements
with high bandwidth rate measurements. Equation (9.1) presents the general idea behind the complemen-
tary filter:
b = C(s) b s X
bgyro
X Xacc (s) +
s + C(s) s + C(s) s
(9.1)
bacc (s) + S(s) Xgyro
b
= T (s)X
s
b is the estimated state vector; X
where: X bacc is the estimation based on accelerometers; X
bgyro /s is the
integration of the estimation based gyroscopes; T (s) is a low pass filter; S(s) is a high pass filter.
The requirement T (s) + S(s) = 1 ensures that the full bandwidth is considered. The simplest filter
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 filter
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 first complementary filter 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 filters are used
for the fusion.
Let us recall some useful properties of the direction cosine matrix defined 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 NI = [1, 0, 0]I then the first column of the direct cosine matrix, S,
corresponds to the projection of the North vector onto the body fixed frame, which is approximated to
the compass measurement vector. The same idea defines the third column of the rotation matrix to the
accelerometers measurement vector, in a hover flight. 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 f req = 50Hz.
Note the consequence of not having a three-axes compass. The present compass has only two sensi-
tive axes, as consequence ψb 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.
B B
S = [N , aB × N , aB ]
S(:, 1) = S(:, 1)/norm(S(:, 1))
S(:, 2) = S(:, 2)/norm(S(:, 2))
S(:, 3) = S(:, 3)/norm(S(:, 3))
φb1 = atan2(S(2, 3), S(3, 3))
θb1 = −asin(S(1, 3))
ψb1 = 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]:
B
error = dot(N , aB )
B B
N = N − aB error/2 (9.2)
B
aB = aB − N error/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 filters. Both the low pass and the high pass filters
are first order systems. Equations (9.3) shows the state space representation of the filters for a cut-off
frequency 1/Tc rad/s:
xhp
˙ =uhp /Tc − xhp /Tc HighP assF ilter
yhp =uhp − xhp
(9.3)
x˙lp =ulp /Tc − xlp /Tc LowP assF ilter

ylp =xlp
where: xhp and xlp are the state variables of the high pass and low pass filters. uhp = [φb2 , θb2 , ψb2 ]T and
ulp = [φb1 , θb1 , ψb1 ]T are the input variables of the high pass and low pass filters. yhp = [φbhp , θbhp , ψbhp ]T and
ylp = [φblp , θblp , ψblp ]T are the output variables of the high pass and low pass filters. Tc 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, first iteration then
S0 = I3
else {t 6= 0, not the first iteration}
St = (I3 + T Ωx )St−T
error = dot(St (:, 1), St (: 3))
St (:, 1) = St (:, 1) − St (: 3)errorg /2
St (: 3) = St (: 3) − St (:, 1)errorg /2
St (:, 2) = cross(St (: 3), St (:, 1))
end if
φb2 = atan2(St (2, 3), S(3, 3))
θb2 = −asin(St (1, 3))
ψb2 = atan2(St (1, 2), S(1, 1))
of the filters in rad/s.
The low pass filter is applied to the three angles estimated from the accelerometer and compass and
the high pass filter to the three angles estimated from on the gyroscopes. The resulting estimation is
presented in equation (9.4):
φb = φbhp + φblp
θb = θbhp + θblp (9.4)
ψb = ψbhp + ψblp
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 first guess the angles
from the accelerometer and compass estimation.
9.1.2 Passive Complementary Filter
The passive complementary filter presented in [18], [19] and [20] is based on the same logic as the
previously described filter, but the formulation is slightly different. Instead of explicitly defining the high and
low pass filters with the cut-off frequency, the filters are implemented as in Figure 9.2, which represents
equation (9.1).
Xgyro
^
^ ^
Xacc + +
X
-
C(s) +
∫
Figure 9.2: Complementary Filter.
58
Let Rs = ST denote the transpose of the rotation matrix. The complementary filter can be written as:
˙ B
R s = (Rs Ω + kp Rs ωcorr )× Rs
c c c
 
0 −Ω3 Ω2
 
(Ω)× = 
 Ω3 0 −Ω1 
 (9.5)
−Ω2 Ω1 0
T
cs Rs − Rs T R
ωcorr = kest /2vex(R cs )
cs is the estimation of the direct cosine matrix; ΩB is the gyroscope measurement vector; kp is a
where: R
proportional control variable, it corresponds to the cut-off frequency of the filters; ωcorr is a correction term;
vex(Ω× ) = Ω is a defined algebraic operator, corresponding to the inverse of ()× .
The general form of this complementary filter is based on the true value of Rs to calculate the error
T
cs Rs . However that term is not available and two options can be used to solve the problem. The
R
first option is to consider the accelerometers and the compass as the best estimation and use the matrix
resulting from those sensors instead of Rs . Let Ry denote that matrix; equation (9.6) defines the matrix
according to [18]:
˙ B
R s = (Ry Ω + kp Rs ωcorr )× Rs
c c c
cs NB |2 + λ2 |gI − R
Ry = arg(min(λ1 |NI − R cs aB |2 )) (9.6)
T
cs Ry − Ry T R
ωcorr = kest /2vex(R cs )
where: arg(min()) is an optimization operation which finds the variable R

cs that minimizes the function
The direct complementary filter 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 filter is dropped.
To simplify the process of calculating the error, equation (9.7) is proposed:
B T T
cs NI ) + λ2 (aB × R
ωcorr = λ1 (N × R cs gI /g0 ) (9.7)
Instead of using Ry , the passive complementary filter considers the estimated matrix R
cs instead of Rs :
˙ c B
R s = Rs (Ω + ωcorr )× (9.8)
c
In addition, a term can be included to compensate for the bias. According to [18], the passive comple-
mentary filter can be written as:
˙ c B
R s = Rs (Ω + ωcorr − ωbias )×
c
B T T
cs NI ) + λ2 (aB × R
ωcorr = λ1 (N × R cs gI /g0 ) (9.9)
ω̇bias = −λi ωcorr
59
9.1.3 Discussion of the Complementary Filters
To evaluate the different filters, 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 filters are implemented in S-functions in a discrete mode. In
Figure 9.3, the behavior of the estimators are compared, and the passive complementary filter is imple-
mented with the bias compensation term in Figure 9.4.
Attitude Estimation with C omplementary F ilters

20
10
φ(deg)
-10
-20
0 10 20 30 40 50
20
10
θ(deg)
-10
-20
0 10 20 30 40 50
80
Ideal sensor
60
ψ(deg)
First Filter
40 Passive Filter
20
0
0 10 20 30 40 50
time(s)
Figure 9.3: Attitude Estimation with Complementary Filters.
Before comparing the filters, it is important to note that the first complementary filter 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/Tc = 1/0.8rad/s. This value was obtained by a
trial and error process led by the trade-off between noise or bias filtering comparing the mean, standard
variation and mean squared error of the estimation.
The passive complementary filter 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 filter appears to be a finer method than the first filter; it allows a discrimi-
nate weighting of the individual sensors and is computationally more efficient to implement due to its lack
of renormalization. On the other hand it is less intuitive.
Both filters depend on inverse trigonometric functions. As seen from the first complementary filter,
which has no bias compensation, the non continuity of the atan() function used to calculate the angle ψ
60
Attitude Estimation withBias C ompensation
20
10
φ(deg) 0
-10
-20
0 10 20 30 40 50
20
10
θ(deg)
-10
-20
0 10 20 30 40 50
80
60
ψ(deg)
Ideal sensor
40 Passive Filter
20
0
0 10 20 30 40 50
time(s)
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
2 2 2
(deg) (deg) (deg) (deg) (deg) (deg) (deg) (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 definition 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 efficient. A comparison
of different parameters are presented in Table 9.1.
Because of that non-continuity, the first complementary filter presents a worse estimation than the
passive filter for the three angles. As a consequence only the passive filter 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 filter 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 filter is better than the first complementary
filter (with the chosen set of parameters) but is still too dependent on trigonometric functions and struggles
with the noise filtering.
9.2 Kalman Filter
An alternative to the complementary filters is to use the Kalman filter as done in [16], [29] and [14].
According to [31] and [30], the Kalman filter 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):
Xk+1 = Ad Xk + Bd Uk + wk
(9.10)
Yk = Cd Xk + Dd Uk + vk
where: Ad , Bd , Cd , Dd are the discrete state space matrices. Xk is the state vector at time k. wk and vk
are respectively the process and measurement noises.
Let Qk and Rk denote the process and measurement noise covariances of the stochastic variables wk
and vk . The Kalman filter can be written as a set of mathematical equations (described by the diagram of
Figure 9.5). First the filter 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 Kk , weights the time prediction and the
measurement correction.
The following approaches to the Kalman filter are all implemented in the discrete mode using S-
functions. The discretization is performed computationally using equations (6.11). The matrices Rk and
Qk are diagonal matrices with values corresponding to the noise characteristics of the sensors. To obtain
the discrete weighting matrices, equation (9.11) is used:
Rd = Rk /T
Z T (9.11)
Qd = eAτ Qk eAτ dτ
τ =0
Figure 9.6 presents the position of the Kalman filter in the general bloc diagram.
62
Time Update (Prediction phase) Measurement Update (Correctionphase)
(1) Compute the Kalman Gain

(1) Project the State ahead
Kk = P-k CdT ( Cd P-k CdT + R )-1
^- ^
X k= Ad Xk-1 + Bk Uk
(2) Update estimate with measurement Yk
^ ^- ^
Xk= X k + Kk ( Yk - Cd X-k - Dd Uk )
(2) Project the Error Covariance ahead
(3) Update the Error Covariance
P-k = Ad Pk-1AdT + Q
Pk = ( I - Kk Ck ) P-k
^
Initial Estimates Xk-1 and Pk-1
Figure 9.5: Mathematical Formulation of the Kalman Filter[31].
Uk Xk
Xk=Ad Xk+ Bd Uk
Yk=Cd Xk+ Dd Uk
Yk
+
+
U0 - Kalman Filter - Y
0
^
+ Xk
X0 +
Figure 9.6: Block Diagram of the Kalman Filter.
63
9.2.1 Simple Linear Kalman Filter
A first approach suggests using the full observable system with Y = [ax , ay , 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:
Qk = diag([.001, .001, 1, .001, .001, 10])
Rk = diag([0.09325, 0.08853, 0.245, 0.3012, 0.1961, 0.295]) (9.12)
P0 = eye(6)
The values used for Qk 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,
Xb0 = [0, 0, 0, 0, 0, 0]T . The sensors are affected by noise and bias; the matrices Qk and Rk are used to
weight the confidence in the measurements or in the model.
The performance of this filter is presented and compared with other options of filters in further sections.
This configuration of the Kalman filter 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 filters,
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 filters 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:
   
0 0 0 1 0 0
   
Ẋ = 
 0 0 X +  0
0   1 U
0  (9.13)
0 0 0 0 0 1
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 benefits the robustness of the
estimation. The covariance weights are set using the noise characteristics as:
Qk = diag([.02, .02, 2])

Rk = diag([0.09325, 0.08853, 0.245, 0.3012, 0.1961, 0.295]) (9.14)
P0 = zeros(3)
The entries of matrix Qk 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 coefficient is given to this one.
The algorithm is implemented as the previous version of the Kalman filter.
The improved Kalman filter is computationally more efficient than the first approach; on the other side,
only the estimation of the attitude is obtained, a control strategy based on this configuration of the Kalman
filter requires using the output of the gyroscopes as direct measurement of the angular velocities.
64
9.2.3 Extended Kalman Filter
The Kalman filter is a very powerful tool, which has been greatly studied and used over the past
decades. One of the successful modification of the basic Kalman filter is the Extended Kalman filter (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 Ad , Bd , Cd , Dd , the nonlinear output and dynamic functions are used to
linearize around the current state, resulting in the use of Ak and Ck , obtained at each iteration.
Time Update (Prediction phase) Measurement Update (Correctionphase)
(1) Compute the Kalman Gain

(1) Project the State ahead
Kk = P-k CkT ( Ck P-k CkT + R )-1
^- ^
X k= g( Xk-1 ,Uk , 0)
(2) Update estimate with measurement Yk
^ ^- ^
Xk= X k + Kk ( Yk - h(Xk-1 ,Uk , 0)
(2) Project the Error Covariance ahead
(3) Update the Error Covariance
P-k = Ak Pk-1AkT + Q
Pk = ( I - Kk Ck ) P-k
^
Initial Estimates Xk-1 and Pk-1
Figure 9.7: Bloc Diagram of the EKF [31].
This application of EKF is based on the same logic as the improved Kalman filter, 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 Ak = Ad and Bk = Bd . The same logic can
be applied to Dk = Dd .
However Ck will have to be defined at each step. The algorithm 9.3 is the one used.
The algorithm is simple but requires the use of trigonometric functions for each iteration. The covariance
matrices are presented by equation (9.15):
Qk = diag([.02, .02, 2])

Rk = diag(([0.09325, 0.08853, 0.245, 0.3012, 0.1961, 0.295])) (9.15)
P0 = zeros(3)
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 filter and the improved filter 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.
Yk = [ax , ay , g x , g y , g z , N y ]
uk = gB
defining the state trim condition for iteration k X0k = [0, 0, ψbk−1 ]T
defining the output trim condition for iteration k Yk0 = [0, 0, 000, −sin(ψk−1 )]T
Time Update:
X− 0 0
k = Xk + Ad (Xk−1 − Xk ) + Bd uk
b
Pk− = Ad Pk−1 ATd + Qk
Linearization:
 
0 −g0 0
 
g0 0 0 
 
0 0 0
 

Ck =  
0 0 0
 

 
0 0 0 
 
−
0 0 − cos(Xk (3))
Measurement Update:
Kk = Pk− CTk (Ck Pk− CTk + Rk )− 1
Xk = X− − 0 0
k + Kk (Yk − (Ck (Xk − Xk ) + Dd uk ) − Yk )
Pk = (I(3) − Kk Ck )Pk−
Attitude Estimation via Kalman F ilter

20
10
φ(deg)
-10
-20
0 10 20 30 40 50
20
10
θ(deg)
-10
-20
0 10 20 30 40 50
80
60 Ideal sensor
Linear Filter
ψ(deg)
40 Improved Filter
20 Extended Filter
0
0 10 20 30 40 50
time(s)
Figure 9.8: Attitude Estimation with Kalman Filters.
66
The linear filter has the advantage of estimating all necessary states whereas the improved filter has no
processing on the angle rates. On the other side, the improved filter is computationally more efficient and
presents a better estimate; the mean squared error, the variance and the mean error are smaller using the
improved filter. It is interesting to notice that although part of the model was abandoned, the filter 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 filter
although the mean error and the mean squared error are smaller. It suggests that the EKF filters out slightly
less noise but reaches a better stationary estimation. A trade off seems to exist between filtering the bias
and filtering the noise.
Table 9.2: Comparison of Kalman Filters
Estimated Angles
Estimator φ θ ψ
2 2 2
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 filter even more the noise, an EKF with bias estimation is implemented. In this
strategy, a greater importance is given to the noise filtering 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 = [φ, θ, ψ, bg1 , bg2 , bg3 ] and U = [P, Q, R]T :
  
0 0 0 −1 0 0 1 0 0
   
0 0 0
 0 −1 0

0
 1 0
0 0 0 0 0 −1 0 0 1
   
Ẋ =  X +  U (9.16)
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
The improvement obtained with the bias estimation is not significant. With the considered values for
the bias affecting the gyroscopes, this new strategy presents no significant improvements when compared
to the standard EKF.
From the different variations of the Kalman filter, one can conclude that the standard EKF is the best
estimator. It is computationally more efficient than the complementary filter 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 finder is used as a direct estimate. However that is not sufficient, the
LQR controller needs also an estimation of the vertical velocity W . Different options can be used. A first
hypothesis is to derive the range finder 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 filter will not be implemented to define with precision
the height. The implemented approach consists in considering that a high pass filter works as a derivation.
In fact the high pass filter basically filters 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 filter is used to estimate the vertical veloc-
ity and the range finder is used to estimate directly the height. It would however be possible to use a
complementary filter or Kalman filter could fuse the accelerometer measurement and the range finder.
9.4 Conclusion about the Estimation
In this section different complementary filters and Kalman filters have been presented. To chose the
estimator, the computational requirements and the behavior of the estimator were taken into account.
Because the complementary filter 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 filter over the complementary filter is the ability to weight independently the different output
variables and the state variables, while the complementary filter mostly sets the cut-off frequency and
correcting coefficient.
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,
first 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 flight 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:
   
0 0 0 0 0 0 0 0 −0.0307 −0.0310 −0.0421 −0.0307
   
 0 0 0 0 0 0 0 0 
 

 0 1.0851 0 −1.0743 

   
 0 0 0 0 0 0 0 0 
 
 1.0742
 0 −1.4713 0 

 0 0 0 0 0 0 0 0   0.0483 −0.0488 0.0661 −0.0483 
   
Ẋ = 

X + 
  U (10.1)
 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
The weighting matrices of the 8 states controller are obtained applying the Bryson’s rule, followed by a
fine tunning as for the 12 states controller. They are written as:
Rlqr = diag([0.01, 0.01, 0.01, 0.01])

(10.2)
Qlqr = diag([10, 100, 100, 50, 10, 150, 150, 60])
To test the controller, first a reference is set to Xref = [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 first test gives a first idea about the stability
of the controller and generates data to be compared with the discrete controller. The second test confirms
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
first 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 figure corresponds to 4 simulations
each setting a different state with its initial condition at the time.
4
[U, V, W ]T
U
2 V
W
0
-2
0 10 20 30 40 50
1
[P, Q, R]T
P
Q
0 R
-1
0 10 20 30 40 50
10
[X, Y, −Z]T
X
Y
0 -Z
-10
0 10 20 30 40 50
0.5
[φ, θ, ψ]T
φ
θ
0
ψ
-0.5
0 10 20 30 40 50
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 first test is presented in Figure 10.3 removing the quantizer first 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 significant 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
20
φ(deg)
Response
Reference
0
-20
0 1 2 3 4 5
θ(deg)
20
Response
10 Reference
-10
0 1 2 3 4 5
20
ψ(deg)
Response
10 Reference
-10
0 1 2 3 4 5
2
Response
-Z(m)
Reference
1.5
1
0 1 2 3 4 5
Figure 10.2: Continuous Closed Loop Response to Initial Conditions.
Stabilization Via LQR without Quantizer

[X, Y, −Z]T
1 X
Y
0 -Z
-1
0 5 10 15 20 25
0.1
[φ, θ, ψ]T
φ
θ
0
ψ
-0.1
0 5 10 15 20 25
Stabilization Via LQR without any Nonlinearities
[X, Y, −Z]T
1 X
Y
0 -Z
-1
0 5 10 15 20 25
0.1
φψ
[φ, θ, ψ]T
θ
0
ψ
-0.1
0 5 10 15 20 25
time(s)
Figure 10.3: Closed Loop Response.
71
Moreover, the 12 states controller from Chapter 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 M4 and reducing M2 , 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 justifies the remote control
as an interesting stabilization tool.
From Figure 10.2, it is concluded that, with settling times (within 2% of the final 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 defined 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 Ad and Bd 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: Xref = [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 justified 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 φ θ ψ
2 2 2
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
[U, V, W ]T
ang vel(rad/s) velocity(m/s)

4
U
2 V
W
0
-2
0 10 20 30 40 50
[P, Q, R]T
1
P
Q
0 R
-1
0 10 20 30 40 50
[X, Y, −Z]T
10
position(m)
X
Y
0 -Z
-10
0 10 20 30 40 50
[φ, θ, ψ]T
attitude(deg)
0.5
φ
θ
0
ψ
-0.5
0 10 20 30 40 50
time(s)
Figure 10.4: Stabilization via Discrete LQR.
15
Response
10 Reference
φ(deg)
0
0 2 4 6 8 10
20
Response
10 Reference
θ(deg)
-10
0 2 4 6 8 10
20
Response
10 Reference
ψ(deg)
-10
0 2 4 6 8 10
2
Response
1.5
−Z(m)
Reference
0.5
0 2 4 6 8 10
time(s)
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, confirming that
the controller stabilizes the system.
Discrete Close Loop Poles

1
0.8
0.6
imaginary axis 0.4
0.2
-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
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 filter. Applying a high pass filter, the bias can
be eliminated without augmenting the order of the system. Figure 10.7 presents the simulation, using
Xref = [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:
Qlqr = diag([60, 5, 5, 20, 100, 10, 10, 30])

(10.3)
Rlqr = diag([0.01, 0.01, 0.01, 0.01])
Qk = diag([.05, .05, .1])

(10.4)
Rk = diag([.2, .2, .05, .05, .1, .01])
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.5
W(m/s)
0
-0.5
0 10 20 30 40 50
0.5
ang vel(rad/s)
P
Q
0 R
-0.5
0 10 20 30 40 50
Z − step
1.1
−Z(m)
0.9
0 10 20 30 40 50
10
attitude(deg)
φ
0
θ
ψ
-10
0 10 20 30 40 50
time(s)
Figure 10.7: Attitude Stabilization.
values cause the controller to fight the high frequency variations and unstablizes the quadrotor. Table 10.3
presents some characteristics of the attitude error.
Table 10.3: Attitude Error
φ θ ψ
2 2 2
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 f req = 50Hz except the range finder which is set at 25Hz. The
high pass filter used to obtain the vertical velocity uses a cut-off frequency 1/0.005rad/s and the high pass
filter used for the gyroscopes uses a cut-off frequency 1/1rad/s.
This simulation suggests that the quadrotor could be stabilized for a hover flight 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 difficult to correctly measure a rising time or settling time for φ or θ. However the test confirms that
75
10
φ
0
-10
0 5 10 15 20
10
θ
0
-10
0 5 10 15 20
10
psi
-10
0 5 10 15 20
2
Response
Reference
-Z
0
0 5 10 15 20
time(s)
Figure 10.8: Attitude Stabilization with Initial Conditions.
the LQR and EKF solution for attitude and height estimation is an efficient method.
10.3 Remote Control
It has already been seen that the control of the position has a positive influence 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
1
small programs are used to communicate with the board. The main program, modified from 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 modifications 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
Quadrotor Ground Station virtual USB
connector
COM 5 Python Main COM 17 COM 7 C++ Visualization

XBee XBee Program Satellite Program
Shield Shield
wireless
Arduino
Board COM 3
Joystick
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,
confirming that the protocol is efficient.
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 difficult 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 first, 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, φ,
b 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 finally 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 φb 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 difficulty in
defining 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 difficult 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.
5
φ
θ
ψ
-5
Attitude(deg)
-10
-15
-20
10 11 12 13 14 15 16 17 18
time(s)
Figure 10.10: Implementation of the Estimator.
Table 10.4: Attitude Estimation Error with null Reference
φ θ ψ
φ std mean θ std mean ψ std mean
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 confirm
that the initialization routine, which compensates for any constant bias, is efficient.
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 filters 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 efficient 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 difficulty 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
confirms 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 identification of the sensors models
is correct.
To validate the estimation of the height, the prototype is taken to 10cm, 20cm and finally 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 finder. 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 difficulty of correctly position the prototype at the defined height. Since the range finder is intended to
be used to keep the quadrotor hovering at a defined height, any constant bias can be corrected by remote
control as already explained. As expected, because the behavior of the range finder is not linear, the
standard deviation increases with the distance to the floor, 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 finder may not be accurate and the height estimation should
include a noise filtering 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 filtering 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 floor
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, confirming 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 finder 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 flight with small
drift in position and a stabilized flight when remotely controlled, different steps were reached:
• The model of prototype was obtained defining 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 flight
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 first 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 flapping of the blades made it difficult.
• 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 confirmed 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 filters including the pas-
sive complementary filter and different Kalman filters as the linear, the improved and the extended
Kalman filters. It was concluded that the EKF was the best estimator being insensitive to the bias of
the gyroscopes contrarily to the other Kalman filters.
• 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 efficient 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 file 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 efficient 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 finder 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 flight. It is expected to be
necessary some fine tuning of the controller to achieve a hover flight.
• 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. 1345–1351. 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. 4393–4398. 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. 2247–2252. 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 filter for attitude
estimation of a fixed-wing uav. pp. 340–345. 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=http%3A%2F%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. 185–192. IEEE
2000 Position Location and Navigation Symposium.
[12] Green, J. (2008). Slip resistance: What specifiers should know. Stone Source.
[13] G.Wahba (1966). A least squares estimate of satellite attitude. SIAM Review Vol n3, 384–386.
[14] Haessig, D. and B. Friedland (1998). Separate-bias estimation with reduced-order Kalman filters.
Volume 43, pp. 983–987. 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 filter-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. Pflimlin (2005). Complementary filter design on the special orthogonal
group so(3). pp. 1477–1484. IEEE Conference on Decision and Control.
[20] Mahony, R., T. Hamel, and J.-M. Pflimlin (2008). Nonlinear complementary filters on the special
orthogonal group. Volume 53 n-5, pp. 1203–1218. 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.
1623–1629. IEEE International Conference on Robotics and Automation (ICRA).
[23] Mian, A. A. and W. Daobo (2008). Nonlinear flight control strategy for an underactuated quadrotor
aerial robot. pp. 938–942. 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. 859–876.
[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. 1–6. 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 flight. MAV 07.
[29] R.Yadlin. Attitude determination and bias estimation using Kalman filtering. 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 filter. 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 Técnico 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 specifications 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 fiberglass 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 M1 and M2 have FlightTech
10amp, motor M3 has a Maxpower 10amp and motor M4 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- (b) FlightTech Speed Con-

Outrunner 2824-34. 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 specific 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 define 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 compass.

R
Figure A.6: Sensors.
• Since the motors are DC motors the battery must fit 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 modifications, justified in Chapter 4, correspond to the installation of the IMU board, the
range finder 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 finder is a Sharp GP2d12. The justified improvements of the processing board
allows the allocation of the extra-sensor used to define the height of the quadrotor during hovering flight. 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 define the 1024 intervals that the Arduino is capable of reading.
The outputs of the IMU, corresponding to the accelerometer’s and gyroscope’s 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
finder is connected to the 5V supply tension and to pin 8 of the Arduino board.
The compass, the range finder 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 it’s 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
2 2 2 23 3 3 33 4 44 4 45 5
2 4 6 80 2 4 68 0 24 6 80 2
3 2 1 0 9 8 7 6 5 4 32 1 0 45678901
1111 11111122
GND
AREF
PWM
SDA
TX3
SCL
RX3
RX2
RX1
TX2
TX1
TX
RX
COMMUNICATION HMC6352
3 33 3 34 4 4 4 4 55
1 35 7 91 3 5 7 9 13
N SharpGp2d12
sparkfun.com
DIGITAL
GND
GND
3.3V
GND
Gyro A
Vcc
SCL
Vcc
SDA
V0
ISCP
x
y
z
y
ANALOG IN
RESET
POWER 01
1121
3141
5
01234567 891 GND
3V3
5V GND vin
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
PWM
motor1
5V +
PWM
GND
_
Signal
PWM
motor2
5V +
2 2 2 23 3 3 33 4 44 4 45 5
2 4 6 80 2 4 68 0 24 6 80 2
3 2 1 0 9 8 7 6 5 4 32 1 0 45678901
1111 11111122 PWM
GND
AREF
PWM GND
SDA
TX3
SCL
RX3
RX2
RX1
TX2
TX1
TX
_
RX
COMMUNICATION
Signal
3 33 3 34 4 4 4 4 55
1 35 7 91 3 5 7 9 13
12V DC Battery
DIGITAL
PWM +
ISCP
motor3
-
5V +
ANALOG IN PWM
GND
RESET
POWER 11
01 21
3141
5
01234567 891 GND
3V3
5V GND vin
_
Signal
PWM
motor4
5V +
PWM
GND
_
Signal
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 defined
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 first 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
battery’s 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 fine.
It is also convenient to make sure the screws are not missing before each flight 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 defined on the sensors do not necessarily coincide with the defined body-fixed frame. Indeed
a simple test was undertaken to evaluate the orientation of the defined 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 identified.
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 defined 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 defined 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 filter and the LQR. Since it is set to run at a frequency of
f req = 50Hz, two options were available. The first 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 filter 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 recalcula
// We consider the symetry to take care of the cos and sin of the accelerometer. Therefore the equati
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];
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_v
// 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
-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);
// 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 it’s 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 device’s 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 HMC6352’s 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 define 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 define 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, confirming that the center of mass
is located on their intersection.
M3
M2
M3 M1
M4 M2
M4
M1
(a) Center of Mass’ location relative to ux axis (b) Center of Mass’ location relative to uy axis
Figure B.1: Determination of the Center of Mass.
To define the location along the uz 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 uz axis (b) Center of Mass’ location along uz axis
Figure B.2: Determination of the Center of Mass along uz axis.
To determine the location, the picture must be analyzed. It has been confirmed that the center of
mass is located along the uz 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 ux uz or its
uy uz plane. As a consequence, the lines are not too biased if taken along that plane. The vertical is
defined 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 defining uz axis must be found since the center of mass will be defined
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 defining the width of one arm (exactly
1cm) is copied several times to join the first 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 define 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

Estimation and Control of A Quadrotor Attitude - Lisboa PDF

Încărcat de

Informații document

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

Estimation and Control of A Quadrotor Attitude - Lisboa PDF

Încărcat de

Drepturi de autor:

Formate disponibile

Bernardo Sousa Machado Henriques

Dissertação para obtenção do Grau de Mestre em

Keywords: Quadrotor, VTOL, Extended Kalman filter, Complementary filter.

Palavras chave: Quadrirotor, VTOL, Filtro de Kalman extendido, Filtros complementares.

List of Tables xvii

I Modeling the Dynamics of a Quadrotor 5

6 State Space Model 37

II Flight Control of a Quadrotor 47

A Practical Usage of the Prototype 87

B Determination of the Center of Mass 103

1.1 Development of the Quadrotor Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2.1 Inertial and Body-Fixed Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1 Dynamics and Kinematics Block Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4.1 Frame of the Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

5.1 Configuration of the Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

7.1 Simulink Implementation of the System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

8.1 Reference Set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

10.1 Stabilization via Continuous LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

A.1 Frame of the Prototype . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87

B.1 Determination of the Center of Mass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103

4.1 Gyroscopes Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5.1 Dead Zone of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

6.1 Dead Zone of the Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

8.1 Closed Loop Poles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

9.1 Comparison of Complementary Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

10.1 Stabilization Closed Loop Poles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

(UN , UE , UD ) North, East and Down axes of the NED frame

m Mass of the prototype

FB Forces applied to the quadrotor (excluding gravity); FB = [Fx , Fy , Fz ]T

Kspring Spring constant of the undercarriage model

KT Constant which relates thrust and angular velocity of the propeller

X State vector of the quadrotor

JLQR Cost function of the LQR controller

B Defined in the Body-Fixed frame (ABC frame)

1.1 Context and Motivation

Figure 1.1: Development of the Quadrotor Concept.

Figure 1.2: Further Development of the Quadrotor Concept.

Figure 1.3: Quadrotors in the Private Sector and in Universities.

1.2 Objectives and Contributions

In order to reach the objectives, different steps have to be reached:

1.3 Structure of the Dissertation

Modeling the Dynamics of a Quadrotor

2.1 Conventions and Characteristics of a Quadrotor

2.1.1 Coordinate Systems

(a) Inertial Frame (b) Body Fixed frame

Figure 2.1: Inertial and Body-Fixed Frames.

PI = [X, Y, Z]T (2.1)

Ψ = [φ, θ, ψ]T (2.3)

The angular velocity is measured in the body-fixed frame:

ΩB = [P, Q, R]T (2.4)

2.1.2 Maneuvering a Quadrotor

(a) (c) (e) (g)

(b) (d) (f) (h)

(a) Positive Roll (e) Negatice Yaw

Figure 2.2: Quadrotor Rotation System (from [3]).

2.2 Prototype Identification

Figure 2.3: Determination of the Center of Mass.

Figure 2.4: Determination of the Center of Mass along uz .

Figure 2.5: Dimensions of the Prototype.

2.3 General Assumptions

• In hover, the accelerations of the quadrotor can be neglected.

Figure 3.1: Dynamics and Kinematics Block Diagram.