Quadrotor Control using Optical-Flow Navigation · Quadrotor Control using Optical-Flow Navigation...

Quadrotor Control using Optical-Flow Navigation

António Henrique Kavamoto Fayad

Thesis to obtain the Master of Science Degree in

Aerospace Engineering

Supervisor(s): Prof. Rita Maria Mendes de Almeida Correia da CunhaProf. Carlos Jorge Ferreira Silvestre

Examination Committee

Chairperson: Prof. João Manuel Lage de Miranda LemosSupervisor: Prof. Rita Maria Mendes de Almeida Correia da CunhaMember of the Committee: Prof. Pedro Manuel Urbano de Almeia Lima

October of 2014

Acknowledgments

Gostaria primeiramente de agradecer a todas as pessoas que estiveram comigo no ISR(Instituto de

Sistemas e Robotica) durante a realizacao desta tese, pois sempre se mostraram disponıveis para

transportar o seu conhecimento para o meu trabalho. Dentro deste grupo, gostaria de salientar o tra-

balho exımio do Bruno Gomes, sem o qual todo o processo experimental serıa impossıvel. Um especial

agradecimento a minha orientora, a Prof. Rita Cunha, que foi incansavel e paciente durante todo o

processo da tese.

Queria tambem agradecer a minha mae e ao meu pai por tudo o que me proporcionaram e pelo esforco

que tem feito ao longo de toda a vida para que chegasse ate aqui. Ao meu irmao, um agradecimento

pelo apoio e companheirismo a todos os nıveis. Apesar da distancia, a vossa presenca faz-se sentir

todos os dias atraves daquilo que me transmitiram e continuam a transmitir.

Gostaria tambem de agradecer a todas aquelas pessoas que ao longo destes 5 anos contribuiram

para que a minha vida no Instituto Superior Tecnico fosse uma experiencia inesquecıvel. Ao Ricardo

Pedrosa, sem o qual os primeiros anos de faculdade nao teriam sido os mesmos. Ao grupo formado pelo

Afonso Ferreira, Pedro Isidro, Telma Oliveira, Pedro Caldeira, Diogo Santiago, Ines Cadilha, Alexandre

Pacheco, Luis Zilhao, Rui Jaulino, Pedro Miller, Joao Luis, Teresa Guegues, Hugo Pina, Luisa Barbosa

, Margarida Pedro, Andre Tribolet e Manel Morais pela presenca constante em todos os aspectos. Uma

palavra tambem para a Mariana Marques pela sua visao unica das coisas, pelas constantes reflexoes

que trocamos e por toda a disponibilidade ao longo destes anos.

For my international friends, the whole university experience wouldn’t be the same without you. I’m

deeply thankful for all the knowledge that I’ve absorbed being part of ”La Familia”.

Last, but not the least, a very special thank you note to Daniela Hallak without whom the last two years

wouldn’t be the same. Your unconditional support, patience and affection was vital in the whole thesis

process.

iii

Resumo

O trabalho aqui descrito propoe um estimador linear de velocidade e de posicao baseado em informacoes

de um conjunto de sensores instalados a bordo para ambientes em que o GPS nao esta disponıvel .

O estimador de velocidade recorre a informacoes do modelo dinamico do veıculo e observacoes de

velocidade efectuadas por um sensor de fluxo optico usualmente utilizado em ratos de computador. A

velocidade medida pelo sensor de fluxo optico tem componentes rotacionais e lineares da velocidade,

mas a componente rotacional e retirada atraves de medicoes de velocidade angular. O estimador de

posicao usa somente informacoes do modelo dinamico do veıculo, existindo a possibilidade de ser cor-

rigido por dados de GPS. Para alem do sensor de fluxo optico, os estimadores propostos requerem

dados da altitude do veıculo e de um conjunto de sensores basicos para quadrirotores composto por

acelerometros e giroscopios.

Um vasto conjunto de experiencias e descrito e os seus resultados apresentados. Experiencias em

ambientes indoor permitiram avaliar a precisao do sensor de fluxo optico e da compensacao angular

necessaria. Nas experiencias a ceu aberto os dados de GPS permitiram verificar a precisao das es-

timativas de velocidade e de posicao para voos reais. Os resultados obtidos provam que a solucao

proposta para o estimador de velocidade e adequada para o seu uso em veıculos autonomos. No en-

tanto, os resultados obtidos para o estimador de posicao revelam que a precisao obtida atraves de uma

integracao da velocidade nao e a mais adequada a um voo completamente autonomo.

Palavras-chave: Estimacao de Velocidade, Estimacao de Posicao, Fluxo Optico, Ambientes

sem GPS, Teoria de Lyapunov,Quadrirotor

v

Abstract

The work presented here proposes a position and velocity estimator using on board sensors for au-

tonomous quadrotors on GPS-denied environments. The velocity estimator uses a dynamic model of

the vehicle and corrects its estimates with velocity measurements made by an optical flow mouse sen-

sor. The optical flow measurements are influenced by the rotational movement of the camera, but are

corrected by fusing angular rate data. The position estimates are based in the dynamic model of the ve-

hicle, but depending on GPS availability its estimates can be corrected by position data. Apart from the

optical flow sensor, the proposed estimators requires altitude data and a basic set of sensors composed

by accelerometers and gyroscopes.

A wide range of indoor and outdoor experimental results are presented. Indoor experiments are used

to access the optical flow sensor and the angular compensation accuracy. In the outdoor tests, GPS

information is used to verify the accuracy of the velocity and position estimates in remote controlled

flights. The obtained results prove the suitability of the proposed velocity estimator for autonomous

vehicle’s but show that in order to have a fully automated flight the position estimate should not be the

result of pure integration of velocity values.

Keywords: Velocity Estimation, Position Estimation, Optical Flow, GPS-denied environments, Lya-

punov Theory, Quadrotor

vii

Contents

Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii

Resumo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v

Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii

List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xii

List of Acronyms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii

List of Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiv

1 Introduction 1

1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2 State-of-the-art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.3 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4 Proposed Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.5 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2 Quadrotor 3D Motion 7

2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.1.1 Rotation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.1.2 Rotation Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.1.3 Kinematics Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3 Optical Flow 14

3.1 Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.2 Sensor Avago ASDN3080 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4 Estimation Algorithms 17

4.1 Mathematical Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

ix

4.2 Velocity Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4.2.1 Linear Movement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4.2.2 Angular Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4.2.3 Filtering Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

4.3 Trajectory Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

5 Experimental Validation 33

5.1 K estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

5.1.1 Experiment Description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

5.1.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.2 Angular Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37


5.2.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.3 Flight Tests . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40


5.3.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

6 Conclusions 49

Bibliography 53

x

List of Figures

1.1 Proposed Solution Flow Chart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.1 Rotation of a Reference Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.2 Referentials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.1 Example of Optical Flow Fields . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.2 Schematic of OF Sensor Displacement detection . . . . . . . . . . . . . . . . . . . . . . . 15

3.3 Schematic of OF Sensor Image Deterioration . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.1 Projection of a Single Point Schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4.2 Optical Flow Rotational Inconsistency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4.3 Proposed Solution Block Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.4 M2 Block Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.1 Mikrocopter QuadroKopter XL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

5.2 Gain KOF estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.3 Angular Compensation Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.4 Angular Compensation Experiment Results . . . . . . . . . . . . . . . . . . . . . . . . . . 38

5.5 Angular Compensation Parameter Estimation . . . . . . . . . . . . . . . . . . . . . . . . . 39

5.6 Modelled Pitch Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.7 Modelled Roll Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.8 Compared Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

5.9 Manual 5m x 3m Trajectory Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

5.10 Manual 5m x 3m Trajectory(2 turns) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.11 Velocity Estimation Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

5.12 ψ(t) during Flight Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

5.13 Angular Compensation Action During Flight . . . . . . . . . . . . . . . . . . . . . . . . . . 46

xi

5.14 Velocity Filter Action During Flight . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

5.15 Proposed Solution Position Estimates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

5.16 M2 and M3 Position Estimates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

xii

List of Acronyms

UAV - Unmanned Air Vehicle

GPS - Global Positioning System

GNSS - Global Navigation Satellite System

IMU - Inertial Measuring Unit

OF - Optical Flow

LRF - Laser Range Finder

SLAM - Simultaneous Localization and Mapping

LTI - Linear Time Invariant

cpi - counts per inch

RTK - Real Time Kinetics

ISS - Input to State Stability

xiii

List of Symbols

{I} Inertial Reference frame

{B} Body Reference frame

pi Position of a single Point in the OF sensor Surface Reference frame m

Pi Position of a single Point in the camera reference frame m

Ip Vehicle’s position in the Inertial reference frame m

Bp Vehicle’s position in the Body reference frame m

Bv Vehicle’s velocity in the Body reference frame ms−1

IvB Vehicle’s Body frame Velocity written in the Inertial reference frame ms−1

vOF Vehicle’s velocity as measured by the Optical Flow sensor ms−1

d Optical Flow Sensor Pixel Array Length m

φ,θ,ψ Roll, Pitch and Yaw angles rad

BIR Inertial to Body reference frame Rotation matrix

ω Vehicle’s Angular Velocity in the Body Reference fame rads−1

∆XOF ,∆YOF Optical Flow sensor Output in x and y axis m

h LRF altitude output m

fs Accelerometer output ms−2

f Focal Lens Distance m

Vi ith Lyapunov Function

g gravity of Earth Nkg−1

xiv

Chapter 1

Introduction

Autonomous civil Unmanned Air Vehicles (UAV) are already part of our daily routine in several outdoor

applications. Its operational cost and drastic reduction of human life risk make it a viable option in

fields where in the past the helicopter played a major role. TV broadcasting, surveillance, farming and

inspection of infrastructure are example of it.

These outdoor operations depend on the stabilization and control of the vehicle, which nowadays is

only made possible due to GPS information. However, GPS data is not available in urban canyons and

indoor environments. In order to expand the applicability of autonomous UAV’s to those situations it

is necessary that the scientific community finds suitable alternatives for it. GPS-denied environment

applications are specially relevant in exploration/search and rescue in hostile environments (e.g. mines,

disaster sites), on demand inspection and problem identification of buildings/plants. Although there are

other commercial applications, the use of UAV’s in these situations plays a major role because it prevents

the loss of humans lives without putting others in jeopardy.

Throughout this thesis the problem of position and velocity estimation using on-board sensors in un-

known GPS-denied environments is addressed. As far as this author is concerned, at the moment only

systems that require previous on-site installation of camera network and/or Global Navigation Satellite

System repeater can accomplish the necessary precision in the estimation process on those environ-

ments. Although this work does not present a full working solution, it experimental data provide in-

dications that this type of solution may one day fulfil all the necessary requirements for automation in

GPS-denied environments. The proposed solution uses an Inetial Measuring Unit (IMU), an optical mice

Optical Flow (OF) sensor and a Laser Range Finder (LRF) placed on board of the UAV.

1

1.1 Motivation

The increasing number of outdoor applications for autonomous UAV’s in the last decade naturally brings

the question of transferring this technology into indoor environment. Its price and adaptability allow it to

be used in hostile environments in which humans cannot go or would take longer to access. Although

the transition may seem logical, the indoor environment constrains clearly reduce the performance of the

existing outdoor autonomous UAV’s. Apart from GPS signal deterioration, indoor environments usually

require hover capabilities, higher manoeuvrability and size limitations that do not exist outdoor.

Quadrotors flight properties fit the indoor flight needs and close the previous gap in UAV indoor require-

ments. The use of rotor propellers allow it to hover and its thrust-to-weight ratio provide great stability

and high manoeuvrability. These qualities allow quadrotors to navigate through narrow corridors, doors

and windows.

When solving complex engineering problems humans have always looked to nature to find inspiration.

For example, the use of visual cues as navigation aid has been verified in several animals like flies,

honeybees and birds. The optical flow retrieved from the visual sensors is used to estimate velocity and

distance to objects as it is shown in [1][2]. Then, it should be possible to perform similar tasks with the

UAV’s by moving all sensors and intelligence on board.

The proposed solution in this thesis is based in the extraction of OF information from images of the

surface below the UAV. This information fused with IMU data and height of the vehicle is sufficient to

successfully estimate the velocity of the vehicle. The estimated velocity can then be used to define the

trajectory of the vehicle and control its velocity.

1.2 State-of-the-art

Several fields of scientific research that focus on quadrotors have the common goal of achieving accurate

autonomous navigation of the vehicle. The range of applications in which this goal is possible to achieve

depends on two different but interconnected fields: Control and Estimation.

Due to the non linear, multi variable and coupled dynamics of the quadrotor, its stabilization is essential

even to human controlled missions. Several work has been done in the control field in terms of stabiliza-

tion of the vehicle both with standard linear and non linear approaches [3, 4]. Non linear back-stepping

and sliding mode is also presented in [5]. More modern approaches using quaternions feedback con-

trol [6] or adaptive fuzzy control [7] have also managed to aid human control to be more accurate.

2

Although there are already important applications that can be performed by human control, the use of

quadrotors in bigger scale depends on the ability to automate these processes.

The estimation field deals with providing approximated values of quantities that are necessary to auto-

mate a certain action. In UAV’s applications, this field is mainly divided into attitude estimation and posi-

tion/velocity estimation. The first one regards the 3D orientation of the vehicle disregarding its position.

Accurate estimates are already achieved with low cost sensors even in the presence of biased mea-

sures recurring to several versions of Kalman Filters as in [8, 9]. In terms of position/velocity estimates

GPS presents itself as the state of the art system and together with an attitude estimator already makes

possible to perform waypoint navigation [10, 11], pursuit-evasion missions and target tracking [11]. Nev-

ertheless, there is still the need to find a suitable replacement for GPS in indoor environments. Off-board

alternatives like Vicon1 provide really accurate and high frequency estimates for attitude,velocity and po-

sition that makes possible to perform extreme control actions like ball juggling [12] or static and dynamic

equilibrium of an inverted pendulum [13].

In order to be able to perform the control actions mentioned above in unknown indoor environments

without prior on-site hardware installation, it is necessary to find onboards sensors that provide the

same level of information as systems like Vicon and GPS do. Approaches similar to the ones applied

in ground robots are usually adapted to aerial vehicles. For example, Simultaneous Localization and

Mapping(SLAM) algorithms are presented in [14] using on board video and in [15] using LRF data.

The state estimation is done by filtering the information from the SLAM algorithm together with IMU

data. Both works provide interesting results in which position hold and position set point missions are

executed with common PID controllers. Due to the computational complexity of the SLAM algorithms,

the retrieved data has to be sent to a ground station in order to be processed. This increases the

limitations in the system applicability since it is difficult to ensure that the data link is available at all times

in unknown environments.

Another alternative is to retrieve ego motion information from visual cues. In [16] a fish eye camera

is used to retrieve optical flow information with the final goal of building a short depth map of the sur-

roundings in corridor like environment. However, the necessary computations to the retrieved images

are also done off board. In [17] the same methodology is used but optical flow information is retrieved

by using pre defined landmarks. So the transmission of data to a ground station is avoided, the use

of dedicated sensor to retrieve image frames and compute optical flow on board were also explored in

[18, 19]. In [18] the optical flow vector field is calculated in spherical coordinates which makes possible

to retrieve additional information regarding the distance to the overflown surface. Set point missions are

performed although ground markers are necessary. In [19] a dedicated optical flow hardware is used

1more information at www.vicon.com

3

and compared with optical mouse sensors. The vehicle’s dynamics are neglected but a Kalman filter

approach is applied to provide the final velocity estimates. The position estimates are direct integration

of the velocity. Nevertheless, it is not possible to access the accuracy of the presented solution since no

estimate error information is provided.

The necessity of image transmission over the air and the use of landmarks are common limitations of

the consulted literature.The presented work tries to achieve similar accuracy as the one achieved

before using cheaper sensors like optical mouse sensors and without constrains in the overflown

surface. A novel approach is proposed in terms of the connection between attitude estimation and

estimated trajectory accuracy by separating the problem of computing the body and inertial velocity.

This work presents detailed results in terms of angular compensation accuracy, velocity and position

estimates error that is not available in the research mentioned above.

1.3 Objectives

An estimation problem is defined by the unknown quantity or quantities that need to be approximated

and the purpose for which the estimation values will be used. In this particular case, the unknown

variables are the inertial position and velocity of the vehicle. The estimated velocity values will be used

to reconstruct the vehicle’s trajectory. Any of the UAV’s and quadrotors applications mentioned in chapter

1 require position and velocity estimations to automate its procedures.

The required precision and accuracy of the estimation is highly dependent on the envisaged use of

the estimated values. Throughout this thesis the estimation will be consider adequate if the velocity

estimated values can be used to control the vehicle’s velocity in a desired set point. The trajectory

estimate does not have desired accuracy for the direct purpose of this work. In order to achieve the final

goal, several middle points have to be achieved:

• Model the optical flow sensor behaviour so its output can be used as a velocity sensor in an

unknown environment;

• Model the influence of angular rates in the measured optical flow and define a compensation law

such that the optical flow measures provide only linear velocity components;

• Design an asymptotic stable velocity estimator in which the velocity observations are provided by

the optical flow sensor. The estimates accuracy should be in the same order of magnitude as the

GPS outdoor performance.

4

The mentioned goals are meant to be achieved when using real flight acquired data.

1.4 Proposed Solution

The solution proposed in this thesis for the velocity estimation in GPS-denied environments is based

on the direct measurements made by the OF sensor, IMU and LRF in which the problem is decoupled

into the X-Y body frame plane and Z body axis, Figure 1.1. The estimation in the X-Y plane is the main

focus of this thesis. For the Z component a simple numeric derivation approach from the LRF data will

be used. The estimation is done by extracting the rotational velocity component from the UAV’s optical

flow field using only data from one sample time. This way, only the translational component is left out.

Thorough explanations of the compensation method are presented in chapter 4. The trajectory estimator

is based on the integration of the inertial frame velocity but corrects its estimates whenever GPS data is

available.

vOF

Computations

ωComp.

ddt

Sensors

vx,vy

Estimations

vz

ddt

Measurements

ω

∆XOF , ∆Y OF

h

OF

IMU

LRF

TrajectoryEstimator

Ip

Figure 1.1: Proposed Solution Flow Chart

1.5 Thesis Outline

The remaining work presented here is divided into five more chapters.

Chapter 2 derives the 3D Kinematic and Dynamic equations that define the quadrotors motion. Detailed

information on rotation matrices and referential transformations is provided as it is extensively used in

this work.

Chapter 3 provides a deeper insight of the optical flow concept and the ego motion information that can

be retrieved from it. The optical flow sensor used in the proposed solution is presented and a small

summary of its internal procedures is provided.

5

Chapter 4 contains all the core information regarding the estimation processes applied in this work.

Mathematically detailed information regarding the origin and synthesis of velocity and position estimators

is provided. The necessary angular compensation is included in the velocity estimator algorithm. Finally

the stability of both estimator is evaluated.

Chapter 5 presents the information regarding all the practical experiments done in this thesis. The aerial

vehicle used and all used sensors are specified and afterwards each of the experiments is described

and its results presented.

The general conclusions of the work are presented in Chapter 6. The direct consequences of the

achieved results are addressed as well as future developments of the presented work.

6

Chapter 2

Quadrotor 3D Motion

One of the main focus of the work presented in this thesis is the future automation of UAV’s motion in a

3D environment. In order to better comprehend the challenges and solutions presented, it is important

to summarize the kinematic and dynamic laws that govern this motion. Apart from that, the mathemat-

ical model of the quadrotor used to simulate and predict the quadrotor motion and the mathematical

nomenclature used in the remaining of this thesis will be introduced.

2.1 Kinematics

The Kinematics field deals with the motion of objects disregarding the aspects that cause or change the

current state of motion [23]. Nevertheless, it contains important aspects on how to transform vectors

that are represented in one reference frame into another one. This transformation can happen in two

ways: translation and rotation. Since the frame transformations in this work will be mainly applied to free

vectors like velocity and angular velocity only the rotation procedure will be explain in detail.

2.1.1 Rotation Matrix

In figure 2.1 it is possible to see reference frame {1} and {2} in which the last one is obtained by rotating

the first one by θ in the counter clock wise direction. At the same time, the vector ~v ≡ v components are

represented as seen from both reference frames. It is possible to exchange between one representation

and the other by using a rotation matrix. Using the same notation as in figure 2.1 in which 1v represents

the vector v as seen from the reference frame {1} it is possible then to write:

7

2v = 21R

1v (2.1)

1Y2Y

1X

2X

v1y

1x

2x

2y

θ

Figure 2.1: Rotation of a Reference Frame

A matrix is defined as a rotation matrix if and only if it the following conditions are true:

• (BAR)−1 = (BAR)T = ABR

• CBR

BAR = C

AR

• det(BAR) = 1

Although the rotation matrix presented here defines a 2D rotation, its use in 3D environment follows

the same rules but with higher complexity since instead of being defined by only one parameter (θ) it is

defined by three. In the next section the rotation representation in 3D will be introduced since it is useful

for vector transformation and also to understand the vehicle’s orientation in a 3D space.

2.1.2 Rotation Representation

The 3D rotation representation does not focus on the centre of mass position but on the body space

orientation. Simple and correct methods to represent the body orientation by a set of rotations are

essential to calculate rotation matrices mentioned in section 2.1.1 and provide physical intuition to 3D

body motion.

In the aerospace industry the usual representation is the one introduced by Leonard Euler in which the

rotation is represented by three parameters φ, θ and ψ. They represent three consecutive rotations

about the axes of a coordinate system [24].

In figure 2.2 is possible to see the direction in which the Euler angles are measured and the two reference

frame used throughout this thesis:

• Inertial Reference frame, {I}, considers a flat earth approximation from the earth ellipsoid WGS84

in which the origin is given by the initial latitude, longitude and altitude. The X axis has the direction

8

of the magnetic north, Z axis is perpendicular to the surface and the Y axis is the resultant from the

cross product IZ × IX. From now on the vectors measured in this frame will have the superscript

I, e.g. Ia

• Body reference frame, {B}, origin is coincident with the vehicle’s center of mass position. X axis

has the direction of the red bar in the vehicle in figure 2.2, the Y axis has the direction of the first

rotor when moving clockwise from the red bar and the Z axis is the resultant from the cross product

BX × BY . From now on the vectors measured in this frame will have the superscript B, e.g. Ba

YBZI

XI

YI

{I}

ZB

XB

θ

φψ

{ B }

IpB

Figure 2.2: Referentials

The Euler angles are essential to interchange vectors between the body and the inertial reference frame.

The need to perform this action comes from the fact that most of the sensor’s on board provide data

expressed in the body frame. However, calculations like vehicle’s trajectory only make sense in the

inertial frame. The rotation matrix that perform this action is built using the three consecutive rotations

over X,Y and Z in this exact order and follow the properties introduced in section 2.1.1.

IBR = B

IRT =

IF1R(ψ)

F1F2R(θ)

F2BR(φ) (2.2)

where {F1} and {F2} are intermediate reference frames

Substituting the rotation by its matrices yields:

9

IBR =

cψ −sψ 0

sψ cψ 0

0 0 1

cθ 0 sθ

0 1 0

−sθ 0 cθ

1 0 0

0 cφ −sφ

0 sφ cφ

=

=

cψcθ cψsθsφ− sψcφ sψsφ+ cψsθcφ

sψcθ cψcφ+ sψsθsφ sψsθcφ− cψsφ

−sθ cθsφ cθcφ

(2.3)

where ”c” and ”s” symbols represent cos(.) and sin(.) functions

Knowing IBR or λ = [ψ, θ, φ] is then enough to fully define the body space orientation. It is possible to

interchange between the two representations using equation 2.3 or applying trigonometric relations to

the rotation matrix elements in equation 2.3:

θ = atan2(−r31,√

(r11)2 + r21)2)

ψ = atan2(r21, r11)

φ = atan2(r3,2, r33)

(2.4)

where rij refers to the i-th row and j-th column of the matrix IBR

2.1.3 Kinematics Variables

Throughout this thesis the vehicle’s motion will be extensively studied both in {I} and {B} reference

frames. It is then necessary to define the kinematic variables that will be used and how they relate with

each other:

• p = [x, y, z]T - position in {I}

• v = [u, v, w] - velocity in {B}

• λ = [θ, φ, ψ] - Euler angles with respect to {F1},{F2} and {I}

• ω = [p, q, r] - angular velocity in {B}

The 3D motion has six degrees of freedom, in which three degrees comes from linear motion and the

remaining from angular motion. It is obvious then that the defined variables fully define the vehicle’s

10

position and orientation both in {B} and {I}. The summary of the role of each variable in the body

motion is presented in tables 2.1 and 2.2.

Linearx direction u xy direction v yz direction w z

Table 2.1: Linear Motion Variables

Angularx direction p φy direction q θz direction r ψ

Table 2.2: Angular Motion Variables

Since the velocity data from the vehicle is usually provided by sensor’s placed on board, its data come in

the body reference frame. Nevertheless, using the concepts of rotation matrix introduced in subsection

2.1.1, it is straightforward to relate the body frame and inertial frame velocity.

x

y

z

= IBR

u

v

w

(2.5)

It is possible as well to find a similar relationship between the body and inertial frame angular variables

but it is not practical to use it because each of the Euler angles is defined in a different reference frame.

Therefore this relation is usually replaced by a set of differential equations that define the rotation matrix

over time:

R = RS(ω) (2.6)

where S(.) is a skew-symmetric matrix defined such that S(x)y = x× y

By having the rotation matrix defined over time, it is then possible to define the Euler angles over time

using equations provided in 2.4.

11

2.2 Dynamics

In the previous sections the body orientation and motion was defined but its causes were left apart.

The dynamic field provides the relation between the actuating external forces and the motion caused by

them. Here it will be assumed a rigid body approximation with body frame {B} placed at the center of

mass and the inertial frame {I} introduced in section 2.1.2.

From the inertial reference frame point of view the relation between external applied forces and acceler-

ation in a translational movement is given by Newton’s second law:

If = m Ia = md

dtIv (2.7)

However, external forces are usually known in the body reference frame and it is also more intuitive to

analyse their effect in that same reference frame. Therefore, equation 2.7 is usually written as function of

body frame vectors using the relationship between the rate of change of a vector expressed in a rotating

frame as seen from the inertial frame shown in [25]:

Bf = m(d

dtBv + Bω × Bv) (2.8)

Substituting in the above equation the definitions for v and ω introduced in subsection 2.1.3 yields:

u

v

w

=

rv − qw

pw − ru

qu− pv

+1

m

fx

fy

fz

(2.9)

It is possible to specify equation 2.9 derived above to the quadrotor case studied here. The considered

external forces are the thrust provided by the quadrotor rotors and the gravitational force. Since the

gravitational force is defined in the inertial reference frame, in order for it to be included in equation 2.9 it

has to be transformed for the body frame referential using the rotation matrix introduced in equation 2.3.

Note that the quadrotor can only apply thrust in the z axis direction of {B}:

Bf =

0

0

−T

+mBIR

0

0

g

(2.10)

12

The same reasoning applied for the translational movement can be applied for the angular one in order

to find a relationship between the external applied torque and the angular motion from the vehicle:

Bτ = Jd

dtBω + Bω × (J Bω) (2.11)

where J is the 3D moment of inertia and τ is the external applied torque

Solving the equation ford

dtBω, replacing the external product for the skew-symmetric matrix multipli-

cation and using the notation introduced in section 2.1.3 yields the body angular rates with respect to

time:

p

q

r

= J−1[S(ω)Jω + τ ] (2.12)

The model of the quadrotor used in this thesis is then achieved by joining all the kinematic and dynamic

equations derived in chapter 2:

x

y

z

= IBR

u

v

w

u

v

w

=

rv − qw

pw − ru

qu− pv

+

−gsθ

gcθsφ

gcθcφ

− 1

m

0

0

T

R =

r12ωz − r13ωy −r11ωz + r13ωx r11ωy − r12ωx



p

q

r

= J−1[S(ω)Jω + τ ]

(2.13)

13

Chapter 3

Optical Flow

3.1 Concept

The psychologist J. J. Gibson realized that changes in the retinal image due to motion contain rich details

about the surrounding world. The optical flow concept derived from that and he defined it as the animal

visual feedback due to its movement [26]. From an engineering point of view the definition needed to

go further, so one can define which physical quantities can be retrieved from these images. Therefore,

optical flow was later defined as the velocity gradients of physical surfaces on a scene due to relative

motion between the observer and the scene [27][28]. This definition is extremely relevant to this work

since it implies that by analysing images it is possible to extract information about the movement of the

camera regarding the surrounding environment.

The main cues of motions are the optical flow gradient and the optical flow pattern. The first one indicates

the value of the relative velocity of the observer with respect to the surface and the second the direction

of that velocity. These cues are represented by arrows in Figure 3.1. In this thesis the concept of optical

flow will be used to estimate the velocity components u and v.

3.2 Sensor Avago ASDN3080

Optical flow technology is widely used in computer optical mice since the beginning of the 21st century,

making possible to find sensors that suits the aerial vehicle needs in terms of price, weight and accuracy.

In order to use the sensor’s information in real-time, its output rate has also to be adequate with the

14

(a) Movement towards the horizon (b) Touchdown Movement

Figure 3.1: Example of Optical Flow Fields

vehicle dynamics. In this case, Avago’s ADSN3080 communicates at 50Hz which does not impose any

constrains to this work. Its resolution is 30x30 pixel and it internally computes the x and y displacement

by comparing detected features positions in consecutive frames as it is represented in Figure 3.2.

y

x

∆x

∆y

tktk+1

Figure 3.2: Schematic of OF Sensor Displacement detection

Although in this case only the movement detection is used, the sensor provides several outputs related

to the captured frames:

• x and y displacement in counts per inch (cpi);

• Measure of the image quality provided (considering features detected and light conditions) in a

variable named SQUAL;

• Grey scale version of the captured frame;

An important aspect about the displacement outputs is that it result of an integration over the x and y

direction of the optical field, meaning that if the sensor detects two features and they move in opposite

15

directions, the output for displacement would be zero. This situation occurs, for example, if the camera

moves on the normal direction of the surface or rotates over the optical axis. Therefore, a limitation of

using this sensor is that it doesn’t provide the optical flow vector field. For more technical details about

ADSN3080, the data sheet can be found in [29].

Since ADSN3080 is suppose to work as an optical mouse, it is optimized to detect features while being

on top of a table, which differs from its application in UAV’s. As one moves the sensor a few centimetres

from the captured surface, the image loses definition to a point in which no features can be detected.

A representation of this problem is shown in Figure 3.3 while using the same situation shown in Figure

3.2. In order to solve this problem a lens was coupled to the sensor and it was calibrated to an ideal

distance of 70 cm. The influence of the lens in the acquire data will be more clear in the next chapter.

y

x

tktk+1

Figure 3.3: Schematic of OF Sensor Image Deterioration

16

Chapter 4

Estimation Algorithms

4.1 Mathematical Background

For reasons of completeness and to improve text readability, this section introduces some mathematical

notation and background on dynamic systems theory, which will be used to describe the estimation

algorithms in the following sections.

Since quantities in R3 will be extensively used, the usual vector notation, ~v, will be replaced from now

on for bold variable v. The dynamics of a free rigid body in a 3D environment is necessarily associated

with angular motion. The mathematical operator S(.) transforms a 3D vector into a skew symmetric

matrix and has special relevance in this application. Its properties are described below and the resultant

skew-symmetric matrix follows S(v)T + S(v) = 0.

• v × ω = S(v)ω

• S−1(v) = v

• −S(v) = S(v)T

• tr(S(v)ω) = tr(ωS(v)) = −vTS−1(ω − ωT )

• S(v)2 = (I − vvT )

A Dynamical System is defined by one or a set of differential equations that describe the evolution of

a state (vector) over time. In this particular case, the system is composed by first order differential

equations in which the state vector is denoted by x. The derivative of the state is then function of the

state value, the input u and the time.

17

x(t) = f(x, t, u) (4.1)

Usually there is another set of equations associated to equation 4.1, which defines the evolution of the

output vector y. The output vector contains values of variables of interest for the state vector and usually

is composed by physical quantities that can be directly measured.

y(t) = h(x, t, u) (4.2)

Equations 4.1 and 4.2 form the state space model of the system.

Systems are modelled to predict its behaviour or make them behave in a certain way, which is usually

referred as controlling the system.

It is possible to predict the behaviour of an observable dynamical system for all t > t0, where t0 rep-

resents the initial time, if the initial pair solution x(t0, x0) and the input vector u(t) are known for all

t > t0.

The study of the stability of a system is commonly done by studying its equilibrium points. An equilibrium

point, x, is a configuration of the state vector where the following condition is satisfied:

x(t) = f(x) = 0 (4.3)

Equilibrium points can be classified by three different levels in terms of stability:

• Stable: If for every ε > 0 there exists a δ > 0 such that

||x0 − x|| < δ ⇒ ||x(t, x0)− x|| < ε, for all t > t0

• Asymptotically Stable: If it is stable and there exists δ1 such that:

||x0 − x|| < δ1 ⇒ limx→∞

||x(t, x0 − x)|| = 0

• Unstable: If it is not stable

Although the stability conditions for equilibrium point are intuitive and simple, to be able to mathematically

prove those conditions is not. The study of systems equilibrium points stability if often done recurring to

Lyapunov Functions, V(x), named after the Russian mathematician Aleksandr Lyapunov.

18

V(x) has the following properties:

• V : Rn → R be a continuous scalar function;

• V(0) = 0;

• V (x) > 0 ∀x ε U\{0}

These functions can then be used to prove stability using Lyapunov Stability for Autonomous Sys-

tems theorem:

Theorem 1. Let x be an equilibrium point of a system x = f(x) on Rn and let V : D → R be a Lyapunov

function on a region D around the origin then x = 0 is:

• Asymptotically Stable: if V (x) < 0 along the trajectories of the system, except at x = 0;

• Stable if V (x) 6 0 along the trajectories of the system, except at x = 0;

• The existence of a Lyapunov function is a sufficient condition to prove stability or asymptotically

stability;

Considering a function V (x) = xTPx, for some symmetric n x n P matrix, function V(x) fulfils Lyapunov

function conditions if and only if:

• P is positive definite matrix;

• V (x) = xTPx+ xTPx = xT (ATP + PA)x 6 0;

Considering now a general dynamical system x(t) = Ax andQ = −ATP+PA, the conditions of theorem

1 are equivalent to Q being positive definite which means that eig(Q) > 0.

Real systems are usually time dependent and actuated by external forces. Depending on how the

system is actuated, it is possible to access its stability by using the concept of input-to-state stability(ISS).

Let’s consider the system from equation 4.4 and its unforced version in equation 4.5.

x = f(t, x, u) (4.4) x = f(t, x, 0) (4.5)

Considering that the input is piecewise continuous and bounded, the system in equation 4.4 can be seen

as a perturbation from its unforced version. It is logical then to think that if the unforced version of the

system is asymptotically stable, that depending on the structure of the system, bounded perturbations

will result in bounded errors.. In [22] the notions of input-to-state stable system is introduced:

19

The system in equation 4.4 is said to be input-to-state stable if there exist a class KL function

β and a class K function γ such that for any state x(t0) and any bounded input u(t), the

solution x(t) exists for all t ≥ t0 and satisfies:

||x(t)|| ≤ β (||x(t0||, t− t0)) + γ(supt0≤τ≤t ||u(τ)||

)

4.2 Velocity Estimation

The goal of the velocity estimation problem addressed here is to continuously be able to estimate the

velocity in a known reference frame with bounded error using only on-board sensor information. When

looking at the usual sensors available in UAV’s like gyroscopes and accelerometers, it is not possible to

continuously estimate the velocity due to unbounded growth of errors [30]. Therefore the incorporation

of additional sensors is inevitable.

The solution proposed in this work comprises two separated actions, in which the first is to compute the

body frame velocity from the OF sensor output. The second part subtracts the influence of rotations

in the OF sensor by fusing its data with IMU data (top part of Computations in figure 1.1). These two

actions will be thoroughly explained in sections 4.2.1 and 4.2.2.

4.2.1 Linear Movement

Mapping the OF sensor’s data to physical units is essential to its use in any engineering system. In

this case, besides the conversion from cpi to meters one has to model the influence of the lens in the

measured data, which has focal distance f and is at distance h from the floor.

Let {B} be the frame attached to the camera in which the Z-axis is coincident with the optical axis and

Pi=[Xi Yi Zi]T a stationary point on that same reference frame. The projection of Pi into the surface

S defined by the camera is shown in equation 4.6. The term ξ(Pi) represents the camera geometry

projection. The camera used by the OF sensor in this work projects the captured image in a flat plane

at a distance f as shown in figure 4.1. It is then obvious that ξ(Pi) =h

f. Finally, the goal is to map the

displacements measured by the sensor on S to the camera displacements in {B}.

pi =1

ξ(Pi)Pi ⇔

⇔ pi =f

ZiPi

(4.6)

20

As it was briefly explained in section 3.2 the sensor’s output is function of the number of pixels moved

by the detected features between consecutive frames. Since DNS3080 datasheet is not clear about

Avago’s interpretation of the cpi unit and the internal computations of the sensor are not available to the

public, it can only be assumed that the sensor’s output is directly proportional to the length of one pixel.

Defining d as the known length of the sensor’s 30x30 pixel array and ∆XOF and ∆YOF as the sensor’s

body frame outputs, the displacement of Pi on S, in meters, is given by:

pixpiy

=d

30KOF

∆XOF

∆YOF

(4.7)

4.2.2 Angular Compensation

Quadrotor dynamics have some particularities that need to be taken into account when retrieving velocity

information from optical flow data. For example, assuming that BZ axis is aligned with IZ axis and that

the quadrotor is hovering, it is not possible for it (like it happens with most of ground robots) to move on

the X-Y plane direction without an angular rotation. This is direct consequence of equation 2.10.

The problem introduced by rotations is that the OF sensor can not distinguish translational and rotational

movements. This occurs because in terms of captured images the rotation movement is the same as

the translational one. In figure 4.2 it is possible to see that the camera covers approximately the same

area in the initial and final position on both circumstances.

This phenomena can be better understood when looking back at the dynamics of the projected point pi

from equation 4.6. Differentiating with respect to time leads to:

h

{B}

ZX

Pi

x

z

y

piS

f

Figure 4.1: Projection of a Single Point Schematic

21

Figure 4.2: Optical Flow Rotational Inconsistency

pi = f

(ZiPi − ZiPi

Z2i

)(4.8)

Taking now in account that Pi is defined in the rotating frame {B}, its time derivative has one component

due to the position change over time and another one due the angular velocity of the frame:

Pi = −v − S(ω)Pi (4.9)

Using the result from equation 4.9 in equation 4.8 yields:

pi = f

(Zi[−v − S(ω)Pi]− (−(v)z + ωyXi + ωxYi)Pi

Z2i

)(4.10)

The influence of the camera movement in the optical flow output is clearer when looking at the individual

components of the equation above (the Z axis term is not represented since there are no optical flow

readings in that axis). After some mathematical simplifications it yields:

pixpiy

=

vzxi − vxf

Zi− ωyf + ωzyi +

ωxxiyi − ωyx2i

fvzyi − vyf

Zi+ ωxf − ωzxi +

ωxy2i − ωyxiyif

(4.11)

As it was mentioned in chapter 3, the optical flow output is an integration of the movement of the detected

features. Therefore, it can be defined as:

vOF =1

N

N∑i

pi (4.12)

The relation shown in equation 4.11 is extremely important since it shows what can be easily

seen in experimental data. The measured optical flow has components due the translational

22

movement and rotational movement. The last term on the right from equation 4.11 can be neglected

becauseN∑i

xi ≈ 0,N∑i

yi ≈ 0,N∑i

xiyi ≈ 0,N∑i

x2i ≈ const,

N∑i

y2i ≈ const and therefore its order of

magnitude is one time smaller compared to the other terms. As it was explained in chapter 3.2 when

the optical flow field is integrated over X and Y the displacement caused by rotations around the Z axis

sum zero, and therefore the terms related to ωz and vz are also neglected. Thus, using the angular

velocity values obtained by IMU one can write the translational velocity of the vehicle as function

of the optical flow indicated velocity and angular velocity values.

Solving equation 4.11 for the translational components, assuming Zi = h and applying the assumption

mentioned above leads to:

vxvy

=

−vxOF hf − wyh−vyOF

hf + wxh

(4.13)

The final equations for the translational velocity estimation are then achieved by modelling the terms

vyOF and vyOF as numeric differentiation from the displacements measured by the OF sensor in equa-

tion 4.6:

vx

vy

=d

30

h

fKOF

−∆XOF

∆t

−∆YOF∆t

+ h

−ωyωx

(4.14)

The equation above directly maps the sensor’s output to travelled distance as function of the height and

its angular speed. The results provided by this estimation process will be discussed in chapter 5.

4.2.3 Filtering Algorithm

Although the velocity estimate provided by equation 4.14 already provides a trend of the true velocity , it

contains undesired noise for its use in a real-time controller and it solely depends on the OF sensor being

able to capture correct information from the overflown surface. When travelling above surfaces that are

approximately uniform (e.g. single coloured floor), the data provided by sensor Avago ASDN3080 has

less quality than usual, which leads to less accurate estimations. In order to reduce noise and expand

the applicability of the velocity estimation procedure, a state observer was introduced.

As it was mentioned in the beginning of section 4.2, the accelerometers are not used as stand alone

information to provide velocity estimations due to unbounded growth of errors. Nevertheless, this infor-

23

mation can be used to filter velocity estimates provided by equation 4.14.

Accelerometers provide measures of the specific force that actuate on the body. Although it has accel-

erations units, its value is given by the acceleration caused by non-gravitational forces:

fs = Ba− Bg (4.15)

Based on the dynamical model introduced in chapter 2 and using the same notation as the one defined

in the same chapter, the specific force measured by the accelerometers in the quadcopter is given by:

f

m= Ba = v + S(ω)v⇔

⇔ fs = v + S(ω)v − Bg

(4.16)

Note that in order to use the accelerometers measures, the gravity vector in the body reference frame

has to be known. By using the rotation matrix BIR introduced in chapter 2 it is possible to calculate Bg:

Bg = BIR

Ig (4.17)

The dynamical model assumed for quadcopter body frame velocity can be calculated by solving equation

4.16 for v:

v = −S(ω)v + fs + Bg (4.18)

Based on the equation above, it is possible to design a velocity estimator in which the velocity observa-

tions retrieved from the optical flow sensor as considered as the model observations:

˙v = −S(ω)v + fs + Bg +Kob(vOF − v) (4.19)

The velocity estimation error and its time evolution can be defined as in equation 4.20, if it is considered

that the measured optical flow velocity equals the true velocity.

v = v − v

˙v = −S(ω)v −Kobv

(4.20)

It is possible to define Lyapunov function V1 with the following properties for v 6= 0:

24

V1 =1

2vT v > 0

V1 = − vTS(ω)v︸︷︷︸=0

−KobvT v =

= −KobvT v < 0

(4.21)

V1 meets the requirements set by theorem 1 and prove the asymptotic stability of the velocity

observer defined in equation 4.19.

Although analytically this observer produces accurate estimations, the practical results presented a drift

caused by the integration of angular rates measurements. It is possible to estimate the sensor’s drift

by evaluating different obtained values for the rotation matrix. The real time evolution of IBR is given

by equation 2.6, but it is possible to define a rotation matrix observer as in equation 4.22, where u is a

chosen input function and IBR = R. The goal is to find a function b(t) that defines the sensor’s drift

over time.

ˆR = RS(ω + u) (4.22)

Consider the relation between the measured angular rates, ωm, true angular rates and the existing drift:

ωm = ω + b

ω = ωm − b

(4.23)

It is possible now to define the estimated drift error b and the rotation matrix error R:

b = b− b = ω − ω

R = RTR

(4.24)

Using the notation introduced in equation 4.23, it is possible to substitute the unknown true angular rate

in equation 4.22 for known quantities:

ˆR = RS(ωm − b + u) (4.25)

So the estimator in equation 4.22 is evaluated in terms of stability, its estimation error time evolution is

25

considered:

˙R = S(ωm − b + u)T RTR+ RTRS(ω)

= −S(ωm − b + u)R+ RS(ω)

(4.26)

A Lyapunov Function V2 is defined and its time derivative evaluated:

V2 = tr(I − R) (4.27)

taking the time derivative,

V2 = −tr(

˙R)

= −tr(

(−ωm + b− u + ω)R) (4.28)

Using now the notation from equation 4.23 and having in mind the goal of achieving a law for the angular

rate sensor drift, it is possible to rewrite the equation above as:

V2 = −tr(S(−ωm + b− u + ωm − b)R

)= tr

(S(b + u)R

) (4.29)

Choosing u = k1S−1(R− RT ) with k1 > 0 yields:

V2 = −bTS−1(R− RT )− k1S−T (R− RT )S−1(R− RT )︸︷︷︸

≥ 0

(4.30)

V2 does not meet the stability conditions of theorem 1. Therefore another Lyapunov function V3 is

defined:

V3 = V2 +1

k2bT b (4.31)

Taking its time derivative:

26

V3 = −k1S−T (R− RT )S−1(R− RT )− bTS−1(R− RT ) +

bT

k2

˙b (4.32)

Defining ˙b = k2S

−1(R− RT ) and k2 > 0 results in:

V3 = −k1S−T (R− RT )S−1(R− RT )−bTS−1(R− RT ) + bTS−1(R− RT )︸︷︷︸

= 0

V3 = −k1S−T (R− RT )S−1(R− RT ) ≤ 0

(4.33)

The Lyapunov Function V3 meets the conditions of theorem 1 for stability but not for asymptotic

stability for any values of k1, k2 > 0. Nevertheless, it is possible to prove its asymptotic stability using

Barbalat’s theorem defined in [22]. This yields the final drift time law:

ˆR = RS(ωm − b + k1S

−1(R− RT ))

˙b = −k2S

−1(R− RT )

(4.34)

Since the drift is now defined over time, it is possible now to use the velocity observer defined in equation

4.19 by updating it with the result of equation 4.34:

˙v = −S(ω − b)v + fs + Bg +Kob(vOF − v) (4.35)

The stability of the new observer has also to be evaluated, and therefore the dynamics of the estimate

error v is evaluated again considering vOF = v:

˙v = −S(ω − b)v + S(b)v −Kobv (4.36)

Using the same Lyapunov function as the one used to prove the asymptotic stability of equation 4.19 in

the new observer law yields:

V1 = − vTS(ω − b)v︸︷︷︸=0

+vS(b)v −KobvT v =

= vS(b)v −KobvT v

(4.37)

The inclusion of the angular rate sensor drift in the velocity observer changes V1. This way, the function

27

does not meet the requirements of Lyapunov’s Stability Theorem defined in theorem 1. Nevertheless, it

is possible to rewrite equation 4.37 to evaluate if the observer is input-to-state stable:

V1 = −Kob(1− ε)vT v − εKobvT v + vS(b)v (4.38)

Setting an upper bound to the equation above yields:

V1 ≤ −Kob(1− ε)||v||2 −Kobε||v||2 + ||v||||b||||v|| =

= −Kob(1− ε)||v||2 − ||v||(Kobε||v|| − ||b||||v||

) (4.39)

Then, the system described by equation 4.37 is ISS in which S(b) is the input. Assuming that v is

bounded and b converges to zero it is possible to state that V1 is definite negative if the following condi-

tions is met:

V1 < 0 , if ||v|| > 1

Kobε||b||||v|| (4.40)

Equation 4.40 shows that V1 applied to the observer from equation 4.36 meets the ISS requirements

and it follows that the system described by 4.35 is ISS with b as the input. Since b is converging to zero,

v also converges to zero.

The proposed estimator for v is then achieved by combining equations 4.14,4.34 and 4.36:

(vOF )x =d

30

h

fKOF (

−∆XOF

∆t) + hwy

(vOF )y =d

30

h

fKOF (

−∆YOF∆t

)− hwx

I

B˙R =

I

BRS[ωm − b + k1S−1(R− RT )]

˙b = −k2S

−1(R− RT )

ˆv = −S(ω − b)v + fs + Bg +Kob(vOF − v)

(4.41)

28

4.3 Trajectory Estimation

The goal of the trajectory estimator proposed here is to continuously provide the vehicle’s centre of mass

position in the inertial reference frame. Nevertheless, the possible estimator properties have to take in

account the set of sensor’s available in the quadcopter. In this case, there is not complete position

or distance data in the body or inertial reference frame as it can be achieved using inertial positioning

systems like GPS or Vicon. Therefore, it is necessary that the trajectory estimator base its estimates in

the integration of the velocity provided by equation 4.41.

Since the velocity estimator and the sensors provide information in the body reference frame, {B}, the

dynamics of the trajectory estimator will also be written in the body frame. The desired inertial frame

trajectory it will be obtain by using the rotation matrix BIR. The position of the inertial frame with respect

to the body frame is written as BpI and is given by:

BpI = −BIR

T IpB (4.42)

Differentiating the equation above results in the position dynamics:

BpI = −(

[BIRS(ω)]T IpB + BIR

T I pB

)⇔

⇔ BpI = S(ω)BIRT IpB︸︷︷︸−BpI

−BIR

T BIR︸︷︷︸

I33

v⇔

⇔ BpI = −S(ω)BpI − v

(4.43)

Adding now the real problem constrains in terms of available data to equation 4.43, yields the

proposed trajectory estimator:B ˙pI = S(ω − b)BpI + v (4.44)

The necessary information for equation 4.44 is provided by the velocity estimator in equation 4.41. This

estimator cascade formed by equation 4.14 and equation 4.44 fully defines the proposed solution

shown in figure 1.1. A block diagram of the observer can be seen in figure 4.3:

Another alternative position estimator was designed in which low rate GPS information is used to update

the filter. This way, it is also possible to simulate applications in which GPS is available but not under

ideal conditions. Local blind spots or low rate data transmissions are possible real situations. The multi

29

OF

IMU

LRF

VelocityObserver

PositionObserver

v p

Figure 4.3: Proposed Solution Block Diagram

rate observer dynamics is given in equation 4.45, in which the GPS data is considered as the true

position Ip and the velocity estimate is provided by equation 4.41. A block diagram of the observer can

be seen in figure 4.4

B ˙pI = −S(ω)BpI − v +KGPS(p− BpI) , if GPS is available

B ˙pI = −S(ω)BpI − v , if GPS is NOT available(4.45)

For the stability analysis of the multi rate observer it will only be considered the dynamics when GPS

information is available. The estimation error and its time evolution are given by:

p = BpI − BpI

˙p = −S(ω)p− v +KGPSp

(4.46)

Using the same methodology as the one use for the velocity estimators in the previous section, it is

possible to define a Lyapunov Function V4:

V4 =1

2pT p (4.47)

Taking its time derivative yields:

V4 = −KGPSpT p− pT v (4.48)

As it is possible to see in equation 4.48, V4 is not definite negative and therefore does not meet the

stabilities requirement of theorem 1. Its input-to-state stability can be accessed by rewriting equation

4.48 as:

V4 = −KGPS(1− ε)pT p−KGPSεpT p− pT v (4.49)

The upper bound of V4 is given by:

V4 ≤ −KGPS(1− ε)||p||2 −KGPSε||p||2 − ||p||||v|| =

= −KGPS(1− ε)||p||2 − ||p|| (εKGPS ||p|| − ||v||)(4.50)

30

Therefore V4 is definite negative if:

V4 < 0 , if ||p|| > ||v||εKGPS

(4.51)

The equation above shows that the body frame position dynamics shown in equation 4.46 is ISS with v

as the input. Since v converges to zero then p converges to zero as well, which implies that the observer

defined in equation 4.45 is asymptotic stable.

VelocityObserver

GPSData

vIMU

LRF

pPositionObserver

Figure 4.4: M2 Block Diagram

In order to achieve the desired inertial position, the estimates provided by equation 4.45 have to be

projected on the inertial reference frame. This step would not add error if the real rotation matrix was

known, but in any real experiment this does not happen. Assuming that p = 0 yields:

BpI = BpI (4.52)

In this particular case, the inertial position estimation error is due to rotation only and it is given by:

I pB = I pB − IpB = −I

BRBpI +− I

BRBpI

= −(I

BR−IBR)BpI

(4.53)

The maximum value for this error is written as:

max || I pB || = max IpTB

(I

BR−IBR)T ( I

BR−IBR)IpB

= 2 IpTB

I − I

BRIBR︸︷︷︸R

IpB

(4.54)

The rotation matrix error can be written using Rodriguez’s Formula [31] which parametrizes the rotation

matrix R as a rotation of angle τ about the unitary axis γ:

31

I − R = sin(τ)S(γ) + (1− cos(τ))S(γ)2 (4.55)

The maximum inertial position estimation error assuming that the body frame position is known equals

to:

max || I pB || = 2(1− cos(θ))|| IpB ||2 if BpI = BpI (4.56)

32

Chapter 5

Experimental Validation

This chapter describes and presents the results from all experiments made in the framework of this

thesis. The procedures took place in indoor environment at IST and outdoors at the aeromodelling

track Hangar 13 in Montijo. First, in section 5.1 and 5.2 indoor estimation procedures are presented.

Afterwards in section 5.3, outdoor tests results are shown.

The tests were made with quadrotor Mikrocopter QuadroKopter XL, figure 5.1, and it was equipped with

the following sensors:

• IMU: Microstrain 3DM-GX3

Used Outputs: ω,fs,φ,θ,ψ

Output Frequency: 50 Hz

• LRF: Hokuyo UTM 30LX

Used Outputs: h


• Optical Flow Sensor: Avago ASDN3080 coupled with 16mm lens

Used Outputs: ∆XOF ,∆YOF


• GPS System1

Software: ASHTEC MB100 with Real Time Kinematic (RTK) corrections

Antenna: Trimble AV33

Used Outputs: Ip


1used in the outdoor flight tests

33

The flights tests were performed by controlling the quadrotor with 2.4 GHz radio control and the goal was

to simulate a flight with approximately constant height in which a rectangular shaped area was covered.

The continuous observers defined in the previous chapter were implement in a user defined function in

MATLAB® and simulated in SIMULINK® using standard ode45 integration algorithm with ∆t = 0.02s.

The solution presented here can be implemented in others UAV’s that are equipped with the neces-

sary sensors. Nevertheless the experiments made in the first two sections of this chapter are essential

in order to obtain more accurate results when using a different experimental set up, since the velocity

estimation algorithm depends on sensor calibration (distance measurements and angular compensa-

tion) and position of the OF sensor regarding the IMU. The trajectory estimation procedures can be

implemented in any dynamic system provided that the necessary dynamic variables are known.

Figure 5.1: Mikrocopter QuadroKopter XL

5.1 K estimation

5.1.1 Experiment Description

As it was stated in section 4.2, due to lack of knowledge of the OF sensor internal computations the

constant KOF was introduced in equation 4.14 to correctly scale the sensor’s output to the real travelled

distance.

The estimation of KOF can be easily achieved by looking at the output data when travelling a known

distance without angular movement. In this case, a person walked in a pre-defined straight line while

keeping one of the OF axis aligned with the desired path. This way it is known that the output data from

the referred axis should provide the length covered by the person after the necessary computations.

34

5.1.2 Results

When travelling a known distance, the only unknown in equation 4.14 is KOF . Evaluating it over the

experiment time yields:

∆sreal =d

30

Ns∑i=1

∆sofh

fKof (5.1)

where ∆sreal represents the real displacement, ∆sof the optical flow sensor output and Ns is the

number of samples

Each row from table 5.1 presents the data from a single experiment used for the estimation process. It is

possible to see the values for the real travelled distance, OF sensor’s output, distance from the surface

and light conditions. These experiment conditions were intentionally altered from one trial to the other

so the data reflect the diversity of conditions in which the sensor could be used.

∆sreal [m]Ns∑i=1

∆sof [cpi] h [m] Light Conditions

2.5 1266 0.85 indoor2.5 1300 0.82 indoor3 1477 1.03 indoor3 1731 0.72 outdoor3 1776 0.73 outdoor3 1507 0.81 indoor3 1466 0.82 indoor3 1499 0.99 outdoor3 1438 1 outdoor3 1553 1.01 outdoor3 1388 1.01 outdoor5 2558 0.99 outdoor5 2727 1.01 outdoor5 2606 1 outdoor6 2809 1.04 indoor9 4584 1.01 indoor

Table 5.1: Sensor Calibration Data

For each experiment data shown in table 5.1 there is a value of KOF that fulfils equation 5.1. In order

to estimate a constant value it is possible to use a maximum likelihood estimator. Assuming that the

measurements are corrupted with Additive White Gaussian Noise, the estimated value of KOF is given

by:

35

Kof =

30fd

Ne∑j=1

(∆sreal)j

hj

Ns∑i=1

∆sof

Ne

⇔

⇔ Kof =

Ne∑j=1

(Kof )j

Ne= 0.0569 [

1

cpi]

(5.2)

where Ne is the total number of experiments

Figure 5.2 provides a graphical view of values distribution achieved for (KOF )j in equation 5.2. Apart

from that, it is possible to see the final estimated value and additional statistical information of the

estimation process.

Figure 5.2: Gain KOF estimation

Assuming that the measurement errors are normally distributed and that the data shown in table 5.1 and

figure 5.2 fully represent the sensor’s behaviour, it is possible to say that 95.4% of the measurements

will have less than 20% error.

36

5.2 Angular Compensation


The angular compensation procedure demonstrated in section 4.2.2 was tested experimentally in the

set up shown in figure 5.3. It is possible to see that the quadcopter is fixed laterally so the only degree

of freedom is the rotation with respect to the line that connect the fixed points.

The goal of this experiment is to use the previous knowledge that v = 0 to test the performance

of the optical flow angular compensation. The OF sensor will detect movement due to the pure

rotation, but using the angular compensation shown in equation 4.14 the computed translational velocity

should be zero.

The angular movement done by the UAV was approximately sinusoidal varying pitch or roll angles sep-

arately. In an standard UAV flight, the pitch and roll angles are only punctually bigger than ±10◦ . Nev-

ertheless, in order to make sure that the angular compensation deals with the all regular angular flight

positions the tested angles were extended to ±20◦ .

Figure 5.3: Angular Compensation Experiment

5.2.2 Results

Figure 5.4 present the angular variations and the computed linear velocity for pitch and roll movements

when applying the set of equations shown in 4.14.

It is possible to see in the figures that the rotation induces sinusoidal velocities with peak values of

1 ∼ 1.5 ms−1. Although the computed compensated velocity has a sinusoidal shape as well, the peak

values range between 0.1 ∼ 0.2 ms−1.

The mean of the relative error for the angular compensation is presented in equation 5.3, in which the

subscript ”p” and ”r” correspond to pitch and roll respectively.

37

(a) u(t) and (vOF )x(t) (b) v(t) and (vOF )y(t)

Figure 5.4: Angular Compensation Experiment Results

εr = 12.9171% (5.3a)

εp = 12.9106% (5.3b)

Due to the fact that the quadcopter is under actuated and its dynamics is influenced by vibrations modes,

the angular compensation plays a major role in the estimates accuracy. Although the compensation

errors whose source are the mathematical approximations explained in section 4.2.2 can not be reduced,

the ones that come from how the data is acquired can. For example, in the previous results it is

assumed that both IMU and OF sensor are installed at the vehicle centre of mass. More important,

it is assumed that the optical flow measures are not influenced by the current angular speed of

the camera. It is possible to take in account these differences from the ideal model to the experimental

set up by estimating the parameters in equation 4.14.

Since the linear velocity of the quadcopter is known to be zero, the set of equations shown in 4.14 are

reduced to:

1

f

vOF xvOF y

=

wy

−wx

(5.4)

It is possible to estimate this relationship experimentally if1

fis replaced for a general affine function

y = mx + b. This yields equation 5.5 and makes possible to take in account differences between the

38

ideal model and the experimental set up.

vxmp

vymr

+

bpbr

=

wy

−wx

(5.5)

Figure 5.5 shows the relationship between the OF measured velocity and the angular rates for the same

experiment as in figure 5.4. The calculated linear fitting is marked in red and the estimated values are:

mr = 55.32

mp = 56.03

br = −0.001398

bp = −0.003213

(5.6)

The parameters estimated are rich in a way that its values are similar to the theoretical ones but

show the expected small deviations from it. During this process it was also possible to see that the

quantization of the OF readings influences the angular compensation. However, it is not possible to

diminish its influence without changing the sensor.

(a) Pitch (b) Roll

Figure 5.5: Angular Compensation Parameter Estimation

Applying the identified angular compensation model, it is possible to see that the induce velocity is the

same, but the computed translational velocities peak values range is reduced from 0.1 ∼ 0.2 ms−1 to

0.05 ∼ 0.01 ms−1. Figures 5.6 and 5.7 compare the results from the theoretical compensation to the

estimated one. The average relative error of the estimated model is shown in equation 5.7

39

εr = 5.8958%

εp = 7.88%

(5.7)

Figure 5.6: Modelled Pitch Compensation Figure 5.7: Modelled Roll Compensation

5.3 Flight Tests


Although the results presented in section 5.2 and 5.1 can already provide a reference for the estimation

performance, they result of experiments done in a set of conditions that are completely different from

an actual flight. Therefore, flight tests were performed in real outdoor environment in Hangar 13 in

Montijo. A ground GPS station was set to provide RTK corrections to the GPS data received by

the quadrotor and the flight was manually controlled using radio control.

The results will be divided in two different parts. The performance of the estimation process will be

addressed first and secondly it will be compared with current alternatives. In all the results presented

it will be assumed that the GPS information retrieved is the ground truth since it is the most precise

information available.

In order to have a deeper understanding of the proposed solution performance, it is important to compare

it with current procedures for velocity and position estimation that rely on the same data as the one

available in the used experimental set up. Apart from the proposed solution other two different models

for the estimation process will be evaluated. The one introduced in equation 4.45 will be referred as M2

and the solution shown in figure 5.8 will be referred as M3. The observer structure and dynamics is the

40

same as M2 but it does not use optical flow information. Therefore, the velocity estimates that are used

to estimate the vehicle’s trajectory are based only in the vehicle’s dynamic equation 4.18.

VelocityObserver

GPSData

vIMU

LRF

pPositionObserver

Figure 5.8: Compared Solutions

The constants defined throughout this work were set as following:

• k1 = 1

• k2 = 0.1

• Kob = 10

• KGPS = 75

All the results shown in the next section were computed by post-processing data retrieved and logged

during the flight. MATLAB® and SIMULINK® were used to perform the results shown in the following

chapters.

5.3.2 Results

Since it was not possible to test the trajectory estimates in a indoor environment due to lack of true

trajectory values and external influences in the magnetometer (eg. cellphones and metal structures), a

rectangular trajectory of 5m of length and 3m of width was performed while carrying the quadrotor in an

outdoor environment while receiving GPS data. The direction of movement was always aligned with the

X body axis, so the influence of non constant heading can also be evaluated.

In figure 5.9 it is possible to see the 2D trajectory data retrieved from the first manual experiment.

The blue line shows GPS obtained data and the green line was obtained by using the proposed solution

trajectory estimator. The measured travelled distance error is given by:

ε(%) =|∆s−∆s|

∆s=

=|16.3925− 16.9675|

16.3925100

= 3.5077%

(5.8)

It is clear that trajectory error estimated is considerably larger than the travelled distance error, which

means that the error is caused by the projection of the body frame estimated velocity to the inertial

frame. It is possible to see in figure 5.9a that the estimated heading angle in green is not consisted

with rectangular shaped trajectory. In order to access the reason for this large estimate error, the

41

heading angle was estimated using only the rotational matrix dynamics from equation 2.6 and

a second estimated trajectory computed using the same velocity estimates from the proposed

solution. The results obtained with this approach is shown in red in figure 5.9. It is possible to see that

the general trend of the heading angle is the one expected and consequently the estimated trajectory is

more accurate than the previous estimate.

The proposed solution rotation matrix didn’t present clear deviations during the flight tests, which indi-

cates that the error observed in the manual tests were due external influences like cellphones close to

the quadrotor magnetometer.

This result proves that the accuracy of the trajectory estimates using on board sensor is highly

dependent on the accuracy of the rotation matrix. Therefore, even if more accurate techniques are

found to estimate the body frame velocity, it will not necessarily result into more accurate estimated

trajectories.

A second manual experiment was performed in the same trajectory at a bigger speed but in which the

rectangular trajectory was travelled two consecutive times in the same direction. The results presented

in figure 5.10 show that the travelled distance measures were not affected by the increasing speed of the

experiment and its travelled distance error shown in equation was smaller regarding the first experiment.

ε(%) =|30.9183− 31.42|

30.9183100 = 1.62% (5.9)

The second experiment results confirmed the conclusions retrieved with the first experiment that the

heading estimates limits the estimated trajectory accuracy.

After the manual experiments, flight tests were performed. The left column of figures in 5.11 show both

the norm and inertial components of the velocity estimates. The proposed solution data is presented in

red, the M3 velocity is presented in green and the GPS velocity is presented in blue. The right column

shows the estimate error of the proposed solution.

The mean of the estimate velocity norm error has an approximated value of −0.0066 ms−1 and the

estimate error in each of the inertial component has an approximated average of −0.025 ms−1. The

higher order of magnitude in the inertial velocity components error can be explained by the

induced error in the projection of the body frame velocity. It is possible to see in figure 5.9a that the

magnetometer readings have a considerable settling time. Looking at the big heading changes around

10 and 40 seconds during the flight test in figure 5.12, it is expected that the heading angle errors

on those periods are considerably large. Using the equation 4.56 from section 4.3, a 3◦ error already

42

(a) ψ(t) (b) Travelled Distance

(c) 2D Trajectory

Figure 5.9: Manual 5m x 3m Trajectory Experiment

provides an 0.0027 ms−1 error, which is the same order of magnitude as the obtained estimate velocity

norm error.

The peaks observed in the estimate error coincide with more abrupt manoeuvres since there is additional

error originated by the angular compensation. This can be easily seen in figure 5.13 in which the non

compensated velocity is presented in brown. This doesn’t present limitations in terms of the estimator

applicability since angular movements of that magnitude are rare in a standard quadrotor flight.

The norm of the estimated velocity can still be used to shown the importance of the velocity observer

introduced in section 4.2.3. It is possible to see in figure 5.14 that the observer considerably

reduces the noise originated in the numeric differentiation of the OF displacement measures

and its output quantization. It is important to state that the observer does not introduce visible delay

43

(a) 2D Trajectory (b) Travelled Distance

Figure 5.10: Manual 5m x 3m Trajectory(2 turns)

in the velocity estimation.

Figure 5.15 shows the 3D position estimation for the proposed solution estimator and its error regard-

ing the GPS data. The colours follow the same designation as in the previous images. Although the

estimated trajectory provides good indications of the quadrotors general trajectory, the estimation error

grows in time since the position estimate is originated from pure integration of the velocity data. Even in

the ideal case that the velocity estimator has mean zero error, it is not possible to bound the error.

The results presented in figure 5.16 show M2 and M3 results for four different TGPS values. In the left

column the red and green line represent M2 and M3 trajectory estimates and in the right column the

estimate error for M2 is shown. For TGPS = 25s, the trajectory provided by M3 is not presented because

its accuracy is several order of magnitudes lower than M2.

It is possible to see that the introduction of an OF sensor to the usual set of sensor’s composed by

gyroscopes and accelerometers considerably improves the trajectory estimate for any of the cases. In

fact, while M3 can only provide a acceptable trajectory with GPS updates every second, M1 estimates

error are under 1 m in the entire scope of the experiment with the lower update rate tested.

44

(a) ||Iv(t)|| (b) || Iv(t)|| Estimation Error

(c) Ivy(t) (d) Iv(t)y Estimation Error

(e) Ivx(t) (f) Iv(t)x Estimation Error

Figure 5.11: Velocity Estimation Data

45

Figure 5.12: ψ(t) during Flight Experiment

(a) Flight Pitch Compensation (b) Flight Roll Compensation

(c) θ(t) (d) φ(t)

Figure 5.13: Angular Compensation Action During Flight

46

Figure 5.14: Velocity Filter Action During Flight

(a) Trajectory Estimation (b) Estimation Error

Figure 5.15: Proposed Solution Position Estimates

47

(a) TGPS = 15 s (b) Estimation Error

(c) TGPS = 5 s (d) Estimation Error

(e) TGPS = 1 s (f) Estimation Error

Figure 5.16: M2 and M3 Position Estimates

48

Chapter 6

Conclusions

This thesis proposed a velocity and position estimator based on low cost on-board sensors. Apart from

the standard sensors composed by accelerometer and gyroscopes, an mice optical flow sensor is used

as a velocity indicator. The induced velocity due to rotation movements is compensated by fusing IMU

data. The proposed solution is tested in a quadrotor as the ability to estimate position and velocity with

on board sensors would open the door for several new applications in indoor/GPS-denied environments.

The proposed estimators are linear estimators in which global stability is proven by assuming that true

values of velocity and position are received.

Experiments that go from sensor calibration, modelling the angular compensation and manual tests are

presented and form a set of necessary experiments for anyone who intends to use a similar solution

to the one proposed here. It is shown that the angular velocity of the camera influences the quality

of the measured data by presenting more accurate angular compensations with the identified model

than the ones achieved with theoretical results. Consequently, the defined modelling objectives were

accomplished.

Outdoor tests were made in order to be able to compare the proposed solutions results to the ones

achieved with GPS data. The velocity estimator presents promising results, but its use in automation

processes could not be accessed. The outdoor tests show that the heading estimate might be a bottle-

neck for the accuracy of the position estimator since the estimated velocities are necessarily projected

to the inertial frame in order to estimate the position. Due to lack of accuracy in the proposed solution

in terms of position estimates, GPS data at a lower rate was included as true position measurements.

This intermediate solution simulates environments where although GPS is available, its transmission rat

is not constant and might even not be available long periods.

49

Regarding future developments of the proposed solution, a SLAM algorithm could be added by using

LRF data. Similar algorithms for mapping were already used in [14, 15]. Since the used set up already

contains an LRF to provide altitude readings, its information could be used for mapping algorithms

without changing the experimental set up. The use of a more robust estimator for heading values could

also be applied in order to check the influence of the heading accuracy in the observers implemented in

the presented work.

50

Bibliography

[1] Partha S Bhagavatula, Charles Claudianos, Michael R Ibbotson, and Mandyam V Srinivasan. Op-

tic flow cues guide flight in birds. Current biology : CB, 21(21):1794–9, November 2011. ISSN

1879-0445. doi: 10.1016/ j.cub.2011.09.009. URL http: // www. ncbi. nlm. nih. gov/ pubmed/

22036184 .

[2] E.W. Justh and P.S. Krishnaprasad. Steering laws for motion camouflage. Proceedings of

the Royal Society A: Mathematical, Physical and Engineering Sciences, 462(2076):3629–3643,

December 2006. ISSN 1364-5021. doi: 10.1098/ rspa.2006.1742. URL http: // rspa.

royalsocietypublishing. org/ cgi/ doi/ 10. 1098/ rspa. 2006. 1742 .

[3] Jan Wendel, Oliver Meister, Christian Schlaile, and Gert F. Trommer. An integrated gps/mems-

imu navigation system for an autonomous helicopter. Aerospace Science and Technology, 10

(6):527 – 533, 2006. ISSN 1270-9638. doi: http:// dx.doi.org/ 10.1016/ j.ast.2006.04.002. URL

http: // www. sciencedirect. com/ science/ article/ pii/ S1270963806000484 .

[4] Stabilization of a mini rotorcraft with four rotors”. IEEE Control Systems Magazine, 25:45–50, 2005.

[5] Samir Bouabdallah and Roland Siegwart. Backstepping and Sliding-mode Techniques Applied to

an Indoor Micro Quadrotor. (April):47–52, 2005.

[6] a. Tayebi and S. McGilvray. Attitude stabilization of a VTOL quadrotor aircraft. IEEE Transactions

on Control Systems Technology, 14(3):562–571, May 2006. ISSN 1063-6536. doi: 10.1109/ TCST.

2006.872519. URL http: // ieeexplore. ieee. org/ lpdocs/ epic03/ wrapper. htm? arnumber=

1624481 .

[7] Cosmin Coza and C J B Macnab. A New Robust Adaptive-Fuzzy Control Method Applied to Quadro-

tor Helicopter Stabilization guide X4a2Q )/ bi rF ( x ) Tc1 Ei ( X ). pages 475–479.

[8] Pedro Batista, Carlos Silvestre, and Paulo Oliveira. Sensor-based Complementary Globally Asymp-

totically Stable Filters for Attitude Estimation. (978):7563–7568, 2009.

51

http://www.ncbi.nlm.nih.gov/pubmed/22036184

http://www.ncbi.nlm.nih.gov/pubmed/22036184

http://rspa.royalsocietypublishing.org/cgi/doi/10.1098/rspa.2006.1742

http://rspa.royalsocietypublishing.org/cgi/doi/10.1098/rspa.2006.1742

http://www.sciencedirect.com/science/article/pii/S1270963806000484

http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=1624481


[9] Kenneth D Sebesta and Nicolas Boizot. A Real-Time Adaptive High-Gain EKF , Applied to a Quad-

copter Inertial Navigation System. 61(1):495–503, 2014.

[10] Jong-hyuk Kim, Salah Sukkarieh, and Stuart Wishart. Real-Time Navigation , Guidance , and

Control of a UAV Using Low-Cost Sensors. pages 299–309, 2006.

[11] H.Jin Kim and David H. Shim. A flight control system for aerial robots: algorithms and experiments.

Control Engineering Practice, 11(12):1389 – 140, 2003. ISSN 0967-0661. doi: http:// dx.doi.org/

10.1016/ S0967-0661(03)00100-X. URL http: // www. sciencedirect. com/ science/ article/

pii/ S096706610300100X . Award winning applications-2002 {IFAC}World Congress.

[12] Mark Muller, S. Lupashin, and R. D’Andrea. Quadrocopter ball juggling. 2011 IEEE/RSJ Interna-

tional Conference on Intelligent Robots and Systems, pages 5113–5120, September 2011. doi:

10.1109/ IROS.2011.6094506. URL http: // ieeexplore. ieee. org/ lpdocs/ epic03/ wrapper.

htm? arnumber= 6094506 .

[13] Markus Hehn and Raffaello D’Andrea. A flying inverted pendulum. 2011 IEEE International Con-

ference on Robotics and Automation, (2):763–770, May 2011. doi: 10.1109/ ICRA.2011.5980244.

URL http: // ieeexplore. ieee. org/ lpdocs/ epic03/ wrapper. htm? arnumber= 5980244 .

[14] Jakob Engel and Daniel Cremers. Camera-Based Navigation of a Low-Cost Quadrocopter. pages

2815–2821, 2012.

[15] Abraham Bachrach, Samuel Prentice, Ruijie He, and Nicholas Roy. RANGE-Robust autonomous

navigation in GPS-denied environments. Journal of Field Robotics, 28(5):644–666, September

2011. ISSN 15564959. doi: 10.1002/ rob.20400. URL http: // doi. wiley. com/ 10. 1002/ rob.

20400 .

[16] Simon Zingg, Davide Scaramuzza, Stephan Weiss, and Roland Siegwart. MAV Navigation through

Indoor Corridors Using Optical Flow. pages 3361–3368, 2010.

[17] Spencer Ahrens, Daniel Levine, Gregory Andrews, and Jonathan P How. Vision-Based Guidance

and Control of a Hovering Vehicle in Unknown , GPS-denied Environments. pages 2643–2648,

2009.

[18] R. Mahony, P. Corke, and T. Hamel. Dynamic Image-Based Visual Servo Control Using Cen-

troid and Optic Flow Features. Journal of Dynamic Systems, Measurement, and Control, 130

(1):011005, 2008. ISSN 00220434. doi: 10.1115/ 1.2807085. URL http: // dynamicsystems.

asmedigitalcollection. asme. org/ article. aspx? articleid= 1475501 .

52

http://www.sciencedirect.com/science/article/pii/S096706610300100X

http://www.sciencedirect.com/science/article/pii/S096706610300100X




http://doi.wiley.com/10.1002/rob.20400

http://doi.wiley.com/10.1002/rob.20400

http://dynamicsystems.asmedigitalcollection.asme.org/article.aspx?articleid=1475501

http://dynamicsystems.asmedigitalcollection.asme.org/article.aspx?articleid=1475501

[19] Dominik Honegger, Lorenz Meier, Petri Tanskanen, Marc Pollefeys, and Z Eth. An Open Source

and Open Hardware Embedded Metric Optical Flow CMOS Camera for Indoor and Outdoor Appli-

cations.

[20] J. G. Maks D. Jeltsema G. J. Olsder, J. W. van der Woude. Mathematical Systems Theory, 4th

Edition. VSSD, 2011.

[21] David C. Lay. Linear Algebra and Its Applications, Fourth Edition. Assison - Wesley, 2012.

[22] H.K. Khalil. Nonlinear Systems. Prentice Hall, 2002. ISBN 9780130673893. URL http: // books.

google. pt/ books? id= t_ d1QgAACAAJ .

[23] Raymond A. Serway John W. Jewett. Physics for Scientists and Engineers with Modern Physics,

Seventh Edition. David Harris, 2008.

[24] E. Russell David Mazurek Phillip Cornwell Ferdinand Beer, Jr. Johnston. Vector Mechanics for

Engineers Dynamics: Statics and Dynamics. McGraw-Hill Education, 2012.

[25] R.C. Hibbeler. Engineering Mechanics - Dynamics, Thirteenth Edition. Prentice Hall, 2012.

[26] James J. Gibson. The Perception of the Visual World. Houghton Mifflin, 1950.

[27] John Radford Andrew Burton. Thinking in Perspective: Critical Essays in the Study of Thought

Processes. Methuen, 1978.

[28] Edward R. Strelow D.H. Warren. Electronic Spatial Sensing for the Blind: Contributions from Per-

ception, Rehabilitation, and Computer Vision. Springer, 1985.

[29] Avago Technologies. Adns-3080, high-performance optical mouse sensor, October 2008.

datasheet.octopart.com/ADNS-3080-Avago-datasheet-14184657.pdf.

[30] Mitch Bryson and Salah Sukkarieh. Vehicle Model Aided Inertial Navigation for a UAV using Low-

cost Sensors. 2006.

[31] Johan E. Mebius. Derivation of the euler-rodrigues formula for three-dimensional rotations from the

general formula for four-dimensional rotations. 2007.

53

http://books.google.pt/books?id=t_d1QgAACAAJ

http://books.google.pt/books?id=t_d1QgAACAAJ

Quadrotor Control using Optical-Flow Navigation · Quadrotor Control using Optical-Flow Navigation...

Documents

Transcript of Quadrotor Control using Optical-Flow Navigation · Quadrotor Control using Optical-Flow Navigation...