This action might not be possible to undo. Are you sure you want to continue?

Welcome to Scribd! Start your free trial and access books, documents and more.Find out more

Master’s Thesis

Fredrik Bajers Vej 7

Telephone +45 96 35 86 90

http://es.aau.dk

Title:

Autonomous Control of a Miniature

Quadrotor Following Fast Trajectories

Project period:

Fall 2009 and spring 2010

Written by:

Anders Friis Sørensen

Group number:

10gr939b

Supervisors:

Asst. Prof. Morten Bisgaard, AAU

Prof. Jakob Stoustrup, AAU

Asst. Prof. Pieter Abbeel, UC Berkeley

Publications: 5

Pages: 78 (98 including appendix)

Handend in: June 3

rd

, 2010

Abstract:

The purpose of this thesis is to investigate how au-

tonomous robust ﬂight with a miniature quadrotor

can be done while minimizing the lag in time of a

fast moving reference.

A general introduction to how quadrotors work and

how they are controlled are given. A mathematical

model is derived for the X-3D quadrotor from As-

cending Technology and an Extended Kalman Fil-

ter is used with a IMU-driven model to estimate the

state doing ﬂight, handle sensor fusion and handle

failing sensors.

A LQR using various reference models and a Ro-

bust H

∞

controller are derived and tested. Good

results was obtained with the LQR, but the H

∞

con-

troller was not able to stabilize the quadrotor in real

ﬂight, only in simulation. It is assumed to be caused

by the assumption made that the on-board controller

is fast enough to ignore any dynamics caused by the

controller, motor controller and aerodynamics.

The derived estimator in combination with the

LQR, using feed forward of the reference velocity,

was able to follow a fast trajectory with a minimum

of lag in time.

Preface

This thesis is made as the ﬁnal step towards my Master’s degree. It contains much of the knowledge I have

gained in the past two years. Much of my work with quadrotors and rotating frames has initiated in my previous

project concerning Autonomous Landing on a Moving Platform. I would therefore like to give a special thank

to my previous group consisting of Johannes Friis, Ebbe Nielsen, Jesper Bønding, Anders Jochumsen and

Rasmus Foldager for their contributions.

My supervisor Asst. Prof. Morten Bisgaard has been a great support through my entire Master’s degree and for

that I am very thankful. I would also like to thank my supervisor Prof. Jakob Stoustrup for his inspiration and

help with understanding the more advanced control theory. With his help I was allowed to spend one semester at

the department of Electrical Engineering and Computer Science at U.C. Berkeley. There, with the help of Asst.

Prof. Pieter Abbeel, I gained a more general perspective on control theory and in the ﬁeld of Reinforcement

Learning.

Finally I want to thank Jan Stumpf and Ascending Technology for their help with making the hardware work

as intended.

Throughout the thesis the Harvard method has been used to indicate citations. Citations are denoted in square

brackets containing the surname of the prime author and the year of publication. All mathematical expressions

are described the ﬁrst time they are used, but a complete list can be found in the nomenclature list in the back of

the thesis. All vectors and matrices is denoted with bold font to indicate that they contain multiple dimensions.

Anders Friis Sørensen

Contents

1 Introductory 2

1.1 Focus of the thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 The X-3D Quadrotor Test Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

I Building a Model 6

2 Modelling: An Introduction 9

2.1 Structure of the Modelling Chapters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.2 Rotating and Fixed Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.3 Orientation and Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Modelling of a Quadrotor 15

3.1 How do a Quadrotor Fly? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.2 Model Structure and Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.3 The On-board Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.4 Forces and Accelerations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.5 Model Summery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4 The Linear Quadrotor Model 24

4.1 state-space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5 Veriﬁcation of the Quadrotor Model 27

5.1 Time Derivative of the Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5.2 Time Derivative of the Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

6 Sensor Model 31

6.1 Position and Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

6.2 Angular Velocity and Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

II State Estimation 34

7 State Estimation and Sensor Fusion 37

7.1 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

7.2 The IMU-driven Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

7.3 Fault Handling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

7.4 Analysis of Estimator Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

III Control Algorithms 48

8 Controlling a Quadrotor 51

9 Linear Quadratic Control 53

9.1 The General LQR Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

9.2 Following a Fast Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

10 Robust H

∞

Control 58

10.1 The Standard Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

10.2 Matrix Inequalities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

10.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

11 Evaluation of Controllers 65

11.1 Hover using Various Vicon Sampling Frequencies . . . . . . . . . . . . . . . . . . . . . . . . 65

11.2 Step Response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

11.3 Following a Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

11.4 Hovering in Low Height . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

11.5 Overall Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

IV Epilogue 71

12 Conclusion 75

12.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

Bibliography 78

V Appendices 79

A The X-3D Interface 83

A.1 Classes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

A.2 Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

A.3 Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

B Motion Tracking Lab (MTLab) 86

B.1 Simulink Block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

C Measurements Journals 88

C.1 Polynomial Relation of u

trust

and the force F

lift

. . . . . . . . . . . . . . . . . . . . . . . . 88

C.2 Determination of Induced Inﬂow Constant . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

C.3 Model Veriﬁcation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92

D CD Contents 95

Chapter 1

Introductory

Autonomous controlled aerial vehicles are not a new invention. They were ﬁrst introduced doing World War I

(1917) [Valavanis, 2007, p. 3] as simple and unreliable systems. Throughout the history the Unmanned Aerial

Vehicle (UAV) has been an invisible player in many wars, usually used for gathering information, and in a few

cases actively in combat. One of the more recent platforms to appear is the quadrotor. The basic idea of the

quadrotor has existed since 1907 [Bouabdallah, 2007], but not until recently been ﬂown autonomously. Au-

tonomous ﬂight of the quadrotor has previously been investigated through projects on various universities such

as MIT [MIT, 2010], Standford/Berkeley [STARMAC, 2010] and others. Especially more advanced manoeu-

vres such as obstacle avoidance [STARMAC, 2010] and Simultaneous Localization and Mapping have been

investigated using the quadrotor platform. One reason why the quadrotor is good for these applications is that

the small size and simple structure makes it easy to obtain very stable ﬂight.

Also outside the academic world is the interest for the quadrotor platform growing. One of the ﬁrst commer-

cializations of the quadrotor is the Silverlit X-UFO (shown on Figure 1.1), that was sold as a toy from Germany.

Figure 1.1: The Silverlit X-UFO

A few German students visiting MIT used this frame to create a more advanced control board using fast Piezo

gyros and Piezo Accelerometers. The students continued this development and in 2003 they started their own

company called Ascending Technologies (AscTec). AscTec has since been developing quadrotor frames along

with other companies such as Draganﬂyer and AR Drone.

Introductory 3

Recently also the industry has gained an interest in the quadrotor frame, especially when it is combined with

autonomous control. Like any other remote controlled ﬂying vehicle a quadrotor is not easy to ﬂy. It takes

much time to learn how to ﬂy in conﬁned spaces and not every place is accessible by manual ﬂight, not even

with an expert pilot.

Within the past year the technology has allowed microcomputers to become fast enough and sensors small

enough that a full autonomous controlled quadrotor can be made to ﬁt in a lunch box and obtain more than

half an hours of ﬂight time. New companies such as Sky-Watch (DK) and Microkopter (DE) is aiming directly

towards the industrial market. Currently the main target is companies who have the need for inspections in

places that is not easily accessed. This could be in areas where it is expensive or dangerous to send up people.

Quadrotors also serves as an easy access to live overhead footage or high quality still pictures.

Control of a quadrotor is typically done using either simple hand-tuned feedback compensators or a more

advanced model based feedback compensators. Throughout this thesis advantages for both types of control

will be discussed.

1.1 Focus of the thesis

The main focus of this thesis can be described as the solution to the following question:

Can robust ﬂight with a miniature quadrotor be done in such a way that a fast moving reference can be followed

with minimum lag in time.

This question give rise to several challenges. Robust ﬂight is deﬁned such that the quadrotor is to remain

stabilized even when exposed to disturbances in the form of a reference and sensor noise. Robust ﬂight, in this

thesis, also includes handling failing sensors and continuing ﬂight with the limited sensibility.

A fast moving reference is deﬁned to be a reference that is moving with a velocity, that would cause a normal

feedback controller to lag signiﬁcantly in time. The challenge with regard to the fast moving reference is to

minimize this lag without compromising the steady-state performance of a constant reference.

To obtain robust ﬂight with a minimum lag in time two types of controllers is developed (part III). A robust

H

∞

controller and a Linear Quadratic Regulator (LQR) using a reference model. To minimize the time lag

feed forward of the velocity of the reference will be used.

An Extended Kalman Filter (EKF) is developed to both minimize the sensor noise and to handle failing sensors

(part II). To minimize computation time and to suppress modelling errors the EKF will be based on measure-

ments from the Inertial Measurement Unit (IMU).

Both the controllers and the estimator is based upon knowledge of the dynamics of the system. A general under-

standing and a mathematical model is therefore being derived to improve the performance of both controllers

and estimator (part I).

1.2 The X-3D Quadrotor Test Environment

The quadrotor chosen for experiments is manufactured by the German company AscTec [Gurdan et al., 2007].

The company supplies various models with different versions of ﬁrmware. The version used in this project is

installed with the ResearchPilot ﬁrmware which enables serial communication with the quadrotor. In this way

it is possible to read out sensor measurements and send commands over a digital interface. If a pair of X-bee

4 1.2 The X-3D Quadrotor Test Environment

communication devices are added this can be done over a wireless data link.

In this setup, all sensors are connected to a control PC on which is installed Matlab/Simulink. Matlab/Simulink

is used to implement the estimation of states and calculation of the control commands for the quadrotor. An

overview of the setup is shown in Figure 1.2.

Figure 1.2: Overview of the MTLab

1.2.1 Control Commands

Through the serial communication connector on the X-3D it is possible to communicate ﬂight commands to

the quadrotor using the proper protocol [AscTec, 2009]. The ﬂight commands are given as values which are

otherwise send from the RC-transmitter for roll, pitch, yaw and thrust. The deﬁnition of roll, pitch and yaw

commands is explained in more detail in a later chapter. Each value is, according to the protocol, deﬁned as in

Table 1.1. In this setup positive roll is deﬁned as a positive rotation around the x-axis and so forth.

Command Value

roll -2048 to 2047

pitch -2048 to 2047

thrust 0 to 4096

yaw -2048 to 2047

Table 1.1: Commands send to quadrotor

The quadrotor can be controlled by the commands send, but only as long as a command is received at least once

every 100ms. If this is not the case the control is given back to RC-transmitter.

Ascending Technologies supplies support for the protocol on the quadrotor, but no public available implementa-

tion for the PC exists. In order to establish communication an interface from Matlab/Simulink to the quadrotor

has therefore been developed. This interface enables conﬁguration of which sensors to be read, and loading

them directly into Simulink. The interface also supplies an easy way to conﬁgure which channels that should

be autonomously controlled, and which should be controlled manually. This feature is very helpful when devel-

oping controllers to a single channel. Finally it also enables the possibility to send commands to the quadrotor

directly form Simulink. Additional information on the communication and the development of the interface

can be found in Appendix A.

Introductory 5

1.2.2 On-board Sensors

Multiple sensors can be read from the quadrotor both in raw and in processed values. An entire list can be found

in the manual for the ResearchPilot ﬁrmware [AscTec, 2009]. For the estimator developed in this thesis only

the raw gyro and acceleration measurements will be used. These will be measured with an update frequency of

100 Hz.

1.2.3 The Motion Tracking Lab (MTLab)

For measurements of the position and orientation the MTLab is used. The MTLab is a room at Aalborg Uni-

versity that is equipped with 7 cameras recording positions of small reﬂectors mounted on various objects. The

reﬂectors can be seen as the silver balls mounted on the X-3D quadrotor in Figure 1.3.

Figure 1.3: The X-3D quadrotor

The MTLab measures the position in meters with an accuracy better then 1 mm and the orientation, given as

either a 3-2-1 Euler rotation or a quaternion. More on the different orientations will be discussed in the follow-

ing chapter. Part of the development of both controllers and estimator have been conducted at UC Berkeley in

California. At UC Berkeley a similar system, called PhaseSpace, was used. The PhaseSpace system uses active

markers but is otherwise similar to the Vicon MX system and will not be discussed any further in this thesis.

A more in-depth description of the MTLab can be found in Appendix B.

Part I

Building a Model

Table of Contents

2 Modelling: An Introduction 9

2.1 Structure of the Modelling Chapters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.2 Rotating and Fixed Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.3 Orientation and Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Modelling of a Quadrotor 15

3.1 How do a Quadrotor Fly? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.2 Model Structure and Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.3 The On-board Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.4 Forces and Accelerations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.5 Model Summery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4 The Linear Quadrotor Model 24

4.1 state-space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5 Veriﬁcation of the Quadrotor Model 27

5.1 Time Derivative of the Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5.2 Time Derivative of the Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

6 Sensor Model 31

6.1 Position and Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

6.2 Angular Velocity and Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

8 TABLE OF CONTENTS

Chapter 2

Modelling: An Introduction

Any mathematical kind of model is simply an approximation of the real world, describing any relevant inﬂuence

of some input signal on an output signal. In this case the model will describe how the quadrotor moves in a

6 dimensional space depending on the four control commands mentioned in the previous chapter. Throughout

the chapters a number of assumptions will be made. This will be noted and discussed to the extent it is found

relevant.

2.1 Structure of the Modelling Chapters

This introduction serves to guide the reader and to give an overview of the modelling part. Before the actual

modelling of the quadrotor initiates, are some general concepts such as the coordinate systems deﬁned.

The ﬁrst modelling chapter covers the basics a quadrotor, how is it controlled and the effect of the control

commands described from a pilots perspective. Throughout the chapter various details of the X-3D quadrotor

are included, in particular the on-board PD-controller that closes the angular rate loop and attenuates some

aerodynamic effects. The chapter concludes by deﬁning a continuous-time model. Later the model is linearized

and put on state-space form.

In the chapter concerning the sensor modelling, a stochastic model is derived of the sensors available. The

sensors in focus are the accelerometers, gyroscopes and Vicon measurements. Together they form the basis for

estimating the states of the quadrotor.

2.2 Rotating and Fixed Frames

When dealing with ﬂying objects it is convenient to be able to describe vectors not only in a global frame, but

also in a rotating local body frame. Any experiments made in this thesis are done in one speciﬁc room. This

room is deﬁned to be the inertial frame from where the model will be described. The rotation of the earth is

assumed not to affect the ﬂight of the quadrotor. The earth ﬁxed inertial frame is from now referred to as the

earth frame. The rotating frame following the attitude of the quadrotor is denoted the body frame. For this

assumption to make sense the structure of the quadrotor is assumed to be rigid. That a body is rigid is deﬁned

10 2.3 Orientation and Rotations

such that the distance between any two points in the structure will always remain the same no matter how the

body is positioned or oriented.

Figure 2.1 shows the two frames. To the left the earth ﬁxed frame denoted E and to the right the body ﬁxed

frame denoted B. When a vector is seen with respect to the earth ﬁxed frame it will either be denoted with an

e

in front of the vector or nothing at all. Likewise if the vector is seen with respect to the body frame it will be

denoted with a

b

.

Figure 2.1: The earth frame and body frame

The earth ﬁxed frame is aligned with the earth with the x- and y-axis drawn perpendicular and in the horizontal

plane. The z-axis is pointing down which leaves a normal right hand coordinate system in which to describe

vectors. This axis-alignment is typically used when dealing with ﬂying objects and is denoted North East

Down (NED). In the body frame a similar right hand coordinate system is aligned with the quadrotor as shown

in Figure 2.2.

Figure 2.2: The X-3D quadrotor in the body frame

The coordinate system is aligned such that rotor 1 is pointing in the same direction as the positive x-axis. And

the center is positioned at the same level as the landing pads. When the quadrotor is located in the center of the

test area it will be positioned in position

_

0 0 0

_

. The position of the quadrotor is denoted P = [x, y, z]

.

The orientation of the quadrotor will be described as a parametrization of the 3 × 3 transformation matrix,

transforming vectors from the earth frame to the body frame. The parametrizations used in this thesis is either

a 3-2-1 Euler parametrization Θ =

_

φ θ ψ

_

**or the quaternion parametrization q =
**

_

q

0

q

1

q

2

q

3

_

.

The angular velocity of the body frame is denoted

b

ω and the translatory velocity

b

v.

2.3 Orientation and Rotations

The orientation of the quadrotor will as previously mentioned be described as a parametrization of the transfor-

mation from the earth frame to the body frame. In this thesis the quaternion parametrization is used primarily,

but also the Euler angle parametrization will be described.

Modelling: An Introduction 11

The Euler angles are widely used, since they have a very clear physical interpretation and are of minimum

dimensionality. The minimum required dimensionality for describing an orientation in 3 dimensions is 3.

However, the orientation can not be both global and non-singular with less then 4 dimensions [Bak, 2002, p.

29]. The parametrizing of the rotational matrix, using Euler angles, includes multiple trigonometric functions,

which leaves the transformation non-linear and is subject to gimbal lock.

The quaternion parametrization only involves quadratic expressions, but is still non-linear. Using the quater-

nions leaves a close to linear kinematic equations and no singularities.

In the remaining sections of this chapter the Euler angle and quaternion parametrizations will be described in

further detail.

2.3.1 Euler Angles

The Euler angle parametrization utilizes that the orientation of one cartesian coordinate system, with respect

to another, can always be described by three successive rotations. The orientation of a coordinate system can

therefore be described by the z,y,x (also called 3-2-1) right-hand rotation sequence that is required to get from

earth frame into alignment with the body frame. Other sequences can be used as well, but the 3-2-1 sequence

is commonly used when dealing with UAVs. The rotations are denoted as follows:

• Right-hand rotation about the z-axis (positive ψ)

• Right-hand rotation about the new y-axis (positive θ)

• Right-hand rotation about the new x-axis (positive φ)

Matrices describing the rotations about the three axises are deﬁned by [Bak, 2002, p. 15] as

C

x

(φ) =

_

¸

_

1 0 0

0 cos(φ) sin(φ)

0 −sin(φ) cos(φ)

_

¸

_ (2.1)

C

y

(θ) =

_

¸

_

cos(θ) 0 −sin(θ)

0 1 0

sin(θ) 0 cos(θ)

_

¸

_ (2.2)

C

z

(ψ) =

_

¸

_

cos(ψ) sin(ψ) 0

−sin(ψ) cos(ψ) 0

0 0 1

_

¸

_ (2.3)

The successive rotations and the rotational matrix from earth frame to body frame is hereby deﬁned as

b

C

e

(Θ) = C

x

(φ)C

y

(θ)C

z

(ψ) (2.4)

=

_

¸

_

cθcψ cθsψ −sθ

(−cφsψ +sφsθsψ) (cφcψ +sφsθsψ) sφcθ

(sφsψ +cφsθcψ) (−sφcψ +cφsθsψ) cφcθ

_

¸

_

where c = cos and s = sin. Because

b

C

e

is orthonormal [Bak, 2002, p. 13] the inverse transformation can be

described at the transpose of

b

C

e

.

b

C

−1

e

(Θ) =

b

C

e

(Θ) (2.5)

12 2.3 Orientation and Rotations

The rotation from body to earth frame is given by

e

C

b

(Θ) =

b

C

e

(Θ) =

_

¸

_

cθcψ (−cφsψ +sφsθsψ) (sφsψ +cφsθcψ)

cθsψ (cφcψ +sφsθsψ) (−sφcψ +cφsθsψ)

−sθ sφcθ cφcθ

_

¸

_ (2.6)

where Θis the vector of Euler angles

2.3.2 Euler Rates

The relationship between Euler angle time derivatives (

˙

Θ) and the angular velocity (

b

ω) can in a similar way

be described with a rotational matrix [Bak, 2002, p.25].

b

ω =

_

¸

_

˙

φ

0

0

_

¸

_ +C

x

(φ)

_

_

_

_

¸

_

0

˙

θ

0

_

¸

_ +C

y

(θ)

_

¸

_

0

0

˙

ψ

_

¸

_

_

_

_ (2.7)

=

_

¸

_

1 0 −sθ

0 cφ sφcθ

0 −sφ cφcθ

_

¸

_

_

¸

_

˙

φ

˙

θ

˙

ψ

_

¸

_

=

b

H

e

(Θ)

˙

Θ

Where C

x

and C

y

are presented in Equation 2.1 and 2.2.

b

H

e

is the transformation matrix from earth frame to

body frame. By inverting

b

H

e

the transformation matrix from body rates to the derivative of the Euler angles

are found.

e

H

b

(Θ) =

b

H

−1

e

(Θ) (2.8)

Where t = tan, and Θare Euler angles. It can be shown that [Stevens and Lewis, 2003, p. 28]

˙

Θ =

e

H

b

(Θ)

b

ω (2.9)

=

_

¸

_

1 tθsφ tθcφ

0 cφ −sφ

0 sφ/cθ cφ/cθ

_

¸

_

b

ω

In Equation 2.9 it is seen that when θ reaches

π

2

the matrix will be singular. Also given the trigonometric

functions this parametrization is not linear.

2.3.3 Quaternions

An alternative way of parametrizing the rotation matrix is by using quaternions. The quaternion has its basis in

the Euler axis/angle representation. The Euler axis/angle parametrization is a unit vector (e) orthogonal to the

plane of rotation and an angle (θ). The direction of the vector gives the direction of the rotation and the angle

the amplitude of the rotation.

Quaternions are similar but the rotation axis and angle have been combined to a single 4 dimensional vector of

unit length. Three of the dimensions describe the direction of the rotation and the last is used to scale the vector

Modelling: An Introduction 13

to have length 1. Equation 2.10 illustrates different representations of the quaternion.

q =

_

¸

¸

¸

_

q

0

q

1

q

2

q

3

_

¸

¸

¸

_

= q

0

+q

1

i +q

2

j +q

3

k (2.10)

The conversions between the Euler axis/angle and the quaternion might help with the understanding. This is

shown in Equation 2.11 and 2.12.

q = cos(

θ

2

) +sin(

θ

2

) · e (2.11)

e =

q

1..3

q

1..3

θ = 2 arccos(q

0

) (2.12)

This deﬁnition of the quaternions ensures a unique quaternion for every value of θ in the range of ±π [Stevens and Lewis, 2003,

p. 18].

Quaternions can be multiplied together using the quaternion product operation deﬁned as in Equation 2.13 and

2.14 [Stevens and Lewis, 2003] where ∗ denotes the quaternion multiplication.

p ∗ q Λ

p

q (2.13)

Λ

p

=

_

p

0

I −[p

1..3

×] p

1..3

−p

1..3

p

0

_

(2.14)

Quaternions are not like Euler angles caught in a gimbal lock and the parametrization of the rotational matrix

contains no trigonometric functions.

Rotations of vectors using quaternions are convenient, given the unit length. A Euclidean vector written as a

quaternion can be seen as a normal quaternion with zero scalar part as shown in Equation 2.15.

u = [0 u

1..3

]

(2.15)

A requirement for a valid transform is that the transformed vector also will have a zero scalar part. The most

commonly used transform is the one shown in Equation 2.16 [Bak, 1999, p. 31][Stevens and Lewis, 2003,

p.19].

v = q

−1

uq (2.16)

=

_

0

2q

1..3

(q

1..3

u) + (q

2

0

−q

1..3

q

1..3

)u −2q

0

(q

1..3

×u)

_

(2.17)

This transformation can be seen as the parametrization of the rotational matrix C(q).

C(q) = 2q

1..3

q

1..3

+ (q

2

0

−q

1..3

q

1..3

)I −2q

0

[q

1..3

×] (2.18)

Where [q

1..3

×] is the cross product matrix shown in Equation 2.19.

[q

1..3

×] =

_

¸

_

0 −q

3

q

2

q

3

0 −q

1

−q

2

q

1

0

_

¸

_ (2.19)

14 2.3 Orientation and Rotations

Evaluating the rotational matrix in 2.18 results in the following rotational matrix from earth frame to body

frame

b

C

e

[Stevens and Lewis, 2003, p. 31].

b

C

e

(q) =

_

¸

_

q

2

0

+q

2

1

−q

2

2

−q

2

3

2(q

1

q

2

−q

0

q

3

) 2(q

1

q

3

+q

0

q

2

)

2(q

1

q

2

+q

0

q

3

) q

2

0

−q

2

1

+q

2

2

−q

2

3

2(q

2

q

3

−q

0

q

1

)

2(q

1

q

3

−q

0

q

2

) 2(q

2

q

3

+q

0

q

1

) q

2

0

−q

2

1

−q

2

2

+q

2

3

_

¸

_ (2.20)

The reverse rotation from body frame to earth frame is given as Equation 2.21.

e

C

b

(q) =

b

C

e

(q)

=

_

¸

_

q

2

0

+q

2

1

−q

2

2

−q

2

3

2(q

1

q

2

+q

0

q

3

) 2(q

1

q

3

−q

0

q

2

)

2(q

1

q

2

−q

0

q

3

) q

2

0

−q

2

1

+q

2

2

−q

2

3

2(q

2

q

3

+q

0

q

1

)

2(q

1

q

3

+q

0

q

2

) 2(q

2

q

3

−q

0

q

1

) q

2

0

−q

2

1

−q

2

2

+q

2

3

_

¸

_ (2.21)

2.3.4 Quaternion Kinematics

Previously the conversion from an Euler axis/angle was described in Equation 2.11. This conversion will now

be the basis for a closer look at how the quaternion is changes over time. Let the quaternion q(t) describe

the orientation at the current time and let an inﬁnitesimal rotation be described as the unit vector ˆ s and the

magnitude be given by ˆ ω. Then for a small time interval ∆t the rotation can be described as in Equation 2.22

using small angle approximation.

∆q(∆t) =

_

1

1

2

ˆ ωˆ s

_

(2.22)

As previously done with Euler angle rotations the quaternions can be combined in a similar way. Therefore

q(t + ∆t) can be described as a quaternion multiplication of the two quaternions.

q(t + ∆t) = q(t) ∗ [∆q(∆t) −I

q

] (2.23)

Where I

q

= [1 0 0 0]

**is the unity quaternion.
**

The time derivative of the quaternion q(t) can be described as in Equation 2.24.

˙ q =

dq

dt

= lim

∆t→0

q(t) ∗ [∆q(∆t) −I

q

]

∆t

(2.24)

Deﬁning angular velocity as:

ω lim

∆t→0

ˆ ωˆ s

∆t

(2.25)

and substituting ∆q(∆t) with 2.22 leaves Equation 2.26 where the time indices are ignored.

˙ q =

1

2

q ∗ ω (2.26)

Using the deﬁnition of the quaternion multiplication from 2.14 Equation 2.26 can be written in matrix form.

˙ q =

1

2

_

0 −ω

ω [ω×]

_

q =

1

2

_

¸

¸

¸

_

0 −ω

1

−ω

2

−ω

3

ω

1

0 ω

3

−ω

2

ω

2

−ω

3

0 ω

1

ω

3

ω

2

−ω

1

0

_

¸

¸

¸

_

q =

1

2

Ω

ω

q (2.27)

Where Ω

ω

is the matrix describing the quaternion multiplication with the angular velocity vector.

Chapter 3

Modelling of a Quadrotor

3.1 How do a Quadrotor Fly?

A quadrotor is equipped with four motors with each a rotor attached. When the rotor spins lift is generated.

When the quadrotor is aligned with the horizontal plane and the sum of the lift generated (F

lift

) is equal to the

gravitational force the quadrotor is hovering. If F

lift

is increased the quadrotor will start to climb, if F

lift

is

decreased the quadrotor will start to decent.

Movement in the horizontal plane is done by tilting the quadrotor in the desired direction. When the quadrotor

is tilted the direction of F

lift

is no longer aligned with the earth frame z-axis but can be dissolved in a z-

axis component and a horizontal component as seen in Figure 3.1. The horizontal component result in an

acceleration in the horizontal plane.

Figure 3.1: F

lift

dissolved in a z-axis component and a horizontal component

The rotors spin in opposite direction in pairs as illustrated in Figure 3.2. This is to prevent the quadrotor from

spinning around the z-axis as an effect of the counter torque generated by the spinning rotors. When the rotors

spin in opposite direction the counter torque is equalized when all rotors rotates with equal speed. It should be

noted that, in the following ﬁgures, the arrows indicate the direction of the rotation and the size is proportional

to the speed of the rotor.

16 3.1 How do a Quadrotor Fly?

1

2

3

4

Figure 3.2: Quadrotor in hover

Tilting of the quadrotor is done by altering the speed of the rotors. In the following are the effect of the inputs

roll pitch and yaw deﬁned.

Roll is deﬁned as a rotation around the x-axis in the body frame. This is done by increasing the speed of one

of the rotors placed along the y-axis and decreasing the speed of the opposite, while maintaining speed on the

remaining rotors. Figure 3.3 illustrates the direction of positive roll, which means that rotor 4 increases speed

while rotor 2 decreases speed. Note that the white mark indicates the front of the quadrotor.

1

2

3

4

Figure 3.3: Quadrotor performing positive roll

Pitch is deﬁned as the rotation around the y-axis in the body frame. The rotation is done by increasing

the speed on one of the rotors placed along the x-axis and decreasing the thrust on the opposite rotor, while

maintaining speed on the remaining rotors. The positive pitch is illustrated in Figure 3.4, where rotor 1 increases

speed while rotor 3 decreases speed.

Modelling of a Quadrotor 17

1

2

3

4

Figure 3.4: Quadrotor performing positive pitch

Yaw is deﬁned as the rotation around the z-axis in the body frame. Yaw is done by altering speed of the rotors

along the x-axis compared to the speed on the rotors along the y-axis. The resulting counter torque generated

by the rotors will no longer be zero and will result in a rotation around the z-axis. If the rotors on the y-axis are

rotating faster than the rotors on the x-axis the body will rotate clockwise (positive yaw) as illustrated in Figure

3.5.

1

2

3

4

Figure 3.5: Diagram of a quadrotor performing a positive yaw motion

3.2 Model Structure and Assumptions

The structure of the model for the quadrotor that will be derived is illustrated in Figure 3.6. The purpose of

this illustration is to give an overview of how the model is structured and how it is derived. The input vector is

deﬁned as u =

_

u

φ

u

θ

u

thrust

u

ψ

_

containing commands for roll, pitch, thrust and yaw. The input serves

as references to the on-board heading hold controller holding the desired angular velocity in the body frame

(

b

ω). The force generated by the rotors (F

lift

) is derived using the knowledge about the on-board controller

and imperial experiments. Then an expression of the acceleration in body frame (

b

˙ v) will be derived from the

force generated by the lift. The model concludes by including the ﬁnal integrations and rotations into the earth

ﬁxed frame deriving the time derivative of the orientation ( ˙ q) and position (

˙

P) in the earth frame. The model

will then be linearized and converted to state-space form.

18 3.3 The On-board Controllers

Figure 3.6: Illustration of the model structure for the X-3D quadrotor

In the derivation a number of assumptions are made to simplify the model. These assumptions are either based

on prior models derived for quadrotors [Bouabdallah, 2007][Friis et al., 2009] or by empirical observations.

• The quadrotor is assumed to be a rigid body. With the exeption of the roters, that both rotates and are

bendable, this is close to be true. The effect of the bending roters is assumed to very small due to the

on-board controller.

• The gyro effect due to the angular momentum of the rotors [Bouabdallah, 2007] is neglected due to the

on-board controller will compensate for this.

• The on-board angular rate controller and the motor controllers are considered fast enough to neglect both

the dynamics of the motors and the dynamic in changing the angular rate. This is further discussed and

supported in Section 3.3.

• Due to the small size of the quadrotor and the controlled environment in which it is ﬂying, it is assumed

that external effects like wind or turbulence do not affect the quadrotor.

3.3 The On-board Controllers

The purpose of this section is to determine the relation from the input given to the quadrotor (u

φ

, u

θ

, u

thrust

and u

ψ

) to angular rate

b

ω and the lift force F

lift

of the quadrotor. Both factors are depending on the on-

board controllers. The knowledge of the on-board software of the X-3D quadrotor is gathered from a research

paper about the development of the X-3D quadrotor [Gurdan et al., 2007], the manual [AscTec, 2009] and from

personal contact with the developers.

Both the angular velocity controllers and the thrust controller are located on the sensor board that handles all

sensor data, ﬁltering, and control. The board is equipped with a 60 MHz ARM7 microprocessor and runs the

control loop with an update frequency of 1 kHz.

3.3.1 The Angular Rate Controllers

The rate controllers are structured as three independent PD-controllers. One for each axis. In each controller

a value K

stick

is used as a proportional gain that controls how aggressive the quadrotor reacts on the input.

Both K

stick

and the controller values (K

p

and K

d

) can be conﬁgured when setting up the X-3D quadrotor. The

block diagram describing the structure of the controller can be seen in Figure 3.7

From observations it is concluded that the doing normal ﬂight (input bandwidth ∼ 0.1 Hz) the quadrotor had

no problem following the input references. To verify this the bandwidth was estimated by measuring the rise

time of the maximum roll input step. The step response can be seen in Figure 3.8.

Modelling of a Quadrotor 19

Figure 3.7: Block diagram of the PD-controller structure

8.5 8.55 8.6 8.65 8.7 8.75 8.8 8.85 8.9

−0.5

0

0.5

1

1.5

2

2.5

3

t

r

Time [s]

b

ω

1

u

φ

⋅ K

φ

b

ω

1

Figure 3.8: Step response of the maximum roll input

The input was chosen to ﬁnd the worst case bandwidth. It is estimated that with the maximum input, the

dynamics of the motor controllers, motors and rotors will have the maximum dampening. On Figure 3.8 the

input step is not a step over 1 sample, but more made over several samples. This is cased by the step being made

manually doing manual ﬂight. The step corresponds to an angular velocity of 2.25

rad

s

(130

o

s

) and causes the

quadrotor to quickly move towards the wall. This can therefore only be done in manually ﬂight where a pilot

is ready to recover the quadrotor. Typically a natural system like this can be estimated with a second order

behaviour. The bandwidth of a second order system can according to [Franklin et al., 2006] be approximated

as in Equation 3.1.

f

bw

≈

1.8

t

r

· 2π

≈

0.29

t

r

(3.1)

Given Equation 3.1 the bandwidth can be estimated.

f

bw

≈

0.29

0.14

≈ 2.07 [Hz] (3.2)

The rate controller is considered fast enough for the dynamics of the controller to be neglected in this quadrotor

model given that the model is to be used for normal ﬂight. The steady state angular rate can according to the

manual [AscTec, 2009] be described as in Equations 3.3, 3.4 and 3.5.

b

ω

φ

=

−K

stickφ

2048

·

2π

360

· u

φ

= K

φ

u

φ

(3.3)

b

ω

θ

=

−K

stickθ

2048

·

2π

360

· u

θ

= K

θ

u

θ

(3.4)

b

ω

ψ

=

−K

stickψ

2048

·

2π

360

· u

ψ

= K

ψ

u

ψ

(3.5)

20 3.3 The On-board Controllers

However the values was found not to be very accurate and new values of K

φ

, K

θ

and K

ψ

was determined using

the system identiﬁcation toolbox in Matlab with a zero order transfer function model with a time delay.

Figure 3.9 shows how the angular velocity around the x-axis follows u · K

φ

. Part of the delay is assumed to

be the communication delay in the series consisting of the transmitter, quadrotor and the Vicon system. The

short areas where the values keep constant is due to data corruption in the interface. This is further discussed

in Appendix A.

10 11 12 13 14 15 16 17 18 19 20

−3

−2

−1

0

1

2

3

Time [s]

b

ω

1

b

ω

u ⋅ K

φ

Figure 3.9: Demonstration of how the angular velocity follows the input

3.3.2 Thrust Force Relation

The input determining thrust is proportional to the average speed of the rotors (in perfect hover) when no other

input is given. The steady-state relation between the average speed in RPM (Ω

rotor

) and the thrust input is

according to the X-3D developers given as Equation 3.6.

Ω

rotor

u

thrust

= 1.953 (3.6)

This however will only be an average, since the quadrotor changes the orientation by in pairs changing the

speed of the rotors from the average.

The lift generated by each rotor could be estimated by an aerodynamic model of the rotor, but instead it is chosen

to estimate the entire relation from thrust input the lift force. The total force F

lift

is estimated empirically from

the thrust input by measuring how the lift force change while stepping through the input. The experiment is

described in more detail in Appendix C.1.

The estimated relation is expressed as the 3rd order polynomial in Equation 3.7 and shown in Figure 3.10

together with the measurements. This however can only be assumed to be correct when a full battery is used.

b

F

lift

=

_

¸

_

0

0

2.073 · 10

−10

· u

3

thrust

−1.143 · 10

−6

· u

2

thrust

−1.054 · 10

−3

· u

thrust

−0.903

_

¸

_ (3.7)

Modelling of a Quadrotor 21

0 1000 2000 3000 4000

−12

−10

−8

−6

−4

−2

0

u

thrust

[ ]

F

l

i

f

t

[

N

]

Measurement

Polynomial estimation

Figure 3.10: Relation between input u

thrust

and F

lift

3.4 Forces and Accelerations

The main force apart from F

lift

affecting the quadrotor is the gravitational force. The gravitational force is

always pointed down in the earth ﬁxed frame and can be described as in Equation 3.8.

e

F

g

= m·

_

¸

_

0

0

g

_

¸

_ (3.8)

Where g is the gravitational acceleration deﬁned to be 9.82

m

s

2

in Denmark.

Doing normal ﬂight it has been observed that the quadrotor do not rise with a constant acceleration, when the

thrust is larger then the gravitational force. This is due to a phenomenon known as induced inﬂow. Induced

inﬂow is an effect affecting the force generated by a rotating rotor when it starts to move up through the air.

When the rotor starts to climb, the airﬂow through the rotor becomes higher and a smaller force is generated.

With a smaller force the vertical speed of the rotor becomes less and the force is once again increased. This

effect induces an equilibrium depending on the vertical velocity. Figure 3.11 illustrates this principal.

Figure 3.11: Illustration of the phenomenon known as induced inﬂow

The phenomenon causes the quadrotor to ﬁnd an equilibrium approximately with a constant velocity. The

phenomenon of induced inﬂow is well researched and good approximations have been made. In this model

22 3.5 Model Summery

however it is found sufﬁcient to use a constant feedback from the vertical velocity to estimate the force gener-

ated by the induced inﬂow. The constant I

i

is found by imperial tests in the MTLab and found to approximately

I

i

= −0.423. Further information of the experiment can be found in Appendix C.2.

b

F

i

= I

i

b

v

3

(3.9)

The total force acting upon the quadrotor, using this model, can be described as in Equation 3.10.

b

F =

b

F

lift

+

e

F

g

+

b

F

i

(3.10)

The rotational matrix

b

C

e

can be used to describe the gravitational force in the body frame. The rotational

matrix was previously derived in Section 2.3. Equation 3.11 describes the summed forces.

b

F =

b

F

lift

+m

b

C

e

_

¸

_

0

0

g

_

¸

_ +I

i

_

¸

_

0

0

v

3

_

¸

_ (3.11)

The translatory acceleration of the body frame with respect to the body frame (

b

˙ v) is now derived.

e

v de-

scribes the translatory velocity of the body frame relative to the earth frame. Newtons second law states that

acceleration is force divided by mass [Newton, 1833, p. 15].

˙ v =

1

m

F (3.12)

The velocity

e

v is described relative to the earth frame in Equation 3.13 and the derivative in Equation 3.14.

e

v =

e

C

b

b

v (3.13)

e

˙ v =

e

C

b

b

˙ v +

e

˙

C

b

b

v (3.14)

The derivative of the direct cosine matrix can be expressed as the cross product seen in Equation 3.15

[Hughes, 1986, p.23].

e

˙

C

b

b

v =

e

C

b

(

b

ω ×

b

v) (3.15)

Equation 3.12 and Equation 3.15 is inserted into Equation 3.14 and the expression is transformed to body frame

and simpliﬁed.

1

m

e

F =

e

C

b

b

˙ v +

e

C

b

b

ω ×

b

v (3.16)

1

m

b

C

e

e

F =

b

˙ v +

b

ω ×

b

v (3.17)

b

˙ v =

1

m

b

F −

b

ω ×

b

v (3.18)

3.5 Model Summery

The previously described rotational matrices and kinematics can be used to describe the entire relations between

the input commands

_

u

φ

u

θ

u

ψ

u

thrust

_

**and the change in position P, orientation q and velocity
**

b

v. In

Modelling of a Quadrotor 23

Equations 3.19 to 3.22 this relation is summarized.

˙

P =

e

C

b

b

v (3.19)

˙ q =

1

2

Ω

ω

q (3.20)

b

˙ v =

1

m

b

F

lift

+

b

C

e

_

¸

_

0

0

g

_

¸

_ +

I

i

m

_

¸

_

0

0

v

3

_

¸

_ −

b

ω ×

b

v (3.21)

b

ω =

_

¸

_

K

φ

· u

φ

K

θ

· u

θ

K

ψ

· u

ψ

_

¸

_ (3.22)

F

lift

is an expression for the 3rd order polynomial that maps the thrust input to the force generated by the

rotors. Because the polynomial is highly nonlinear the thrust force will be used as input to the system, such

that the polynomial will be kept out of the model and the controller.

Chapter 4

The Linear Quadrotor Model

To use the model to tune a linear controller requires a strictly linear model. Therefore a general linearization is

done here with the purpose of describing the model as a linear state-space model. First step will be identifying

the non-linear parts and ﬁnd appropriate linearization methods and operating points.

Since a quadrotor is usually required to be stabilized around hover this is chosen as the operating point. Equa-

tions 4.1 to 4.3 shows the values of states in the operating point.

¯ q =

_

1 0 0 0

_

(4.1)

b

¯ v =

_

0 0 0

_

(4.2)

b

¯ ω =

_

0 0 0

_

(4.3)

Most of the non-linearities of the model previously described is due to states multiplied together. This is

linearized using a 1st order Taylor approximation as illustrated in Equation 4.4.

a · b ≈ (¯ a + ˜ a)(

¯

b +

˜

b) = ¯ a

¯

b + ¯ a

˜

b + ˜ a

¯

b + ˜ a

˜

b (4.4)

≈ ¯ a

¯

b + ¯ a

˜

b + ˜ a

¯

b

Before the actual linearization the matrices used is calculated when the operating point is inserted.

e

C

b

(q)

¸

¸

¸

q=¯ q

=

b

C

e

(q)

¸

¸

¸

q=¯ q

= I (4.5)

b

Ω

ω

¸

¸

¸

ω=¯ ω

= 0 (4.6)

[

b

ω×]

¸

¸

¸

ω=¯ ω

= 0 (4.7)

Linearization of the position and orientation can be done very simple given the chosen method as shown in

Equations 4.8 and 4.9.

˙

P ≈

e

C

b

(q)

¸

¸

¸

q=¯ q

b

v +

e

C

b

(q)

b

¯ v =

b

v (4.8)

˙ q ≈

1

2

_

b

Ω

ω

¸

¸

¸

ω=¯ ω

q +Ω

ω

¯ q

_

=

1

2

_

0 ω

1

ω

2

ω

3

_

(4.9)

The Linear Quadrotor Model 25

The acceleration is divided into multiple steps treating each segment by it self. Firstly is the values of the

rotational matrix is inserted from Equation 2.20 to linearize the gravitational force.

b

C

e

e

F

g

=

b

C

e

_

¸

_

0

0

g

_

¸

_m =

_

¸

_

2(q

1

q

3

−q

0

q

2

)

2(q

2

q

3

+q

0

q

1

)

q

2

0

+q

2

1

+q

2

2

−q

2

3

_

¸

_g · m (4.10)

The 1st order Taylor approximation is again used when two different states are multiplied. For the last expres-

sion it is assumed that q

2

0

can be estimated to be 1 to avoid an afﬁne expression with a bias.

b

C

e

e

F

g

≈

_

¸

_

−2 · q

2

2 · q

1

1

_

¸

_g · m (4.11)

The cross product can be approximated like the position and orientation using the 1st order Taylor.

b

ω ×

b

v = [

b

ω×]

b

v ≈ [

b

ω×]

¸

¸

¸

ω=¯ ω

b

v +

b

ω

b

¯ v = 0 (4.12)

When the segments are put together the resulting expression is not linear but afﬁne with the bias of the gravita-

tional force. But by redeﬁning the input force as F

z

= (F

lift

+g · m) this afﬁne relation becomes linear.

˙ v ≈

1

m

_

_

_

b

F

lift

+

_

¸

_

−2 · q

2

2 · q

1

1

_

¸

_g · m+I

i

_

¸

_

0

0

v

3

_

¸

_

_

_

_ =

_

¸

_

−2g · q

2

2g · q

1

1

m

(F

lift

+g · m+I

i

v

3

)

_

¸

_ (4.13)

=

_

¸

_

−2g · q

2

2g · q

1

1

m

(F

z

+I

i

v

3

)

_

¸

_ (4.14)

For an overview the equations 4.8, 4.9 and 4.14 are shown together in the following equations.

˙ x =

b

v

1

(4.15)

˙ y =

b

v

2

(4.16)

˙ z =

b

v

3

(4.17)

˙ q

0

= 0 (4.18)

˙ q

1

=

1

2

ω

φ

(4.19)

˙ q

2

=

1

2

ω

θ

(4.20)

˙ q

3

=

1

2

ω

ψ

(4.21)

b

˙ v

1

= −2q

2

· g (4.22)

b

˙ v

2

= 2q

1

· g (4.23)

b

˙ v

3

=

1

m

(F

z

+I

i

v

3

) (4.24)

˙ ω

φ

= K

φ

· u

φ

(4.25)

˙ ω

θ

= K

θ

· u

θ

(4.26)

˙ ω

ψ

= K

ψ

· u

ψ

(4.27)

26 4.1 state-space Model

4.1 state-space Model

The typical way of describing a Multiple Input Multiple Output (MIMO) system is with the state-space repre-

sentation. This form is also required in calculating the model based controllers developed in a later chapter.

The state-space form consists of the following system:

˙ x

s

= Ax

s

+Bu

s

(4.28)

(4.29)

Where x

s

is the state vector described in Equation 4.30 and u

s

the input described in Equation 4.31.

x

s

=

_

P q

v

v

_

(4.30)

u

s

=

_

u

φ

u

θ

F

z

u

ψ

_

(4.31)

The system matrices are described below.

A =

_

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

_

0 0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 0 1

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 −2g 0 0 0 0

0 0 0 0 2g 0 0 0 0 0

0 0 0 0 0 0 0 0 0

I

i

m

_

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

_

(4.32)

B =

_

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

_

0 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

1

2

K

φ

0 0 0

0

1

2

K

θ

0 0

0 0 0

1

2

K

ψ

0 0 0 0

0 0 0 0

0 0

1

m

0

_

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

¸

_

(4.33)

Chapter 5

Veriﬁcation of the Quadrotor Model

In the previous two chapters both the linearized and non-linear models have been derived. To evaluate how well

the two models perform it is desired to compare them with the behaviour of the real quadrotor. This will be the

main focus in this chapter.

Comparing a quadrotor model with a real quadrotor is not straight forward because the quadrotor is not a stable

system without a controller. Slight difference in the modelling of the angular velocity will very quickly turn the

quadrotor upside down and the states depending on the translatory acceleration will be ﬂawed.

In this chapter the performance of the models is evaluated by modeling the time derivative velocity (

b

˙ v) and

time derivative quaternion ( ˙ q) from a state vector from real ﬂight. The true derivative of the velocity and

quaternion is calculated from the measurements and compared with the result of the model.

In this way the state of the model will never be allowed to run away. It is in this way a 1 step prediction.

Verifying the model in this way have some limitations. It will be difﬁcult to see much dynamic from the

modeled derivatives, but since the derivatives are mostly kinematic equations without much dynamic this is

accepted.

To record the behaviour of the real quadrotor two test ﬂight where made. One where large input was applied to

roll and pitch input and one with a more calm ﬂight close to the operating point. The ﬁrst test ﬂight is the basis

on which the time derivative of the quaternion is further examined and the second ﬂight is used when the time

derivative of the velocity is examined. A more detailed description on the test ﬂight and the full measurements

can be seen in Appendix C.3.

5.1 Time Derivative of the Quaternion

In general both the non-linear and the linear model follows the tendencies of the system well. Figure 5.1 shows

a part of the measurement with the result of the two models. There is small deviations in amplitude, but this is

estimated not to be enough to have signiﬁcant effect.

The biggest difference can be seen when ˙ q

3

is examined. Here the assumption that there is no dynamics in the

input angular velocity relation is not very accurate. This is shown in detail on Figure 5.2. The extra angular

28 5.1 Time Derivative of the Quaternion

20 25 30 35 40

−1.5

−1

−0.5

0

0.5

1

1.5

˙

q

1

Time [s]

Measured

Non−linear model

Linear model

Figure 5.1: Sample of modelled and measured ˙ q

1

velocity is most likely caused by the angular momentum being build up as the quadrotor rotates around the

z-axis and the on-board controllers lag of compensating for this momentum. The linear model is very close to

the non-linear model, which indicated that the most dominating effects remain even after the linearization. The

overall result is considered acceptable for use in the estimator and controllers.

Veriﬁcation of the Quadrotor Model 29

35 40 45 50

−1.5

−1

−0.5

0

0.5

1

1.5

˙

q

3

Time [s]

Measured

Non−linear model

Linear model

Figure 5.2: Sample of modelled and measured ˙ q

3

5.2 Time Derivative of the Velocity

Figure 5.3 and 5.4 shows the measured acceleration in the body frame with the result of the models. The

measured horizontal accelerations are very close to the result of the models and both follows the dynamic very

well.

The vertical acceleration however seem to have an offset and parts where the models deviates from the measure-

ment. This offset can very well be contributed the battery. The effect of the battery being discharged over time

can be represented as the force generated by the quadrotor slowly declines as time pass. On a down pointing

z-axis this will result in a general higher value of the measured acceleration compared with the model. This is

what is indicated on Figure 5.4.

If the total measurements found in Appendix C.3 is further examined the offset can be seen to be increasing as

time passes which also indicates that the battery could be the cause. This corresponds well with the assumption

made of the force polynomial only being accurate when a fully charged battery is used.

On Figure 5.4 close to 46 and 57 seconds in the measurement, the non-linear model seems almost to go in the

opposite direction of the measurements. At these two areas the quadrotor was not in the range of the Vicon

system. The ﬁrst area the quadrotor ﬂew out of range in the y-axis and the second it was ﬂown to close to the

ceiling. In this period the measurements are not considered valid and will not be further discussed.

Even considering the offset on the acceleration in the vertical axis it is estimated that both the linear and the

non linear model will be sufﬁcient to use in the estimator and controllers.

30 5.2 Time Derivative of the Velocity

20 30 40 50 60 70

−3

−2

−1

0

1

2

3

4

5

˙

v

1

Time [s]

Measured

Non−linear model

Linear model

Figure 5.3: Sample of modelled and measured ˙ v

1

25 30 35 40 45 50 55 60

−5

0

5

˙

v

3

Time [s]

Measured

Non−linear model

Linear model

Figure 5.4: Sample of modelled and measured ˙ v

3

Chapter 6

Sensor Model

The purpose of this chapter is to present the various sensors used in the estimation and derive any scaling

needed.

6.1 Position and Orientation

For measuring the position and orientation the Vicon system in the MTLab is used. Being part of the MTLab

it is already calibrated and scaled to use SI units. To give an impression of how accurate these sensors are the

position and orientation measured and the variance recorded. The result is listed in Table 6.1.

x-axis y-axis z-axis

Variance P 4.16 · 10

−7

3.28 · 10

−9

2.91 · 10

−7

q

0

q

1

q

2

q

3

Variance q 1.09 · 10

−9

5.17 · 10

−6

9.74 · 10

−6

3.62 · 10

−6

Table 6.1: Variance for the Vicon system

6.2 Angular Velocity and Acceleration

For measuring angular velocity and acceleration the on-board gyroscopes and accelerometers are used. The val-

ues are retrieved through the X-3D interface described in Appendix A. The data received through the interface

are raw values directly sampled from the on-board ADC. The values are, in the ResearchPilot documentation,

stated to be withing the range of 0 and 1023.

Further analysis of the actual gyro and the accelerometer shows that they both have a linear output, and the

data sheets states a conversion factor to SI units. But given the lag of detailed information on the ADC, the

conversion factors are determined using Matlab’s system identiﬁcation toolbox. The results of the estimated

conversion factors are listed in Table 6.2. A comparison of the converted sensor values and the true values

(based on Vicon measurements) are shown in Figure 6.1 and 6.2 for the gyroscope and accelerometer.

32 6.2 Angular Velocity and Acceleration

1st axis 2nd axis 3rd axis

K

gyro

0.0144 0.0143 0.0127

K

acc

0.0086 0.0077 0.0111

Table 6.2: Sensor scales for the gyroscope and accelerometer

On the graf showing the on-board sensor some vertical periods are present. This is due to short communication

failures resulting in no new values are received from the quadrotor. The phenomenon is further described in

Appendix A concerning the X-3D quadrotor interface. Also on the lowest plot in Figure 6.2 the on-board

sensor measurement can be seen to always stay above approximately 4

m

s

2

. This is assumed to be caused by the

saturation of the sensor. In this case the gravitational acceleration have been subtracted from the measurement

(knowing the orientation from the Vicon system) to compare with the second derivative of the position. The

accelerometer measures originally always the −9.82

m

s

2

on the z-axis when the quadrotor is close to hover. This

indicates that the saturation of the sensor, given the measurements, is close to −14

m

s

2

.

28.5 29 29.5 30 30.5

−5

0

5

ω

1

[

r

a

d

/

s

e

c

]

Vicon based

On−board sensor

32 33 34 35 36 37 38 39

−5

0

5

ω

2

[

r

a

d

/

s

]

40 40.5 41 41.5 42 42.5 43 43.5 44

−5

0

5

ω

3

[

r

a

d

/

s

]

Time [s]

Figure 6.1: Converted gyroscope measurement compared with Vicon measurement

The conversion factor is in both sensors depending on the temperature, but in this setup the quadrotor is always

used in the same room, with a constant temperature. Any changes in the conversion factor due to temperature

are therefore ignored.

As with the Vicon system the variance of the sensors have been measured as it is needed to determine the

co-variance matrix for the estimator in the next Part. The variances are listed in Table 6.3.

Sensor Model 33

6 6.5 7 7.5 8 8.5 9

−10

−5

0

5

10

A

c

c

1

[

m

/

s

2

]

Vicon based

On−board sensor

13.5 14 14.5 15 15.5 16 16.5 17 17.5 18

−10

−5

0

5

10

A

c

c

2

[

m

/

s

2

]

21.5 22 22.5 23 23.5 24 24.5 25

−10

−5

0

5

10

A

c

c

3

[

m

/

s

2

]

Time [s]

Figure 6.2: Converted accelerometer measurement compared with Vicon measurement

Variance 1st axis 2nd axis 3rd axis

Gyro 1.24 · 10

−4

1.94 · 10

−4

8.54 · 10

−4

Accelerometer 4.60 · 10

−3

2.62 · 10

−3

3.54 · 10

−3

Table 6.3: Variance for the gyro and accelerometer sensors

Part II

State Estimation

Table of Contents

7 State Estimation and Sensor Fusion 37

7.1 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

7.2 The IMU-driven Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

7.3 Fault Handling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

7.4 Analysis of Estimator Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

36 TABLE OF CONTENTS

Chapter 7

State Estimation and Sensor Fusion

An important step in obtaining a good ﬂight performance for the quadrotor, is to know where it is. In other

words to have a good estimate of the state vector. This chapter describes how this estimate is made given the

system model and measurements made in ﬂight.

One might question why an estimator is necessary when measurements are available. If perfect measurements

(no noise or uncertainty) could be made for all states an estimator would not be necessary but unfortunately

perfect measurements do not exist in the real world. Usually it is not possible to have sensors measuring all

states, and the states that are measured are corrupted by measuring noise from the sensor, or some disturbing

effects from the surrounding world. All these factors cause uncertainty in the measurements. Instead of per-

ceiving the measurements as the actual truth, they are thought of as noisy samples of the actual state. In this

way the measurements become random variables whose mean are the true state.

Several tools exist dealing with estimation problems such as this. A group of ﬁlters that are especially efﬁcient

are the Bayes ﬁlters. Using Bayes ﬁlters it is possible to fuse the predicted state based on the previous estima-

tion, with the measurements form the sensors [Thrun et al., 2006]. This is done using Bayes theorem (shown

in Equation 7.1).

P(x|z) =

P(z|x)P(x)

P(z)

(7.1)

P(x) is typically referred to as the prior probability distribution, z the data, and P(x|z) the posterior probability

distribution.

One particular kind of Bayes ﬁlter is the Kalman ﬁlter. The Kalman ﬁlter assumes that the state and mea-

surements are random variables with a Gaussian distribution. Not only does the ﬁlter propagate the state (the

mean of the random variable) but it also propagates the uncertainty in form of a covariance matrix. The type of

ﬁlter used to estimate the system states for the quadrotor is an EKF. Where as the normal Kalman ﬁlter only

supports linear systems and sensor models, the EKF supports nonlinear models. As shown later the ﬁlters are

very closely related. The EKF simply linearizes the models using a ﬁrst order Taylor approximation.

The EKF is placed in between the sensors and the controller as shown in the control scheme in Figure 7.1.

38 7.1 The Extended Kalman Filter

Figure 7.1: Control scheme with estimator

7.1 The Extended Kalman Filter

The EKF is a very powerful and elegant tool to know when dealing with estimation. The mathematical sim-

plicity and intuitive procedure leaves the EKF as a simple way to not only estimate the state of the system but

also to merge multiple sensors and also to handle failing sensors. How all of this is done in more detail will

be revealed throughout the remainder of this chapter. This section will primarily describe the mathematical

background of the EKF which hopefully helps the understanding of why this is such a powerful tool.

To give a short overview, all the essential EKF equations are listed in Table 7.1 [Grenwal and Andrews, 2008]

and can be divided into three categories: Estimation model, prediction and update.

Estimation Model

x

k

= φ

k−1

(x

k−1

) +w

k−1

, w

k

∼ N(0, Q

k

)

z

k

= h

k

(x

k

) +v

k

, v

k

∼ N(0, R

k

)

Φ

[1]

k−1

≈

∂φ

k

∂x

¸

¸

¸

x=ˆ x

+

k−1

H

[1]

k

≈

∂h

k

∂x

¸

¸

¸

x=ˆ x

−

k

Prediction step

ˆ x

−

k

= φ

k−1

(ˆ x

+

k−1

)

P

−

k

= Φ

[1]

k−1

P

+

k−1

Φ

[1]

k−1

+Q

k−1

Update step

K

k

= P

−

k

H

[1]

k

_

H

[1]

k

P

−

k

H

[1]

k

+R

k

_

−1

ˆ x

+

k

= ˆ x

−

k

+K

k

(z

k

−H

[1]

k

ˆ x

−

k

)

P

+

k

=

_

I −K

k

H

[1]

k

_

P

−

k

Table 7.1: Discrete EKF Equations

The notation used in Table 7.1 will be used throughout this chapter. The indices

k

and

k−1

indicate the current

sample and the previous sample. When a matrix is denoted with an

[1]

it is a 1st order linear Taylor approxi-

mation of a nonlinear function. Ex. Φ

[1]

is a linearization of φ(x). The

+

and

−

indicate a priori or a posterior

estimate respectively. That a variable is an estimate is also indicated by theˆ. By this deﬁnition ˆ x

−

k

is the priori

State Estimation and Sensor Fusion 39

estimate of the state vector at the k sample.

The basic function of both the EKF and the Kalman ﬁlter is to merge information from a prediction model

with the information from the sensors. The procedure is illustrated in Figure 7.2 and can be described by the

following three steps.

1. The current priori state ( ˆ x

−

k

) is predicted using the previously estimated state ( ˆ x

+

k−1

) and the prediction

model.

2. The merging factor also called the Kalman gain (K

k

), is calculated.

3. The priori state is updated with the sensor information (z

k

) using the Kalman gain

Figure 7.2: Procedure of the EKF and the Kalman ﬁlter

7.1.1 Estimation model

The most essential function of a Kalman ﬁlter is to merge the information, known from the model, with the

information obtained from the sensors. Therefore an important part of the ﬁlter is the prediction model. As the

name reveals the purpose is to predict the next state given the previous state. The normal Kalman ﬁlter uses a

linear transition matrix for the prediction where as the EKF supports a non-linear prediction model by doing

on-line linearization using the Jacobian matrix. Since the both ﬁlters are Gaussian ﬁlters both models need to

have a zero mean Gaussian noise model as shown in Equation 7.2 and 7.3.

x

k

= φ

k−1

(x

k−1

) +w

k−1

(7.2)

w

k

∼ N(0, Q

k

) (7.3)

Also a sensor model is needed to relate the measurements to the state and likewise the noise model is required

to be Gaussian.

z

k

= h

k

(x

k

) +v

k

(7.4)

v

k

∼ N(0, R

k

) (7.5)

The Jacobian matrix needed for the on-line linearization is the ﬁrst order partial derivatives of all states as

shown in Equations 7.6 and 7.7. The linearization of the system matrix is done with the previous a posteriori

state and the linearization of the sensor model is done using the current a priori state.

Φ

[1]

k−1

≈

∂φ

k

∂x

¸

¸

¸

x=ˆ x

+

k−1

(7.6)

H

[1]

k

≈

∂h

k

∂x

¸

¸

¸

x=ˆ x

−

k

(7.7)

40 7.1 The Extended Kalman Filter

7.1.2 Prediction

The prediction of the state is done using the nonlinear prediction model such as Equation 7.8 describes.

ˆ x

−

k

= φ

k−1

(ˆ x

+

k−1

) (7.8)

Also the estimation covariance P

k

is propagated through the model. This time however, the linearized model

is to be used for the prediction. The estimation covariance indicates how reliable the estimate is. To under-

stand how this covariance is propagated through the model one can start with the deﬁnition of the estimated

covariance being the squared residual as shown in Equation 7.9.

P

−

k

= E[ ˜ x

−

k

˜ x

−

k

] (7.9)

Where ˜ x

−

k

= ˆ x

−

k

− x

k

is the residual between the estimate and the true state. The residual can be introduced

by subtracting x

k

from the linearized state prediction.

ˆ x

−

k

−x

k

= Φ

[1]

k−1

ˆ x

+

k−1

−x

k

˜ x

−

k

= Φ

[1]

k−1

(ˆ x

+

k−1

−x

k−1

) −w

k−1

= Φ

[1]

k−1

˜ x

+

k−1

−w

k−1

The actual covariance propagation can be described by multiplying with ˜ x

−

k

and talking the estimated value on

both sides.

P

−

k

= E[ ˜ x

−

k

˜ x

−

k

] = Φ

[1]

k−1

E[ ˜ x

+

k−1

˜ x

+

k−1

]Φ

[1]

k−1

+E[w

k−1

w

k−1

]

= Φ

[1]

k−1

P

+

k−1

Φ

[1]

k−1

+Q

k−1

(7.10)

7.1.3 Update

The ﬁnal step to the sensor updated estimate is a balance between the model prediction and the sensor mea-

surement. The weight called the Kalman gain is calculated on the basis of the error covariance (how good the

model prediction is) and the sensor covariance (how much the sensors are trusted) as shown in Equation 7.11.

K

k

= P

−

k

H

[1]T

k

_

H

[1]

k

P

−

k

H

[1]T

k

+R

k

_

−1

(7.11)

The derivation of the Kalman gain and the following Kalman equations are left out for simplicity but they can

be found in many textbooks concerning this subject [Grenwal and Andrews, 2008][Thrun et al., 2006].

The update of the priori state to the posteriori state is done using Equation 7.12 where the posteriori state is the

sum of the priori state and the weighted difference between the sensor measurement and the modelled sensor

measurement.

ˆ x

+

k

= ˆ x

−

k

+K

k

(z

k

−H

[1]

k

ˆ x

−

k

) (7.12)

The error covariance update is like with the prediction based on the update of the state. The error covariance

update is given in Equation 7.13.

P

+

k

=

_

I −K

k

H

[1]

k

_

P

−

k

(7.13)

State Estimation and Sensor Fusion 41

7.2 The IMU-driven Estimator

An IMU-driven estimator has previously been used with success to estimate the states of a UAV[Bisgaard, 2007][Van Der Merwe et al., 2004].

The basic idea, of using IMU-driven estimators, is to use the measured acceleration and angular velocity as the

input to the model. In the case of the quadrotor this leaves only the rigid body kinematic and dynamics. With

this simpliﬁed model it is estimated that the prediction will be more accurate than with the previously described

model that is based on the input to the quadrotor. With the IMU-driven model the errors caused by battery

discharging and slightly off input scaling will have no effect.

Given that part of the model is now changed also the state vector have to be reviewed. When dealing with

sensors it can be useful to estimate any offsets to compensate for these in the model. To the previously described

model state is therefore appended six extra states to estimate the offset of the gyroscope and accelerometer. The

state vector now contains the position (P), the translatory velocity (

b

v), the orientation (q), the gyro bias (ω

b

)

and the accelerometer bias (a

b

).

x =

_

P

[1×3]

q

[1×4]

b

v

[1×3]

ω

b[1×3]

a

b[1×3]

_

(7.14)

The measurements of acceleration and angular velocity is preprocessed (see Chapter 6) with a scale conver-

sion factor to convert to SI units. The input vector for the IMU-driven model includes the processed gyro

measurement ( ¯ ω) and the processed accelerometer measurement (¯ a).

u =

_

¯ ω

[1×3]

¯ a

[1×3]

_

(7.15)

The remaining sensors are used without any preprocessing and affects the ﬁlter through the sensor state vector.

The sensor state vector contains the measurements of the position (P

vicon

) and orientation (q

vicon

) from the

Vicon system.

z =

_

P

vicon[1×3]

q

vicon[1×4]

_

(7.16)

7.2.1 IMU-driven Model

The model used for the IMU-driven Estimator is based on the model derived in Chapter 3, but deviates in the

case of input. The IMU-driven model uses inertial sensors as input and not the control commands send to the

quadrotor. The model equations are listed in Equations 7.17 to 7.21

˙

P =

e

C

b

b

v (7.17)

b

˙ v = ¯ a +a

b

+

b

C

e

_

0 0 g

_

−

b

ω ×

b

v (7.18)

˙ q =

1

2

Ω

¯ ω

q (7.19)

˙ ω

b

= 0 (7.20)

˙ a

b

= 0 (7.21)

The transformation matrices between earth and body frame

b

C

e

and

e

C

b

are described in Section 2.3 on page

10. ¯ a and ¯ ω is the processed measurements of the acceleration and angular velocity. The combined non-linear

continuous model is deﬁned as f

t

(x).

For the IMU-driven model to be used in the EKF a few derivations are necessary. Previously described equa-

tions form the basis for the non-linear prediction model, but a linearization in form of a Jacobian matrix is still

needed.

42 7.2 The IMU-driven Estimator

7.2.2 Derivation of the Jacobian

As previously mentioned the online linearization is done using the Jacobian matrix of the system. The Jacobian

matrix consists of the partial derived states given the previous a posteriori state. This described mathematically

looks like in Equation 7.22.

Φ

[1]

k−1

≈

∂φ

k

∂x

¸

¸

¸

x=ˆ x

+

k−1

(7.22)

=

_

¸

¸

_

∂φ

1,k

∂x

1

∂φ

1,k

∂x

2

· · ·

∂φ

2,k

∂x

1

∂φ

2,k

∂x

2

· · ·

.

.

.

.

.

.

.

.

.

_

¸

¸

_

(7.23)

The prediction model needed for the described EKF must be a discrete model. The discretization is done using

the Zero-Order-Hold (ZOH) method in a way such that the discrete prediction model can be described as in

Equation 7.24.

Φ

[1]

k−1

= I +T

s

·

∂f

t

(x)

∂x

¸

¸

¸

x=ˆ x

+

k−1

(7.24)

The sample time of the EKF is chosen to be 100 Hz (T

s

= 0.01), and given the slow changes of the states it is

estimated that a ZOH discretization is sufﬁcient. When the linearization of f

t

(x) is written on matrix form it

becomes more apparent where the actual linearization happens.

_

¸

¸

¸

¸

¸

¸

_

˙

P

˙ q

b

˙ v

˙ ω

b

˙ a

b

_

¸

¸

¸

¸

¸

¸

_

=

_

¸

¸

¸

¸

¸

¸

_

0

∂

e

C

b

b

v

∂q

e

C

b

0 0

0

1

2

Ω

¯ ω

0

1

2

∂Ω

¯ ω

q

∂ω

b

0

0

∂

b

C

e

g

∂q

[ω×]

∂[ω×]

b

v

∂ω

b

I

0 0 0 0 0

0 0 0 0 0

_

¸

¸

¸

¸

¸

¸

_

_

¸

¸

¸

¸

¸

¸

_

P

q

b

v

ω

b

a

b

_

¸

¸

¸

¸

¸

¸

_

+

_

¸

¸

¸

¸

¸

¸

_

0 0

1

2

∂Ω

¯ ω

q

∂ ¯ ω

0

∂[ω×]

b

v

∂ ¯ ω

I

0 0

0 0

_

¸

¸

¸

¸

¸

¸

_

_

¯ ω

¯ a

_

(7.25)

The partial derivatives can each be calculated as a vector of partial derivatives of the single states as done in

Equations 7.26 to 7.29.

∂

e

C

b

b

v

∂q

=

_

∂

e

C

b

∂q

0

b

v

∂

e

C

b

∂q

1

b

v

∂

e

C

b

∂q

2

b

v

∂

e

C

b

∂q

3

b

v

_

(7.26)

∂Ω

¯ ω

q

∂ω

=

_

∂Ω

¯ ω

∂ω

1

q

∂Ω

¯ ω

∂ω

2

q

∂Ω

¯ ω

∂ω

3

q

_

(7.27)

∂

b

C

e

g

∂q

=

_

∂

b

C

e

∂q

0

g

∂

b

C

e

∂q

1

g

∂

b

C

e

∂q

2

g

∂

b

C

e

∂q

3

g

_

(7.28)

∂[ω×]

b

v

∂ω

=

_

∂[ω×]

∂ω

1

b

v

∂[ω×]

∂ω

2

b

v

∂[ω×]

∂ω

3

b

v

_

(7.29)

Each of the above stated partial derivatives have been calculated using Maple and is for convenience not listed

here, but can be found in the ﬁle phi.m on the attached CD.

The remaining sensors used for the EKF already have a linear relation to the states and is given by the matrix

H used in the relation in Equation 7.30.

z = Hx (7.30)

_

P

vicon

q

vicon

_

=

_

I 0 0

0 I 0

_

_

¸

_

P

q

b

v

_

¸

_

State Estimation and Sensor Fusion 43

Finally for the EKF is the previously determined variance of the Vicon system used as the sensor covariance

matrix Rand the model covariance matrix Qis determined by manually tuning the estimator.

7.3 Fault Handling

While testing the IMU-based EKF a few situations occurred that affected the estimated too much to be accept-

able. One situation were while using an additional sensor measuring the air pressure. The pressure sensor was

not able to be update the pressure information at the same rate as the EKF. An EKF uses every sample as a new

and equally weighted sample, which in this case gave a faulty result.

To ensure the sustainability of the EKF a small algorithm was implemented to handle variating sampling times.

Algorithm 1 Handling of variating sampling times

if z

i,k

== z

i,k−1

then

R

i,i

= 10

8

end if

Algorithm 1 basically states that if a sensor reading is detected to be equal to the prior, the variance for this

sensor is set sufﬁciently high for the EKF to ignore the sensor reading. After each sample the sensor covariance

matrix Ris reset and the algorithm will have no permanent effect.

7.3.1 Flying Outside Vicon Range

Another more frequently occurring fault is the Vicon sensors failing due to the limited Vicon range. As ex-

plained in a previous chapter, the Vicon system relies on reﬂectors on the quadrotor to see the orientation and

position using cameras. When either the quadrotor is ﬂying with to great of an angle or the quadrotor ﬂies too

close to a wall the Vicon system is not capable of delivering good measurements. Whenever the quadrotor is

lost from the sight of the Vicon system the position is kept constant and the orientation is reset. In this way it is

possible to determine whenever the Vicon system fails.

To compensate for the failures a similar algorithm as Algorithm 1 is implemented to make sure the EKF ignores

the Vicon sensors whenever the measurement fails.

Algorithm 2 Handling failing Vicon measurements

if q ==

_

1 0 0 0

_

then

R

1..7,1..7

= 10

8

end if

Using this algorithm when ever the quadrotor is ﬂying out of Vicon range results in an estimation based only

on the on-board sensors. Several test have been made and a satisfactory estimation performance have been

obtained.

44 7.4 Analysis of Estimator Performance

7.4 Analysis of Estimator Performance

To verify that the estimator performs as intended the quadrotor is ﬂown manually around both within and

outside the range of the Vicon system. The Figures 7.3 and 7.4 document one of these measurements.

0 5 10 15 20 25 30 35

−3

−2

−1

0

1

2

3

Time [s]

x

[

m

]

0 5 10 15 20 25 30 35

−3

−2

−1

0

1

2

3

Time [s]

y

[

m

]

0 5 10 15 20 25 30 35

−3

−2

−1

0

Time [s]

z

[

m

]

Quaternion measurement and estimate

sensor

estimate

sensor

estimate

sensor

estimate

Figure 7.3: A test ﬂight with the position shown

On these ﬁgures several occasions of Vicon failures can be found. At approximately 4 and 7 sec the boundary

of the range is reached on the z-axis and the y-axis. This do, as predicted, result in short fall outs. Figures 7.5

and 7.6 shows a closer look at the fall out at 7 sec.

On these ﬁgures it can be seen that the estimator follows a continuous path when the sensor fails and the

path includes dynamics that enables the orientation and the position to still be close when the Vicon sensor is

restored.

State Estimation and Sensor Fusion 45

0 5 10 15 20 25 30 35

0

0.2

0.4

0.6

0.8

1

Time [s]

q

0

0 5 10 15 20 25 30 35

−0.2

−0.1

0

0.1

0.2

Time [s]

q

1

0 5 10 15 20 25 30 35

−0.2

−0.1

0

0.1

0.2

Time [s]

q

2

0 5 10 15 20 25 30 35

−0.4

−0.2

0

0.2

0.4

Time [s]

q

3

sensor

estimate

sensor

estimate

sensor

estimate

sensor

estimate

Figure 7.4: A test ﬂight with the quaternion shown

46 7.4 Analysis of Estimator Performance

7 7.5 8 8.5 9 9.5

0

0.5

1

1.5

2

2.5

3

Time [s]

y

[

m

]

sensor

estimate

Figure 7.5: A position sample from the test ﬂight

7 7.5 8 8.5 9 9.5

−0.2

−0.1

0

0.1

Time [s]

q

1

sensor

estimate

Figure 7.6: A quaternion sample from the test ﬂight

On Figure 7.4 can also be seen other effects. When the Vicon sensors fail around 4 sec into the test ﬂight the

q

1

and q

2

values continues further than intended. This is because the previously discussed gyroscope bias not

yet have been estimated. With the current settings it takes around 3 sec where the quadrotor should be steady

on the ground. If the quadrotor is ﬂying, the time for the bias estimate to settle is longer as indicated by Figure

7.7.

Given this and the performance of other estimation tests the IMU-based estimator is considered good enough

to use in combination with the controllers.

State Estimation and Sensor Fusion 47

0 5 10 15 20 25 30 35

−0.2

−0.1

0

0.1

ω

b

,

2

[

r

a

d

/

s

]

Time [s]

Figure 7.7: Estimate of the gyro bias on the 2nd axis

Part III

Control Algorithms

Table of Contents

8 Controlling a Quadrotor 51

9 Linear Quadratic Control 53

9.1 The General LQR Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

9.2 Following a Fast Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

10 Robust H

∞

Control 58

10.1 The Standard Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

10.2 Matrix Inequalities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

10.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

11 Evaluation of Controllers 65

11.1 Hover using Various Vicon Sampling Frequencies . . . . . . . . . . . . . . . . . . . . . . . . 65

11.2 Step Response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

11.3 Following a Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

11.4 Hovering in Low Height . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

11.5 Overall Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

50 TABLE OF CONTENTS

Chapter 8

Controlling a Quadrotor

The controller is the ﬁnal step towards enabling autonomous ﬂight. In the previous chapters details of how

the quadrotor is able to navigate depending on the input signal have been discussed. As well as how to more

accurately determine the actual state of the quadrotor depending on the measurements from the sensors. This

information will in the next few chapters be employed when developing three different controllers.

All the controllers will be feedback controllers. In other words, they will all be based on the difference between

part of the state vector and some given reference describing the desired trajectory. The basic idea of the feedback

controller is illustrated in Figure 8.1. Where r is the reference, e is the error between the current state and the

reference, K(s) is the controller, u is the input to the system, G(s) is the system and y is the output of the

system.

Figure 8.1: The classical feedback controller

The most simple form of a controller that is able to stabilize the quadrotor and have it follow a reference is

the PID controller. The PID controller developed for the quadrotor is in fact a controller consisting of 10 PID

controllers all connected in a particular way and together form controllers for the position, orientation and

velocity. The PID controller is fairly intuitive to understand, but on a more complex MIMO system as the

quadrotor the intuitive understanding becomes less transparent. The tuning of PID controller is usually done

manually for each controlled state, and this becomes difﬁcult as the complexity of the system increases.

The PID controller is an easy way of making the quadrotor ﬂy, but the performance is usually not as good as

with model based controllers. There have been developed a PID controller for the quadrotor which enables it

to be stabilized, but the controller is not discussed further in this thesis. It will be used only as a reference to

evaluate the performance of the other two controllers.

Another way of formulating the feedback controller is in the state-space form as illustrated in Figure 8.2.

The state-space formulation is in principle the same as the classical feedback controller, only the controller

K becomes a feedback matrix weighting the states in a particular way to generate the input. When using a

52

reference, part of the control matrix is used to feed forward the reference.

Figure 8.2: The state-space feedback controller

In calculating the feedback matrix many methods exist. The two methods that will be discussed further the

Linear Quadratic method and the H

∞

method.

Chapter 9

Linear Quadratic Control

The ﬁrst controller to be derived for the quadrotor is the discrete LQR. The LQR is a well known method

to obtain a simple optimal controller. The LQR is a model based controller and through out this chapter the

discrete linear models listed in Equations 9.1 and 9.2 are used as the models for the quadrotor and the reference.

x

s

(k + 1) = Φ

s

x

s

(k) +Γ

s

u(k) (9.1)

x

r

(k + 1) = Φ

r

x

r

(k) (9.2)

Section 9.1 concerns the general derivation of the LQR algorithm and can be skipped. In Section 9.2 the general

LQR algorithm is appended with a reference model, allowing the quadrotor to follow a desired trajectory. The

ﬁnal evaluation of the controller will be done together with the other controllers in Chapter 11.

9.1 The General LQR Algorithm

LQR is an interesting case of an optimal controller. What makes it so interesting is the quadratic cost function

shown in Equation 9.3.

J = x

Qx +u

Ru (9.3)

A cost function describes the immediate cost of being in state x and talking input u. The quadratic cost function

of the LQR consists of the quadratic weights Qand R, weighing respectively the current state and input. What

makes this quadratic cost function so interesting is the simple methods to ﬁnd the minimum (in this case the

optimal) value by ﬁnding the value where the derivative reaches zero. This is basically the essential step in how

the LQR algorithms are derived. The ﬁst step is to deﬁne a value function as in Equation 9.4.

V (x) =

∞

k=0

x

k

Qx

k

+u

k

Ru

k

(9.4)

In general, a value function is a function of the state describing the total cost going from the current state and

following a control policy either through some horizon or until an equilibriumwith zero cost is obtained (inﬁnite

horizon) [Thrun et al., 2006]. In this case the value function describes the sum the cost function through all

54 9.1 The General LQR Algorithm

samples until the desired stabilized state is reached where no input is necessary. More speciﬁc it is an expression

of the value function using the optimal control policy that is sought, called the optimal value function (Equation

9.5).

V

(x) = min

u

∞

k=0

x

k

Qx

k

+u

k

Ru

k

(9.5)

The expression of value function can be expressed in the current cost and cost for all future samples as in

Equation 9.6. Where x

0

and u

0

describes the current state and the current input.

V (x) = x

0

Qx

0

+u

0

Ru

0

+

∞

k=1

x

k

Qx

k

+u

k

Ru

k

(9.6)

Using the control policy described in Equation 9.7, the future cost can be described by a quadratic parametriza-

tion P of the state as in Equation 9.8 [Andersen, 2009].

u = Kx (9.7)

V (x) = x

Px (9.8)

Combining 9.8 and 9.6 yields an expression of the value function depending only on the current state, the

current input and the next state. Including the model the expression can be simpliﬁed to only depend on the

current state and input as shown in Equation 9.9. The indices

0

and

1

denotes the current and next state/input

respectively.

V (x) = x

0

Qx

0

+u

0

Ru

0

+x

1

P

1

x

1

V (x) = x

Qx +u

Ru + (Φx +Γu)

P

1

(Φx +Γu) (9.9)

The optimal value function can be described, as previously mentioned, as the smallest possible value function

hence where the partial derivative with regards to the input is zero (given the quadratic structure). The partial

derivative is calculated in Equation 9.10 and the optimal input given as the structure from 9.7 in Equation 9.11.

∂V (x)

∂u

= 0 (9.10)

2Ru + 2Γ

P

1

(Φx +Γu) = 0

Ru +Γ

P

1

Γu = −Γ

P

1

Φx

u = −[R+Γ

P

1

Γ]

−1

Γ

P

1

Φx (9.11)

To derive ﬁnal expression of the optimal value function in Equation 9.13 and the feedback matrix in Equation

9.12 are combined with 9.9. The optimal parametrization P can be expressed as in Equation 9.14.

K = −[R+Γ

P

1

Γ]

−1

Γ

P

1

Φ (9.12)

V

(x) = x

Qx + (Kx)

R(Kx) + (Φx +ΓKx)

P

1

(Φx +ΓKx) (9.13)

P

0

= Q+K

RK + (Φ+ΓK)

P

1

(Φ+ΓK) (9.14)

The actual values of P and K can be found by simply iterating from a guessed P until the feedback matrix do

no longer change signiﬁcantly as illustrated in Algorithm 3.

Linear Quadratic Control 55

Algorithm 3 Iterates towards a feedback matrix K

P = Q

K

= 0

while K −K

> 10

−5

do

K

= K

K = −[R+Γ

P

1

Γ]

−1

Γ

P

1

Φ

P = Q+K

RK + (Φ+ΓK)

P(Φ+ΓK)

end while

9.2 Following a Fast Trajectory

Throughout this section the model for the quadrotor is appended with a model of a reference in a way such

that an expression for the error between the reference state and the quadrotor state becomes apparent. For this

combined models a new weight matrix is derived such that the general LQR algorithm can be used to calculate

matrices K

s

and K

r

illustrated in 9.1. Figure 9.1 also illustrates how the matrices from the LQR are connected

with the quadrotor and the EKF.

Figure 9.1: Overview of how the feedback matrices, derived through LQR, is connected

Additionally to the models described in Equations 9.1 and 9.2 is the output of the models now described. The

matrices H

s

and H

r

describes the part of the system state that is to follow the reference and the reference

respectable.

y

r

(k) = H

s

x

s

(k)

r(k) = H

r

x

r

(k)

Even though the quadrotor have 6 Degrees of Freedom (DoF) the 4 inputs limits the controllable DoF to 4. The

states chosen to control is the position (x, y and z) and the orientation around the z-axis (q

3

). This yields the

following H

s

matrix.

H

s

=

_

¸

¸

¸

_

1 0 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0 0

0 0 1 0 0 0 0 0 0 0

0 0 0 0 0 0 1 0 0 0

_

¸

¸

¸

_

(9.15)

The state of the reference (x

r

) is appended to the system state (x

s

) to make a combined prediction model as

done in Equation 9.16.

_

x

s

(k + 1)

x

r

(k + 1)

_

=

_

Φ

s

0

0 Φ

r

__

x

s

(k)

x

r

(k)

_

+

_

Γ

s

0

_

u(k) (9.16)

56 9.2 Following a Fast Trajectory

The error (e) is deﬁned as the difference between the output of the models. In the combined model, this can be

described as a single matrix H as done in Equation 9.17.

e(k) = H

r

x

r

−H

s

x

s

=

_

−H

s

H

r

_

_

x

s

(k)

x

r

(k)

_

= Hx(k) (9.17)

The cost function can now be redeﬁned to be a quadratic weighting of the error and the input as described in

Equation 9.18.

J = e

Q

e

e +u

Ru (9.18)

Combining 9.17 and 9.18 yields a cost function with a similar structure as the default LQR algorithm.

J = (Hx)

Q

e

Hx +u

Ru

= x

(H

Q

e

H)x +u

Ru (9.19)

From Equation 9.19 can a new expression for the weight matrix easily be isolated as shown in Equation 9.20.

Q = H

Q

e

H =

_

−H

s

H

r

_

Q

e

_

−H

s

H

r

_

(9.20)

Using the LQR algorithm described in Section 9.1 a combined feedback matrix K can be calculated. The

matrix will be a combination of the matrices K

r

and K

s

as shown in Equation 9.21.

u =

_

K

s

K

r

_

_

x

s

x

r

_

= K

s

x

s

+K

r

x

r

(9.21)

The feedback matrices will interact with the quadrotor as previously shown on Figure 9.1, but can also be

described with the linear models as in Figure 9.2.

Figure 9.2: Structure of the feedback matrices with the linear models

9.2.1 The Reference Model

The quadrotor is desired to be able to follow a trajectory and not only stay at one point. Therefore the reference

is initially modelled as constantly moving reference with a constant velocity, as described in Equation 9.22.

r(k) −r(k −1) = r(k −1) −r(k −2) (9.22)

Linear Quadratic Control 57

This expression can easily be rearranged to become a discrete state-space model.

r(k) = 2r(k −1) −r(k −2)

r(k + 1) = 2r(k) −r(k −1)

(9.23)

By deﬁning the reference state as in Equation 9.24 the state-space model can be described as in Equation 9.25.

x

r

(k) =

_

r(k)

r(k −1)

_

(9.24)

x

r

(k + 1) =

_

2I −I

I 0

_

x

r

(k) (9.25)

Where the reference can be described as in Equation 9.26.

r(k) = H

r

x

r

=

_

I 0

_

x

r

(9.26)

In practise the reference used must be the state of the reference (x

r

), so the previous sample must be included.

Because the sample rate is much faster then the changes in the reference it is chosen to ﬁlter the previous state

to avoid the otherwise noisy residual.

A similar model is calculated using a constant position model (x

r

(k) = r(k)) to compare the effect of the

models.

Chapter 10

Robust H

∞

Control

The second type of controller to be derived is the robust H

∞

controller. H

∞

control is based on minimizing a

performance channel, in this case, including a disturbance in the form of a reference and sensor noise.

The structure of the system and the controller are illustrated in Figure 10.1.

Figure 10.1: The basic conﬁguration for the Robust H

∞

controller

The disturbances can be seen as the reference (w

r

) and the sensor noise (w

n

). The frequency content is

described using the wights W

r

(s) and W

n

(s). W

r

(s) has a low-pass characteristic and W

n

(s) has a high-pass

characteristic.

The method chosen to derive the controller can be described in three steps:

1. Convert setup to standard 2 × 2 block form

2. Calculating the Matrix Inequality describing the H

∞

solution

3. Use a Linear Matrix Inequality (LMI) solver to derive the feed forward and feedback matrices

The standard 2 × 2 block form, also known as the standard problem in robust control theory, is a form that

encapsulates the entire system as a linear system with two inputs and to outputs as illustrated in Figure 10.2.

The channel from w to z is called the performance channel, and the purpose of deriving a controller is to

minimize this channel. The general input to the system is u and the measurable output is y. The challenge of

deriving the standard problem is to incorporate the disturbances and to deﬁne the performance channel.

In this computation of the controller, a particular type of solution have been selected. A solution the requires

solving a LMI. The steps to do this involves setting up one LMI containing all requirements and solving it

Robust H

∞

Control 59

Figure 10.2: The standard problem

using a LMI solver. These last two steps are very general and will only be described shortly.

60 10.1 The Standard Problem

10.1 The Standard Problem

The standard problem is a way to formulate a more complex system on a standardized form with only a few

input and output. When it is written on state-space form it can be described with 8 matrices with the correlation

shown in Equation 10.1. The top row is the state propagation and the remaining are the correlation of the

general input/output and the performance channel.

_

¸

_

˙ x

z

y

_

¸

_ =

_

¸

_

A B

1

B

2

C

1

D

11

D

12

C

2

D

21

0

_

¸

_

_

¸

_

x

w

u

_

¸

_ (10.1)

The state-space formulation is shown on a block diagram form in Figure 10.3.

Figure 10.3: The structure of the standard problem

The normal states space formulation can be recognized as the matrices A, B

1

and C

1

. The colors symbolizes

the function of the matrices. red indicates a feedback matrix, blue an input matrix, green, an output matrix and

purple a feed forward matrix.

The weights shown on Figure 10.1 are implemented as state-space ﬁlters with each their own matrices propa-

gation, input and output matrices. To include all variables are the input redeﬁned as the following, where x

s

is the states of the system, x

r

the low-pass weighted reference state, x

n

the high-pass weighted sensor noise

state, w

r

the reference and w

n

the sensor noise.

x =

_

¸

_

x

s

x

r

x

n

_

¸

_ w =

_

w

r

w

n

_

(10.2)

With this deﬁnition the matrices A, B

1

and B

2

can be redeﬁned as in Equation 10.3

A =

_

¸

_

A

s

0 0

0 A

r

0

0 0 A

n

_

¸

_ B

1

=

_

¸

_

0 0

B

r

0

0 B

n

_

¸

_ B

2

=

_

¸

_

B

s

0

0

_

¸

_ (10.3)

The performance channel is based on the residual of the state and the reference as shown in Figure 10.1 and

can be described as the output of the system subtracted from the ﬁltered reference (C

r

x

r

−C

s

x

s

) as shown in

Robust H

∞

Control 61

Equation 10.5. The matrix C

s

is a sub-matrix of C

s

, but only describing the outputs that is used as a reference

as illustrated in Equation 10.4.

C

s

=

_

¸

¸

¸

_

1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0

0 0 1 0 0 0 0 0 0

0 0 0 0 0 1 0 0 0

_

¸

¸

¸

_

(10.4)

A requirement of the general H

∞

-solution is for the D

12

matrix to have full column rank

[Toeffner-Clausen et al., 2001]. This is done by adding a factor . This also serves as a precaution to limit the

input since the u will be part of the performance channel.

C

1

=

_

−C

s

C

r

0

0 0 0

_

D

11

=

_

0 0

0 0

_

D

12

=

_

0

_

(10.5)

The out is described at the measurable system output added with the high pass ﬁltered sensor noise (C

s

x

s

+

C

n

x

s

+D

n

w

n

) as described in Equation 10.6.

C

2

=

_

C

s

0 C

n

_

D

21

=

_

0 D

n

_

(10.6)

10.1.1 Weights as ﬁlters

The weights are as previously mentioned implemented as ﬁlters. The types of ﬁlters chosen are to be ﬁrst order

high-pass and low-pass ﬁlters with the structure shown in Equation 10.7 where a denotes a pole, b a zero and k

a scalar gain.

W

r

=

y(s)

u(s)

=

k

s +a

W

n

=

y(s)

u(s)

=

k(s +b)

s +a

(10.7)

The transfer functions are converted to state-space form like in the following example:

y(s)

u(s)

=

k

s +a

y(s)(s +a) = k · u(s)

˙ y +a · y = k · u

˙ y = −a · y +k · u

When x is deﬁned to x = y the following standard state-space matrices can be used.

A = −a B = k C = 1 D = 0 (10.8)

For the high-pass case with both a pole and a zero the model can be described with the state-space model in

Equation 10.9.

A = −a B = b −a C = 1 D = k (10.9)

The weight for the reference consists of several low-pass ﬁlters since every state of the reference is weighted.

The state-space weight for the reference is listed in Equation 10.10.

A

r

= −

_

¸

¸

¸

_

a

1

0 0 0

0 a

2

0 0

0 0 a

3

0

0 0 0 a

4

_

¸

¸

¸

_

B

r

=

_

¸

¸

¸

_

k

1

0 0 0

0 k

2

0 0

0 0 k

3

0

0 0 0 k

4

_

¸

¸

¸

_

C

r

=

_

¸

¸

¸

_

1 0 0 0

0 1 0 0

0 0 1 0

0 0 0 1

_

¸

¸

¸

_

(10.10)

62 10.2 Matrix Inequalities

Similar do the state-space noise wight consist of diagonal matrices containing the values from Equation 10.9.

10.2 Matrix Inequalities

Given a standard state-space system G(s) given as

˙ x = Ax +Bw

z = Cx +Dw

the nominal H

∞

condition G(s)

∞

< γ is, according to [Stoustrup, 2010], satisﬁed if and only if there exists

a vector X = X

∗

> 0 such that

A

∗

X +XA+C

∗

C −(XB +C

∗

D)(D

∗

D−γ

2

I)

−1

(B

∗

X +D

∗

C) < 0 (10.11)

This inequality can be transfered to robust control theory as done in [Stoustrup, 2010] and yield the following

LMI

_

¸

_

F

11

F

12

F

13

F

12

−R

1

0

F

13

0 −I

_

¸

_ < 0 (10.12)

Where the matrices F

11

, F

12

, F

13

and R

1

is described as in Equations 10.13 to 10.16.

R

1

= I −D

11

D

11

(10.13)

F

11

= Y A

+AY +W

B

2

+B

2

W (10.14)

F

12

= B

1

+Y C

1

D

11

+W

D

12

D

11

(10.15)

F

13

= Y C

1

+W

D

12

(10.16)

One feedback matrix (including reference and noise feedback) can be calculated as in Equation 10.17

F = WY

−1

(10.17)

10.3 Simulation

To verify that the controller is able to stabilize the quadrotor the controller is tested with the nonlinear model.

In Figure 10.4 is the simulation of a step on each axis shown. Since a step in the yaw reference would bring the

quadrotor permanently out of the operating point this is disregarded.

All simulations show satisfying results, with a performance on the x and y-axis that indicates a bandwidth

of approximately 0.2 Hz. This controller is on purpose tuned this slow to have as low a feedback matrix as

possible. This H

∞

controller have been tested on the quadrotor in real ﬂight with an adverse result. The

controller was not capable of stabilizing the quadrotor in hover.

The feed forward and feedback controller matrices was investigated to ﬁnd the cause of this result. The feedback

matrix was compared with the one of the LQR and the feedback matrix of the H

∞

controller was signiﬁcantly

higher.

Robust H

∞

Control 63

68 70 72 74 76 78

−0.5

0

0.5

x

[

m

]

28 30 32 34 36 38

−0.5

0

0.5

y

[

m

]

48 50 52 54 56 58

−2

−1.5

−1

−0.5

z

[

m

]

Time [s]

Reference

Simulation

Figure 10.4: Simulation of step response of H

∞

controller

−1000 −900 −800 −700 −600 −500 −400 −300 −200 −100 0 100

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

Real Axis

I

m

a

g

i

n

a

r

y

A

x

i

s

Figure 10.5: Pole/zero map of the closed loop transfer function from reference to position on the

y-axis

If the closed loop system poles are analysed an indication for this behaviour can be found. In Figure 10.5 is

shown a pole/zero map for the closed loop transfer function from reference to position on the y-axis.

The pole/zero map indicates that the controller dampens much of the effect of the original poles in zero (from

the integrators), and only one pole remains to have a dominating effect. The actual movement of the poles,

depending on the parameters that can be adjusted for the H

∞

controller, is very difﬁcult to predict. But in the

case that all the other poles remains dampened by the controller and the pole original is moved from zero and

is following the normal root-locus behaviour the increasing feedback will cause the pole to further increase

the bandwidth. If the normal root-locus is followed by all poles and the zeros are not further moved (not very

likely), this system is not able to become unstable as further feedback gain is increased.

64 10.3 Simulation

The reason why this controller becomes unstable in the real system, is most likely that some dynamics are

absent in the current model. One place where a dampening have been identiﬁed, but not modelled was in

Section 3.3 on page 18 where the on-board controller was discussed. The choice of not modelling the dynamics

of the controller, motor controller and aerodynamic effect can very well be the cause of the H

∞

controller not

being able to stabilize the real quadrotor. The performance of the H

∞

controller is not investigated further, and

only the LQR and PID will be evaluated doing real ﬂight in the following chapter.

Chapter 11

Evaluation of Controllers

The developed controllers are tested under various conditions in real ﬂight to evaluate the performance and to

identify areas that may be improved. Firstly in Section 11.1 are the controllers tested in hover using various

sampling frequencies for the Vicon measurements. This is done to partly evaluate the controller in hover, but

also to determine how well the estimator and the controller collaborate. Section 11.2 demonstrates the step

response of the closed loop system. In Section 11.3 are the controllers tested with various types of trajec-

tories. In this section it is illustrated how the various structure of the controller affects the performance. In

Section 11.4 shortly discusses how the quadrotor would perform doing take-off and landing operations. A short

demonstration of hovering in low hight shows how well the quadrotor can be controlled.

The main tests are documented on video, and can be found on the CD in the folder Videos.

11.1 Hover using Various Vicon Sampling Frequencies

To evaluate the performance of the estimator combined with the controllers and to estimate whether a 100

Hz sampling frequency on the Vicon system is a requirement, three different sampling frequencies was tested

doing hover with the developed LQR with a constant position reference model. The reference was locked to

the position

_

0 0 −1

_

doing the entire ﬂight. Figure 11.1 shows the x-position doing an autonomous ﬂight

of 20 seconds.

30 35 40 45 50

−0.6

−0.4

−0.2

0

0.2

0.4

Time [s]

x

−

p

o

s

i

t

i

o

n

[

m

]

1 Hz

10 Hz

100 Hz

Figure 11.1: Hovering with Vicon sample frequencies of 1 Hz, 10 Hz and 100 Hz

66 11.1 Hover using Various Vicon Sampling Frequencies

The x-position shown is the result of the estimator, in other words the state that is used by the controller. From

the ﬁgure a clear difference can be seen in using 1 Hz and 10 Hz sampling frequency. This difference is also

illustrated on Figure 11.2, where the ﬁgure is shown as if it was seen from the ceiling of the MTLab.

−0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8

−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

y−position [m]

x

−

p

o

s

i

t

i

o

n

[

m

]

1 Hz

10 Hz

100 Hz

Figure 11.2: Hovering with Vicon sample frequencies of 1 Hz, 10 Hz and 100 Hz

The very low sampling frequency allows the estimator to drift from the actual state, and this is believed to be

the cause for this inaccurate ﬂight when only using 1 Hz Vicon measurements. Figure 11.3 shows the estimate

and measurements time correlated to better illustrate this belief. In almost every sample, the estimate is shown

to be far from the true state, where the furthest is more the 20 cm. This is believed to be an effect of the

accelerometers and gyroscopes being scaled with a little error and that the estimator continuously is correcting

the acceleration and gyro bias. When using this low of a sampling frequency the update will be unreliable,

since the movements of the quadrotor are fast compared to the sampling frequency.

30 32 34 36 38 40 42 44 46 48 50

−0.6

−0.4

−0.2

0

0.2

0.4

Time [s]

x

−

p

o

s

i

t

i

o

n

[

m

]

1 Hz estimate

1 Hz measurement

Figure 11.3: Hovering with Vicon sample frequency of 1 Hz. Figure shows how estimates drifts

between samples

Also on the Figure 11.2 the accuracy of the controller using 10 Hz and 100 Hz Vicon update frequencies can be

seen to be approximately similar. From this it can be concluded that a 100 Hz update frequency is not necessary

when using an observer. What sampling frequency that would be enough is not further investigated, but a 10

Hz sampling frequency is used for the remaining ﬂight tests.

Evaluation of Controllers 67

11.2 Step Response

On Figure 11.4 are 3 step responses shown, one for each controlled axis. Both a simulation done with the

nonlinear model, and an actual measurements are shown. The x and y-axis control indicate a bandwidth of

approximately 0.2 Hz. With the LQR this can be done much faster, but an increasing feedback of the orientation

makes the orientation of the quadrotor slightly oscillate. This is not further analyzed, but it is assumed that a

better dynamics model would help the problem.

68 70 72 74 76 78

−0.5

0

0.5

x

[

m

]

28 30 32 34 36 38

−0.5

0

0.5

y

[

m

]

48 50 52 54 56 58

−2

−1.5

−1

−0.5

z

[

m

]

Time [s]

Reference

Simulation

Measurement

Figure 11.4: Simulation and measurement of step response of LQR controller

11.3 Following a Trajectory

The controllers are tested when following trajectories. The controllers tested are the PID controller, the LQR

with a constant position reference and the LQR with a constant velocity reference. All controllers have been

tuned to be general applicable, but not in particular to this test scenario. In Figure 11.5 is the ﬁrst test trajectory

shown. The reference is moved clockwise around in a continuous square of 0.75m × 0.75m with a period time

of 20 sec. The trajectories of the controllers shown is the mean of 5 complete round-trips.

All paths of the controllers can be seen to be outside the square. This is believed to an effect of the not so

tight tuning of the controller. When the controllers are turned more tight the error becomes less, but the general

ﬂight more unstable. The PID and the LQR with constant position reference model can be seen to have a similar

ﬂight pattern. They both cut the corners a bit, and this effect is caused by a short lag in time. This is illustrated

in Figure 11.6.

The LQR with the constant velocity model can be seen to follow the reference not only in position, but also in

68 11.3 Following a Trajectory

−1 −0.5 0 0.5 1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

x

−

p

o

s

i

t

i

o

n

[

m

]

y−position [m]

Reference

LQR vel model

LQR vel model simulation

LQR pos model

LQR pos model simulation

PID

Figure 11.5: Tracking a square reference in x and y-axis with different controllers

25 30 35 40 45 50 55 60

−1

−0.5

0

0.5

1

x

−

p

o

s

i

t

i

o

n

[

m

]

Time [s]

Reference

LQR vel model

LQR pos model

PID

Figure 11.6: Tracking a square reference in x and y-axis with different controllers

velocity. This however results in a overshoot at every corner when the reference suddenly changes direction.

The controller however quickly compensates.

A similar trajectory in the y/z-plane is tested as shown in Figure 11.7. Because of the much more direct control

the reference hight can be seen to be followed much closer then the y-reference.

However there is still an overshoot from all controllers. It can also be seen more easily how the quadrotor

cuts corners with the PID and LQR with the constant position model. The lower right corner is very close to be

outside Vicon range which caused the quadrotor to loose position and orientation very shortly when descending.

Evaluation of Controllers 69

−0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1

−1.6

−1.4

−1.2

−1

−0.8

−0.6

−0.4

y−position [m]

z

−

p

o

s

i

t

i

o

n

[

m

]

Reference

LQR vel model

LQR pos model

PID

Figure 11.7: Tracking a rectangular reference in z and y-axis with different controllers

11.4 Hovering in Low Height

To demonstrate how the quadrotor behaves in doing landing and take-off a short ﬂight was made in low height.

The reference was locked in the center off the room with a height of 2 cm. Figure 11.8 illustrates how the

quadrotor moved in the x/y-plane and how the quadrotor was able to hover very close the reference.

15 20 25 30

−0.06

−0.05

−0.04

−0.03

−0.02

−0.01

0

y−position [m]

z

−

p

o

s

i

t

i

o

n

[

m

]

−0.15 −0.1 −0.05 0 0.05 0.1

−0.1

−0.05

0

0.05

0.1

y−position [m]

x

−

p

o

s

i

t

i

o

n

[

m

]

Reference

Estimate

Reference

Estimate

Figure 11.8: Hovering with a hight reference of 2 cm

When the quadrotor hovers this close to the ground, it is affected by the ground effect. The ground effect is the

effect of the rotors pushing down the air faster then the air can be transfered away. This is commonly referred

to as a high pressure cushion. The cushion gives more lift to the quadrotor, but leaves it more unstable in the

x/y-plane.

11.5 Overall Evaluation

The controllers are all able to stabilize the quadrotor, but in following moving trajectories the LQR with a

constant velocity model clearly stands out being able to match the position time correlated with the reference.

70 11.5 Overall Evaluation

The overshoot on the test trajectories could, to some extent, be avoided if the test was done with trajectories

that were feasible for the quadrotor.

When the quadrotor was ﬂown with a disturbance from the ground effect the LQR seemed capable of keeping

the quadrotor stabilized.

Part IV

Epilogue

72

Table of Contents

12 Conclusion 75

12.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

Bibliography 78

74 TABLE OF CONTENTS

Chapter 12

Conclusion

In the ﬁrst part of this thesis was a general introduction to quadrotors given. How they are able to ﬂy and

navigate. The 6 DoF was described mathematically so a prediction of the behaviour could be made depending

on the input.

The mathematical model was used to estimate the state of the quadrotor, even when some sensors was failing,

and to be the basis for the model based controllers. This chapter will shortly summarize the most important

conclusion from the thesis, compare these with the original objective and discuss what future improvement can

be made.

The model derived for the X-3D quadrotor encapsulates most of the behaviours of the quadrotor, but in using

the model for the model based controllers it was found that the dynamic description was not sufﬁcient. It is

estimated that the assumption made regarding the on-board controller being fast enough to ignore the dynamics

do not hold. The transfer function including the effects of the on-board controller, the motor controller and

the aerodynamics must be modelled. Also models was derived for the on-board sensors, and these were found

satisfactory.

The on-board sensors was used for the IMU-based estimator. The result of the estimator being driven by the

IMU gave good results. The EKF turned out be an elegant way of handling sensor failures. The quadrotor is

capable of ﬂying without the position and orientation sensor for a short while (∼ 1 second) and use various

asynchrony sampling frequencies for the sensors. It was however, observed that it becomes very difﬁcult to

estimates the constantly changing on-board sensor bias with a update frequency of the position sensor as low

as 1 Hz. This resulted in an estimate that drifted much between the measurements.

Two model based controllers was derived for the quadrotor, a LQR with both a constant position and constant

velocity reference model and a robust H

∞

-controller. The LQR stabilized the quadrotor without problems. The

measurements done with the LQR controllers shows similar results both in real ﬂight and in simulations. When

using a reference model that modelled constant velocity the quadrotor was capable of following a reference

with less lag given the feed forward of the velocity of the reference. This concludes that a fast trajectory can

be followed, as long as the quadrotor and the controller allows the necessary velocity. Both controllers ﬂies

well as long as the state is kept close to the operating point. If further navigation is needed when ﬂying in

high velocity it might become necessary to change the time invariant LQR to a time varying on-line linearizing

LQR to be able to move outside the chosen operating point. If only few of the states are changed alternative

control strategies can be considered such as gain scheduling or Linear Parameter Varying (LPV) control. As

76 12.1 Future Work

the frequency of the reference increases a small prediction horizon might contribute to keeping the quadrotor

stabilized.

The H

∞

controller derived showed good results in the simulation, but failed to work in real ﬂight. It have been

reasoned that the main factor of the effect shown in real ﬂight is based in the missing poles in the model. H

∞

was in general found to be an simple way of incorporating speciﬁc types of structure or performance channels,

but the calculation is more time consuming then a quadratic minimization.

In general the quadrotor platform combined with the MTLab was found to be a good combination to try out

control strategies. Features such as stabilized autonomous landing and take-off should be no problemto perform

with the quadrotor as it is able to hover stable in a hight of 2 cm. However before actual fast trajectories can

be tested, a larger Vicon range is needed. The range at the given time is limited to 1m × 1m × 2m which is to

limited. Through test and simulation it have been shown that it is possible to obtain robust ﬂight, even when

sensors shortly fail, and be able to follow a desired fast moving trajectory with a minimum of lag in time.

12.1 Future Work

The quadrotor is an interesting platform and will most likely soon become a more commonly sight in peoples

everyday life. But before this happens some issues are still to be takes care of. First of all the X-3D quadrotor

used in this thesis was only equipped with an on-board accelerometer and a gyroscope. If a quadrotor is to be

used outside a Vicon system sensors such as magnetometers and GPS must be used. To further stabilize the

quadrotor laser range ﬁnders or sonars can be considered to ﬁnd ﬁxed distances to surrounding objects.

Especially the estimation problem becomes a challenge with ﬂying quadrotors outside the Vicon MX system.

If the quadrotor intend to base the orientation on the magnetometer it becomes subject to much magnetically

interference when ﬂying indoor or close to objects with electrical signals. This and other estimation challenges

needs further to be addressed.

When quadrotors are ﬂying freely subjects as object avoidance and fault detection also becomes increasingly

important. These as well as many other application speciﬁc areas are already now being researched for future

use.

The quadrotor has great potential as an autonomous drone in the future, the stable and relatively harmless

platform allows it to ﬂy places where people is present and do task such as video surveillance or industrial

inspection.

Bibliography

[Andersen, 2009] Andersen, P. (2009). Optimal control. internet (pdf).

[AscTec, 2009] AscTec (2009). ResearchPilot Manual. http://asctec.de/downloads/

researchpilot_manual.pdf.

[Bak, 1999] Bak, T. (1999). Spacecraft Attitude Determination - a Magnetometer Approach. PhD thesis,

Aalborg University.

[Bak, 2002] Bak, T. (2002). Lecture Notes - Modeling of Mechanical Systems. AAU, 1 edition.

[Bisgaard, 2007] Bisgaard, M. (2007). Modeling,Estimation,and Control of a Helicopter Slung Load System.

PhD thesis, Aalborg University.

[Bouabdallah, 2007] Bouabdallah, S. (2007). Design and control of quadrotors with application to autonomous

ﬂying. Master’s thesis, ÉCOLE POLYTECHNIQUE FÉDÉRALE DE LAUSANNE.

[Franklin et al., 2006] Franklin, G. F., Powell, J. D., and Emami-Naeini, A. (2006). Feedback Control of

Dynamic Systems. Prentice Hall, 5 edition.

[Friis et al., 2009] Friis, J., Nielsen, E., Andersen, R., Boending, J., Jochumsen, A., and Friis, A. (2009).

Autonomous landing on a moving platform. Technical report, Aalborg University.

[Grenwal and Andrews, 2008] Grenwal, M. S. and Andrews, A. P. (2008). Kalman Filtering, Theory and

Practice Using MATLAB. Wiley, third edition. ISBN-13: 978-0-470-17366-4.

[Gurdan et al., 2007] Gurdan, D., Stumpf, J., Achtelik, M., Klaus-Michael, Doth, Hirzinger, G., and Rus, D.

(2007). Energy-efﬁcient autonomous four-rotor ﬂying robot. IEEE.

[Hughes, 1986] Hughes, P. C. (1986). Space Attitude Dynamics. Wiley.

[MIT, 2010] MIT (2010). Uav swarm health management project. http://vertol.mit.edu.

[Newton, 1833] Newton, I. (1833). Philosophiae naturalis principia mathematica. G. Brookman, 1 edition.

[STARMAC, 2010] STARMAC (2010). http://hybrid.eecs.berkeley.edu/starmac.

[Stevens and Lewis, 2003] Stevens, B. L. and Lewis, F. L. (2003). Aircraft Control and Simulation. Wiley, 2

edition.

[Stoustrup, 2010] Stoustrup, J. (2010). Robust lecture slides. http://www.control.aau.dk/

~jakob/courses/robust/robust5.pdf.

78 BIBLIOGRAPHY

[Thrun et al., 2006] Thrun, S., Burgard, W., and Fox, D. (2006). Probabilistic Robotics. The MIT Press.

ISBN-13: 978-0-262-20162-9.

[Toeffner-Clausen et al., 2001] Toeffner-Clausen, S., Andersen, P., and Stoustrup, J. (2001). Robust control.

internet (pdf).

[Valavanis, 2007] Valavanis, K. P. (2007). Advances in Unmanned Aerial Vehicles, volume 33 of Intelligent

Systems, Control and Automation: Science and Engineering. Springer. ISBN: 978-1-4020-6113-4.

[Van Der Merwe et al., 2004] Van Der Merwe, R., Wan, E., and Julier, S. (2004). Sigma-point Kalman ﬁlters

for nonlinear estimation and sensor-fusion: Applications to integrated navigation. In Proceedings of the

AIAA Guidance, Navigation & Control Conference. Citeseer.

Part V

Appendices

80

Table of Contents

A The X-3D Interface 83

A.1 Classes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

A.2 Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

A.3 Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

B Motion Tracking Lab (MTLab) 86

B.1 Simulink Block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

C Measurements Journals 88

C.1 Polynomial Relation of u

trust

and the force F

lift

. . . . . . . . . . . . . . . . . . . . . . . . 88

C.2 Determination of Induced Inﬂow Constant . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

C.3 Model Veriﬁcation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92

D CD Contents 95

82 TABLE OF CONTENTS

Appendix A

The X-3D Interface

The X-3D quadrotor from Ascending Technologies that have been used in all practical experiments was modi-

ﬁed with the ResearchPilot ﬁrmware developed by Ascending Technologies. This enables serial communication

with the sensor board handling the on-board control of the quadrotor. This communication was used to read

some of the on-board sensors so the information could be used in the high-level control. The quadrotor was

further equipped with a X-bee serial module enabling wireless serial communication. Unfortunately Ascend-

ing Technology only supplies the ﬁrmware enabling the communication and a short description of the protocol.

They do not supply any implemented software to handle the communication on the control PC.

To be able to use the on-board sensors as part of the estimation a interface have been developed and further

implemented as a Matlab/Simulink block. The purpose of this appendix is to shortly describes the main classes

and the ﬂow of the interfaces so if others intend to modify the interface this would ease their task.

The actual communication protocol can be found in the documentation of the ResearchPilot [AscTec, 2009]

and is not discussed further in this appendix expect when it is relevant to the development of the interface.

A.1 Classes

The main classes used in this interface are the following: Message, SerialSocket and Serial. Serial

is the operating system speciﬁc way of sending and receiving bytes on a serial line. SerialSocket is a

wrapper for the serial communication, allowing complete messages to be send and received with one command.

The class Message is the container of data that is used doing transitions.

A.1.1 Message

The class Message is parent to several sub-classes as illustrated in Figure A.1. The main class includes basic

functions such as reading the data and length as well as getting the type of the class. The sub classes represent

the three kinds of transfers used from the protocol. ConfigMsg is the conﬁguration message determining

what data that will be send from the quadrotor. CommandMsg is the command send from Matlab/Simulink

to the quadrotor with control commands. DataMsg is the message from the quadrotor to Matlab/Simulink

containing the measurements of the local sensors.

84 A.1 Classes

Figure A.1: The Message class

A.1.2 SerialSocket

The SerialSocket class is the only access to the serial port, therefore this class also contains the only

instance of the Serial object. When no new messages are received the SerialSocket contains an instance

of the old message and this is returned. Newly received data is stored in a buffer containing all data from the

previous received message and forward. Before a message is returned it is tested whether the CRC matches to

make sure no data corruption has occurred.

Figure A.2: The SerialSocket class

The class diagram for the SerialSocket is illustrated in Figure A.2

A.1.3 Serial

As previously mentioned the Serial class contains all the information necessary to communicate with the

serial connection on the speciﬁc operating system. Currently the class only supports the Linux operating sys-

tem, but would be easy to implement support for others such as Windows or Mac OS X. The class diagram is

illustrated in Figure A.3.

The X-3D Interface 85

Figure A.3: The Serial class

A.2 Procedure

The procedure for communicating with the quadrotor is illustrated in the ﬂow chart in Figure A.4. Initially the

structure of the data message conﬁgured by choosing the sensors that is to be read. For the experiments done

the following sensors have been read.

• The gyroscopes (3 axis)

• The accelerometer (3 axis)

• The pressure sensor

• The manual transmitter commands

The conﬁguration is send as a ConfigMsg using the SerialSocket. This is done in the initialization of

the Simulink simulation. For every sample the control commands from the Simulink send to the quadrotor as a

CommandMsg and the sensor measurements are received as a DataMsg.

Figure A.4: The ﬂow of the X-3D interface

A.3 Evaluation

The interface have proven stable under both long and short ﬂight. Even though the bandwidth should support

the amount of data transfered, the rate of sending commands had to be slowed down to 33 Hz to avoid breaks

in the received sensor measurements. Because 33 Hz is still very fast compared to the desired ﬂight trajectory

it is estimated that this will have no affect on the ﬁnal result.

Some times the receiving rate of 100 Hz of the sensor measurements is not kept by the quadrotor and two or

three following samples end up being identical. When this was further analyzed it turned out to be minor data

errors causing the SerialSocket to discard the messages.

Appendix B

Motion Tracking Lab (MTLab)

The MTLab was developed for testing real-time ﬂight of small autonomous helicopters. Original it consists of

both the Vicon MX system and a remote controller both linked to Matlab/Simulink. For the experiments done

in this thesis however only the Vicon MX system is used in combination with the X-3D interface.

The MTLab is a 5x6 m room with 7 cameras mounted on the walls and connected through gigabit network to

the Vicon MX system. The cameras record the position of reﬂectors mounted on the object. These reﬂectors

reﬂect the infrared light send out by the cameras. In the ViconIQ software an object is deﬁned on the basis

of the position of the reﬂectors. The position and orientation of this object is then send out on the ordinary

network, when a connection is made form a client computer running Matlab/Simulink. The Vicon MX system

is currently capable of sending this information with an update rate of 100 Hz.

B.1 Simulink Block

Figure B.1: The MTLab Vicon Simulink block

The Vicon Simulink block supplied by the MTLab outputs position, the orientation in a 3-2-1 Euler rotation,

the orientation in a quaternion and the orientation in the direct cosine matrix (earth to body rotation). In this

project only the position, the Euler rotation and the quaternions will be used. In Table B.1 is the boundaries

and units listed.

Both the Euler rotation and the quaternions are calculated from the Direct Cosine Matrix in the Simulink

interface.

Motion Tracking Lab (MTLab) 87

Data symbols Unit Boundary

Position [x] m ±1.5

[y] m ±1.5

[z] m 0..2

Orientation (Euler) [φ θ ψ] rad ±π

Orientation (quaternion) [q

0

q

1

q

2

q

3

] - −1..1

Table B.1: Data units and boundaries

Appendix C

Measurements Journals

C.1 Polynomial Relation of u

trust

and the force F

lift

The objective of this measurement journal is to ﬁnd a polynomial relation from the collective input u

thrust

from the transmitter to the thrust force acting on the quadrotor in near hover ﬂight. To ﬁnd this, the quadrotor

is attached to a box that is more heavy than the quadrotor can lift. Measurements of changes in the weight are

then recorded while the input is increasing. The amount of weight loss is proportional to the force induced as

long as the acceleration is kept constant. This originates from Newtons second law.

F = m· a (C.1)

Where

F is the force [N]

m is the mass [kg]

a is the acceleration [m/s

2

]

C.1.1 Procedure

The procedure for the experiment is as follows.

1. MTLab is powered on (Vicon information must be available).

2. A control PC is started with Matlab/Simulink.

3. The X-3D quadrotor is equipped with a newly charged battery and attached to a box with a bigger mass

than the quadrotor is capable of lifting.

4. The quadrotor and box is then placed on a scale on a elevated (0.75 m) platform as shown in Figure C.1.

The rotors needs to be free from the platform to avoid any ground effect.

5. The test signal for thrust is applied while the scale is read off and recorded.

6. When the test signal is done, the motors are returning to idle and the experiment is done.

Measurements Journals 89

ON 2000 g OFF

BOX

h = 0.75m

Bin

Figure C.1: The set-up used in the thrust test

The scale used is a Rådvad Elevtronic IS075 kitchen scale measuring at a resolution of 1 gram.

C.1.2 Results

The thrust force can then be expressed as in Equation C.2

F

t

= ∆m∗ g (C.2)

Were: F

t

is the thrust force [N]

∆m is the change in mass [kg]

g is the gravitational acceleration [m/s

2

]

The results of the measurements can be seen in Table C.1.

Measurement 1 Measurement 2

Initial weight [kg] 2.0040 2.0050

RC input m [kg] ∆m [kg] m [kg] ∆m [kg]

0 1.9060 0.0980 1.9040 0.1010

500 1.8460 0.1580 1.8490 0.1560

1000 1.6950 0.3090 1.7030 0.3020

1500 1.5440 0.4600 1.5400 0.4650

2000 1.3780 0.6260 1.3800 0.6250

2500 1.2400 0.7640 1.2300 0.7750

3000 1.1100 0.8940 1.1100 0.8950

3500 1.0100 0.9940 1.0000 1.0050

4000 0.9800 1.0240 0.9700 1.0350

Table C.1: The results of the force measurement, where the initial weight is the total mass of the

quadrotor and the box. ∆m is the total mass minus the measured mass at a given input. m is the

mass measured by the scale

90 C.2 Determination of Induced Inﬂow Constant

The calculated force of each input step is shown on Figure C.2 as a continuous graph. To estimate the graph as

a polynomial the least squares problem for the following equation is minimised.

a

3

x

3

+a

2

x

2

+a

1

x +a

0

= f(x) (C.3)

Where f(x) is the result of one measurements and a

0

- a

3

is parameters to a 3rd order polynomial. Figure C.2

also shows the 3rd order polynomial that is ﬁtted to the measured graph. The polynomial can be described by

the parameters a

3

= 2.07 · 10

−10

, a

2

= −1.43 · 10

−6

, a

1

= −1.05 · 10

−3

, and a

0

= −0.90.

0 1000 2000 3000 4000

−12

−10

−8

−6

−4

−2

0

u

thrust

[ ]

F

l

i

f

t

[

N

]

Measurement

Polynomial estimation

Figure C.2: The measured results plotted together with the F

lift

C.2 Determination of Induced Inﬂow Constant

The objective of this test is to determine the constant that approximates the best mathematical description of

the vertical behaviour of the quadrotor. Doing manually ﬂight it has been observed that the quadrotor do not

continue to accelerate when a constant input (more than hover) is applied. A more precise description would

be that the quadrotor ﬁnds a constant vertical velocity with a constant input. With this observation in mind a

constant is to be found that results in a constant velocity at a constant input.

The formulation of the total force can be described as in Equation C.4 following the derivation in Section 3.4

in the main thesis.

b

F =

b

F

lift

+m

b

C

e

_

¸

_

0

0

g

_

¸

_ +I

i

_

¸

_

0

0

v

3

_

¸

_ (C.4)

The induced inﬂow constant is found doing steps on the vertical axis. Therefore the quadrotor is assumed

parallel with the horizontal plane. This eliminates forces on the x- and y-axis. When a constant velocity is

obtained the acceleration must be zero. Therefore the total force must also be zero.

0 = F

lift

+mg +I

i

v

3

(C.5)

Under these circumstances the induced inﬂow constant (I

i

) can be derived knowing the input force and the

velocity.

I

i

=

−(F

lift

+mg)

v

3

(C.6)

Measurements Journals 91

C.2.1 Procedure

Measurements of the input force and the velocity under the previous described conditions are done in the

following way:

1. MTLab is powered on

2. The control PC is started with Matlab Simulink and the x3d_interface.mdl model is opened

3. The quadrotor ﬂown near hover in the center of the room

4. From hover multiple steps on the thrust input is made resulting in the quadrotor slowly moving upward

5. All steps are logged and saved

The input force is calculated using the polynomial approximation derived in Appendix C.1.

F

lift

= f(u

thrust

) (C.7)

C.2.2 Results

A measurement of the vertical steps can be seen in Figure C.3.

10 15 20 25 30 35 40 45 50 55 60

−1

−0.5

0

0.5

1

Vertical velocity and force

V

e

r

t

i

c

a

l

f

o

r

c

e

[

N

]

10 15 20 25 30 35 40 45 50 55 60

−1

−0.5

0

0.5

1

Time [s]

V

e

l

o

c

i

t

y

[

m

/

s

]

Figure C.3: Thrust steps and vertical velocity

In total 7 measurements have been made. The input force, estimated velocity and the calculated I

i

is listed in

Table C.2.

The induced inﬂow constant used in the model is the mean of the calculated values.

¯

I

i

= −0.423 σ

2

= 0.004 (C.8)

92 C.3 Model Veriﬁcation

F

lift

[N] v

3

[

m

s

] I

i

-0.450 -1.20 -0.375

-0.439 -1.12 -0.392

-0.490 -1.15 -0.426

-0.294 -0.92 -0.320

-0.180 -0.36 -0.500

-0.210 -0.44 -0.477

-0.330 -0.70 -0.471

Table C.2: Result of measurements

C.3 Model Veriﬁcation

The model veriﬁcation is made from two measurements. One where large amplitude input signals on roll and

pitch is used and one where only small input signals are used. In recording the orientation and velocity the

developed EKF is used to minimize noise.

In both measurements the time derivatives are calculated using the Matlab command diff. The model output

is calculated using the equations listed in the end of the Chapters 3 and 4. The result of the entire ˙ q measurement

is shown in Figure C.4. And the result of the entire

b

˙ v measurement is shown in Figure C.5.

Measurements Journals 93

0 10 20 30 40 50 60 70

−0.4

−0.2

0

0.2

0.4

0.6

˙

q

0

0 10 20 30 40 50 60 70

−2

−1

0

1

2

˙

q

1

0 10 20 30 40 50 60 70

−1

−0.5

0

0.5

1

˙

q

2

0 10 20 30 40 50 60 70

−2

−1

0

1

˙

q

3

Time [s]

Measured

Non−linear model

Linear model

Figure C.4: Modelled and measured ˙ q

94 C.3 Model Veriﬁcation

10 20 30 40 50 60 70

−4

−2

0

2

4

6

˙

v

1

10 20 30 40 50 60 70

−5

0

5

˙

v

2

10 20 30 40 50 60 70

−5

0

5

˙

v

3

Time [s]

Measured

Non−linear model

Linear model

Figure C.5: Modelled and measured ˙ v

Appendix D

CD Contents

The CD attached to the report contains relevant information, Matlab scripts, videos and measurements results.

Following is a description of the folders on the CD and their content.

x3d Contains all Matlab ﬁles and measurement results used throughout the project

Report A pdf version of the report

Videos Videos of the hover, step and square test made

Pictures Pictures of the quadrotor ﬂying outdoor

Nomenclature

φ Nonlinear progression model

Φ

[1]

k

First order Taylor approximation

Θ Euler angle parameterization

A System matrix continuous time

B Input matrix continuous time

C Output matrix continuous time

D Feed forward matrix continuous time

F Total force affecting the quad rotor in body frame

H Output matrix discrete time

H

r

Reference model output matrix discrete time

K

r

Reference compensation (LQR)

K

s

Feedback matrix that minimises the performance function (LQR)

K

acc

Scaling of raw accelerometer measurements to

m

sec

2

K

gyro

Scaling of raw gyro measurement to

rad

sec

N

r

Reference compensation (H

∞

)

N

s

Feedback matrix that minimises the performance function (H

∞

)

P Position in earth frame

Q Weight matrix of the states

q Quaternion parameterization

q

0

, q

1

, q

2

, q

3

Quaternion parameterization

R Weight matrix of the inputs

u Input vector

CD Contents 97

x

r

Reference state vector

x

s

Quadrotor state vector

y Output vector

Γ Input matrix discrete time

ˆ x

+

A posterior estimate

ˆ x

−

A priori estimate

J LQR cost function

Ω

rotor

Angular velocity of rotors

Φ System matrix discrete time

φ, θ, ψ Euler angle parameterization

b

C

e

Direct cosine transformation from earth to body frame

b

F

g

Gravitational force acting on the body

b

F

i

Force generated by the induced inﬂow

b

F

lift

Force generated by the rotors

b

H

e

Transformation of rates and angles from earth to body frame

e

C

b

Direct cosine transformation from body to earth frame

e

H

b

Transformation of rates and angles from body to earth frame

g Gravitational acceleration

I

i

Induced inﬂow coefﬁcient

m Mass of quad rotor

P(x) A priori probability

P(z) Probability of z

P

k

Estimate covariance matrix

Q

k

Model covariance matrix

R

k

Sensor covariance matrix

u

ω

Vector containing S

φ

, S

θ

, S

ψ

u

φ

Roll input to the on-board controller

u

ψ

Yaw input to the on-board controller

u

θ

Pitch input to the on-board controller

u

thrust

Collective input to the on-board controller

x, y, z Position in earth frame

98

b

ω Body rates

b

v Translatory velocity in body frame

e

ω Euler rates

e

v Translatory velocity of body frame in earth frame

Control Engineering Master’s Thesis Fredrik Bajers Vej 7 Telephone +45 96 35 86 90 http://es.aau.dk

Abstract:

Title: Autonomous Control of a Miniature Quadrotor Following Fast Trajectories Project period: Fall 2009 and spring 2010 Written by: Anders Friis Sørensen Group number: 10gr939b Supervisors: Asst. Prof. Morten Bisgaard, AAU Prof. Jakob Stoustrup, AAU Asst. Prof. Pieter Abbeel, UC Berkeley Publications: 5 Pages: 78 (98 including appendix) Handend in: June 3rd , 2010

The purpose of this thesis is to investigate how autonomous robust ﬂight with a miniature quadrotor can be done while minimizing the lag in time of a fast moving reference. A general introduction to how quadrotors work and how they are controlled are given. A mathematical model is derived for the X-3D quadrotor from Ascending Technology and an Extended Kalman Filter is used with a IMU-driven model to estimate the state doing ﬂight, handle sensor fusion and handle failing sensors. A LQR using various reference models and a Robust H∞ controller are derived and tested. Good results was obtained with the LQR, but the H∞ controller was not able to stabilize the quadrotor in real ﬂight, only in simulation. It is assumed to be caused by the assumption made that the on-board controller is fast enough to ignore any dynamics caused by the controller, motor controller and aerodynamics. The derived estimator in combination with the LQR, using feed forward of the reference velocity, was able to follow a fast trajectory with a minimum of lag in time.

.

There. Anders Friis Sørensen . Finally I want to thank Jan Stumpf and Ascending Technology for their help with making the hardware work as intended. Ebbe Nielsen. All vectors and matrices is denoted with bold font to indicate that they contain multiple dimensions. I would also like to thank my supervisor Prof. Jesper Bønding. Jakob Stoustrup for his inspiration and help with understanding the more advanced control theory. Much of my work with quadrotors and rotating frames has initiated in my previous project concerning Autonomous Landing on a Moving Platform. Berkeley. Pieter Abbeel. Morten Bisgaard has been a great support through my entire Master’s degree and for that I am very thankful. I gained a more general perspective on control theory and in the ﬁeld of Reinforcement Learning.C. It contains much of the knowledge I have gained in the past two years. I would therefore like to give a special thank to my previous group consisting of Johannes Friis. Anders Jochumsen and Rasmus Foldager for their contributions. My supervisor Asst. Prof. Throughout the thesis the Harvard method has been used to indicate citations.Preface This thesis is made as the ﬁnal step towards my Master’s degree. Citations are denoted in square brackets containing the surname of the prime author and the year of publication. but a complete list can be found in the nomenclature list in the back of the thesis. with the help of Asst. All mathematical expressions are described the ﬁrst time they are used. Prof. With his help I was allowed to spend one semester at the department of Electrical Engineering and Computer Science at U.

. .1 Position and Orientation . . . . . . . . . . . . .2 Rotating and Fixed Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 The Linear Quadrotor Model 4. . . . . . . . . . . . Sensor Model 6. . . . . . . . . . . . . . . 6. . . . . . . . . . . . Veriﬁcation of the Quadrotor Model 5. . . . . . . . . . . . . . . . . 34 37 38 41 . . . . . . . . . . . 5. . . .3 Orientation and Rotations .2 Time Derivative of the Velocity . . . . . . . . . . .1 Structure of the Modelling Chapters . . . . . . . . . . . . . .4 Forces and Accelerations . . . . .Contents 1 Introductory 1. . .2 The X-3D Quadrotor Test Environment . . . . . .1 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3. . . . . . . . . . . . . 1. . . . . . . . . . . . . . . . .1 How do a Quadrotor Fly? . . . . Modelling of a Quadrotor 3. . . . . . . . . . . 6 9 9 9 10 15 15 17 18 21 22 24 26 27 27 29 31 31 31 3 . . . . . .2 The IMU-driven Estimator . . .2 Angular Velocity and Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Model Structure and Assumptions 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2. . . . . . . . .1 Focus of the thesis . . . . . . . . . . . . . . . . . . . .1 Time Derivative of the Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 3 3 I 2 Building a Model Modelling: An Introduction 2. . . . . . . . . .1 state-space Model . 5 6 II 7 State Estimation State Estimation and Sensor Fusion 7. . . . 7. . . . . . . . . . 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .5 Model Summery . . . .3 The On-board Controllers . . . . . . 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. 11. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 Future Work . . . . . . . . . . . . . . . . . . . . . . . .2 Following a Fast Trajectory . . . . . . . . . . . . . . . . . . . .2 Procedure . . . . . . . . . . . . . . . . . . . . 9. . . . . . . . . . . . . . . . . . . . .1 Hover using Various Vicon Sampling Frequencies 11. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bibliography V Appendices 79 83 83 85 85 86 86 88 88 90 92 95 A The X-3D Interface A. C. . . . . . . . . . . . . . . . . . . . . . . . .2 Step Response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Determination of Induced Inﬂow Constant . . . . . . . . . . . . . .7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .3 Model Veriﬁcation . IV Epilogue 71 75 76 78 12 Conclusion 12. . . . . . . . . . . . . . . . . . . . . . . . . . . .1 Simulink Block . . . . . . . . . . . . . . . . B Motion Tracking Lab (MTLab) B. . . . . . . . . . . . . . . 48 51 53 53 55 58 60 62 62 65 65 67 67 69 69 10 Robust H∞ Control 10. . . . . 11. . . . . . . . . . . . . . . . . . . . 11 Evaluation of Controllers 11. . . . A. . . .5 Overall Evaluation . .1 Classes . . . . . . . . . . . . . . . . . . . . . . . .3 Following a Trajectory . . . . . . . . . . .4 Fault Handling . . . . . . . . . . . . . . . . . . . . . . C. . . . . . . . . . . . . . . . . . . . . . .1 The Standard Problem . .3 Evaluation . . C Measurements Journals C. . . . . . . . . . . . 11.3 7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A. .1 The General LQR Algorithm . . . . . . . . Analysis of Estimator Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . D CD Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . .4 Hovering in Low Height . . . . . . . . . . . . . . . . . . . . . . . . .1 Polynomial Relation of utrust and the force Flif t . . . . . . . . . . . . 43 44 III 8 9 Control Algorithms Controlling a Quadrotor Linear Quadratic Control 9. . . . . . . . . . . . .3 Simulation . . . . . . . . . . . . . . . . 10. . . . . . . . . . . . . . . . . . . . . . . .2 Matrix Inequalities . . .

1: The Silverlit X-UFO A few German students visiting MIT used this frame to create a more advanced control board using fast Piezo gyros and Piezo Accelerometers. 2010] and others. Figure 1. Also outside the academic world is the interest for the quadrotor platform growing. 2010] and Simultaneous Localization and Mapping have been investigated using the quadrotor platform.Chapter 1 Introductory Autonomous controlled aerial vehicles are not a new invention. One of the ﬁrst commercializations of the quadrotor is the Silverlit X-UFO (shown on Figure 1. 2007. Autonomous ﬂight of the quadrotor has previously been investigated through projects on various universities such as MIT [MIT. 2010]. The students continued this development and in 2003 they started their own company called Ascending Technologies (AscTec). p. and in a few cases actively in combat. 2007]. that was sold as a toy from Germany. Especially more advanced manoeuvres such as obstacle avoidance [STARMAC. AscTec has since been developing quadrotor frames along with other companies such as Draganﬂyer and AR Drone. usually used for gathering information. 3] as simple and unreliable systems.1). One reason why the quadrotor is good for these applications is that the small size and simple structure makes it easy to obtain very stable ﬂight. They were ﬁrst introduced doing World War I (1917) [Valavanis. . Standford/Berkeley [STARMAC. The basic idea of the quadrotor has existed since 1907 [Bouabdallah. Throughout the history the Unmanned Aerial Vehicle (UAV) has been an invisible player in many wars. but not until recently been ﬂown autonomously. One of the more recent platforms to appear is the quadrotor.

To minimize computation time and to suppress modelling errors the EKF will be based on measurements from the Inertial Measurement Unit (IMU). In this way it is possible to read out sensor measurements and send commands over a digital interface. Both the controllers and the estimator is based upon knowledge of the dynamics of the system. 1. A robust H∞ controller and a Linear Quadratic Regulator (LQR) using a reference model. This question give rise to several challenges. 1. especially when it is combined with autonomous control.1 Focus of the thesis The main focus of this thesis can be described as the solution to the following question: Can robust ﬂight with a miniature quadrotor be done in such a way that a fast moving reference can be followed with minimum lag in time. The version used in this project is installed with the ResearchPilot ﬁrmware which enables serial communication with the quadrotor. The challenge with regard to the fast moving reference is to minimize this lag without compromising the steady-state performance of a constant reference. 2007]. Within the past year the technology has allowed microcomputers to become fast enough and sensors small enough that a full autonomous controlled quadrotor can be made to ﬁt in a lunch box and obtain more than half an hours of ﬂight time. This could be in areas where it is expensive or dangerous to send up people. that would cause a normal feedback controller to lag signiﬁcantly in time. To minimize the time lag feed forward of the velocity of the reference will be used. New companies such as Sky-Watch (DK) and Microkopter (DE) is aiming directly towards the industrial market. Robust ﬂight is deﬁned such that the quadrotor is to remain stabilized even when exposed to disturbances in the form of a reference and sensor noise. Throughout this thesis advantages for both types of control will be discussed. It takes much time to learn how to ﬂy in conﬁned spaces and not every place is accessible by manual ﬂight. A fast moving reference is deﬁned to be a reference that is moving with a velocity.Introductory 3 Recently also the industry has gained an interest in the quadrotor frame. An Extended Kalman Filter (EKF) is developed to both minimize the sensor noise and to handle failing sensors (part II).. Like any other remote controlled ﬂying vehicle a quadrotor is not easy to ﬂy. Currently the main target is companies who have the need for inspections in places that is not easily accessed.2 The X-3D Quadrotor Test Environment The quadrotor chosen for experiments is manufactured by the German company AscTec [Gurdan et al. The company supplies various models with different versions of ﬁrmware. A general understanding and a mathematical model is therefore being derived to improve the performance of both controllers and estimator (part I). Robust ﬂight. If a pair of X-bee . also includes handling failing sensors and continuing ﬂight with the limited sensibility. not even with an expert pilot. Control of a quadrotor is typically done using either simple hand-tuned feedback compensators or a more advanced model based feedback compensators. in this thesis. Quadrotors also serves as an easy access to live overhead footage or high quality still pictures. To obtain robust ﬂight with a minimum lag in time two types of controllers is developed (part III).

Additional information on the communication and the development of the interface can be found in Appendix A. yaw and thrust. In this setup. In this setup positive roll is deﬁned as a positive rotation around the x-axis and so forth. The ﬂight commands are given as values which are otherwise send from the RC-transmitter for roll. The deﬁnition of roll.2: Overview of the MTLab 1.4 1. This feature is very helpful when developing controllers to a single channel. Matlab/Simulink is used to implement the estimation of states and calculation of the control commands for the quadrotor. Command roll pitch thrust yaw Value -2048 to 2047 -2048 to 2047 0 to 4096 -2048 to 2047 Table 1. 2009]. but only as long as a command is received at least once every 100ms. Figure 1.2 The X-3D Quadrotor Test Environment communication devices are added this can be done over a wireless data link.2. If this is not the case the control is given back to RC-transmitter. Finally it also enables the possibility to send commands to the quadrotor directly form Simulink. according to the protocol. pitch and yaw commands is explained in more detail in a later chapter.1: Commands send to quadrotor The quadrotor can be controlled by the commands send. but no public available implementation for the PC exists. and which should be controlled manually. . An overview of the setup is shown in Figure 1. and loading them directly into Simulink. Ascending Technologies supplies support for the protocol on the quadrotor. In order to establish communication an interface from Matlab/Simulink to the quadrotor has therefore been developed.1. Each value is. pitch. all sensors are connected to a control PC on which is installed Matlab/Simulink.1 Control Commands Through the serial communication connector on the X-3D it is possible to communicate ﬂight commands to the quadrotor using the proper protocol [AscTec. deﬁned as in Table 1. The interface also supplies an easy way to conﬁgure which channels that should be autonomously controlled. This interface enables conﬁguration of which sensors to be read.2.

2 On-board Sensors Multiple sensors can be read from the quadrotor both in raw and in processed values. 2009]. A more in-depth description of the MTLab can be found in Appendix B. The MTLab is a room at Aalborg University that is equipped with 7 cameras recording positions of small reﬂectors mounted on various objects. . was used. called PhaseSpace. More on the different orientations will be discussed in the following chapter.2. These will be measured with an update frequency of 100 Hz. The PhaseSpace system uses active markers but is otherwise similar to the Vicon MX system and will not be discussed any further in this thesis. Part of the development of both controllers and estimator have been conducted at UC Berkeley in California.3: The X-3D quadrotor The MTLab measures the position in meters with an accuracy better then 1 mm and the orientation.2. Figure 1.Introductory 5 1.3 The Motion Tracking Lab (MTLab) For measurements of the position and orientation the MTLab is used. The reﬂectors can be seen as the silver balls mounted on the X-3D quadrotor in Figure 1. For the estimator developed in this thesis only the raw gyro and acceleration measurements will be used. An entire list can be found in the manual for the ResearchPilot ﬁrmware [AscTec. 1.3. given as either a 3-2-1 Euler rotation or a quaternion. At UC Berkeley a similar system.

Part I Building a Model .

. . . . . . . . . . . . . . . . . . . . . . . . .1 Position and Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 The Linear Quadrotor Model 4. . . . . . . .2 Rotating and Fixed Frames . . . . . . .3 Orientation and Rotations . . . . . . . . . . . . . . . . . . . . . 2. . . . . . . . . . . . . . . . . .1 Structure of the Modelling Chapters . . . . . . . Modelling of a Quadrotor 3. . . 3. . . . . . . . . . . . . . . . 2. .1 Time Derivative of the Quaternion . . . . . . 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 How do a Quadrotor Fly? . . . . . . . . . . . . . . . . . .2 Angular Velocity and Acceleration . . . . . . . . . 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Model Structure and Assumptions 3. . . . . . . . . . .1 state-space Model . . . . . . . . . . . . . .3 The On-board Controllers . . . . . 6.4 Forces and Accelerations . .2 Time Derivative of the Velocity . . . . . . . . . . . . . . . 5 6 . . . . . . . . . . . . . . . . . . . . . . 5. . . Veriﬁcation of the Quadrotor Model 5. . . . . . . . . Sensor Model 6. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .Table of Contents 2 Modelling: An Introduction 2. . . . . . . . . . . . . . . . . . . .5 Model Summery . . . . . . . . . 9 9 9 10 15 15 17 18 21 22 24 26 27 27 29 31 31 31 3 . . . . . . . . . . . . . .

8 TABLE OF CONTENTS .

That a body is rigid is deﬁned . 2. are some general concepts such as the coordinate systems deﬁned. This room is deﬁned to be the inertial frame from where the model will be described. in particular the on-board PD-controller that closes the angular rate loop and attenuates some aerodynamic effects. For this assumption to make sense the structure of the quadrotor is assumed to be rigid. Any experiments made in this thesis are done in one speciﬁc room. Later the model is linearized and put on state-space form. gyroscopes and Vicon measurements. describing any relevant inﬂuence of some input signal on an output signal. Before the actual modelling of the quadrotor initiates. The rotation of the earth is assumed not to affect the ﬂight of the quadrotor. In the chapter concerning the sensor modelling. The ﬁrst modelling chapter covers the basics a quadrotor. Throughout the chapter various details of the X-3D quadrotor are included. The sensors in focus are the accelerometers.2 Rotating and Fixed Frames When dealing with ﬂying objects it is convenient to be able to describe vectors not only in a global frame. how is it controlled and the effect of the control commands described from a pilots perspective. 2. The chapter concludes by deﬁning a continuous-time model.1 Structure of the Modelling Chapters This introduction serves to guide the reader and to give an overview of the modelling part. This will be noted and discussed to the extent it is found relevant. a stochastic model is derived of the sensors available. The earth ﬁxed inertial frame is from now referred to as the earth frame.Chapter 2 Modelling: An Introduction Any mathematical kind of model is simply an approximation of the real world. but also in a rotating local body frame. Together they form the basis for estimating the states of the quadrotor. Throughout the chapters a number of assumptions will be made. The rotating frame following the attitude of the quadrotor is denoted the body frame. In this case the model will describe how the quadrotor moves in a 6 dimensional space depending on the four control commands mentioned in the previous chapter.

When the quadrotor is located in the center of the test area it will be positioned in position 0 0 0 . When a vector is seen with respect to the earth ﬁxed frame it will either be denoted with an e in front of the vector or nothing at all.2: The X-3D quadrotor in the body frame The coordinate system is aligned such that rotor 1 is pointing in the same direction as the positive x-axis.1: The earth frame and body frame The earth ﬁxed frame is aligned with the earth with the x. And the center is positioned at the same level as the landing pads. z] . The z-axis is pointing down which leaves a normal right hand coordinate system in which to describe vectors.3 Orientation and Rotations such that the distance between any two points in the structure will always remain the same no matter how the body is positioned or oriented. In the body frame a similar right hand coordinate system is aligned with the quadrotor as shown in Figure 2. transforming vectors from the earth frame to the body frame. Figure 2. To the left the earth ﬁxed frame denoted E and to the right the body ﬁxed frame denoted B. y.10 2.3 Orientation and Rotations The orientation of the quadrotor will as previously mentioned be described as a parametrization of the transformation from the earth frame to the body frame.1 shows the two frames. but also the Euler angle parametrization will be described. Figure 2. In this thesis the quaternion parametrization is used primarily. The parametrizations used in this thesis is either a 3-2-1 Euler parametrization Θ = φ θ ψ or the quaternion parametrization q = q0 q1 q2 q3 .and y-axis drawn perpendicular and in the horizontal plane. Likewise if the vector is seen with respect to the body frame it will be denoted with a b . . The position of the quadrotor is denoted P = [x. The angular velocity of the body frame is denoted b ω and the translatory velocity b v. 2.2. This axis-alignment is typically used when dealing with ﬂying objects and is denoted North East Down (NED). The orientation of the quadrotor will be described as a parametrization of the 3 × 3 transformation matrix. Figure 2.

4) cθsψ −sθ (cφcψ + sφsθsψ) sφcθ (−sφcψ + cφsθsψ) cφcθ where c = cos and s = sin.2) (2. b −1 Ce (Θ) = b Ce (Θ) (2. but the 3-2-1 sequence is commonly used when dealing with UAVs. p.3. Because b Ce is orthonormal [Bak. includes multiple trigonometric functions. but is still non-linear.x (also called 3-2-1) right-hand rotation sequence that is required to get from earth frame into alignment with the body frame. However.1) (2.3) Ce (Θ) = = Cx (φ)Cy (θ)Cz (ψ) cθcψ (−cφsψ + sφsθsψ) (sφsψ + cφsθcψ) (2. 2002. which leaves the transformation non-linear and is subject to gimbal lock. with respect to another. The rotations are denoted as follows: • Right-hand rotation about the z-axis (positive ψ) • Right-hand rotation about the new y-axis (positive θ) • Right-hand rotation about the new x-axis (positive φ) Matrices describing the rotations about the three axises are deﬁned by [Bak. 2. 2002. The minimum required dimensionality for describing an orientation in 3 dimensions is 3. The orientation of a coordinate system can therefore be described by the z. using Euler angles.Modelling: An Introduction 11 The Euler angles are widely used. 2002. The parametrizing of the rotational matrix. 29].5) . 15] as 1 0 0 = 0 cos(φ) sin(φ) Cx (φ) 0 − sin(φ) cos(φ) cos(θ) 0 − sin(θ) = 0 Cy (θ) 1 0 sin(θ) 0 cos(θ) cos(ψ) sin(ψ) 0 = − sin(ψ) cos(ψ) 0 Cz (ψ) 0 0 1 The successive rotations and the rotational matrix from earth frame to body frame is hereby deﬁned as b (2.1 Euler Angles The Euler angle parametrization utilizes that the orientation of one cartesian coordinate system. Other sequences can be used as well. 13] the inverse transformation can be described at the transpose of b Ce . Using the quaternions leaves a close to linear kinematic equations and no singularities. p. can always be described by three successive rotations. the orientation can not be both global and non-singular with less then 4 dimensions [Bak.y. p. In the remaining sections of this chapter the Euler angle and quaternion parametrizations will be described in further detail. The quaternion parametrization only involves quadratic expressions. since they have a very clear physical interpretation and are of minimum dimensionality.

b H e is the transformation matrix from earth frame to body frame.9 it is seen that when θ reaches functions this parametrization is not linear.25]. p. The direction of the vector gives the direction of the rotation and the angle the amplitude of the rotation. It can be shown that [Stevens and Lewis. 28] ˙ Θ = H b (Θ)b ω 1 tθsφ tθcφ = 0 cφ −sφ b ω 0 sφ/cθ cφ/cθ e π 2 (2.3.3 Quaternions An alternative way of parametrizing the rotation matrix is by using quaternions.1 and 2. and Θ are Euler angles. By inverting b H e the transformation matrix from body rates to the derivative of the Euler angles are found. e H b (Θ) = b H e (Θ) −1 (2. 2003. ˙ 0 0 φ ˙ = 0 + Cx (φ) θ + Cy (θ) 0 ˙ ψ 0 0 ˙ 1 0 −sθ φ ˙ = 0 cφ sφcθ θ ˙ 0 −sφ cφcθ ψ = b b ω (2.7) ˙ H e (Θ)Θ Where Cx and Cy are presented in Equation 2. 2002. The Euler axis/angle parametrization is a unit vector (e) orthogonal to the plane of rotation and an angle (θ).3 Orientation and Rotations e Cb (Θ) (sφsψ + cφsθcψ) (−sφcψ + cφsθsψ) cφcθ (2.3. p.2. Quaternions are similar but the rotation axis and angle have been combined to a single 4 dimensional vector of unit length. the matrix will be singular.9) In Equation 2. Three of the dimensions describe the direction of the rotation and the last is used to scale the vector .2 Euler Rates ˙ The relationship between Euler angle time derivatives (Θ) and the angular velocity (b ω) can in a similar way be described with a rotational matrix [Bak.8) Where t = tan. Also given the trigonometric 2.12 The rotation from body to earth frame is given by cθcψ b = Ce (Θ) = cθsψ −sθ (−cφsψ + sφsθsψ) (cφcψ + sφsθsψ) sφcθ 2. The quaternion has its basis in the Euler axis/angle representation.6) where Θ is the vector of Euler angles 2.

3 = θ = 2 arccos(q0 ) q1.3 q1.. 2003. 31][Stevens and Lewis..Modelling: An Introduction to have length 1. 2003] where ∗ denotes the quaternion multiplication...10) The conversions between the Euler axis/angle and the quaternion might help with the understanding. u = [0 u1. p.3 ] (2. C(q) = 2 2q1. 1999. p.16 [Bak.14 [Stevens and Lewis.3 p0 (2.3 q1.13) (2.15) A requirement for a valid transform is that the transformed vector also will have a zero scalar part. A Euclidean vector written as a quaternion can be seen as a normal quaternion with zero scalar part as shown in Equation 2. The most commonly used transform is the one shown in Equation 2. p∗q Λp = Λp q p0 I − [p1.12) This deﬁnition of the quaternions ensures a unique quaternion for every value of θ in the range of ±π [Stevens and Lewis. Equation 2. q0 q 1 = = q0 + q1 i + q2 j + q3 k q2 q3 13 q (2.. q e θ θ = cos( ) + sin( ) · e 2 2 q1.3 (q1.3 ×] (2.3 ×] = q3 0 −q1 −q2 q1 0 (2.3 )I − 2q0 [q1.19) . 2003..3 )u − 2q0 (q1..18) Where [q1.16) − q1..10 illustrates different representations of the quaternion.14) Quaternions are not like Euler angles caught in a gimbal lock and the parametrization of the rotational matrix contains no trigonometric functions..17) This transformation can be seen as the parametrization of the rotational matrix C(q).. given the unit length.15.11) (2. 0 −q3 q2 [q1.... 18]. Quaternions can be multiplied together using the quaternion product operation deﬁned as in Equation 2.. This is shown in Equation 2.11 and 2..12.3 + (q0 − q1.19]. Rotations of vectors using quaternions are convenient.3 q1. p.19.3 ×] p1.3 −p1...3 u) + 2 (q0 (2.3 ×] is the cross product matrix shown in Equation 2..13 and 2.3 (2. v = q −1 uq = 0 2q1.3 × u) (2.

p. The time derivative of the quaternion q(t) can be described as in Equation 2.14 Evaluating the rotational matrix in 2. q(t + ∆t) Where Iq = [1 0 0 = q(t) ∗ [∆q(∆t) − Iq ] (2. 0 −ω1 −ω2 −ω3 0 ω3 −ω2 1 0 −ω 1 ω1 1 ˙ q = q= (2.25) = dq q(t) ∗ [∆q(∆t) − Iq ] = lim ∆t→0 dt ∆t (2.22 leaves Equation 2.21) 2. 2003.22) As previously done with Euler angle rotations the quaternions can be combined in a similar way.26) Using the deﬁnition of the quaternion multiplication from 2. Then for a small time interval ∆t the rotation can be described as in Equation 2.27) q = Ωω q 2 ω [ω×] 2 ω2 −ω3 2 0 ω1 ω3 ω2 −ω1 0 Where Ωω is the matrix describing the quaternion multiplication with the angular velocity vector.3.21.22 ˆ using small angle approximation.11.23) 0] is the unity quaternion.24) and substituting ∆q(∆t) with 2. .26 where the time indices are ignored. ˙ q = (2. ˙ q Deﬁning angular velocity as: ω ωs ˆˆ ∆t→0 ∆t lim 1 q∗ω 2 (2.14 Equation 2. 2 2 2 2 q0 + q1 − q2 − q3 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) e 2 2 2 2 Cb (q) = b Ce (q) = 2(q1 q2 − q0 q3 ) q0 − q1 + q2 − q3 2(q2 q3 + q0 q1 ) 2 2 2 2 2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) q0 − q1 − q2 + q3 (2.20) The reverse rotation from body frame to earth frame is given as Equation 2.3 Orientation and Rotations in the following rotational matrix from earth frame to body 2(q1 q2 − q0 q3 ) 2 2 2 2 q0 − q1 + q2 − q3 2(q2 q3 + q0 q1 ) 2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) 2 2 2 2 q0 − q1 − q2 + q3 (2. 31]. 2 2 2 2 q0 + q1 − q2 − q3 b Ce (q) = 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) 2. This conversion will now be the basis for a closer look at how the quaternion is changes over time.4 Quaternion Kinematics Previously the conversion from an Euler axis/angle was described in Equation 2.24.18 results frame b Ce [Stevens and Lewis. ∆q(∆t) = 1 1 ˆˆ 2 ωs (2. Therefore q(t + ∆t) can be described as a quaternion multiplication of the two quaternions.26 can be written in matrix form. Let the quaternion q(t) describe ˆ the orientation at the current time and let an inﬁnitesimal rotation be described as the unit vector s and the magnitude be given by ω .

. Figure 3. When the quadrotor is aligned with the horizontal plane and the sum of the lift generated (Flif t ) is equal to the gravitational force the quadrotor is hovering. This is to prevent the quadrotor from spinning around the z-axis as an effect of the counter torque generated by the spinning rotors. When the rotors spin in opposite direction the counter torque is equalized when all rotors rotates with equal speed. Movement in the horizontal plane is done by tilting the quadrotor in the desired direction. It should be noted that.1.1: Flif t dissolved in a z-axis component and a horizontal component The rotors spin in opposite direction in pairs as illustrated in Figure 3. in the following ﬁgures.2. The horizontal component result in an acceleration in the horizontal plane. If Flif t is increased the quadrotor will start to climb. When the rotor spins lift is generated. if Flif t is decreased the quadrotor will start to decent.1 How do a Quadrotor Fly? A quadrotor is equipped with four motors with each a rotor attached.Chapter 3 Modelling of a Quadrotor 3. the arrows indicate the direction of the rotation and the size is proportional to the speed of the rotor. When the quadrotor is tilted the direction of Flif t is no longer aligned with the earth frame z-axis but can be dissolved in a zaxis component and a horizontal component as seen in Figure 3.

Figure 3. where rotor 1 increases speed while rotor 3 decreases speed. In the following are the effect of the inputs roll pitch and yaw deﬁned. . The positive pitch is illustrated in Figure 3. This is done by increasing the speed of one of the rotors placed along the y-axis and decreasing the speed of the opposite.3: Quadrotor performing positive roll Pitch is deﬁned as the rotation around the y-axis in the body frame. while maintaining speed on the remaining rotors. which means that rotor 4 increases speed while rotor 2 decreases speed.3 illustrates the direction of positive roll.1 How do a Quadrotor Fly? 1 3 2 Figure 3.2: Quadrotor in hover Tilting of the quadrotor is done by altering the speed of the rotors.16 4 3.4. 4 3 1 2 Figure 3. while maintaining speed on the remaining rotors. Note that the white mark indicates the front of the quadrotor. Roll is deﬁned as a rotation around the x-axis in the body frame. The rotation is done by increasing the speed on one of the rotors placed along the x-axis and decreasing the thrust on the opposite rotor.

Modelling of a Quadrotor 4 17 3 1 2 Figure 3. Yaw is done by altering speed of the rotors along the x-axis compared to the speed on the rotors along the y-axis. pitch. The purpose of this illustration is to give an overview of how the model is structured and how it is derived.2 Model Structure and Assumptions The structure of the model for the quadrotor that will be derived is illustrated in Figure 3. thrust and yaw. The force generated by the rotors (Flif t ) is derived using the knowledge about the on-board controller ˙ and imperial experiments. Then an expression of the acceleration in body frame (b v) will be derived from the force generated by the lift. If the rotors on the y-axis are rotating faster than the rotors on the x-axis the body will rotate clockwise (positive yaw) as illustrated in Figure 3.4: Quadrotor performing positive pitch Yaw is deﬁned as the rotation around the z-axis in the body frame.5.5: Diagram of a quadrotor performing a positive yaw motion 3. The resulting counter torque generated by the rotors will no longer be zero and will result in a rotation around the z-axis.6. . 4 3 1 2 Figure 3. The input vector is deﬁned as u = uφ uθ uthrust uψ containing commands for roll. The model will then be linearized and converted to state-space form. The model concludes by including the ﬁnal integrations and rotations into the earth ˙ ˙ ﬁxed frame deriving the time derivative of the orientation (q) and position (P ) in the earth frame. The input serves as references to the on-board heading hold controller holding the desired angular velocity in the body frame (b ω).

7 From observations it is concluded that the doing normal ﬂight (input bandwidth ∼ 0. that both rotates and are bendable. uθ . and control. the manual [AscTec. • The on-board angular rate controller and the motor controllers are considered fast enough to neglect both the dynamics of the motors and the dynamic in changing the angular rate.3 The On-board Controllers The purpose of this section is to determine the relation from the input given to the quadrotor (uφ . ﬁltering. The effect of the bending roters is assumed to very small due to the on-board controller. 2007][Friis et al.6: Illustration of the model structure for the X-3D quadrotor In the derivation a number of assumptions are made to simplify the model. Both factors are depending on the onboard controllers. The step response can be seen in Figure 3.18 3.3. 2009] and from personal contact with the developers. 3.1 The Angular Rate Controllers The rate controllers are structured as three independent PD-controllers.8. this is close to be true. With the exeption of the roters. Both Kstick and the controller values (Kp and Kd ) can be conﬁgured when setting up the X-3D quadrotor.3 The On-board Controllers Figure 3. • The gyro effect due to the angular momentum of the rotors [Bouabdallah. The board is equipped with a 60 MHz ARM7 microprocessor and runs the control loop with an update frequency of 1 kHz. . it is assumed that external effects like wind or turbulence do not affect the quadrotor.3. Both the angular velocity controllers and the thrust controller are located on the sensor board that handles all sensor data. 2009] or by empirical observations. One for each axis. • Due to the small size of the quadrotor and the controlled environment in which it is ﬂying. • The quadrotor is assumed to be a rigid body. The knowledge of the on-board software of the X-3D quadrotor is gathered from a research paper about the development of the X-3D quadrotor [Gurdan et al. To verify this the bandwidth was estimated by measuring the rise time of the maximum roll input step. uthrust and uψ ) to angular rate b ω and the lift force Flif t of the quadrotor. 2007]. These assumptions are either based on prior models derived for quadrotors [Bouabdallah. The block diagram describing the structure of the controller can be seen in Figure 3. This is further discussed and supported in Section 3. 2007] is neglected due to the on-board controller will compensate for this.. 3. In each controller a value Kstick is used as a proportional gain that controls how aggressive the quadrotor reacts on the input.1 Hz) the quadrotor had no problem following the input references..

On Figure 3.6 8.Modelling of a Quadrotor 19 Figure 3.3.5 1 0.5 8.8: Step response of the maximum roll input The input was chosen to ﬁnd the worst case bandwidth.7 Time [s] 8.55 8.8 the input step is not a step over 1 sample.29 ≈ tr · 2π tr (3.2) The rate controller is considered fast enough for the dynamics of the controller to be neglected in this quadrotor model given that the model is to be used for normal ﬂight.1) Given Equation 3.75 8.1 the bandwidth can be estimated. the dynamics of the motor controllers.25 rad (130 s ) and causes the s quadrotor to quickly move towards the wall.07 0.8 0.29 ≈ 2. 3. motors and rotors will have the maximum dampening. Typically a natural system like this can be estimated with a second order behaviour.4 and 3.1..85 8.65 8. This is cased by the step being made o manually doing manual ﬂight.3) (3. b ωφ ωθ = = = b b ωψ −Kstickφ 2π · · uφ = Kφ uφ 2048 360 −Kstickθ 2π · · uθ = Kθ uθ 2048 360 −Kstickψ 2π · · uψ = Kψ uψ 2048 360 (3. fbw ≈ 1.7: Block diagram of the PD-controller structure 3 2. 2009] be described as in Equations 3.5. fbw ≈ 0.5 r b ω1 uφ ⋅ Kφ b ω 1 8.4) (3. but more made over several samples. The steady state angular rate can according to the manual [AscTec. This can therefore only be done in manually ﬂight where a pilot is ready to recover the quadrotor. It is estimated that with the maximum input.5 2 1.14 [Hz] (3.5) . The bandwidth of a second order system can according to [Franklin et al. 2006] be approximated as in Equation 3.8 8.9 Figure 3. The step corresponds to an angular velocity of 2.5 t 0 −0.

The total force Flif t is estimated empirically from the thrust input by measuring how the lift force change while stepping through the input.2 Thrust Force Relation The input determining thrust is proportional to the average speed of the rotors (in perfect hover) when no other input is given.7 and shown in Figure 3. but instead it is chosen to estimate the entire relation from thrust input the lift force. Part of the delay is assumed to be the communication delay in the series consisting of the transmitter.7) −10 3 −6 2 −3 2. 0 b Flif t = 0 (3.953 (3.3 The On-board Controllers However the values was found not to be very accurate and new values of Kφ . 3 b 2 1 ω1 b ω u ⋅ Kφ 0 −1 −2 −3 10 11 12 13 14 15 Time [s] 16 17 18 19 20 Figure 3. This is further discussed in Appendix A. The steady-state relation between the average speed in RPM (Ωrotor ) and the thrust input is according to the X-3D developers given as Equation 3.073 · 10 · uthrust − 1.3.20 3. Ωrotor uthrust = 1.9: Demonstration of how the angular velocity follows the input 3. quadrotor and the Vicon system.10 together with the measurements. since the quadrotor changes the orientation by in pairs changing the speed of the rotors from the average. This however can only be assumed to be correct when a full battery is used.903 . The lift generated by each rotor could be estimated by an aerodynamic model of the rotor.6) This however will only be an average. The short areas where the values keep constant is due to data corruption in the interface. The estimated relation is expressed as the 3rd order polynomial in Equation 3.6. Kθ and Kψ was determined using the system identiﬁcation toolbox in Matlab with a zero order transfer function model with a time delay. The experiment is described in more detail in Appendix C.143 · 10 · uthrust − 1. Figure 3.054 · 10 · uthrust − 0.9 shows how the angular velocity around the x-axis follows u · Kφ .1.

The phenomenon of induced inﬂow is well researched and good approximations have been made.82 s2 in Denmark. This is due to a phenomenon known as induced inﬂow.Modelling of a Quadrotor 21 0 −2 −4 Flift [N] −6 −8 −10 −12 0 1000 2000 uthrust [ ] 3000 4000 Measurement Polynomial estimation Figure 3.8.11: Illustration of the phenomenon known as induced inﬂow The phenomenon causes the quadrotor to ﬁnd an equilibrium approximately with a constant velocity. In this model . When the rotor starts to climb.10: Relation between input uthrust and Flif t 3. With a smaller force the vertical speed of the rotor becomes less and the force is once again increased. when the thrust is larger then the gravitational force. This effect induces an equilibrium depending on the vertical velocity. Figure 3. Doing normal ﬂight it has been observed that the quadrotor do not rise with a constant acceleration. the airﬂow through the rotor becomes higher and a smaller force is generated.4 Forces and Accelerations The main force apart from Flif t affecting the quadrotor is the gravitational force. Induced inﬂow is an effect affecting the force generated by a rotating rotor when it starts to move up through the air. 0 e (3. The gravitational force is always pointed down in the earth ﬁxed frame and can be described as in Equation 3. Figure 3.11 illustrates this principal.8) Fg = m · 0 g m Where g is the gravitational acceleration deﬁned to be 9.

In .5 Model Summery The previously described rotational matrices and kinematics can be used to describe the entire relations between the input commands uφ uθ uψ uthrust and the change in position P . Equation 3.16) (3. The rotational matrix was previously derived in Section 2. The constant Ii is found by imperial tests in the MTLab and found to approximately Ii = −0.15 [Hughes.10. p.11 describes the summed forces. p.423.23].3.10) The rotational matrix b Ce can be used to describe the gravitational force in the body frame.2.18) 1b e Ce F m b b ˙ v 1b F − bω × bv m 3. can be described as in Equation 3. b Fi = Ii b v3 (3.17) (3. 1e F m = = = e ˙ Cb b v + e Cb b ω × b v ˙ v + bω × bv (3. Further information of the experiment can be found in Appendix C.5 Model Summery however it is found sufﬁcient to use a constant feedback from the vertical velocity to estimate the force generated by the induced inﬂow. e v describes the translatory velocity of the body frame relative to the earth frame.13) (3. b F = b Flif t + e Fg + b Fi (3. ˙ v= 1 F m (3. Newtons second law states that acceleration is force divided by mass [Newton. 1986.12 and Equation 3. e ˙ Cb b v = e Cb (b ω × b v) (3.11) ˙ The translatory acceleration of the body frame with respect to the body frame (b v) is now derived.13 and the derivative in Equation 3. using this model. e e v ˙ v = = e e Cb b v ˙ ˙ Cb v + Cb v b e b (3.12) The velocity e v is described relative to the earth frame in Equation 3. 1833. orientation q and velocity b v. 15].15 is inserted into Equation 3.14 and the expression is transformed to body frame and simpliﬁed.9) The total force acting upon the quadrotor. 0 0 b Flif t + mb Ce 0 + Ii 0 v3 g b F = (3.15) Equation 3.14) The derivative of the direct cosine matrix can be expressed as the cross product seen in Equation 3.22 3.14.

22 this relation is summarized.20) ˙ v b ω 0 0 1b Ii b b Flif t +b Ce 0 + = 0− ω× v m m g v3 Kφ · uφ = Kθ · uθ Kψ · uψ (3.19) (3. Because the polynomial is highly nonlinear the thrust force will be used as input to the system.Modelling of a Quadrotor Equations 3. ˙ P ˙ q b 23 = = e Cb b v 1 Ωω q 2 (3. such that the polynomial will be kept out of the model and the controller.21) (3.22) Flif t is an expression for the 3rd order polynomial that maps the thrust input to the force generated by the rotors.19 to 3. .

1 to 4. Therefore a general linearization is done here with the purpose of describing the model as a linear state-space model.4.8 and 4.8) (4. Equations 4. First step will be identifying the non-linear parts and ﬁnd appropriate linearization methods and operating points.5) (4.9) 1 2 Ωω ¯ ω=ω .3) ¯ v = = ¯ ω Most of the non-linearities of the model previously described is due to states multiplied together. ˙ P ˙ q ≈ ≈ e Cb (q) b b ¯ q=q ¯ v + e Cb (q)b v = b v ¯ q + Ωω q = 1 0 2 ω1 ω2 ω3 (4. This is linearized using a 1st order Taylor approximation as illustrated in Equation 4.9.1) (4. e Cb (q) b ¯ q=q ¯ ω=ω ¯ ω=ω = b Ce (q) ¯ q=q =I (4.6) (4.3 shows the values of states in the operating point.Chapter 4 The Linear Quadrotor Model To use the model to tune a linear controller requires a strictly linear model.4) Before the actual linearization the matrices used is calculated when the operating point is inserted. ¯ q = b b 1 0 0 0 0 0 0 0 0 0 (4. a·b ≈ ≈ (¯ + a)(¯ + ˜ = a¯ + a˜ + a¯ + a˜ a ˜ b b) ¯b ¯b ˜b ˜b a¯ + a˜ + a¯ ¯b ¯b ˜b (4.7) Ωω = 0 = 0 [b ω×] Linearization of the position and orientation can be done very simple given the chosen method as shown in Equations 4. Since a quadrotor is usually required to be stabilized around hover this is chosen as the operating point.2) (4.

20 to linearize the gravitational force. Firstly is the values of the rotational matrix is inserted from Equation 2.21) v1 ˙ v2 ˙ v3 ˙ = −2q2 · g = = 2q1 · g 1 m (4.12) When the segments are put together the resulting expression is not linear but afﬁne with the bias of the gravitational force. 2(q1 q3 − q0 q2 ) 0 b (4.24) (Fz + Ii v3 ) ωφ ˙ ωθ ˙ ωψ ˙ = Kφ · uφ = Kθ · uθ = Kψ · uψ (4.14) = 2g · q1 1 m (Fz + Ii v3 ) For an overview the equations 4. −2 · q2 0 −2g · q2 1 b ˙ v ≈ (4.20) (4.11) Ce e Fg ≈ 2 · q1 g · m 1 The cross product can be approximated like the position and orientation using the 1st order Taylor. −2 · q2 b (4.The Linear Quadrotor Model 25 The acceleration is divided into multiple steps treating each segment by it self.14 are shown together in the following equations. x ˙ y ˙ z ˙ q˙0 q˙1 q˙2 q˙3 b b b = = = = = = = b b b v1 v2 v3 (4.13) 2g · q1 Flif t + 2 · q1 g · m + Ii 0 = m 1 1 (Flif t + g · m + Ii v3 ) v3 m −2g · q2 (4.18) (4.9 and 4.25) (4.15) (4. But by redeﬁning the input force as Fz = (Flif t + g · m) this afﬁne relation becomes linear.22) (4. 4.10) Ce e Fg = b Ce 0 m = 2(q2 q3 + q0 q1 ) g · m 2 2 2 2 q0 + q1 + q2 − q3 g The 1st order Taylor approximation is again used when two different states are multiplied. For the last expres2 sion it is assumed that q0 can be estimated to be 1 to avoid an afﬁne expression with a bias. b ω × bv = [b ω×]b v ≈ [b ω×] b ¯ ω=ω ¯ v + b ωb v = 0 (4.27) .16) (4.23) (4.8.26) (4.19) (4.17) 0 1 2 ωφ 1 2 ωθ 1 2 ωψ (4.

31.28) (4.26 4.32) B= 0 0 0 0 1 Kφ 2 0 0 0 0 0 0 0 0 0 0 1 Kθ 2 0 0 0 0 0 0 0 0 0 0 0 0 0 1 m 0 0 0 0 0 0 1 Kψ 2 0 0 0 (4. xs us The system matrices are described below.33) .1 state-space Model The typical way of describing a Multiple Input Multiple Output (MIMO) system is with the state-space representation.31) A= 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2g 0 0 0 0 0 0 0 0 −2g 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 Ii m (4. The state-space form consists of the following system: ˙ xs = Axs + Bus (4. This form is also required in calculating the model based controllers developed in a later chapter.29) Where xs is the state vector described in Equation 4.30) (4.1 state-space Model 4. = = P uφ q uθ v v Fz uψ (4.30 and us the input described in Equation 4.

but this is estimated not to be enough to have signiﬁcant effect. The true derivative of the velocity and quaternion is calculated from the measurements and compared with the result of the model. Here the assumption that there is no dynamics in the ˙ input angular velocity relation is not very accurate. It will be difﬁcult to see much dynamic from the modeled derivatives. Verifying the model in this way have some limitations. One where large input was applied to roll and pitch input and one with a more calm ﬂight close to the operating point. This will be the main focus in this chapter.2. To record the behaviour of the real quadrotor two test ﬂight where made. The biggest difference can be seen when q3 is examined. There is small deviations in amplitude.1 shows a part of the measurement with the result of the two models. The extra angular . Slight difference in the modelling of the angular velocity will very quickly turn the quadrotor upside down and the states depending on the translatory acceleration will be ﬂawed. Comparing a quadrotor model with a real quadrotor is not straight forward because the quadrotor is not a stable system without a controller. It is in this way a 1 step prediction. Figure 5. 5. The ﬁrst test ﬂight is the basis on which the time derivative of the quaternion is further examined and the second ﬂight is used when the time derivative of the velocity is examined. This is shown in detail on Figure 5. ˙ In this chapter the performance of the models is evaluated by modeling the time derivative velocity (b v) and ˙ time derivative quaternion (q) from a state vector from real ﬂight. To evaluate how well the two models perform it is desired to compare them with the behaviour of the real quadrotor.1 Time Derivative of the Quaternion In general both the non-linear and the linear model follows the tendencies of the system well. but since the derivatives are mostly kinematic equations without much dynamic this is accepted. A more detailed description on the test ﬂight and the full measurements can be seen in Appendix C.3. In this way the state of the model will never be allowed to run away.Chapter 5 Veriﬁcation of the Quadrotor Model In the previous two chapters both the linearized and non-linear models have been derived.

5 q1 ˙ 0 −0.28 5.5 −1 −1.5 Measured Non−linear model 1 Linear model 0.1: Sample of modelled and measured q1 ˙ velocity is most likely caused by the angular momentum being build up as the quadrotor rotates around the z-axis and the on-board controllers lag of compensating for this momentum. which indicated that the most dominating effects remain even after the linearization. The overall result is considered acceptable for use in the estimator and controllers. .5 20 25 30 Time [s] 35 40 Figure 5. The linear model is very close to the non-linear model.1 Time Derivative of the Quaternion 1.

the non-linear model seems almost to go in the opposite direction of the measurements.5 35 40 Time [s] 45 50 Figure 5. This offset can very well be contributed the battery. The effect of the battery being discharged over time can be represented as the force generated by the quadrotor slowly declines as time pass. If the total measurements found in Appendix C.5 q3 ˙ 0 −0. In this period the measurements are not considered valid and will not be further discussed.Veriﬁcation of the Quadrotor Model 29 1. . At these two areas the quadrotor was not in the range of the Vicon system.5 Measured Non−linear model 1 Linear model 0.2 Time Derivative of the Velocity Figure 5. This corresponds well with the assumption made of the force polynomial only being accurate when a fully charged battery is used. On a down pointing z-axis this will result in a general higher value of the measured acceleration compared with the model.3 is further examined the offset can be seen to be increasing as time passes which also indicates that the battery could be the cause. Even considering the offset on the acceleration in the vertical axis it is estimated that both the linear and the non linear model will be sufﬁcient to use in the estimator and controllers. This is what is indicated on Figure 5. The measured horizontal accelerations are very close to the result of the models and both follows the dynamic very well. On Figure 5.3 and 5. The ﬁrst area the quadrotor ﬂew out of range in the y-axis and the second it was ﬂown to close to the ceiling.4 close to 46 and 57 seconds in the measurement.5 −1 −1.2: Sample of modelled and measured q3 ˙ 5. The vertical acceleration however seem to have an offset and parts where the models deviates from the measurement.4.4 shows the measured acceleration in the body frame with the result of the models.

3: Sample of modelled and measured v1 ˙ 5 Measured Non−linear model Linear model v3 ˙ 0 −5 25 30 35 40 45 Time [s] 50 55 60 Figure 5.30 5.2 Time Derivative of the Velocity 5 4 3 2 v1 ˙ 1 0 −1 −2 −3 20 30 40 Time [s] 50 60 70 Measured Non−linear model Linear model Figure 5.4: Sample of modelled and measured v3 ˙ .

and the data sheets states a conversion factor to SI units. The results of the estimated conversion factors are listed in Table 6. The data received through the interface are raw values directly sampled from the on-board ADC.62 · 10−6 Variance P Variance q q0 1.Chapter 6 Sensor Model The purpose of this chapter is to present the various sensors used in the estimation and derive any scaling needed. x-axis 4. A comparison of the converted sensor values and the true values (based on Vicon measurements) are shown in Figure 6. The values are retrieved through the X-3D interface described in Appendix A.2 Angular Velocity and Acceleration For measuring angular velocity and acceleration the on-board gyroscopes and accelerometers are used.74 · 10−6 Table 6.2 for the gyroscope and accelerometer.91 · 10−7 q3 3.16 · 10−7 y-axis 3.17 · 10−6 q2 9. But given the lag of detailed information on the ADC. Further analysis of the actual gyro and the accelerometer shows that they both have a linear output. in the ResearchPilot documentation.28 · 10−9 z-axis 2.1. The values are. Being part of the MTLab it is already calibrated and scaled to use SI units.1: Variance for the Vicon system 6. 6.09 · 10−9 q1 5.1 Position and Orientation For measuring the position and orientation the Vicon system in the MTLab is used.2.1 and 6. . the conversion factors are determined using Matlab’s system identiﬁcation toolbox. stated to be withing the range of 0 and 1023. The result is listed in Table 6. To give an impression of how accurate these sensors are the position and orientation measured and the variance recorded.

This is assumed to be caused by the saturation of the sensor.1: Converted gyroscope measurement compared with Vicon measurement The conversion factor is in both sensors depending on the temperature. is close to −14 s2 . given the measurements. In this case the gravitational acceleration have been subtracted from the measurement (knowing the orientation from the Vicon system) to compare with the second derivative of the position.5 5 29 29.0144 0.3.0086 2nd axis 0.82 s2 on the z-axis when the quadrotor is close to hover. but in this setup the quadrotor is always used in the same room.5 41 41. The variances are listed in Table 6. This is due to short communication failures resulting in no new values are received from the quadrotor.5 44 Figure 6.32 1st axis Kgyro Kacc 0.5 ω2 [rad/s] 0 −5 32 5 33 34 35 36 37 38 39 ω3 [rad/s] 0 −5 40 40. The m accelerometer measures originally always the −9. Also on the lowest plot in Figure 6.0127 0.2: Sensor scales for the gyroscope and accelerometer On the graf showing the on-board sensor some vertical periods are present.5 43 43. .0077 6.2 Angular Velocity and Acceleration 3rd axis 0. Any changes in the conversion factor due to temperature are therefore ignored.2 the on-board m sensor measurement can be seen to always stay above approximately 4 s2 . with a constant temperature.0143 0. 5 ω1 [rad/sec] 0 Vicon based On−board sensor −5 28. The phenomenon is further described in Appendix A concerning the X-3D quadrotor interface. As with the Vicon system the variance of the sensors have been measured as it is needed to determine the co-variance matrix for the estimator in the next Part. This m indicates that the saturation of the sensor.5 30 30.5 42 Time [s] 42.0111 Table 6.

3: Variance for the gyro and accelerometer sensors .5 15 15.5 On−board sensor 1 6.60 · 10−3 −4 2nd axis 1.5 24 24.5 18 3 22 22.94 · 10 2.2: Converted accelerometer measurement compared with Vicon measurement Variance Gyro Accelerometer 1st axis 1.5 10 Acc [m/s2] 5 0 −5 −10 21.54 · 10−4 3.62 · 10−3 −4 3rd axis 8.5 25 Figure 6.5 17 17.5 16 16.Sensor Model 33 10 Vicon based Acc [m/s2] 5 0 −5 −10 6 10 Acc [m/s2] 5 0 −5 −10 13.54 · 10−3 Table 6.5 9 2 14 14.24 · 10 4.5 8 8.5 23 Time [s] 23.5 7 7.

Part II State Estimation .

. . . .4 Analysis of Estimator Performance . . . . . . 7. . . . . .1 The Extended Kalman Filter . . . . . . . . . . . . . .Table of Contents 7 State Estimation and Sensor Fusion 7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 38 41 43 44 . . . . . . . . . . 7. . . .2 The IMU-driven Estimator .3 Fault Handling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7. . . . . . . . . . . . . . . . . . . . . . . . . .

36 TABLE OF CONTENTS .

1.Chapter 7 State Estimation and Sensor Fusion An important step in obtaining a good ﬂight performance for the quadrotor. If perfect measurements (no noise or uncertainty) could be made for all states an estimator would not be necessary but unfortunately perfect measurements do not exist in the real world. Instead of perceiving the measurements as the actual truth. All these factors cause uncertainty in the measurements. A group of ﬁlters that are especially efﬁcient are the Bayes ﬁlters. the EKF supports nonlinear models.1). The EKF is placed in between the sensors and the controller as shown in the control scheme in Figure 7.. 2006]. Usually it is not possible to have sensors measuring all states. One might question why an estimator is necessary when measurements are available. The EKF simply linearizes the models using a ﬁrst order Taylor approximation. The type of ﬁlter used to estimate the system states for the quadrotor is an EKF. with the measurements form the sensors [Thrun et al.1) P (x) is typically referred to as the prior probability distribution. and the states that are measured are corrupted by measuring noise from the sensor. is to know where it is. z the data. As shown later the ﬁlters are very closely related. This is done using Bayes theorem (shown in Equation 7. Several tools exist dealing with estimation problems such as this. Using Bayes ﬁlters it is possible to fuse the predicted state based on the previous estimation. . and P (x|z) the posterior probability distribution. Not only does the ﬁlter propagate the state (the mean of the random variable) but it also propagates the uncertainty in form of a covariance matrix. One particular kind of Bayes ﬁlter is the Kalman ﬁlter. P (x|z) = P (z|x)P (x) P (z) (7. they are thought of as noisy samples of the actual state. This chapter describes how this estimate is made given the system model and measurements made in ﬂight. In this way the measurements become random variables whose mean are the true state. In other words to have a good estimate of the state vector. or some disturbing effects from the surrounding world. Where as the normal Kalman ﬁlter only supports linear systems and sensor models. The Kalman ﬁlter assumes that the state and measurements are random variables with a Gaussian distribution.

When a matrix is denoted with an [1] it is a 1st order linear Taylor approximation of a nonlinear function.1 The Extended Kalman Filter Figure 7. prediction and update. zk = hk (xk ) + vk . To give a short overview.1: Discrete EKF Equations The notation used in Table 7.38 7. This section will primarily describe the mathematical background of the EKF which hopefully helps the understanding of why this is such a powerful tool. . Ex.1 The Extended Kalman Filter The EKF is a very powerful and elegant tool to know when dealing with estimation.1 will be used throughout this chapter. 2008] and can be divided into three categories: Estimation model. Rk ) Prediction step ˆk ˆ k−1 x− = φk−1 (x+ ) [1] [1] − + Pk = Φk−1 Pk−1 Φk−1 + Qk−1 Update step − Kk = Pk Hk [1] − Hk Pk Hk [1] [1] [1] [1] + Rk −1 ˆk ˆk ˆk x+ = x− + Kk (zk − Hk x− ) + Pk = I − Kk Hk − Pk Table 7. That a variable is an estimate is also indicated by theˆ By this deﬁnition x− is the priori . Φk−1 ≈ Hk ≈ [1] [1] ∂φk ∂x ∂hk ∂x ˆ k−1 x=x+ ˆk x=x− wk ∼ N (0. all the essential EKF equations are listed in Table 7. Φ[1] is a linearization of φ(x). The mathematical simplicity and intuitive procedure leaves the EKF as a simple way to not only estimate the state of the system but also to merge multiple sensors and also to handle failing sensors. Qk ) vk ∼ N (0.1: Control scheme with estimator 7.1 [Grenwal and Andrews. Estimation Model xk = φk−1 (xk−1 ) + wk−1 . The indices k and k−1 indicate the current sample and the previous sample. How all of this is done in more detail will be revealed throughout the remainder of this chapter. The + and − indicate a priori or a posterior ˆk estimate respectively.

6) (7. xk wk = φk−1 (xk−1 ) + wk−1 ∼ N (0. with the information obtained from the sensors.2 and can be described by the following three steps. Qk ) (7. The priori state is updated with the sensor information (zk ) using the Kalman gain Figure 7. 39 The basic function of both the EKF and the Kalman ﬁlter is to merge information from a prediction model with the information from the sensors.2) (7.5) The Jacobian matrix needed for the on-line linearization is the ﬁrst order partial derivatives of all states as shown in Equations 7. The procedure is illustrated in Figure 7. The normal Kalman ﬁlter uses a linear transition matrix for the prediction where as the EKF supports a non-linear prediction model by doing on-line linearization using the Jacobian matrix.4) (7. known from the model.2: Procedure of the EKF and the Kalman ﬁlter 7. The current priori state (x− ) is predicted using the previously estimated state (x+ ) and the prediction model.1 Estimation model The most essential function of a Kalman ﬁlter is to merge the information.7) ˆk x=x− . The merging factor also called the Kalman gain (Kk ). zk vk = ∼ hk (xk ) + vk N (0.3) Also a sensor model is needed to relate the measurements to the state and likewise the noise model is required to be Gaussian.6 and 7. 2.1. is calculated. Since the both ﬁlters are Gaussian ﬁlters both models need to have a zero mean Gaussian noise model as shown in Equation 7. Φk−1 Hk [1] [1] ≈ ≈ ∂φk ∂x ∂hk ∂x ˆ k−1 x=x+ (7. The linearization of the system matrix is done with the previous a posteriori state and the linearization of the sensor model is done using the current a priori state. As the name reveals the purpose is to predict the next state given the previous state. Rk ) (7.7.State Estimation and Sensor Fusion estimate of the state vector at the k sample. ˆk ˆ k−1 1. Therefore an important part of the ﬁlter is the prediction model.3. 3.2 and 7.

To understand how this covariance is propagated through the model one can start with the deﬁnition of the estimated covariance being the squared residual as shown in Equation 7.3 Update The ﬁnal step to the sensor updated estimate is a balance between the model prediction and the sensor measurement. The update of the priori state to the posteriori state is done using Equation 7.2 Prediction The prediction of the state is done using the nonlinear prediction model such as Equation 7.11.11) The derivation of the Kalman gain and the following Kalman equations are left out for simplicity but they can be found in many textbooks concerning this subject [Grenwal and Andrews.1 The Extended Kalman Filter 7..13.10) 7. The error covariance update is given in Equation 7. − Pk = ˜k ˜k ˜ k−1 ˜ k−1 E[x− x− ] = Φk−1 E[x+ x+ ]Φk−1 + E[wk−1 wk−1 ] [1] [1] [1] [1] + = Φk−1 Pk−1 Φk−1 + Qk−1 (7.40 7. − Kk = Pk Hk [1]T − Hk Pk Hk [1] [1]T + Rk −1 (7.13) .8) Also the estimation covariance Pk is propagated through the model. ˆk x− − xk ˜k x− = = = ˆ k−1 Φk−1 x+ − xk ˆ k−1 Φk−1 (x+ − xk−1 ) − wk−1 ˜ k−1 Φk−1 x+ − wk−1 [1] [1] [1] ˜k The actual covariance propagation can be described by multiplying with x− and talking the estimated value on both sides. The estimation covariance indicates how reliable the estimate is. The residual can be introduced by subtracting xk from the linearized state prediction.8 describes.12 where the posteriori state is the sum of the priori state and the weighted difference between the sensor measurement and the modelled sensor measurement. ˆk x− ˆ k−1 = φk−1 (x+ ) (7. + Pk = I − Kk Hk [1] − Pk (7.1.1. 2008][Thrun et al. 2006]. The weight called the Kalman gain is calculated on the basis of the error covariance (how good the model prediction is) and the sensor covariance (how much the sensors are trusted) as shown in Equation 7. ˆk ˆk ˆk x+ = x− + Kk (zk − Hk x− ) [1] (7.9) ˜k ˆk Where x− = x− − xk is the residual between the estimate and the true state. This time however. − Pk ˜k ˜k = E[x− x− ] (7.9.12) The error covariance update is like with the prediction based on the update of the state. the linearized model is to be used for the prediction.

With this simpliﬁed model it is estimated that the prediction will be more accurate than with the previously described model that is based on the input to the quadrotor. 2007][Van Der Merwe The basic idea.17) (7. In the case of the quadrotor this leaves only the rigid body kinematic and dynamics.15) The remaining sensors are used without any preprocessing and affects the ﬁlter through the sensor state vector.19) (7.1 IMU-driven Model The model used for the IMU-driven Estimator is based on the model derived in Chapter 3. . the orientation (q). The combined non-linear continuous model is deﬁned as ft (x).3 on page ¯ ¯ 10. a and ω is the processed measurements of the acceleration and angular velocity. a u = ¯ ω[1×3] ¯ a[1×3] (7. the translatory velocity (b v). z = Pvicon[1×3] qvicon[1×4] (7. is to use the measured acceleration and angular velocity as the input to the model.State Estimation and Sensor Fusion 41 7. The input vector for the IMU-driven model includes the processed gyro ¯ measurement (ω) and the processed accelerometer measurement (¯ ).20) (7.17 to 7.2 The IMU-driven Estimator An IMU-driven estimator has previously been used with success to estimate the states of a UAV [Bisgaard. The IMU-driven model uses inertial sensors as input and not the control commands send to the quadrotor.21) ˙ v ˙ q ¯ a + ab + b Ce 0 1 ¯ 2 Ωω q ˙ ωb ˙ ab 0 0 The transformation matrices between earth and body frame b Ce and e Cb are described in Section 2.2. the gyro bias (ωb ) and the accelerometer bias (ab ).16) 7. but deviates in the case of input. The model equations are listed in Equations 7.18) (7. Given that part of the model is now changed also the state vector have to be reviewed. Previously described equations form the basis for the non-linear prediction model.21 ˙ P b = = = = = e Cb b v 0 g − bω × bv (7.14) The measurements of acceleration and angular velocity is preprocessed (see Chapter 6) with a scale conversion factor to convert to SI units. x = P[1×3] q[1×4] b v[1×3] ωb[1×3] ab[1×3] (7. When dealing with sensors it can be useful to estimate any offsets to compensate for these in the model. The sensor state vector contains the measurements of the position (Pvicon ) and orientation (qvicon ) from the Vicon system. With the IMU-driven model the errors caused by battery discharging and slightly off input scaling will have no effect. of using IMU-driven estimators. To the previously described model state is therefore appended six extra states to estimate the offset of the gyroscope and accelerometer. The state vector now contains the position (P ). but a linearization in form of a Jacobian matrix is still needed. For the IMU-driven model to be used in the EKF a few derivations are necessary.

24. . The Jacobian matrix consists of the partial derived states given the previous a posteriori state..30. Φk−1 [1] ≈ ∂φk ∂x ∂φ ˆ k−1 x=x+ ∂φ1.27) ∂Ωω ¯ ∂ω3 q ∂ b Ce ∂q2 g ∂ b Ce ∂q3 g (7.29.29) ∂[ω×] b ∂ω2 v ∂[ω×] b ∂ω3 v Each of the above stated partial derivatives have been calculated using Maple and is for convenience not listed here.k ∂x2 (7.23) . ∂ e Cb b v ∂q ∂Ωω q ¯ ∂ω ∂ b Ce g ∂q ∂[ω×]b v ∂ω = = = = ∂ e Cb b ∂q0 v ∂Ωω ¯ ∂ω1 q ∂ b Ce ∂q0 g ∂[ω×] b ∂ω1 v ∂ e Cb b ∂q1 v ∂Ωω ¯ ∂ω2 q ∂ b Ce ∂q1 g ∂ e Cb b ∂q2 v ∂ e Cb b ∂q3 v (7. When the linearization of ft (x) is written on matrix form it becomes more apparent where the actual linearization happens.22.25) I ∂ω a ˙ ¯ ∂q ∂ωb ¯ ˙ ωb ωb 0 0 0 0 0 0 0 ˙ ab ab 0 0 0 0 0 0 0 The partial derivatives can each be calculated as a vector of partial derivatives of the single states as done in Equations 7. 1. and given the slow changes of the states it is estimated that a ZOH discretization is sufﬁcient. This described mathematically looks like in Equation 7.42 7.26 to 7. but can be found in the ﬁle phi. . .2 The IMU-driven Estimator 7. . .k ∂x2 ∂φ2. z Pvicon qvicon = Hx = I 0 0 I P 0 q 0 b v (7.30) .k = ∂x1 ∂φ2.2 Derivation of the Jacobian As previously mentioned the online linearization is done using the Jacobian matrix of the system. The prediction model needed for the described EKF must be a discrete model. The remaining sensors used for the EKF already have a linear relation to the states and is given by the matrix H used in the relation in Equation 7.m on the attached CD.28) (7.2. Φk−1 [1] = I + Ts · ∂ft (x) ∂x ˆ k−1 x=x+ (7. The discretization is done using the Zero-Order-Hold (ZOH) method in a way such that the discrete prediction model can be described as in Equation 7.22) ··· · · · .24) The sample time of the EKF is chosen to be 100 Hz (Ts = 0.26) (7. .k ∂x1 (7.01). e b ˙ 0 ∂ Cb v e Cb 0 0 0 0 P P ∂q 1 1 ∂Ωω q ω ¯ ¯ 0 q 0 0 q 1 ∂Ωω q 0 ˙ ¯ 2 ∂¯ 2 Ωω 2 ∂ωb ∂[ω×]b v ω ¯ b b b v = 0 ∂ Ce g [ω×] ∂[ω×] v I b v + (7.

. Whenever the quadrotor is lost from the sight of the Vicon system the position is kept constant and the orientation is reset. To compensate for the failures a similar algorithm as Algorithm 1 is implemented to make sure the EKF ignores the Vicon sensors whenever the measurement fails.k−1 then Ri.i = 108 end if Algorithm 1 basically states that if a sensor reading is detected to be equal to the prior.1. the Vicon system relies on reﬂectors on the quadrotor to see the orientation and position using cameras.1 Flying Outside Vicon Range Another more frequently occurring fault is the Vicon sensors failing due to the limited Vicon range. In this way it is possible to determine whenever the Vicon system fails. which in this case gave a faulty result. Algorithm 2 Handling failing Vicon measurements if q == 1 0 0 8 0 then R1. Several test have been made and a satisfactory estimation performance have been obtained. . The pressure sensor was not able to be update the pressure information at the same rate as the EKF.k == zi. 7.7 = 10 end if Using this algorithm when ever the quadrotor is ﬂying out of Vicon range results in an estimation based only on the on-board sensors.. One situation were while using an additional sensor measuring the air pressure. When either the quadrotor is ﬂying with to great of an angle or the quadrotor ﬂies too close to a wall the Vicon system is not capable of delivering good measurements. As explained in a previous chapter. After each sample the sensor covariance matrix R is reset and the algorithm will have no permanent effect. An EKF uses every sample as a new and equally weighted sample. Algorithm 1 Handling of variating sampling times if zi.3. the variance for this sensor is set sufﬁciently high for the EKF to ignore the sensor reading.State Estimation and Sensor Fusion 43 Finally for the EKF is the previously determined variance of the Vicon system used as the sensor covariance matrix R and the model covariance matrix Q is determined by manually tuning the estimator. To ensure the sustainability of the EKF a small algorithm was implemented to handle variating sampling times. 7.3 Fault Handling While testing the IMU-based EKF a few situations occurred that affected the estimated too much to be acceptable.7.

The Figures 7.44 7. 3 2 1 x [m] 0 −1 −2 −3 0 5 10 15 Time [s] 3 2 1 y [m] 0 −1 −2 −3 0 5 10 15 Time [s] Quaternion measurement and estimate 0 sensor estimate −1 z [m] 20 25 30 35 sensor estimate 20 25 30 35 sensor estimate −2 −3 0 5 10 15 Time [s] 20 25 30 35 Figure 7.5 and 7.4 document one of these measurements. . Figures 7. as predicted.6 shows a closer look at the fall out at 7 sec. result in short fall outs. At approximately 4 and 7 sec the boundary of the range is reached on the z-axis and the y-axis. This do. On these ﬁgures it can be seen that the estimator follows a continuous path when the sensor fails and the path includes dynamics that enables the orientation and the position to still be close when the Vicon sensor is restored.4 Analysis of Estimator Performance 7.3 and 7.3: A test ﬂight with the position shown On these ﬁgures several occasions of Vicon failures can be found.4 Analysis of Estimator Performance To verify that the estimator performs as intended the quadrotor is ﬂown manually around both within and outside the range of the Vicon system.

1 −0.4 0.1 q1 0 −0.4 0 sensor estimate 5 10 15 Time [s] 20 25 30 35 Figure 7.2 0.8 0.2 0 0 5 10 15 Time [s] 0.4 0.1 −0.State Estimation and Sensor Fusion 45 1 0.2 −0.6 q0 0.2 0 sensor estimate 5 10 15 Time [s] 20 25 30 35 0.1 q2 0 −0.4: A test ﬂight with the quaternion shown .2 0 sensor estimate 20 25 30 35 sensor estimate 5 10 15 Time [s] 20 25 30 35 0.2 0.2 q3 0 −0.

If the quadrotor is ﬂying.5 2 y [m] 1.4 can also be seen other effects. This is because the previously discussed gyroscope bias not yet have been estimated. With the current settings it takes around 3 sec where the quadrotor should be steady on the ground. .2 7 7.5: A position sample from the test ﬂight 0. When the Vicon sensors fail around 4 sec into the test ﬂight the q1 and q2 values continues further than intended.5 8 Time [s] 8.1 sensor estimate 0 q1 −0.1 −0.5 8 Time [s] 8.5 Figure 7.4 Analysis of Estimator Performance 3 2.5 0 7 sensor estimate 7.5 1 0.7.6: A quaternion sample from the test ﬂight On Figure 7.5 9 9.46 7. the time for the bias estimate to settle is longer as indicated by Figure 7.5 9 9.5 Figure 7. Given this and the performance of other estimation tests the IMU-based estimator is considered good enough to use in combination with the controllers.

State Estimation and Sensor Fusion 47 0.1 −0.2 0 5 10 15 Time [s] 20 25 30 35 Figure 7.2 [rad/s] 0 −0.1 ωb.7: Estimate of the gyro bias on the 2nd axis .

Part III Control Algorithms .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 Evaluation of Controllers 11. . . . . .3 Simulation . . . . . . . . . . . . . . . . . .5 Overall Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .Table of Contents 8 9 Controlling a Quadrotor Linear Quadratic Control 9. . . 51 53 53 55 58 60 62 62 65 65 67 67 69 69 10 Robust H∞ Control 10. . . . . .2 Following a Fast Trajectory . . . . . . . . . .1 Hover using Various Vicon Sampling Frequencies 11. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9. . . . . . . . . . . . . . . . . . . . . . .3 Following a Trajectory . . . . . . 11. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 The General LQR Algorithm . . 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 The Standard Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11. . . . . 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Step Response . .2 Matrix Inequalities . . . . . . . . . . . 11. . .4 Hovering in Low Height . . .

50 TABLE OF CONTENTS .

Where r is the reference. only the controller K becomes a feedback matrix weighting the states in a particular way to generate the input. u is the input to the system.1: The classical feedback controller The most simple form of a controller that is able to stabilize the quadrotor and have it follow a reference is the PID controller. Another way of formulating the feedback controller is in the state-space form as illustrated in Figure 8. This information will in the next few chapters be employed when developing three different controllers.2. K(s) is the controller. The PID controller is an easy way of making the quadrotor ﬂy. The PID controller developed for the quadrotor is in fact a controller consisting of 10 PID controllers all connected in a particular way and together form controllers for the position. G(s) is the system and y is the output of the system. they will all be based on the difference between part of the state vector and some given reference describing the desired trajectory. In the previous chapters details of how the quadrotor is able to navigate depending on the input signal have been discussed. e is the error between the current state and the reference. The PID controller is fairly intuitive to understand. but the controller is not discussed further in this thesis. The tuning of PID controller is usually done manually for each controlled state.1. orientation and velocity. In other words. and this becomes difﬁcult as the complexity of the system increases.Chapter 8 Controlling a Quadrotor The controller is the ﬁnal step towards enabling autonomous ﬂight. It will be used only as a reference to evaluate the performance of the other two controllers. All the controllers will be feedback controllers. As well as how to more accurately determine the actual state of the quadrotor depending on the measurements from the sensors. but the performance is usually not as good as with model based controllers. When using a . but on a more complex MIMO system as the quadrotor the intuitive understanding becomes less transparent. Figure 8. The state-space formulation is in principle the same as the classical feedback controller. There have been developed a PID controller for the quadrotor which enables it to be stabilized. The basic idea of the feedback controller is illustrated in Figure 8.

2: The state-space feedback controller In calculating the feedback matrix many methods exist. The two methods that will be discussed further the Linear Quadratic method and the H∞ method. . Figure 8. part of the control matrix is used to feed forward the reference.52 reference.

The quadratic cost function of the LQR consists of the quadratic weights Q and R. The ﬁst step is to deﬁne a value function as in Equation 9. The LQR is a well known method to obtain a simple optimal controller. 2006]. ∞ V (x) = k=0 xk Qxk + uk Ruk (9.3) A cost function describes the immediate cost of being in state x and talking input u.4. In Section 9. a value function is a function of the state describing the total cost going from the current state and following a control policy either through some horizon or until an equilibrium with zero cost is obtained (inﬁnite horizon) [Thrun et al.4) In general. J = x Qx + u Ru (9.2 are used as the models for the quadrotor and the reference. In this case the value function describes the sum the cost function through all .. allowing the quadrotor to follow a desired trajectory.3. What makes this quadratic cost function so interesting is the simple methods to ﬁnd the minimum (in this case the optimal) value by ﬁnding the value where the derivative reaches zero.2 the general LQR algorithm is appended with a reference model.Chapter 9 Linear Quadratic Control The ﬁrst controller to be derived for the quadrotor is the discrete LQR. weighing respectively the current state and input. What makes it so interesting is the quadratic cost function shown in Equation 9.1) (9.2) Section 9. The LQR is a model based controller and through out this chapter the discrete linear models listed in Equations 9. xs (k + 1) xr (k + 1) = = Φs xs (k) + Γs u(k) Φr xr (k) (9.1 The General LQR Algorithm LQR is an interesting case of an optimal controller. 9. This is basically the essential step in how the LQR algorithms are derived.1 and 9. The ﬁnal evaluation of the controller will be done together with the other controllers in Chapter 11.1 concerns the general derivation of the LQR algorithm and can be skipped.

Including the model the expression can be simpliﬁed to only depend on the current state and input as shown in Equation 9.54 9.6 yields an expression of the value function depending only on the current state. 2009].5). . More speciﬁc it is an expression of the value function using the optimal control policy that is sought.13) (9.8) Combining 9.8 [Andersen.7 in Equation 9. u V (x) = Kx = x Px (9.1 The General LQR Algorithm samples until the desired stabilized state is reached where no input is necessary. V (x) V (x) = = x0 Qx0 + u0 Ru0 + x1 P1 x1 x Qx + u Ru + (Φx + Γu) P1 (Φx + Γu) (9.12) (9.11) To derive ﬁnal expression of the optimal value function in Equation 9.13 and the feedback matrix in Equation 9. ∞ V (x) = x0 Qx0 + u0 Ru0 + k=1 xk Qxk + uk Ruk (9. the future cost can be described by a quadratic parametrization P of the state as in Equation 9. called the optimal value function (Equation 9. The optimal parametrization P can be expressed as in Equation 9.5) The expression of value function can be expressed in the current cost and cost for all future samples as in Equation 9.14) x Qx + (Kx) R(Kx) + (Φx + ΓKx) P1 (Φx + ΓKx) Q + K RK + (Φ + ΓK) P1 (Φ + ΓK) The actual values of P and K can be found by simply iterating from a guessed P until the feedback matrix do no longer change signiﬁcantly as illustrated in Algorithm 3. ∂V (x) = 0 ∂u 2Ru + 2Γ P1 (Φx + Γu) = 0 Ru + Γ P1 Γu u = −Γ P1 Φx = − [R + Γ P1 Γ] −1 (9.7) (9. the current input and the next state. The partial derivative is calculated in Equation 9.6.9) The optimal value function can be described.6) Using the control policy described in Equation 9.7. as the smallest possible value function hence where the partial derivative with regards to the input is zero (given the quadratic structure).14.10 and the optimal input given as the structure from 9. Where x0 and u0 describes the current state and the current input.12 are combined with 9.11. as previously mentioned.8 and 9. The indices 0 and 1 denotes the current and next state/input respectively. ∞ V (x) = min u k=0 xk Qxk + uk Ruk (9. K V (x) P0 = = = − [R + Γ P1 Γ] −1 Γ P1 Φ (9.10) Γ P1 Φx (9.9.9.

is connected Additionally to the models described in Equations 9. The states chosen to control is the position (x.1.16. xs (k + 1) Φs = xr (k + 1) 0 0 Φr xs (k) Γs + u(k) xr (k) 0 (9. For this combined models a new weight matrix is derived such that the general LQR algorithm can be used to calculate matrices Ks and Kr illustrated in 9.1 also illustrates how the matrices from the LQR are connected with the quadrotor and the EKF.1: Overview of how the feedback matrices.2 is the output of the models now described.16) . derived through LQR. Figure 9. Figure 9.1 and 9.2 Following a Fast Trajectory Throughout this section the model for the quadrotor is appended with a model of a reference in a way such that an expression for the error between the reference state and the quadrotor state becomes apparent.15) The state of the reference (xr ) is appended to the system state (xs ) to make a combined prediction model as done in Equation 9. y and z) and the orientation around the z-axis (q3 ). The matrices Hs and Hr describes the part of the system state that is to follow the reference and the reference respectable. 1 0 = 0 0 0 1 0 0 0 0 1 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 Hs (9.Linear Quadratic Control Algorithm 3 Iterates towards a feedback matrix K P =Q K =0 while K − K > 10−5 do K =K −1 K = − [R + Γ P1 Γ] Γ P1 Φ P = Q + K RK + (Φ + ΓK) P (Φ + ΓK) end while 55 9. This yields the following Hs matrix. yr (k) r(k) = = Hs xs (k) Hr xr (k) Even though the quadrotor have 6 Degrees of Freedom (DoF) the 4 inputs limits the controllable DoF to 4.

19 can a new expression for the weight matrix easily be isolated as shown in Equation 9.19) = x (H Qe H)x + u Ru From Equation 9.17) The cost function can now be redeﬁned to be a quadratic weighting of the error and the input as described in Equation 9. u = Ks Kr xs = Ks xs + Kr xr xr (9.18. r(k) − r(k − 1) = r(k − 1) − r(k − 2) (9.18) Combining 9.22) . Q = H Qe H = −Hs Qe −Hs Hr Hr (9.21. Figure 9. Therefore the reference is initially modelled as constantly moving reference with a constant velocity.17 and 9. The matrix will be a combination of the matrices Kr and Ks as shown in Equation 9.1 The Reference Model The quadrotor is desired to be able to follow a trajectory and not only stay at one point. In the combined model.2: Structure of the feedback matrices with the linear models 9.1 a combined feedback matrix K can be calculated.20.21) The feedback matrices will interact with the quadrotor as previously shown on Figure 9. J = (Hx) Qe Hx + u Ru (9.2. J = e Qe e + u Ru (9. this can be described as a single matrix H as done in Equation 9. as described in Equation 9.1.56 9. e(k) = Hr xr − Hs xs = −Hs Hr xs (k) = Hx(k) xr (k) (9. but can also be described with the linear models as in Figure 9.17.20) Using the LQR algorithm described in Section 9.2.18 yields a cost function with a similar structure as the default LQR algorithm.2 Following a Fast Trajectory The error (e) is deﬁned as the difference between the output of the models.22.

25) Where the reference can be described as in Equation 9.25. xr (k) = r(k) r(k − 1) xr (k + 1) = 2I I −I xr (k) 0 (9.Linear Quadratic Control This expression can easily be rearranged to become a discrete state-space model.24 the state-space model can be described as in Equation 9.23) By deﬁning the reference state as in Equation 9. so the previous sample must be included. Because the sample rate is much faster then the changes in the reference it is chosen to ﬁlter the previous state to avoid the otherwise noisy residual. r(k) r(k + 1) = = 2r(k − 1) − r(k − 2) 2r(k) − r(k − 1) 57 (9.26) In practise the reference used must be the state of the reference (xr ). A similar model is calculated using a constant position model (xr (k) = r(k)) to compare the effect of the models.24) (9. r(k) = Hr xr = I 0 xr (9. .26.

Figure 10.1: The basic conﬁguration for the Robust H∞ controller The disturbances can be seen as the reference (wr ) and the sensor noise (wn ). H∞ control is based on minimizing a performance channel. also known as the standard problem in robust control theory. is a form that encapsulates the entire system as a linear system with two inputs and to outputs as illustrated in Figure 10.1. Calculating the Matrix Inequality describing the H∞ solution 3.Chapter 10 Robust H∞ Control The second type of controller to be derived is the robust H∞ controller. The structure of the system and the controller are illustrated in Figure 10. A solution the requires solving a LMI. In this computation of the controller. Use a Linear Matrix Inequality (LMI) solver to derive the feed forward and feedback matrices The standard 2 × 2 block form. The general input to the system is u and the measurable output is y. a particular type of solution have been selected. including a disturbance in the form of a reference and sensor noise. The method chosen to derive the controller can be described in three steps: 1. in this case. The steps to do this involves setting up one LMI containing all requirements and solving it . Wr (s) has a low-pass characteristic and Wn (s) has a high-pass characteristic. The channel from w to z is called the performance channel. The frequency content is described using the wights Wr (s) and Wn (s). The challenge of deriving the standard problem is to incorporate the disturbances and to deﬁne the performance channel. Convert setup to standard 2 × 2 block form 2. and the purpose of deriving a controller is to minimize this channel.2.

2: The standard problem using a LMI solver.Robust H∞ Control 59 Figure 10. These last two steps are very general and will only be described shortly. .

xn the high-pass weighted sensor noise state. an output matrix and purple a feed forward matrix.1 and can be described as the output of the system subtracted from the ﬁltered reference (Cr xr − Cs xs ) as shown in . red indicates a feedback matrix. input and output matrices. Figure 10.2) wn xn With this deﬁnition the matrices A.3. xs wr x = xr w = (10.1 The Standard Problem The standard problem is a way to formulate a more complex system on a standardized form with only a few input and output. green. xr the low-pass weighted reference state. The weights shown on Figure 10. wr the reference and wn the sensor noise.60 10. When it is written on state-space form it can be described with 8 matrices with the correlation shown in Equation 10.1. B1 and C1 .3: The structure of the standard problem The normal states space formulation can be recognized as the matrices A. The top row is the state propagation and the remaining are the correlation of the general input/output and the performance channel.1 The Standard Problem 10. The colors symbolizes the function of the matrices.1 are implemented as state-space ﬁlters with each their own matrices propagation. where xs is the states of the system. B1 and B2 can be redeﬁned as in Equation 10. ˙ x A B1 B2 x (10.1) z = C1 D11 D12 w u C2 D21 0 y The state-space formulation is shown on a block diagram form in Figure 10.3 As 0 0 0 0 Bs A = 0 Ar 0 B1 = Br 0 B2 = 0 0 0 An 0 Bn 0 (10.3) The performance channel is based on the residual of the state and the reference as shown in Figure 10. blue an input matrix. To include all variables are the input redeﬁned as the following.

9) The weight for the reference consists of several low-pass ﬁlters since every state of the reference is weighted. A = −a B=k C=1 D=0 (10.8) For the high-pass case with both a pole and a zero the model can be described with the state-space model in Equation 10. C2 = Cs 0 Cn D21 = 0 Dn (10.10) Br = Cr = 0 0 a3 0 0 0 k3 0 0 0 1 0 0 0 0 a4 0 0 0 k4 0 0 0 1 . Wr = y(s) k = u(s) s+a Wn = y(s) k(s + b) = u(s) s+a (10.4. but only describing the outputs that is used as a reference as illustrated in Equation 10.5) The out is described at the measurable system output added with the high pass ﬁltered sensor noise (Cs xs + Cn xs + Dn wn ) as described in Equation 10.. b a zero and k a scalar gain.1 Weights as ﬁlters The weights are as previously mentioned implemented as ﬁlters. 2001].6.5. This is done by adding a factor .10.9. The state-space weight for the reference is listed in Equation 10. A = −a B =b−a C =1 D =k (10. The matrix Cs is a sub-matrix of Cs . a1 0 0 0 k1 0 0 0 1 0 0 0 0 a 0 k 0 1 0 0 0 0 0 0 2 2 Ar = − (10. The types of ﬁlters chosen are to be ﬁrst order high-pass and low-pass ﬁlters with the structure shown in Equation 10.4) Cs = 0 0 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 A requirement of the general H∞ -solution is for the D12 matrix to have full column rank [Toeffner-Clausen et al. 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 (10. C1 = −Cs 0 Cr 0 0 0 D11 = 0 0 0 0 D12 = 0 (10.1.6) 10.7 where a denotes a pole.Robust H∞ Control 61 Equation 10. This also serves as a precaution to limit the input since the u will be part of the performance channel.7) The transfer functions are converted to state-space form like in the following example: y(s) = u(s) y(s)(s + a) = y+a·y ˙ y ˙ k s+a k · u(s) = k·u = −a · y + k · u When x is deﬁned to x = y the following standard state-space matrices can be used.

13 to 10.2 Hz.14) (10.9.16. according to [Stoustrup. This H∞ controller have been tested on the quadrotor in real ﬂight with an adverse result.11) This inequality can be transfered to robust control theory as done in [Stoustrup.12) 0 <0 F12 −R1 F13 0 −I Where the matrices F11 . F13 and R1 is described as in Equations 10. The feedback matrix was compared with the one of the LQR and the feedback matrix of the H∞ controller was signiﬁcantly higher. Since a step in the yaw reference would bring the quadrotor permanently out of the operating point this is disregarded.4 is the simulation of a step on each axis shown.62 10. with a performance on the x and y-axis that indicates a bandwidth of approximately 0.17 F = W Y −1 (10. satisﬁed if and only if there exists A∗ X + XA + C ∗ C − (XB + C ∗ D)(D ∗ D − γ 2 I)−1 (B ∗ X + D ∗ C) < 0 (10. This controller is on purpose tuned this slow to have as low a feedback matrix as possible. R1 F11 F12 F13 = I − D 11 D 11 = Y A + AY + W B2 + B2 W = B1 + Y C1 D11 + W D12 D11 = Y C1 + W D12 (10.17) 10.3 Simulation To verify that the controller is able to stabilize the quadrotor the controller is tested with the nonlinear model. The controller was not capable of stabilizing the quadrotor in hover.16) One feedback matrix (including reference and noise feedback) can be calculated as in Equation 10. 2010] and yield the following LMI F11 F12 F13 (10.15) (10. 10. F12 . . 2010].2 Matrix Inequalities Similar do the state-space noise wight consist of diagonal matrices containing the values from Equation 10.13) (10. The feed forward and feedback controller matrices was investigated to ﬁnd the cause of this result.2 Matrix Inequalities Given a standard state-space system G(s) given as ˙ x z the nominal H∞ condition G(s) a vector X = X ∗ > 0 such that ∞ = Ax + Bw = Cx + Dw < γ is. In Figure 10. All simulations show satisfying results.

Robust H∞ Control

63

0.5 x [m] 0 −0.5 68 0.5 y [m] 0 −0.5 28 −2 z [m] −1.5 −1 −0.5 48 50 52 Time [s] 54 56 Reference Simulation 58 30 32 34 36 38 70 72 74 76 78

Figure 10.4: Simulation of step response of H∞ controller

1 0.8 0.6 Imaginary Axis 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 −1000 −900 −800 −700 −600 −500 −400 Real Axis −300 −200 −100 0 100

Figure 10.5: Pole/zero map of the closed loop transfer function from reference to position on the y-axis

If the closed loop system poles are analysed an indication for this behaviour can be found. In Figure 10.5 is shown a pole/zero map for the closed loop transfer function from reference to position on the y-axis. The pole/zero map indicates that the controller dampens much of the effect of the original poles in zero (from the integrators), and only one pole remains to have a dominating effect. The actual movement of the poles, depending on the parameters that can be adjusted for the H∞ controller, is very difﬁcult to predict. But in the case that all the other poles remains dampened by the controller and the pole original is moved from zero and is following the normal root-locus behaviour the increasing feedback will cause the pole to further increase the bandwidth. If the normal root-locus is followed by all poles and the zeros are not further moved (not very likely), this system is not able to become unstable as further feedback gain is increased.

64

10.3 Simulation

The reason why this controller becomes unstable in the real system, is most likely that some dynamics are absent in the current model. One place where a dampening have been identiﬁed, but not modelled was in Section 3.3 on page 18 where the on-board controller was discussed. The choice of not modelling the dynamics of the controller, motor controller and aerodynamic effect can very well be the cause of the H∞ controller not being able to stabilize the real quadrotor. The performance of the H∞ controller is not investigated further, and only the LQR and PID will be evaluated doing real ﬂight in the following chapter.

Chapter

11

Evaluation of Controllers

The developed controllers are tested under various conditions in real ﬂight to evaluate the performance and to identify areas that may be improved. Firstly in Section 11.1 are the controllers tested in hover using various sampling frequencies for the Vicon measurements. This is done to partly evaluate the controller in hover, but also to determine how well the estimator and the controller collaborate. Section 11.2 demonstrates the step response of the closed loop system. In Section 11.3 are the controllers tested with various types of trajectories. In this section it is illustrated how the various structure of the controller affects the performance. In Section 11.4 shortly discusses how the quadrotor would perform doing take-off and landing operations. A short demonstration of hovering in low hight shows how well the quadrotor can be controlled. The main tests are documented on video, and can be found on the CD in the folder Videos.

11.1

Hover using Various Vicon Sampling Frequencies

To evaluate the performance of the estimator combined with the controllers and to estimate whether a 100 Hz sampling frequency on the Vicon system is a requirement, three different sampling frequencies was tested doing hover with the developed LQR with a constant position reference model. The reference was locked to the position 0 0 −1 doing the entire ﬂight. Figure 11.1 shows the x-position doing an autonomous ﬂight of 20 seconds.

0.4 0.2 x−position [m] 0 −0.2 −0.4 −0.6 30 35 40 Time [s] 45 1 Hz 10 Hz 100 Hz 50

Figure 11.1: Hovering with Vicon sample frequencies of 1 Hz, 10 Hz and 100 Hz

4 0. When using this low of a sampling frequency the update will be unreliable. since the movements of the quadrotor are fast compared to the sampling frequency. the estimate is shown to be far from the true state.2 −0.2 x−position [m] 0 −0.4 −0.2 y−position [m] 0.4 −0. From this it can be concluded that a 100 Hz update frequency is not necessary when using an observer.3 0.2: Hovering with Vicon sample frequencies of 1 Hz.5 −0. and this is believed to be the cause for this inaccurate ﬂight when only using 1 Hz Vicon measurements. but a 10 Hz sampling frequency is used for the remaining ﬂight tests.4 −0.6 0.3 −0.8 −0. where the furthest is more the 20 cm. Figure shows how estimates drifts between samples Also on the Figure 11. In almost every sample.3: Hovering with Vicon sample frequency of 1 Hz. in other words the state that is used by the controller. where the ﬁgure is shown as if it was seen from the ceiling of the MTLab. 0.2 the accuracy of the controller using 10 Hz and 100 Hz Vicon update frequencies can be seen to be approximately similar. This is believed to be an effect of the accelerometers and gyroscopes being scaled with a little error and that the estimator continuously is correcting the acceleration and gyro bias.2 0 0. .1 −0.1 Hover using Various Vicon Sampling Frequencies The x-position shown is the result of the estimator.4 0. Figure 11. This difference is also illustrated on Figure 11.4 0. 10 Hz and 100 Hz The very low sampling frequency allows the estimator to drift from the actual state. 0.2 −0. From the ﬁgure a clear difference can be seen in using 1 Hz and 10 Hz sampling frequency.1 0 −0.6 −0.2.66 11.3 shows the estimate and measurements time correlated to better illustrate this belief. What sampling frequency that would be enough is not further investigated.6 30 32 34 36 38 40 Time [s] 42 1 Hz estimate 1 Hz measurement 44 46 48 50 Figure 11.8 1 Hz 10 Hz 100 Hz Figure 11.2 x−position [m] 0.

Evaluation of Controllers

67

11.2

Step Response

On Figure 11.4 are 3 step responses shown, one for each controlled axis. Both a simulation done with the nonlinear model, and an actual measurements are shown. The x and y-axis control indicate a bandwidth of approximately 0.2 Hz. With the LQR this can be done much faster, but an increasing feedback of the orientation makes the orientation of the quadrotor slightly oscillate. This is not further analyzed, but it is assumed that a better dynamics model would help the problem.

0.5 x [m] 0 −0.5 68 0.5 y [m] 0 −0.5 28 −2 z [m] −1.5 −1 −0.5 48 50 52 Time [s] 54 Reference Simulation Measurement 56 58 30 32 34 36 38 70 72 74 76 78

Figure 11.4: Simulation and measurement of step response of LQR controller

11.3

Following a Trajectory

The controllers are tested when following trajectories. The controllers tested are the PID controller, the LQR with a constant position reference and the LQR with a constant velocity reference. All controllers have been tuned to be general applicable, but not in particular to this test scenario. In Figure 11.5 is the ﬁrst test trajectory shown. The reference is moved clockwise around in a continuous square of 0.75m × 0.75m with a period time of 20 sec. The trajectories of the controllers shown is the mean of 5 complete round-trips. All paths of the controllers can be seen to be outside the square. This is believed to an effect of the not so tight tuning of the controller. When the controllers are turned more tight the error becomes less, but the general ﬂight more unstable. The PID and the LQR with constant position reference model can be seen to have a similar ﬂight pattern. They both cut the corners a bit, and this effect is caused by a short lag in time. This is illustrated in Figure 11.6. The LQR with the constant velocity model can be seen to follow the reference not only in position, but also in

68

11.3 Following a Trajectory

0.8 0.6 0.4 Reference x−position [m] 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 −0.5 0 y−position [m] 0.5 1 LQR vel model LQR vel model simulation LQR pos model LQR pos model simulation PID

**Figure 11.5: Tracking a square reference in x and y-axis with different controllers
**

1

0.5 x−position [m]

0 Reference −0.5 LQR vel model LQR pos model PID −1 25 30 35 40 Time [s] 45 50 55 60

Figure 11.6: Tracking a square reference in x and y-axis with different controllers

velocity. This however results in a overshoot at every corner when the reference suddenly changes direction. The controller however quickly compensates. A similar trajectory in the y/z-plane is tested as shown in Figure 11.7. Because of the much more direct control the reference hight can be seen to be followed much closer then the y-reference. However there is still an overshoot from all controllers. It can also be seen more easily how the quadrotor cuts corners with the PID and LQR with the constant position model. The lower right corner is very close to be outside Vicon range which caused the quadrotor to loose position and orientation very shortly when descending.

Evaluation of Controllers

69

−1.6 −1.4 −1.2 −1 −0.8 −0.6 −0.4

z−position [m]

Reference LQR vel model LQR pos model PID

−0.8

−0.6

−0.4

−0.2

0 0.2 y−position [m]

0.4

0.6

0.8

1

Figure 11.7: Tracking a rectangular reference in z and y-axis with different controllers

11.4

Hovering in Low Height

To demonstrate how the quadrotor behaves in doing landing and take-off a short ﬂight was made in low height. The reference was locked in the center off the room with a height of 2 cm. Figure 11.8 illustrates how the quadrotor moved in the x/y-plane and how the quadrotor was able to hover very close the reference.

−0.06 Reference −0.05 z−position [m] −0.04 −0.03 −0.02 −0.01 0 15 −0.1 Estimate 0.05 x−position [m] 0.1 Reference Estimate

0

−0.05

20 y−position [m]

25

30

−0.15

−0.1

−0.05 0 y−position [m]

0.05

0.1

Figure 11.8: Hovering with a hight reference of 2 cm

When the quadrotor hovers this close to the ground, it is affected by the ground effect. The ground effect is the effect of the rotors pushing down the air faster then the air can be transfered away. This is commonly referred to as a high pressure cushion. The cushion gives more lift to the quadrotor, but leaves it more unstable in the x/y-plane.

11.5

Overall Evaluation

The controllers are all able to stabilize the quadrotor, but in following moving trajectories the LQR with a constant velocity model clearly stands out being able to match the position time correlated with the reference.

be avoided if the test was done with trajectories that were feasible for the quadrotor.70 11. When the quadrotor was ﬂown with a disturbance from the ground effect the LQR seemed capable of keeping the quadrotor stabilized. to some extent.5 Overall Evaluation The overshoot on the test trajectories could. .

Part IV Epilogue .

72 .

. . . . . . . . . . . . . . . . . .Table of Contents 12 Conclusion 12. . . . . Bibliography 75 76 78 . . . . . . .1 Future Work . . . . . . . . . . . . . . . .

74 TABLE OF CONTENTS .

The on-board sensors was used for the IMU-based estimator. but in using the model for the model based controllers it was found that the dynamic description was not sufﬁcient. and to be the basis for the model based controllers. As . Also models was derived for the on-board sensors. Two model based controllers was derived for the quadrotor. even when some sensors was failing. the motor controller and the aerodynamics must be modelled. This chapter will shortly summarize the most important conclusion from the thesis. and these were found satisfactory. a LQR with both a constant position and constant velocity reference model and a robust H∞ -controller. When using a reference model that modelled constant velocity the quadrotor was capable of following a reference with less lag given the feed forward of the velocity of the reference. This resulted in an estimate that drifted much between the measurements. The 6 DoF was described mathematically so a prediction of the behaviour could be made depending on the input. The quadrotor is capable of ﬂying without the position and orientation sensor for a short while (∼ 1 second) and use various asynchrony sampling frequencies for the sensors. Both controllers ﬂies well as long as the state is kept close to the operating point. The model derived for the X-3D quadrotor encapsulates most of the behaviours of the quadrotor. as long as the quadrotor and the controller allows the necessary velocity. If only few of the states are changed alternative control strategies can be considered such as gain scheduling or Linear Parameter Varying (LPV) control. The measurements done with the LQR controllers shows similar results both in real ﬂight and in simulations. The mathematical model was used to estimate the state of the quadrotor. The result of the estimator being driven by the IMU gave good results. The LQR stabilized the quadrotor without problems. If further navigation is needed when ﬂying in high velocity it might become necessary to change the time invariant LQR to a time varying on-line linearizing LQR to be able to move outside the chosen operating point. It was however. observed that it becomes very difﬁcult to estimates the constantly changing on-board sensor bias with a update frequency of the position sensor as low as 1 Hz. The EKF turned out be an elegant way of handling sensor failures. The transfer function including the effects of the on-board controller. It is estimated that the assumption made regarding the on-board controller being fast enough to ignore the dynamics do not hold. This concludes that a fast trajectory can be followed.Chapter 12 Conclusion In the ﬁrst part of this thesis was a general introduction to quadrotors given. compare these with the original objective and discuss what future improvement can be made. How they are able to ﬂy and navigate.

When quadrotors are ﬂying freely subjects as object avoidance and fault detection also becomes increasingly important. To further stabilize the quadrotor laser range ﬁnders or sonars can be considered to ﬁnd ﬁxed distances to surrounding objects. 12. The H∞ controller derived showed good results in the simulation.76 12. The range at the given time is limited to 1m × 1m × 2m which is to limited. This and other estimation challenges needs further to be addressed. It have been reasoned that the main factor of the effect shown in real ﬂight is based in the missing poles in the model. H∞ was in general found to be an simple way of incorporating speciﬁc types of structure or performance channels. and be able to follow a desired fast moving trajectory with a minimum of lag in time. .1 Future Work the frequency of the reference increases a small prediction horizon might contribute to keeping the quadrotor stabilized. Especially the estimation problem becomes a challenge with ﬂying quadrotors outside the Vicon MX system. even when sensors shortly fail. If the quadrotor intend to base the orientation on the magnetometer it becomes subject to much magnetically interference when ﬂying indoor or close to objects with electrical signals. the stable and relatively harmless platform allows it to ﬂy places where people is present and do task such as video surveillance or industrial inspection. but failed to work in real ﬂight. but the calculation is more time consuming then a quadratic minimization.1 Future Work The quadrotor is an interesting platform and will most likely soon become a more commonly sight in peoples everyday life. The quadrotor has great potential as an autonomous drone in the future. In general the quadrotor platform combined with the MTLab was found to be a good combination to try out control strategies. But before this happens some issues are still to be takes care of. Features such as stabilized autonomous landing and take-off should be no problem to perform with the quadrotor as it is able to hover stable in a hight of 2 cm. a larger Vicon range is needed. However before actual fast trajectories can be tested. First of all the X-3D quadrotor used in this thesis was only equipped with an on-board accelerometer and a gyroscope. If a quadrotor is to be used outside a Vicon system sensors such as magnetometers and GPS must be used. Through test and simulation it have been shown that it is possible to obtain robust ﬂight. These as well as many other application speciﬁc areas are already now being researched for future use.

Optimal control..pdf. [Bisgaard. F.berkeley. ~jakob/courses/robust/robust5. J.. Design and control of quadrotors with application to autonomous ﬂying.. [Grenwal and Andrews. J. Prentice Hall. [Hughes. http://hybrid.de/downloads/ [Bak. Andersen. Boending. Jochumsen.Bibliography [Andersen.a Magnetometer Approach. and Lewis. M.. Hirzinger.. [AscTec. S. Uav swarm health management project. [Bouabdallah. (2003). 2010] Stoustrup.. E.pdf. Lecture Notes . 1986] Hughes. ResearchPilot Manual.. [Franklin et al. A. 2007] Bisgaard. [MIT. (2009).. Aalborg University. 2002] Bak. 2003] Stevens. IEEE. 2010] STARMAC (2010). 1 edition..edu/starmac. Feedback Control of Dynamic Systems. 5 edition.control. F. J. C. Aalborg University. G. D. P.Modeling of Mechanical Systems. T. 2006] Franklin. 2007] Gurdan. (2010). Klaus-Michael. J. (1986).. Space Attitude Dynamics. PhD thesis. [Friis et al. ISBN-13: 978-0-470-17366-4. and Emami-Naeini. D. PhD thesis. I. A. Spacecraft Attitude Determination . (2006). [Stevens and Lewis. G. [Gurdan et al.aau. ÉCOLE POLYTECHNIQUE FÉDÉRALE DE LAUSANNE. 1833] Newton. (2007). [Newton. P. G. Stumpf. T. Powell. [STARMAC. Autonomous landing on a moving platform.. Modeling. 2010] MIT (2010). Wiley.Estimation. and Rus. (2007). Doth. (1999). M. Robust lecture slides. Master’s thesis. S. Brookman. Philosophiae naturalis principia mathematica. (2002). 1999] Bak. third edition. [Bak. Aircraft Control and Simulation. Energy-efﬁcient autonomous four-rotor ﬂying robot. D. 2 edition. internet (pdf).eecs. 2008] Grenwal. 2009] AscTec (2009). Technical report. Kalman Filtering. M.dk/ . L. P.and Control of a Helicopter Slung Load System. (1833). http://asctec. Nielsen. Aalborg University. 2007] Bouabdallah.mit. researchpilot_manual. Theory and Practice Using MATLAB. B. A. J. and Friis. (2009). 2009] Friis. (2007). 2009] Andersen.. (2008). Achtelik. and Andrews. http://vertol.. Wiley. 1 edition.. R. AAU.edu. A. [Stoustrup. http://www. L. Wiley.

ISBN-13: 978-0-262-20162-9.. S. Probabilistic Robotics. and Fox.78 BIBLIOGRAPHY [Thrun et al.. (2007). D. and Stoustrup. E. volume 33 of Intelligent Systems. and Julier. [Valavanis. P. 2006] Thrun. K. (2001). Control and Automation: Science and Engineering. [Toeffner-Clausen et al. Sigma-point Kalman ﬁlters for nonlinear estimation and sensor-fusion: Applications to integrated navigation. [Van Der Merwe et al. Citeseer. S. Robust control.. In Proceedings of the AIAA Guidance. S. Burgard. .. 2004] Van Der Merwe. J. ISBN: 978-1-4020-6113-4. The MIT Press. 2001] Toeffner-Clausen. W.. Springer. P. (2006)... Andersen. 2007] Valavanis. (2004). Navigation & Control Conference. R.. Advances in Unmanned Aerial Vehicles. Wan. internet (pdf)..

Part V Appendices .

80 .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . C Measurements Journals C. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . D CD Contents 83 83 85 85 86 86 88 88 90 92 95 . . . . . .3 Evaluation . . . . . .2 Determination of Induced Inﬂow Constant . . . . B Motion Tracking Lab (MTLab) B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . A. . . . .3 Model Veriﬁcation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 Classes . . . . . . . . . . . .2 Procedure . . . . .Table of Contents A The X-3D Interface A. . . . . . . . . . . . . C. . . . .1 Simulink Block . .1 Polynomial Relation of utrust and the force Flif t . A. . . . . . . . . . . . . . . C. . . . . . . . . . . . . . . . . . . . . .

82 TABLE OF CONTENTS .

CommandMsg is the command send from Matlab/Simulink to the quadrotor with control commands. The quadrotor was further equipped with a X-bee serial module enabling wireless serial communication. They do not supply any implemented software to handle the communication on the control PC. SerialSocket is a wrapper for the serial communication.Appendix A The X-3D Interface The X-3D quadrotor from Ascending Technologies that have been used in all practical experiments was modiﬁed with the ResearchPilot ﬁrmware developed by Ascending Technologies.1. Serial is the operating system speciﬁc way of sending and receiving bytes on a serial line. A.1 Message The class Message is parent to several sub-classes as illustrated in Figure A. This enables serial communication with the sensor board handling the on-board control of the quadrotor. 2009] and is not discussed further in this appendix expect when it is relevant to the development of the interface. The main class includes basic functions such as reading the data and length as well as getting the type of the class. . The sub classes represent the three kinds of transfers used from the protocol. DataMsg is the message from the quadrotor to Matlab/Simulink containing the measurements of the local sensors. The actual communication protocol can be found in the documentation of the ResearchPilot [AscTec. To be able to use the on-board sensors as part of the estimation a interface have been developed and further implemented as a Matlab/Simulink block. ConfigMsg is the conﬁguration message determining what data that will be send from the quadrotor. A. SerialSocket and Serial.1 Classes The main classes used in this interface are the following: Message. The class Message is the container of data that is used doing transitions. The purpose of this appendix is to shortly describes the main classes and the ﬂow of the interfaces so if others intend to modify the interface this would ease their task.1. allowing complete messages to be send and received with one command. This communication was used to read some of the on-board sensors so the information could be used in the high-level control. Unfortunately Ascending Technology only supplies the ﬁrmware enabling the communication and a short description of the protocol.

2 SerialSocket The SerialSocket class is the only access to the serial port.3.1 Classes Figure A.2 A. but would be easy to implement support for others such as Windows or Mac OS X. . Figure A. Currently the class only supports the Linux operating system. Newly received data is stored in a buffer containing all data from the previous received message and forward.3 Serial As previously mentioned the Serial class contains all the information necessary to communicate with the serial connection on the speciﬁc operating system. The class diagram is illustrated in Figure A.1: The Message class A. When no new messages are received the SerialSocket contains an instance of the old message and this is returned.1.2: The SerialSocket class The class diagram for the SerialSocket is illustrated in Figure A.1. therefore this class also contains the only instance of the Serial object. Before a message is returned it is tested whether the CRC matches to make sure no data corruption has occurred.84 A.

Some times the receiving rate of 100 Hz of the sensor measurements is not kept by the quadrotor and two or three following samples end up being identical. For the experiments done the following sensors have been read.3 Evaluation The interface have proven stable under both long and short ﬂight. Because 33 Hz is still very fast compared to the desired ﬂight trajectory it is estimated that this will have no affect on the ﬁnal result.4: The ﬂow of the X-3D interface A. Initially the structure of the data message conﬁgured by choosing the sensors that is to be read. This is done in the initialization of the Simulink simulation. . • The gyroscopes (3 axis) • The accelerometer (3 axis) • The pressure sensor • The manual transmitter commands The conﬁguration is send as a ConfigMsg using the SerialSocket. When this was further analyzed it turned out to be minor data errors causing the SerialSocket to discard the messages.The X-3D Interface 85 Figure A. the rate of sending commands had to be slowed down to 33 Hz to avoid breaks in the received sensor measurements.4. Figure A. For every sample the control commands from the Simulink send to the quadrotor as a CommandMsg and the sensor measurements are received as a DataMsg. Even though the bandwidth should support the amount of data transfered.2 Procedure The procedure for communicating with the quadrotor is illustrated in the ﬂow chart in Figure A.3: The Serial class A.

In Table B. The Vicon MX system is currently capable of sending this information with an update rate of 100 Hz. The cameras record the position of reﬂectors mounted on the object. In this project only the position. the Euler rotation and the quaternions will be used. B. when a connection is made form a client computer running Matlab/Simulink. These reﬂectors reﬂect the infrared light send out by the cameras. In the ViconIQ software an object is deﬁned on the basis of the position of the reﬂectors. The position and orientation of this object is then send out on the ordinary network. Original it consists of both the Vicon MX system and a remote controller both linked to Matlab/Simulink. For the experiments done in this thesis however only the Vicon MX system is used in combination with the X-3D interface.1: The MTLab Vicon Simulink block The Vicon Simulink block supplied by the MTLab outputs position. the orientation in a 3-2-1 Euler rotation.Appendix B Motion Tracking Lab (MTLab) The MTLab was developed for testing real-time ﬂight of small autonomous helicopters.1 Simulink Block Figure B. The MTLab is a 5x6 m room with 7 cameras mounted on the walls and connected through gigabit network to the Vicon MX system. the orientation in a quaternion and the orientation in the direct cosine matrix (earth to body rotation).1 is the boundaries and units listed. Both the Euler rotation and the quaternions are calculated from the Direct Cosine Matrix in the Simulink interface. .

5 ±1..5 0.Motion Tracking Lab (MTLab) 87 Data Position symbols [x] [y] [z] [φ θ ψ] [q0 q1 q2 q3 ] Unit m m m rad - Boundary ±1..1 Orientation (Euler) Orientation (quaternion) Table B.1: Data units and boundaries .2 ±π −1.

The amount of weight loss is proportional to the force induced as long as the acceleration is kept constant.Appendix C Measurements Journals C. 4. the motors are returning to idle and the experiment is done. The quadrotor and box is then placed on a scale on a elevated (0.1 Polynomial Relation of utrust and the force Flif t The objective of this measurement journal is to ﬁnd a polynomial relation from the collective input uthrust from the transmitter to the thrust force acting on the quadrotor in near hover ﬂight. 1. A control PC is started with Matlab/Simulink. F =m·a Where F is the force [N] m is the mass [kg] a is the acceleration [m/s2 ] (C.1. To ﬁnd this. 5.1) C. the quadrotor is attached to a box that is more heavy than the quadrotor can lift. . MTLab is powered on (Vicon information must be available). 3. Measurements of changes in the weight are then recorded while the input is increasing. The rotors needs to be free from the platform to avoid any ground effect.75 m) platform as shown in Figure C. When the test signal is done. This originates from Newtons second law. 2.1 Procedure The procedure for the experiment is as follows.1. The X-3D quadrotor is equipped with a newly charged battery and attached to a box with a bigger mass than the quadrotor is capable of lifting. The test signal for thrust is applied while the scale is read off and recorded. 6.

7030 1.5440 1.0000 0.4650 0.0980 0.2 Ft = ∆m ∗ g Were: Ft is the thrust force [N] ∆m is the change in mass [kg] g is the gravitational acceleration [m/s2 ] (C.0350 Initial weight [kg] RC input 0 500 1000 1500 2000 2500 3000 3500 4000 Table C.2 Results The thrust force can then be expressed as in Equation C.3780 1.0040 m [kg] 1.1: The set-up used in the thrust test The scale used is a Rådvad Elevtronic IS075 kitchen scale measuring at a resolution of 1 gram.0240 Measurement 2 2.1560 0.8490 1.1100 1.0050 1.0050 m [kg] 1. where the initial weight is the total mass of the quadrotor and the box.2) The results of the measurements can be seen in Table C.1.8940 0.1580 0.9060 1.2300 1.9040 1.7750 0.8950 1. m is the mass measured by the scale .6260 0.1010 0.4600 0.5400 1. C.6250 0.2400 1. ∆m is the total mass minus the measured mass at a given input.75m Bin Figure C.1: The results of the force measurement.9800 ∆m [kg] 0.1. Measurement 1 2.8460 1.Measurements Journals 89 BOX ON OFF 2000 g h = 0.3020 0.6950 1.0100 0.7640 0.9940 1.9700 ∆m [kg] 0.3800 1.1100 1.3090 0.

0 −2 −4 Flift [N] −6 −8 −10 −12 0 1000 2000 uthrust [ ] 3000 4000 Measurement Polynomial estimation Figure C.4 in the main thesis.90 C.2 as a continuous graph. Doing manually ﬂight it has been observed that the quadrotor do not continue to accelerate when a constant input (more than hover) is applied. With this observation in mind a constant is to be found that results in a constant velocity at a constant input.2 also shows the 3rd order polynomial that is ﬁtted to the measured graph. To estimate the graph as a polynomial the least squares problem for the following equation is minimised.4 following the derivation in Section 3.and y-axis. Therefore the quadrotor is assumed parallel with the horizontal plane.90. 0 0 b F = b Flif t + mb Ce 0 + Ii 0 (C. a3 x3 + a2 x2 + a1 x + a0 = f (x) (C. A more precise description would be that the quadrotor ﬁnds a constant vertical velocity with a constant input.43 · 10−6 . This eliminates forces on the x. The formulation of the total force can be described as in Equation C. Ii = −(Flif t + mg) v3 (C.2 Determination of Induced Inﬂow Constant The calculated force of each input step is shown on Figure C.2 Determination of Induced Inﬂow Constant The objective of this test is to determine the constant that approximates the best mathematical description of the vertical behaviour of the quadrotor.2: The measured results plotted together with the Flif t C. When a constant velocity is obtained the acceleration must be zero. The polynomial can be described by the parameters a3 = 2. Therefore the total force must also be zero.a3 is parameters to a 3rd order polynomial.05 · 10−3 . a1 = −1.07 · 10−10 .3) Where f (x) is the result of one measurements and a0 . a2 = −1. 0 = Flif t + mg + Ii v3 (C.5) Under these circumstances the induced inﬂow constant (Ii ) can be derived knowing the input force and the velocity. Figure C. and a0 = −0.4) g v3 The induced inﬂow constant is found doing steps on the vertical axis.6) .

1.5 0 −0.7) C.Measurements Journals 91 C.5 0 −0. The quadrotor ﬂown near hover in the center of the room 4.2.423 σ 2 = 0. From hover multiple steps on the thrust input is made resulting in the quadrotor slowly moving upward 5.3.3: Thrust steps and vertical velocity In total 7 measurements have been made. The control PC is started with Matlab Simulink and the x3d_interface. estimated velocity and the calculated Ii is listed in Table C. All steps are logged and saved The input force is calculated using the polynomial approximation derived in Appendix C. Flif t = f (uthrust ) (C.8) .2. Vertical velocity and force 1 Vertical force [N] 0. MTLab is powered on 2.004 (C. The induced inﬂow constant used in the model is the mean of the calculated values.2.1 Procedure Measurements of the input force and the velocity under the previous described conditions are done in the following way: 1. The input force.2 Results A measurement of the vertical steps can be seen in Figure C. ¯ Ii = −0.mdl model is opened 3.5 −1 10 15 20 25 30 35 Time [s] 40 45 50 55 60 Figure C.5 −1 10 15 20 25 30 35 40 45 50 55 60 1 Velocity [m/s] 0.

500 -0. The result of the entire q measurement b˙ is shown in Figure C. The model output ˙ is calculated using the equations listed in the end of the Chapters 3 and 4.15 -0.4. In recording the orientation and velocity the developed EKF is used to minimize noise.375 -0.36 -0.92 -0.477 -0.3 Model Veriﬁcation Table C. One where large amplitude input signals on roll and pitch is used and one where only small input signals are used.3 Model Veriﬁcation The model veriﬁcation is made from two measurements.5. .44 -0.2: Result of measurements C.426 -0.320 -0.294 -0.450 -0.20 -1. And the result of the entire v measurement is shown in Figure C.180 -0.12 -1.330 v3 [ m ] s -1.70 Ii -0.92 Flif t [N ] -0.392 -0. In both measurements the time derivatives are calculated using the Matlab command diff.490 -0.210 -0.439 -0.471 C.

5 −1 0 10 20 30 40 50 60 70 1 Measured Non−linear model Linear model 0 q3 ˙ −1 −2 0 10 20 30 Time [s] 40 50 60 70 ˙ Figure C.4 0 10 20 30 40 50 60 70 2 1 q1 ˙ 0 −1 −2 0 10 20 30 40 50 60 70 1 0.4: Modelled and measured q .2 −0.5 q2 ˙ 0 −0.Measurements Journals 93 0.2 q0 ˙ 0 −0.4 0.6 0.

5: Modelled and measured v .94 C.3 Model Veriﬁcation 6 4 2 v1 ˙ 0 −2 −4 10 20 30 40 50 60 70 5 v2 ˙ 0 −5 10 20 30 40 50 60 70 5 Measured Non−linear model Linear model v3 ˙ 0 −5 10 20 30 40 Time [s] 50 60 70 ˙ Figure C.

Following is a description of the folders on the CD and their content. step and square test made Pictures Pictures of the quadrotor ﬂying outdoor . videos and measurements results. x3d Contains all Matlab ﬁles and measurement results used throughout the project Report A pdf version of the report Videos Videos of the hover.Appendix D CD Contents The CD attached to the report contains relevant information. Matlab scripts.

Nomenclature φ Φk Θ A B C D F H Hr Kr Ks Kacc [1] Nonlinear progression model First order Taylor approximation Euler angle parameterization System matrix continuous time Input matrix continuous time Output matrix continuous time Feed forward matrix continuous time Total force affecting the quad rotor in body frame Output matrix discrete time Reference model output matrix discrete time Reference compensation (LQR) Feedback matrix that minimises the performance function (LQR) Scaling of raw accelerometer measurements to rad sec m sec2 Kgyro Scaling of raw gyro measurement to Nr Ns P Q q Reference compensation (H∞ ) Feedback matrix that minimises the performance function (H∞ ) Position in earth frame Weight matrix of the states Quaternion parameterization q0 . q1 . q2 . q3 Quaternion parameterization R u Weight matrix of the inputs Input vector .

ψ Euler angle parameterization b b b b b e e Ce Fg Fi Direct cosine transformation from earth to body frame Gravitational force acting on the body Force generated by the induced inﬂow Flif t Force generated by the rotors He Cb Hb Transformation of rates and angles from earth to body frame Direct cosine transformation from body to earth frame Transformation of rates and angles from body to earth frame Gravitational acceleration Induced inﬂow coefﬁcient Mass of quad rotor A priori probability Probability of z Estimate covariance matrix Model covariance matrix Sensor covariance matrix Vector containing Sφ . Sθ . Sψ Roll input to the on-board controller Yaw input to the on-board controller Pitch input to the on-board controller g Ii m P (x) P (z) Pk Qk Rk uω uφ uψ uθ uthrust Collective input to the on-board controller x. θ.CD Contents xr xs y Γ ˆ x+ ˆ x− J Reference state vector Quadrotor state vector Output vector Input matrix discrete time A posterior estimate A priori estimate LQR cost function 97 Ωrotor Angular velocity of rotors Φ System matrix discrete time φ. z Position in earth frame . y.

98 b b e e ω v ω v Body rates Translatory velocity in body frame Euler rates Translatory velocity of body frame in earth frame .

- Baayen MSc 2011
- IARC Final Report v6.0
- traj
- Design of a Small Flight Control System
- SHEAR7 Version4.4 2005 TheoryManual A
- IMU_Wk8
- The Design of Sliding Mode Control System Based on Backstepping Theory for Btt Uav
- AeroQuad Manual v10
- AIAA-21457-774
- 02 Hummingbird
- Paternoster 11 Geometric 2
- Quad Rotor
- A finite element method for calculating load distributions in bolted joint assemblies
- ArduCopterManual
- Bruhn Errata by Bill Gran
- Optimization Kinematics for Flapping Wing
- Robust Linear Parameter-Varying Control of Air-breathing Hypersonic Vehicle Considering Input Constraints _IEEE
- Probabilistic Design Hollow of Airfoil Wing by Using Finite Element Method
- Valve Sizing Worksheet Instructions
- Advance Ship Propulsion
- Rung Kutta
- Space Robotics Seminar Report
- Modelling of a Small Unmanned Aerial Vehicle 2168 9695 1000126
- An Experimental Study and Simulation With Fluent
- Fluid Chp 15 Introduction of Computational Fluid Dynamics
- v8n2a2
- Aeroelastic Optimization
- 2010_Trans_intelliget Transportation System_ Visionimu Integration Using Slow-frame Rate Monocular Vision System in an Actual Roadway Setting
- Tilt Flapping Maniobras
- Empennage Sizing and Aircraft Stability Using Matlab

Are you sure?

This action might not be possible to undo. Are you sure you want to continue?

We've moved you to where you read on your other device.

Get the full title to continue

Get the full title to continue listening from where you left off, or restart the preview.

scribd