PhD Thesis - Deterministic Position and Attitude Estimation Methodsusers.isr.ist.utl.pt/~sbras/online_papers/sbras_thesis.pdf · where Doppler measurements are available instead of

UNIVERSIDADE DE LISBOA

INSTITUTO SUPERIOR TECNICO

Deterministic Position and Attitude Estimation

Methods

Sergio Daniel Goncalves Gante Bras

Supervisor: Doctor Carlos Jorge Ferreira SilvestreCo-Supervisor: Doctor Paulo Jorge Coelho Ramalho Oliveira

Thesis approved in public session to obtain the PhD Degree inElectrical and Computer Engineering

Jury final classification: Pass with Distinction

Jury

Chairperson: Chairman of the IST Scientific Board

Members of the committee:Doctor Maria de Fatima da Silva LeiteDoctor Maria Isabel Lobato de Faria RibeiroDoctor Paulo Jorge Coelho Ramalho OliveiraDoctor Silvere BonnabelDoctor Pedro Tiago Martins Batista

2015

UNIVERSIDADE DE LISBOA

INSTITUTO SUPERIOR TECNICO

Deterministic Position and Attitude Estimation

Methods

Sergio Daniel Goncalves Gante Bras

Supervisor: Doctor Carlos Jorge Ferreira SilvestreCo-Supervisor: Doctor Paulo Jorge Coelho Ramalho Oliveira

Thesis approved in public session to obtain the PhD Degree inElectrical and Computer Engineering

Jury final classification: Pass with Distinction

Jury

Chairperson: Chairman of the IST Scientific Board

Members of the committee:Doctor Maria de Fatima da Silva Leite

Full Professor, Faculdade de Ciencias e Tecnologia, Universidade de CoimbraDoctor Maria Isabel Lobato de Faria Ribeiro

Full Professor, Instituto Superior Tecnico, Universidade de LisboaDoctor Paulo Jorge Coelho Ramalho Oliveira

Associate Professor, Instituto Superior Tecnico, Universidade de LisboaDoctor Silvere Bonnabel

Assistant Professor, Mines-Paris Tech, FranceDoctor Pedro Tiago Martins Batista

Invited Assistant Professor, Instituto Superior Tecnico, Universidade de Lisboa

Funding InstitutionFundacao para a Ciencia e Tecnologia

2015

“Quelli che s’innamoran di pratica sanza scienzia son come ’l nocchier ch’entra in

navilio sanza timone o bussola, che mai ha certezza dove si vada.”

“He who loves practice without theory is like the sailor who boards ship without a

rudder and compass and never knows where he may cast.”

- Leonardo da Vinci

Abstract

This thesis proposes new deterministic position and attitude estimation methods. The

devised solutions are based on nonlinear observers and on the methodologies of set-valued

observers (SVOs).

Two nonlinear observers are developed for indoor scenarios. The first observer fuses

inertial information with measurements from an active vision system. Furthermore, a

pan and tilt controller is proposed so that the visual markers are kept in the camera’s

field of view. The second observer is based on range measurements from acoustic sensors

fixed in the moving body reference frame to beacons installed in the inertial frame. The

performance of both solutions is experimentally assessed by resorting to custom-built

experimental prototypes and a high-accuracy motion rate table that provides ground truth

data. Additionally, the vision-based estimator is used in closed-loop with a quadrotor

controller to stabilize this aerial platform.

When an accurate dynamic model is available, it can be exploited to improve the accu-

racy of the pose (position and orientation) estimates. Under this condition, the problem of

3-D motion estimation is firstly addressed by assuming the existence of an accurate model

and full state measurements, and afterwards, by considering a more realistic scenario,

where Doppler measurements are available instead of translational velocity measurements.

Under the framework of SVOs, a different solution to the attitude estimation problem

is proposed that provides a bounding set containing the true rotational state. The sensor

readings are assumed to be corrupted by unknown bounded measurement noise and con-

stant gyro bias. A set containing the rate gyro bias is also obtained and used to reduce

the uncertainty of the estimates. Sufficient conditions for boundedness of the estimated

set for the cases of multi- and singlevector observations are given.

Additionally, two fault detection and isolation (FDI) methods based on bounding sets

vii

are proposed for navigation systems equipped with sensors providing inertial measure-

ments and vector observations.

Keywords: Navigation systems, position and attitude estimation, fault detection and

isolation, inertial sensors, Lie group numerical integrators, nonlinear observers, set-valued

observers.

viii

Resumo

Nesta tese desenvolvem-se novos metodos determinısticos para a estimacao de posicao

e atitude de plataformas autonomas. As solucoes propostas sao baseadas em observadores

nao lineares e na metodologia de observadores de conjuntos.

Neste ambito sao desenvolvidos dois observadores nao lineares de atitude para missoes

no interior de edifıcios. O primeiro observador funde medidas inerciais com medidas prove-

nientes de um sistema de visao activa por computador. Adicionalmente, um controlador

para os eixos de rotacao e elevacao da camara e projectado de forma que as marcas visuais

sejam mantidas no seu campo de visao. O segundo observador, no lugar de visao por

computador, utiliza medidas de distancia entre sensores acusticos fixos no referencial do

veıculo e emissores fixos num referencial inercial. Ambas as solucoes sao validadas atraves

de prototipos experimentais e de uma mesa de calibracao. Esta fornece dados compara-

tivos de alta precisao. A solucao baseada em visao e ainda implementada em conjunto

com um controlador a bordo de um quadrirotor e os resultados das estimativas obtidas

em voo sao comparados com as medidas obtidas por um sistema avancado de captura de

movimento.

Quando existe um modelo fiavel da dinamica do veıculo, este pode ser utilizado para

melhorar as estimativas de posicao e atitude. O problema de estimacao de posicao e

atitude de uma plataforma movel e tratado em dois cenarios distintos. Primeiramente

utilizando um modelo dinamico preciso e informacao de estado completa e, em seguida,

considerando o cenario mais realista em que as medidas de velocidade translacional sao

substituıdas pelas medidas de um sensor de efeito de Doppler.

Uma outra solucao baseada em observadores de conjuntos e tambem proposta. Este

estimador fornece conjuntos que contem, no seu interior, a verdadeira atitude do sistema.

Assume-se que as medidas dos sensores sao corrompidas por ruıdo de magnitude limitada

ix

e polarizacoes constantes. Tambem e calculado um conjunto que contem o valor da polar-

izacao dos giroscopios que e utilizado para reduzir a incerteza das estimativas de atitude.

Sao descritas condicoes suficientes para a obtencao de estimativas sob a forma de conjun-

tos limitados para o caso em que existem multiplas observacoes vectoriais e para o caso

em que existe apenas uma observacao vectorial.

Adicionalmente, propoem-se dois metodos para deteccao e isolamento de falhas em

sistemas de navegacao equipados com sensores inerciais e observacoes vectoriais.

Palavras-chave: Sistemas de navegacao, estimacao de posicao e atitude, deteccao e iso-

lamento de falhas, sensores inerciais, integracao numerica em grupos de Lie, observadores

nao lineares, observadores de conjuntos.

x

Acknowledgements

First and foremost, I would like to thank my advisor Professor Carlos Silvestre and my

co-advisor Professor Paulo Oliveira for encouraging me to embrace this endeavor. I am

very grateful for their guidance and all the insightful discussions we shared as well as for

their trust on my work, for fomenting autonomy, and for giving me the freedom to explore

new ideas and challenges.

I would also like to acknowledge the follow-up thesis committee, Professor Isabel

Ribeiro and Professor Fatima Leite, for the acute and insightful comments and for support-

ing the thesis proposal. Their constructive criticism has greatly contributed to improve

this thesis.

During my doctoral studies, I had the exciting opportunity to study abroad at the

New Mexico State University and collaborate with Doctor Amit Sanyal and Maziar Izadi,

which has welcomed me at his house and has become a good friend. I owe a profound

thanks to Jose Vasconcelos, Rita Cunha, Paulo Rosa and David Cabecinhas, who were

key for the success of our joint work. They have shared with me their great scientific

knowledge and provided me inspiration and advice to overcome all the difficulties. The

experimental apparatus developed to support this research work would not be possible

without the contribution of Pedro Batista, Bruno Cardeira, Jose Tojeira and, in particular,

of David Cabecinhas whose work allowed the in-fight validation of the proposed estimation

algorithms onboard a quadrotor.

I am also thankful to all those who have contributed to a relaxed and joyful working

atmosphere, namely to Tiago Gaspar, Carlos Neves, Joao Almeida, Manuel Rufino, Pedro

Casau, Daniel Viegas, Daniel Silvestre, and Pedro Lourenco. I will always miss the un-

countable jokes, laughs, endless discussions, beach volleyball games, lunches and coffees

we all shared. I also thank to the rest of the DSOR family, in particular to Pedro Serra,

xi

Marco Morgado, Luis Sebastiao, Bruno Guerreiro, Bruno Gomes, Duarte Antunes, Andre

Oliveira, Joao Botelho, Filipa Almeida and Loic Bamde. A special word of gratitude

goes to Joana Coelho, Sergio Pequito and Andre Serranho. Their unique perspective on

research and life were a light beacon on a sometimes foggy path.

The last but not the least thanks goes to my family. For their unconditional love and

support, for being always present and for cheering me up during the lows of life.

Financial support I also greatly acknowledge Fundacao para a Ciencia e Tecnologia for

financially supporting my research work through the PhD grant SFRH/BD/47456/2008,

and also the ISR-Lisbon for hosting me and providing the all the necessary facilities and

equipment.

xii

Contents

1 Introduction 1

1.1 Historical perspective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Literature review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2.1 Algebraic methods for attitude and position estimation . . . . . . . 3

1.2.2 Attitude and position estimation based on Kalman filtering . . . . . 3

1.2.3 Attitude and position estimation using nonlinear observers . . . . . 4

1.2.4 Attitude estimation using a single vector observation . . . . . . . . . 7

1.2.5 Other filtering solutions for attitude estimation . . . . . . . . . . . . 8

1.2.6 Set-valued observers . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.2.7 Navigation systems using cameras . . . . . . . . . . . . . . . . . . . 9

1.2.8 Navigation systems using range measurements . . . . . . . . . . . . 10

1.2.9 Model-based 3-D rigid body estimation . . . . . . . . . . . . . . . . 11

1.2.10 Fault detection and isolation in inertial measurement units . . . . . 11

1.3 Main contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

1.3.1 Attitude estimation using an active vision system . . . . . . . . . . . 13

1.3.2 Attitude estimation using ranges . . . . . . . . . . . . . . . . . . . . 14

1.3.3 3-D rigid body motion estimation . . . . . . . . . . . . . . . . . . . . 15

1.3.4 Robust attitude estimation . . . . . . . . . . . . . . . . . . . . . . . 16

1.3.5 Fault detection and isolation in inertial measurement units . . . . . 16

1.4 Thesis outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2 Rigid body motion 21

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.2 Attitude representations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

xiii

Contents

2.2.1 Rotation matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2.2.2 Euler angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2.2.3 Euler angle-axis representation . . . . . . . . . . . . . . . . . . . . . 23

2.2.4 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

2.3 Rigid body kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.4 Rigid body dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

2.5 Rigid body motion using homogeneous coordinates . . . . . . . . . . . . . . 26

2.6 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3 Nonlinear observer for attitude estimation based on active vision 29

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.2 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

3.2.1 Sensor suite . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.2.2 Attitude kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.2.3 Problem summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.3 Attitude observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3.3.1 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

3.4 Camera pan and tilt controller . . . . . . . . . . . . . . . . . . . . . . . . . 42

3.5 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

3.5.1 Discrete-time algorithm . . . . . . . . . . . . . . . . . . . . . . . . . 45

3.5.2 Lens distortion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

3.6 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

3.6.1 Time-varying rate gyro bias . . . . . . . . . . . . . . . . . . . . . . . 52

3.6.2 Multi-rate algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

3.7 Experimental prototype on a calibration motion rate table . . . . . . . . . . 54

3.7.1 Experimental setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

3.7.2 Using the measurement noise to tune the observer gains . . . . . . . 55

3.7.3 Wahba’s problem and the multiplicative extended Kalman filter . . . 56

3.7.4 Experiment 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

3.7.5 Experiment 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

3.8 Experimental implementation onboard a quadrotor . . . . . . . . . . . . . . 69

3.9 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

4 A nonlinear attitude observer based on range and inertial measurements 79

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79


xiv

Contents

4.3 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

4.4 Tuning the observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88


4.6 Experimental apparatus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92

4.6.1 Discrete-time implementation . . . . . . . . . . . . . . . . . . . . . . 93

4.6.2 MEMSense nIMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

4.6.3 Cricket localization system . . . . . . . . . . . . . . . . . . . . . . . 94

4.6.4 Motion rate table . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95

4.6.5 Calibration of the position of the Cricket motes . . . . . . . . . . . . 96

4.6.6 Observer gains . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96

4.7 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

4.8 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

5 Nonlinear observer for 3-D rigid body motion using full state feedback 103

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103


5.3 Observer synthesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

5.4 Observer properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

5.4.1 Almost global exponential stability . . . . . . . . . . . . . . . . . . . 112

5.4.2 Unmodeled torques and forces . . . . . . . . . . . . . . . . . . . . . 113

5.5 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115

5.6 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115

6 Nonlinear observer for 3-D rigid body motion using Doppler measure-

ments 119

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119

6.2 Problem formulation and measurement model . . . . . . . . . . . . . . . . . 120

6.3 Observer design with Doppler measurements . . . . . . . . . . . . . . . . . 122

6.4 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127

6.5 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128

7 Global attitude and gyro bias estimation based on set-valued observers 131

7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131


7.2.1 System description . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

7.2.2 Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133

7.3 Set-valued observers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134

xv

Contents

7.3.1 SVO formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135

7.3.2 Observability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139

7.4 Attitude and rate gyro bias estimation using SVOs . . . . . . . . . . . . . . 139

7.4.1 Discretization of the system . . . . . . . . . . . . . . . . . . . . . . . 139

7.4.2 Set of states compatible with the measurements . . . . . . . . . . . . 141

7.4.3 SVO design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143

7.4.4 Attitude estimation with exact angular velocity measurements . . . 145

7.4.5 Attitude estimation using a single vector observation . . . . . . . . . 146

7.5 Implementation issues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147


7.7 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151

8 Fault detection and isolation in IMUs based on bounding sets 153

8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153


8.2.1 Measurement model . . . . . . . . . . . . . . . . . . . . . . . . . . . 154

8.2.2 Dynamic model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156

8.2.3 Faults description . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157

8.3 FDI using hardware redundancy . . . . . . . . . . . . . . . . . . . . . . . . 157

8.3.1 Fault detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157

8.3.2 Fault isolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160

8.4 FDI using analytical redundancy and SVOs . . . . . . . . . . . . . . . . . . 163

8.4.1 Fault detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164

8.4.2 Fault isolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167

8.5 Simulations results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170

8.6 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174

9 Conclusions and future work 177

9.1 Future directions of research . . . . . . . . . . . . . . . . . . . . . . . . . . . 180

Appendix A Lie groups and Lie algebras 183

A.1 Lie groups . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183

A.2 Lie algebras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185

A.3 Exponential map and logarithmic map . . . . . . . . . . . . . . . . . . . . . 186

A.4 Actions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187

Appendix B Numerical integration on Lie groups 191

xvi

Contents

B.1 Crouch-Grossman method . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191

B.2 Munthe-Kaas method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193

B.3 Commutator-free method . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194

Appendix C Bound on the exponential map of the sum of two skew-

symmetric matrices 197

References 199

Publications 213

xvii

List of Figures

2.1 Z-Y-X Euler rotations from B to A. . . . . . . . . . . . . . . . . . . . . . . 23

3.1 Diagram of the experimental setup. . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.2 Block diagram of the attitude observer and camera controller. . . . . . . . . . 33

3.3 Projection of the visual markers on the image plane. . . . . . . . . . . . . . . . 34

3.4 Illustration of the image coordinates of four markers and their centroid as seen

in the image plane. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3.5 Algorithm flowchart, where µ is the ratio between the sampling rates of the

rate gyros and of the image acquisition. . . . . . . . . . . . . . . . . . . . . . . 46

3.6 Experimental results for the image distortion compensation. . . . . . . . . . . . 49

3.7 Simulated trajectory and position of the markers (where vB denotes the velocity

of the vehicle with respect to the local frame). . . . . . . . . . . . . . . . . . . 49

3.8 Attitude observer estimates for two different sets of gains. . . . . . . . . . . . . 50

3.9 Distance between the markers center and the image centroid and the pan and

tilt actuation for two different sets of gains. . . . . . . . . . . . . . . . . . . . . 51

3.10 Path followed by the centroid of the markers in the image plane for two different

sets of gains. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

3.11 Simulations with a time varying rate gyro bias. . . . . . . . . . . . . . . . . . . 52

3.12 Simulations with a time varying rate gyro bias. . . . . . . . . . . . . . . . . . . 53

3.13 Attitude estimation error using the continuous-time observer, the single-rate

observer and the multi-rate observer. . . . . . . . . . . . . . . . . . . . . . . . . 53

3.14 Experimental setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

3.15 Hardware used in the real time prototype. . . . . . . . . . . . . . . . . . . . . . 54

3.16 Angular rate measurements of the first experiment. . . . . . . . . . . . . . . . . 56

xix

List of figures

3.17 Average quadratic error for several sets of gains k and kb. . . . . . . . . . . . . 57

3.18 Attitude determination using the solution to Wahba’s problem and ground

truth data. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

3.19 Attitude estimates using the proposed algorithm and ground truth data. . . . . 61

3.20 Comparison of the estimation errors brought about by the use of the Wahba’s

solution and by the use of the proposed observer. . . . . . . . . . . . . . . . . . 62

3.21 Rate gyros bias estimates. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

3.22 Attitude estimates using an MEKF and ground truth data. . . . . . . . . . . . 63

3.23 Comparison of the estimation errors brought about by the use of the MEKF

and by the use of the proposed observer. . . . . . . . . . . . . . . . . . . . . . . 63

3.24 MEKF rate gyros bias estimates. . . . . . . . . . . . . . . . . . . . . . . . . . . 64

3.25 Time evolution of the camera pan and tilt position and velocity and of the

markers’ centroid in the image plane. . . . . . . . . . . . . . . . . . . . . . . . . 66

3.26 Attitude determination using the solution to Wahba’s problem and ground

truth data. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

3.27 Attitude estimation using the proposed algorithm and ground truth data. . . . 67

3.28 Comparison of the estimation errors brought about by the use of the Wahba’s

solution and by use of the observer. . . . . . . . . . . . . . . . . . . . . . . . . 67

3.29 Angular rate bias estimation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

3.30 Attitude estimates using an MEKF and ground truth data. . . . . . . . . . . . 68



3.32 MEKF rate gyros bias estimates. . . . . . . . . . . . . . . . . . . . . . . . . . . 70

3.33 Experimental apparatus at SCORE Lab – University of Macau: AscTec Pelican

quadrotor, fiducial markers, and motion capture system. . . . . . . . . . . . . . 71

3.34 Block diagram of the overall system comprising the nonlinear observer, the

camera controller, the quadrotor controller, the pan and tilt steering platform

and the quadrotor itself. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

3.35 Time evolution of the camera pan and tilt position and velocity and of the

markers’ centroid in the image plane. . . . . . . . . . . . . . . . . . . . . . . . . 73

3.36 Trajectory of the markers in the image plane. . . . . . . . . . . . . . . . . . . . 73

3.37 Attitude estimation using the proposed nonlinear observer and the attitude

ground truth. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74


ground truth. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

xx

List of figures


ground truth. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75



3.41 Proposed observer and MEKF rate gyros bias estimates. . . . . . . . . . . . . . 76

3.42 Position error. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

4.1 Frames and navigation system configuration. . . . . . . . . . . . . . . . . . . . 81

4.2 Normalized histogram of nω and nv using simulation data, where the range

and rate gyro measurements are corrupted by Gaussian noise. . . . . . . . . . . 89

4.3 Block diagram of the transformed system (4.22). . . . . . . . . . . . . . . . . . 90

4.4 Attitude estimation error. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92

4.5 Angular velocity bias estimation error. . . . . . . . . . . . . . . . . . . . . . . . 92

4.6 Lyapunov function VT = ηTPη. . . . . . . . . . . . . . . . . . . . . . . . . . . . 92

4.7 Time derivative of the Lyapunov function. . . . . . . . . . . . . . . . . . . . . . 93

4.8 Time evolution of the estimation error ‖η(t)‖ and the theoretical upper bound. 93

4.9 Inertial unit MEMSense nIMU (a) and Cricket mote (b). . . . . . . . . . . . . 94

4.10 Experimental setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95

4.11 Cricket motes positions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97

4.12 Attitude estimates provided by the MEKF and ground truth obtained with the

calibration table. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

4.13 Attitude estimates provided by the proposed observer and ground truth ob-

tained with the calibration table. . . . . . . . . . . . . . . . . . . . . . . . . . . 99


and the proposed attitude observer. . . . . . . . . . . . . . . . . . . . . . . . . 99

4.15 Angular rate bias estimates from the MEKF and from the proposed nonlinear

observer. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

5.1 Rigid body trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116

5.2 Norm of the estimation error in linear and logarithmic scale. . . . . . . . . . . 117

5.3 Norm of the unmodeled forces and torques, ϕd. . . . . . . . . . . . . . . . . . . 117

5.4 Norm of the estimation error in the presence of unmodeled forces and torques. 117

6.1 Rigid body trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127

6.2 Norm of the estimation error of the observer under nominal conditions and in

the presence of disturbances. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128

xxi

List of figures

7.1 Example of a polytope. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134

7.2 Convex hull of the different set propagations. . . . . . . . . . . . . . . . . . . . 137

7.3 Illustration of an SVO iteration. . . . . . . . . . . . . . . . . . . . . . . . . . . 138

7.4 Attitude trajectory of the rigid body. . . . . . . . . . . . . . . . . . . . . . . . . 150

7.5 Error of the central point of the rotation vector set. . . . . . . . . . . . . . . . 150

7.6 Uncertainty limits along the main axes. . . . . . . . . . . . . . . . . . . . . . . 151

7.7 Error of rate gyro bias estimates. . . . . . . . . . . . . . . . . . . . . . . . . . . 151

8.1 Illustration of the proposed FD method to exploit hardware redundancy: non

faulty rate gyros. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159

8.2 Illustration of the proposed FD method to exploit hardware redundancy: faulty

rate gyros. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159

8.3 Proposed fault detection filter for navigation systems. . . . . . . . . . . . . . . 166

8.4 Proposed fault detection and isolation filter for navigation systems. . . . . . . . 167

8.5 Different stages of the FDI filter based on SVOs. . . . . . . . . . . . . . . . . . 168

8.6 Nominal state v1x, estimate of the nominal SVO and estimate of the robust

SVO. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174

8.7 Estimates of the SVOs tolerant to faults #1 to #6. . . . . . . . . . . . . . . . . 175

xxii

List of Tables

3.1 Number of elementary operations in each step for second order CG and MK

methods. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

3.2 RMS of the estimation errors of the nonlinear (NL) observer and of the Wahba’s

solution and of the MEKF. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

3.3 RMS of the estimation errors of the nonlinear (NL) observer and of the Wahba’s

solution and of the MEKF. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

7.1 RMS of the estimation errors of the SVO and of the MEKF. . . . . . . . . . . 149

8.1 Mean (µ) and standard deviation (σ) of the number of iterations necessary to

detect and isolate the faults and percentage of correct isolations (% Corr. Isol.)

with five gyros and five sensors per vector. . . . . . . . . . . . . . . . . . . . . . 172

8.2 Mean (µ) and standard deviation (σ) of the number of iterations necessary to

detect and isolate the faults and isolate the faults and percentage of correct

isolations (% Corr. Isol.) with three gyros and three sensors per vector. . . . . 172

B.1 Butcher tableau. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193

B.2 Coefficients of a second-order Crouch-Grossman method (CG) method. . . . . 193

B.3 Coefficients of a third-order CG method. . . . . . . . . . . . . . . . . . . . . . 193

B.4 Coefficients of a second-order Munthe-Kaas method (MK) method. . . . . . . 194

B.5 Coefficients of a third-order MK method. . . . . . . . . . . . . . . . . . . . . . 194

B.6 Coefficients of a second-order commutator-free Lie group method (CF) method. 195

B.7 Coefficients of a third-order CF method. . . . . . . . . . . . . . . . . . . . . . 195

xxiii

Notation

Reference frames

I inertial reference frame.

L local reference frame – assumed to be inertial.

B body-fixed reference frame, which is rigidly attached to the vehicle.

C camera reference frame, which has its origin at the camera’s center

of projection and the z-axis is aligned with the optical axis.

Sets, spaces and manifolds

∅ empty set.

N set of natural numbers.

R set of real numbers.

Rn set of real vectors of dimension n.

Rn×m set of real matrices of dimension n×m.

O(n) orthogonal group, O(n) = U ∈ Rn×n : UTU = I, whose dimen-

sion is n(n− 1)/2.

B(n) n-dimensional ball, B(n) = x ∈ Rn : xTx ≤ 1.

S(n) n-dimensional sphere, S(n) = x ∈ Rn+1 : xTx = 1.

SO(3) set of rotation matrices which compose the special orthogonal group

SO(3) = R ∈ R3×3 : R ∈ O(3),det(R) = 1.

SE(3) special Euclidean group composed of a rotation matrix R ∈ SO(3)

and a translational vector p ∈ R3.

xxv

Notation

Set(M,m) polytope defined by x ∈ Rn : Mx ≤ m, where M ∈ R

m×n and

m ∈ Rm.

Matrices and vectors

In identity matrix of dimension n×n – the subscript n can sometimes

be omitted for convenience of notation.

1n×m a block matrix of ones with n rows and m columns – also let 1n =

1n×n, and when the subscripts n and m are omitted, consider 1

with the appropriate implicit dimensions.

0n×m a block matrix of zeros with n rows and m columns – also let 0n =

0n×n, and when the subscripts n and m are omitted, consider 0

with the appropriate implicit dimensions.

diag(A1, . . . ,An) block diagonal matrix with the elements A1, . . . ,An in the main

block diagonal.

rot(θ,λ) rotation matrix defined by Euler angle-axis parametrization with

the angle θ and the unit vector λ.

xx, xy, xz x-, y- and z-axis components of the 3× 1 vector x.

xi ith element of the vector x.

ei unit norm vector with element ei = 1.

Eij Eij denotes the 3× 3 matrices whose elements are zeros except for

the element of row i and column j which is one.

[.]ij matrix element at row i and column j.

Functions

std(.) standard deviation.

expm(.) exponential map that is generalization of the ordinary exponential

function and maps a Lie algebra to the associated Lie group. The

exponential maps on SO(3) and SE(3) are denoted by expmSO(3)(.)

and expmSE(3)(.), respectively.

xxvi

Notation

logm(.) logarithmic map, which is the inverse function of the exponential

map. The logarithmic maps on SO(3) and SE(3) are denoted by

logmSO(3)(.) and logmSE(3)(.), respectively.

dexpm-1(.) inverse of the differential of the exponential map [PC05].

σmin(.) minimum singular value.

σmax(.) maximum singular value.

(.)T standard matrix transpose operator.

(.)−1 matrix inverse, such that for a square matrix A, comes that

A−1A = I.

(.)H conjugate transpose or Hermitian transpose.

(.)† pseudo-inverse of a matrix. Let A ∈ Rn×m with m ≤ n, then ATA

is not singular and A† = (ATA)−1AT . Let B ∈ Rn×m with n ≤ m,

then BBT is not singular and B† = BT (BBT )−1.

(.)× skew symmetric operator such that (x)×y = x× y, x,y ∈ R3.

(.)⊗ unskew operator such that ((x)×)⊗ = x, x ∈ R3.

(.)∨ isomorphic transformation from R6 to se(3) defined as

ξ∨ =

(ω)× v

01×3 0

∈ se(3),

where ξ = [ωT vT ]T and ω,v ∈ R3.

(.)∧ isomorphic transformation from se(3) to R6 that satisfies ((ξ)∨)∧ =

ξ, ξ ∈ R6.

sign(x) sign of x, i.e., sign(x) = x|x| if x 6= 0, and sign(x) = 0 if x = 0.

tr(.) matrix trace.

rank(.) matrix rank.

det(.) matrix determinant.

span(.) vector space spanned by the arguments.

ker(.) kernel or null space of the argument matrix.

vec(.) operator which stacks the columns of a matrix with m columns and

n rows into a mn× 1 vector, from left to right.

xxvii

Notation

A⊗B Kronecker product of matrices A and B. Let A ∈ Rm×n and B ∈

Rp×q, then

A⊗B =

[A]11B · · · [A]1nB...

. . ....

[A]m1B · · · [A]mnB

.

q⊠ p quaternion product of q by p, where q,p ∈ S(3). This non-

commutative product can be computed by q⊠ p = Ψ(q)p, where

Ψ(q)p =

qsI3 + (qv)× qv

−qTv qs

pv

ps

,

and where qv and qs denote the vector and scalar components of q,

respectively, and pv and ps denote the vector and scalar components

of p, respectively.

Norms

‖.‖2 Euclidean norm of vectors and matrices. For a matrix A ∈ Rm×n,

we have ‖A‖2 := supx 6=0

(‖Ax‖2‖x‖2

)

= σmax(A), where x ∈ Rn and

sup(.) denotes the supremum. For sake of simplicity the subscript

is sometimes omitted so that ‖.‖2 ≡ ‖.‖.‖.‖∞ infinity norm of vectors, ‖x‖∞ := max|x1|, . . . , |xn|, x ∈ R

n.

‖.‖max maximum norm of matrices, ‖A‖max := maxi,j|[A]ij |, A ∈ Rn×m.

‖.‖F Frobenius norm of matrices, ‖A‖F :=√

tr(ATA) =√∑m

i=1

∑nj=1[A]ij , A ∈ R

m×n.

|.| absolute value of scalar.

xxviii

List of acronyms

λ-UGES λ-parameterized uniformly globally exponentially stable (system) 39–41

λ-ULES λ-parameterized uniformly locally exponentially stable (system) 39

CF commutator-free Lie group method xxiii, 45, 47, 180, 191, 195

CG Crouch-Grossman method xxiii, 45, 47, 93, 180, 191, 193, 195

EKF extended Kalman filter 3, 4, 8, 9, 11, 57, 148, 149, 180

FD fault detection 11, 12, 158, 166–168

FDI fault detection and isolation 11, 12, 17, 19, 153, 154, 157, 160, 163, 168, 170, 173,

174

GPS global positioning system 3, 4, 6, 7, 10, 77, 180

IBVS image-based visual servoing 44, 178

IEKF invariant extended Kalman filter 4

IMU inertial measurement unit 12, 17, 19, 153, 154, 174, 179

ISS input-to-state stable 30, 44, 77, 177

KF Kalman filter 171, 173

LP linear programming 143, 148, 164

LTV linear time-varying 4, 9, 15, 18, 37, 39, 40, 178

xxix

List of acronyms

MEKF multiplicative extended Kalman filter 4, 15, 56–60, 63–66, 68, 69, 74–76, 98–100,

149–151

MEMS micro-electro-mechanical system 12, 13, 157

MK Munthe-Kaas method xxiii, 45, 47, 180, 191, 193–195

RF radio frequency 10, 81, 94

RMS root-mean-square 61, 64, 65, 68, 149

SLAM simultaneous localization and mapping 10

SVO set-valued observer 9, 16, 17, 19, 132, 135, 136, 138, 139, 144, 145, 147–154, 157,

164–168, 170, 173, 174, 179

UAV unmanned aerial vehicle 3, 13, 29, 30, 57, 69, 77, 79, 119, 120, 149, 177

xxx

Chapter 1

Introduction

The challenges associated with the determination of position and orientation have ac-

companied mankind since the earliest days. However, the technologies devised in the last

few decades, such as powerful computational devices and new and more accurate sensors,

associated with their affordable cost have sparked the development of novel solutions for

a wider range of operational conditions. The objective of this thesis is to explore deter-

ministic methodologies to propose and evaluate new theoretically validated position and

attitude estimation algorithms tailored to different scenarios and using different method-

ologies. In the deterministic framework, noise and other uncertainties are not considered,

only deterministic information such as upper bounds on the magnitude can be taken into

account. The advantage of this approach is the existence of systematic mathematical tools

for analysis of the properties of linear and nonlinear system, such as stability, convergence

speed and performance that are the key drivers of the design of each proposed solution.

1.1 Historical perspective

Ever since humans started migrating throughout the surface of the Earth, there was

the need to determine their location and the direction in which they were traveling.

Ancient travelers relied mainly on the Sun, stars, and topographical landmarks to

determine their position and orientation. The rise and set of the Sun established two of the

four main geographic directions. In fact, some studies suggest that, the names of Europe

and Asia derived from the Phoenicians names Ereb (sunset) and Asu (sunrise), respectively

[BM09, p. 446]. Around 1000 BC, Polynesians and Melanesians were able to sail from

1

1. Introduction

island to island in the Pacific Ocean using a navigation system based on the stars. But with

years of experience and empirical knowledge accumulated over generations, other valuable

navigation clues were used, such as wind characteristics, sea color, and flight path of birds

[Maj13]. On the other side of the world, in the desert, sand dunes morphology, and growth

pattern of any vegetation were used by the nomads crossing the desert to guide them

through the most inhospitable lands [Maj13]. When the early sailors, in particular the

Europeans, started venturing into uncharted waters, more advanced navigation methods

were required. By using a compass to measure the direction of the Earth’s magnetic

field, or by observing the Polaris (North Star) or the Sun, they could determine their

orientation. To know their position they required both latitude (angular distance to the

equator) and longitude (angular distance to a reference geodesic1 connecting both poles).

A ship’s latitude can be determined by the distance of the North Star to the horizon.

One of the earliest instrument to measure latitude was the Kamal, invented around the

10th century, which consisted of a set of small pieces of wood whose center was fixed to

a cord [BD09]. In 1419, the Portuguese initiated a series of sea expeditions along the

coast of Africa aiming to expand Christianity and to establish new commercial routes

with Africa and Asia. During this period, known as Age of Discovery, remarkable feats

were achieved. In 1488, Bartolomeu Dias passed the Cape of Good Hope [Arn13]. During

his first voyage, in 1492, Columbus arrived at the New World instead of reaching Japan

as he had intended, and in 1497–1498 Vasco da Gama established a sea route to Asia

[Arn13]. In 1519–1522, the first circumnavigation of the globe is accomplished by a fleet

initially lead by Fernao de Magalhaes [Arn13]. This series of achievements has motivated

the creation or improvement of many navigation instruments that were used to measured

latitude such as the maritime astrolabe, the backstaff, the sextant, and the octant [BD09].

The determination of longitude, however, posed a much more challenging problem to the

European navigators. Initially, a dead reckoning system was used. A log tied to a rope

with knots at fixed intervals was thrown at the sea and the number of knots that passed

the back of the ship during a fixed time period was used to determine its speed [BD09]. By

knowing the speed, the heading, and the time passed, one could calculate the longitude.

Interesting enough is the fact that the measurement unit commonly used to express the

speed at the sea is still the knot. An alternative way to determine longitude was to

measure the local time of the ship and compare it to the time at a location whose longitude

was known. However, the timing instruments in those days, such as the hourglass, were

not accurate enough. This problem was only solved in 1761 with the invention of the

1Geodesic is a generalization of straight line to curved spaces, i.e., it is the shortest line on a curvedspace connecting two points.

2

1.2 Literature review

maritime chronometer by John Harrison, which was designed such that humidity and

thermal changes would not affect significantly its accuracy [BD09]. The local time was

measured using the Sun high noon or instruments such as the nocturnal.

More recently, the dawn of the digital era associated with new low-cost sensors have

motivated a remarkable development of new solutions to tackle the navigation problem.

State-of-the-art rate gyros, magnetometers, and accelerometers provide invaluable data

that can be used to determine position and attitude. An important navigation tool is the

global positioning system (GPS) developed by the Department of Defense of the United

States of America as well as the GLONASS, Galileo, and COMPASS, from Russia, Euro-

pean Union and People’s Republic of China, respectively. The GPS is composed of at least

24 satellites emitting unique radio signals received by a device that resorting to trilatera-

tion methods can obtain a fix on its position with sub-meter accuracy [VD09]. However,

the GPS is not reliable on situations such as indoors and in the vicinity of tall structures,

and the achieved sampling period may not be compatible with the control of fast dynamics

vehicles, such as unmanned aerial vehicles (UAVs). In such cases, mathematical sensor

fusion techniques (e.g. Kalman filtering) with alternative sensors are necessary.


1.2.1 Algebraic methods for attitude and position estimation

Attitude estimation plays an essential role in many modern platforms such as aircrafts,

satellites, unmanned air vehicles, and underwater autonomous robots. It is a classical esti-

mation problem that, despite its rich historical background, continues to attract intensive

research and experiencing new advances. Over the years, a wide variety of estimation

techniques has been proposed to address this problem. Some of the earliest solutions are

purely algebraic, like the solution to Wahba’s problem [Wah65]. Several alternatives are

described in the literature, such as the TRIAD [SO81], the Davenport’s q-method whose

solution is based on quaternions [MM00], the QUEST algorithm [MM00], the ESOQ algo-

rithms [MM00], and the SVD algorithm [MM00].

The extension of the Wahba’s problem to also include the estimation of position has

been considered in [AHB87] and [Ume91].

1.2.2 Attitude and position estimation based on Kalman filtering

More advanced solutions rely on the system dynamics to obtain better accuracy and

noise reduction. This is the case of the approaches based on Kalman filtering [Far70,

AM79]. The extended Kalman filter (EKF) has been widely used to address nonlinear

3

1. Introduction

estimation problems [LM82, YF03, Cra06]. Other solutions have been proposed to over-

come the limitations of the EKF in estimating states evolving on the group of rotations

SO(3), namely, the multiplicative extended Kalman filter (MEKF) [LM82, Mar03] and

the invariant extended Kalman filter (IEKF) [BMS09]. The work in [RH04] presents a so-

lution for attitude estimation based on a switching architecture of non-standard Kalman

filters, which uses data from a 3-axis rate gyro and a 3-axis accelerometer. An algorithm

for attitude estimation based on sigma-point Kalman filters and measurements from ac-

celerometers and magnetometers is proposed in [HMS07], and a Kalman filter that resorts

on a novel measurement model is described in [CBIO06]. In the latter approach, the mea-

surement noise depends on the quaternion, however, model linearization is avoided. In

[VSO11], a navigation system is proposed where inertial integration is fused with vector

observations and GPS measurements using an EKF.

The EKF is a very powerful estimation technique. However, none of the EKF-based

estimators gives guarantees of optimality, stability or convergence. Using a sensor-based

approach, attitude estimation solutions are proposed in [BSO11b, BSO12c]. These so-

lutions compute the estimation error on the measurement space which is linear. Thus,

standard Kalman filters for linear time-varying (LTV) systems can be applied yielding

global stability of the augmented error system. A similar approach has been success-

fully exploited to address the position estimation problem with different sensor suites

[BSO10, BSO11a, MBOS11].

1.2.3 Attitude and position estimation using nonlinear observers

Among the dynamic estimators, nonlinear observers have emerged as an attractive

alternative given the possibility to establish convergence bounds and provide stability

guarantees. Moreover, the associated computational cost is, in general, smaller than the

computational cost associated with EKF-based solutions, and the tuning of design param-

eters is more straightforward. In the framework of the nonlinear observers, the character-

istics of the sensor noise are taken into account by proper tuning of the feedback gains,

which effectively controls the estimator bandwidth. The stability properties of nonlinear

observers can be analyzed resorting to Lyapunov theory [Kha02], input-to-state-stability

[Son07] and other more recent techniques such as density functions [VRSO11].

Research on the problem of designing a stabilizing law for systems evolving on man-

ifolds, where attitude is represented in its natural space as an element of the special or-

thogonal group SO(3) and the rigid body configuration (pose) is an element of the special

Euclidean group SE(3), can be found in [Kod89, FI04, CM06, MKS06]. These references

4


provide important guidelines for observer design and highlight the topological limitations

to achieve global stabilization on SO(3) and SE(3).

The seminal paper [Sal91] proposes an eventually globally exponentially convergent

attitude observer resorting to the quaternions representation. This work makes use of

attitude, velocity, and torque measurements, and is based on the rigid body kinematics

and dynamics. In [VF00], a nonlinear observer for estimation of position, velocity and

attitude is proposed adopting quaternions as attitude representation. This observer was

later extended in [TS03] to also estimate bias in the rate gyro measurements.

Although quaternions provide a singularity-free attitude representation, their use could

lead to undesired phenomena such as unwinding [BB00]. To overcome this disadvantage,

several solutions were proposed whose attitude estimates evolve directly on the group of

rotations SO(3). Many of these solutions are solely based on the rigid body kinematics,

which has the advantage of being an exact description of the physical quantities evolved.

In [PHS07], a nonlinear observer for attitude and rate gyro bias estimation is proposed,

fusing measurements of rate gyros with accelerometers and magnetometers. The work in

[MHP08] revisits the observer of [PHS07] and proposes an alternative in which the mea-

sured rotation matrix is not present in the feed-forward term of the observer. An explicit

solution based on measurements from accelerometers and magnetometers is proposed and

the case with only accelerometers’ measurements is also studied. A position and atti-

tude observer is proposed based on velocity and landmark measurements in [VCSO10].

In this work, whose attitude error definition is distinct from the ones used in [PHS07]

and [MHP08], the resulting estimation errors are almost globally stable and exponentially

stable in any ball inside the region of attraction. The feedback law is given as an explicit

function of the sensor measurements, avoiding previous attitude or position reconstruction.

The observers in [PHS07, MHP08, VCSO10] exhibit almost global convergence of the ob-

server error, that is, for all initial conditions and system trajectories other than on a set

of zero measure, the estimated states converge to the desirable equilibrium point. As men-

tioned before, topological obstacles hinder global asymptotic stability of smooth dynamic

systems evolving on SO(3) and SE(3). These limitations arise since these manifolds are

not diffeomorphic to an Euclidean vector space. Such condition on the underlying space

is necessary for the existence of a global stabilizing continuous feedback as discussed in

[MKS06] and references therein.

The development of observers on SO(3) and S(3) is closely associated with the study

of symmetry-preserving observers (also denoted as invariant observers in some literature)

[BMR08, BMR09]. The symmetry-preserving formalism is the basis of the work in [MS10],

5

1. Introduction

where an attitude and rate gyro bias observer for a flying rigid body is proposed. This

solution uses quaternions as attitude representation. Moreover, its implementation in a

low-power 8-bit microcontroller demonstrate the good computational efficiency of attitude

filtering based on nonlinear observers.

Even though the bias in the angular velocity measurements is often assumed constant

in the design of attitude observers, in practice, slowly time-varying bias is also properly

estimated. In [NF99], the authors take a different approach and estimate a time-varying

rate gyro bias assuming that its time-evolution is determined by a known linear model

with known inputs. However, in many situations, such model is not available or it depends

on unknown quantities.

Gradient-like observers for Lie-groups are developed in [LTM10]. These observers have

exponential convergence and almost global asymptotic stability. As application examples,

observers in SO(3) and SE(3) are presented. These solutions, however, do not consider

the existence of bias. The work in [Hua10] describes two observers tailored to scenarios

with significant linear accelerations, which use measurements of rate gyros, accelerometers

and magnetometers. Additionally, linear velocity measurements provided by GPS, make

possible the explicit estimation of velocity, which is used to compensate the linear acceler-

ation component in the measurements of the accelerometers. The first observer exhibits

semi-global exponential convergence, whereas the second ensures almost global conver-

gence when the vehicle’s linear acceleration is constant. However, none of the observers

estimates rate gyro bias.

A nonlinear observer for a left-invariant dynamical system with states evolving on

SE(3) and implicit outputs is proposed in [RCAL11]. In such circumstances, the observer

design methodology for symmetry-preserving systems [BMR08, BMR09] cannot be applied.

The solution in [RCAL11] can be exploited in dynamic vision applications such as the

estimation of the motion of a monocular camera from a sequence of images. In case

the system is subject to measurement disturbance and noise, the observer converges to a

neighborhood of the real solution.

The work in [GSJ12] proposes observers for interconnected nonlinear and linear sys-

tems. The authors assume that a corresponding quadratic-type Lyapunov function is

already available for the nonlinear system whose output is the input of a linear system,

whose state measurement is, at least, partially measured. This solution is used to develop

an attitude and position estimator based on inertial and magnetic measurements as wells

as GPS data. However, it does not consider bias in the rate gyros.

The paper [GFJS12] revisits the work in [MHP08] establishing that, by adding a pa-

6


rameter projection to the bias estimation, the observer in [MHP08] is still applicable when

the reference vectors are time-varying and the gyro measurements are biased. The sec-

ond result in the paper is an observer for estimation of bias in the body-fixed vector

measurements.

A sensor-based approach is exploited in [BSO12b, BSO12c], where global exponential

convergence of the (augmented) error system is established. In these works, the estimated

state is the sensor measurement rather than the attitude, which, in turn, can be obtained

from the estimated states resorting to one of the solutions to the Wahba’s problem (see

Section 1.2.1). Additionally, rate gyro bias is also estimated when two or more non-

collinear reference vectors are available. The work in [GTJS15] follows the same strategy as

[BSO12a, BSO12b] by embedding the space of rotation matrices into R3×3 and estimating

attitude in this higher dimensional space. The authors show that time-varying reference

vectors (in addition to inertially-fixed vectors) can be used as inputs to estimate attitude

and gyro bias. Building on the results for interconnected cascaded systems in [GSJ12],

the attitude observer is then augmented to also estimate the rigid body motion exploiting

GPS-like measurements and globally exponentially stability of the ensemble is established.

Nonlinear observers are usually formulated in the continuous-time domain. The com-

putational implementation of these algorithms calls for advanced discrete-time integration

algorithms. The selection of a suitable numerical method is vital since it should preserve

the geometric properties of the differential equations. The development of numerical inte-

gration methods that preserve the geometric properties of Lie groups has witnessed a re-

markable progress in the last decades. These methods were originally proposed by Crouch

and Grossman in [CG93] and their general order conditions can be found in [OM99]. In

[MK98], the author constructs generalized Runge-Kutta methods for numerical integra-

tion of differential equations evolving on Lie groups. In this work, the computations are

performed on the Lie algebra, which is a linear space. More recently, commutator-free

Lie group methods were proposed to overcome some of the problems associated with the

computation of commutators [CMO03, Owr06]. A comprehensive overview of geometric

numerical integration methods and their application to multi-body dynamics evolving on

SE(3) can be found in [PC05]. More applications of Lie group integration methods are

presented in [BP03].

1.2.4 Attitude estimation using a single vector observation

One of the earliest works devoted to the estimation of attitude resorting to a single

vector measurement is based on a least-squares solution and can be found in [CFG+97].

7

1. Introduction

An EKF-based solution is presented in [AP11], and the work in [W07] uses a single vec-

tor observation and an adaptive approach to address the challenges of underwater vehicle

navigation. An adaptive approach is also adopted in [ASZ06]. The consequences of the ex-

istence of time-varying vector observations (with respect to the inertial reference frame) for

attitude estimation is analyzed in [TMHL12]. This paper shows that, a single time-varying

vector observation with a rich enough trajectory is sufficient to design an almost globally

stable attitude observer. The richness of the vector observation trajectory is directly asso-

ciated with the notion of persistence of excitation [Sas99]. A globally exponentially stable

observer is described in [BSO12a]. This solution is based on the embedding of SO(3) into

R9, and even though the observer state does not evolve on the rotation group, an explicit

solution on SO(3) is provided whose error is shown to converge exponentially fast to zero

for all initial conditions. Following a similar approach, another globally exponentially

stable estimator is proposed in [GTJS15]. A single vector observer with rate gyro bias

compensation is presented in [NS13]. A distinct solution using a set-membership approach

is proposed in [LLMS07]. In this thesis, a solution to attitude estimation based only on one

vector observation that also uses a set-membership approach is described in Section 7.4.5.

However, the set containing the state is described using polytopes, whereas in [LLMS07],

the attitude uncertainty is described by ellipsoids.

1.2.5 Other filtering solutions for attitude estimation

The work in [Sol10] provides new conditions defining a diffusion and a Brownian motion

on an implicitly defined Riemannian manifold embedded in an Euclidean space. These

are exploited to develop a state space model for diffusions and Brownian motions on

SO(3). Using these results, a solution to the stochastic attitude estimation problem is

given. Another stochastic approach to the problem of filtering on SO(3) can be found

in [BB13, BB15]. The problem of obtaining minimum-energy attitude estimates defined

on SO(3) is considered in [ZTM11]. This solution requires solving a time-varying Riccati

differential equation that depends on estimates and measurements. A second-order optimal

filtering solution on Lie groups with vectorial outputs is proposed in [STMA13]. This

estimator minimizes the cumulative energy of unknown deterministic system disturbances.

When applied to attitude estimation in SO(3) and with the selection of an appropriate

affine connection, the estimator assumes the form of a gradient observer whose filter gains

are given by a matrix Riccati differential equation.

8


1.2.6 Set-valued observers

While, in the framework of nonlinear observers, the sensor measurements are assumed

to be exact, having a deterministic nature, in EKF-based estimation, the measurement

uncertainty is typically modeled as additive Gaussian noise. However, the probabilistic

distributions of the sensor noise and system disturbances may not be available in some

scenarios, while magnitude bounds are typically known. In these circumstances, optimal

stochastic state estimation is not achievable, and thus the objective of the estimator is

rather to obtain a set of possible state values given the available sensor data.

The set-valued estimation methods, also referred to as set-membership estimation

methods, are based on the main assumption that the noise in the external inputs and

sensor measurements is unknown but bounded. These methods were first introduced in

the late 60’s and early 70’s, in the seminal works [Sch68, BR71]. They were later ex-

ploited to address the problem of system identification by considering measurement noise

bounded by ellipsoids [FH82, DH87] and bounded by a minimal box [MB82]. More re-

cently, methods based on the same principles were successfully developed for a wide range

of fields, such as robust estimation and control [YL09], system identification [MN04], sig-

nal processing [ABA12], information theory [Wel96], robotics [ZD10], navigation systems

[Cal01, CGLP05] and fault detection [NBL+09].

In [ST97], a set-valued observer (SVO) with a formulation close to the one adopted in

Chapter 7 and Chapter 8 of this thesis is proposed for nonlinear systems. In this solution,

the sets of possible state vectors take the form of polytopes whose centers are the optimal

state estimates. The work in [SLLM08b] exploits a different approach and proposes an at-

titude estimator where uncertainty ellipsoids bound the sensor measurements and the filter

state. However, this estimator relies on model linearization to propagate the uncertainty

ellipsoids. In [RSSA09], an SVO is used as identification subsystem in a multiple-model

adaptive control methodology. This methodology was later applied to model falsification

[RSA11] and fault detection and isolation [RSSA10a, RSSA10b]. An algorithm to prop-

agate polytopes containing the set of all possible states of an LTV system with optimal

disturbance rejection is presented in [ST99]. This algorithm is valuable for the efficient

computational implementation of SVOs.

1.2.7 Navigation systems using cameras

The EKF-based estimators and the nonlinear observers are used to fuse measurement

data from different sensors. The non-ideal characteristics of the inertial sensors, namely

bias, misalignment, and noise renders inaccurate the attitude obtained by integration of

9

1. Introduction

inertial data. External aiding sensors, such as GPS receivers, can be used to obtain more

accurate estimates. However, in GPS-denied environments, such as indoors or in the

vicinity of buildings and other structures, alternative aiding sensors are required.

The use of cameras as positioning sensors in robotics has its most significant represen-

tative in the body of work devoted to vision-based control (see for example [CH06, CH07,

CV04, CC05] and references therein). The literature on vision-based rigid body stabiliza-

tion and estimation highlights important questions and indicates possible solutions to i)

keeping marker’s visibility along the system trajectories for a large region of attraction

[CWK02], ii) minimizing the required knowledge about the 3-D model of the observed

object [MC02], iii) guaranteeing convergence in the presence of camera parametric uncer-

tainty and image measurement noise [MC02] and iv) establishing observability conditions

for attitude estimation [RG03]. The problem of pose (position and attitude) estimation in

computer vision has also received considerable attention. The work in [MTR+09] presents

an inertial navigation system aided by computer vision for estimating relative position,

attitude and velocity. A framework to design pose estimators based on vision sensors is

given in [AH06, RCAL11]. A variety of algebraic and iterative methods based on point

and line correspondences has been proposed (see for example [AD03, Dav07, YSKS04]).

Although algorithms based solely on image measurements can determine the position and

attitude of a camera, they can greatly benefit from the integration with inertial sensors,

namely, rate gyros and accelerometers as well as from the use of dynamic filtering and

observer techniques [RG03, LD03, YKK08, MTR+09].

1.2.8 Navigation systems using range measurements

In addition to visual data, many solutions proposed in the field of robotics rely on

range data as aiding sensors. Range measurements are very suitable for algorithms aimed

at simultaneous localization and mapping (SLAM) [PWA10], for extraction of features

such as doors and room corners [LO10], for automated inspection system of manufactured

parts [PBLR02], for obstacle detection and avoidance, and for pose estimation [VCS+11].

In [ZR08], an analytic method to determine the relative position and attitude of a pair of

communicating robots using ranges measurements is described. Range measurements can

be obtained using sensors such as laser range finders and stereo cameras. An affordable

alternative is to use acoustic sensors. In the ranging system described in [PCB00], beacons

transmit simultaneously an acoustic signal and a radio frequency (RF) synchronization

signal. The receiver then measures the time difference of arrival between both signals and

computes the distance to the beacons using the speed of sound. Other advantages of these

10


sensors are their compact size and low-power consumption.

1.2.9 Model-based 3-D rigid body estimation

When an accurate model for the forces and moments is available, one can take advan-

tage of this additional information about the rigid body motion to design more accurate

position and attitude estimators. As mentioned before, the work in [Sal91] combines the

rigid body kinematics and dynamics to obtain attitude and angular velocity estimates

from orientation and torque data. In [HZT+11], a gradient-based observer is designed

directly on the special Euclidean group SE(3), which relies on a global attitude descrip-

tion, similarly to the observer for mechanical systems on Lie groups presented in [MBD04].

The work in [BS04] considers two different approaches for using a dynamic model of the

vehicle to aid pose estimates provided by an inertial navigation system (INS). In [HH11],

a complete model-aided INS for underwater vehicles is presented. A new methodology to

exploit the vehicle dynamics based on the EKF is proposed in [VSOG10]. In [RSP07], the

attitude of an underwater vehicle is estimated by an observer which also takes into account

the rigid body dynamics. Attitude and angular velocity estimation for rigid bodies using a

global representation of the equations of motion based on geometric mechanics is reported

in [San06, SLLM08b]. This estimation scheme is adopted in [SN12] for feedback attitude

tracking control. The aforementioned works assume that a perfect model of the vehicle

dynamics is available. However, in many situations that might not be the case. For exam-

ple, spacecraft dynamics may be affected by solar radiation pressure and poorly-modeled

higher-order gravity effects [LW99]. To address those uncertainties, in [ES04], an EKF for

identification of unmodeled disturbance torques is proposed.

1.2.10 Fault detection and isolation in inertial measurement units

The field of fault detection and isolation (FDI) has been studied since the early 70’s

[Wil76] and several techniques have, since then, been applied to different systems. Com-

mon examples of systems equipped with FDI devices include not only aircrafts and space-

craft but also a wide range of industrial processes such as the ones described in the following

references – [BIZBL97, FD97, PC97, Ise97, CT01, BSW01, MDL06, LM09]. For a survey

of FDI methods in the literature, see for instance, [HKKES10].

An FDI system must be able to bear with different types of faults in sensors and

actuators, which can occur abruptly or slowly in time. Furthermore, model uncertainty

(such as unmodeled dynamics) and disturbances must never be interpreted as faults. An

active deterministic model-based fault detection (FD) system (see [Est04] for a description

11

1. Introduction

of the typical FD classes available in the literature) is usually composed of two parts:

a filter that generates residuals that should be large under faulty environments; and a

decision threshold, which is used to decide whether a fault is present or not – see [Wil76,

FD94, Est04, BB04, BKL+06, NVR08, Duc09] and references therein. The isolation of

the fault can, in some cases, be accomplished by using a dual approach, i.e., by designing

filters for families of faults, and identifying the most likely fault as the one associated with

the filter with the smallest residual [May99]. In previous years, new FDI methods based

on observers have been proposed – cf. [HKEY99], [DPI01], [LM09], and [ZDL12].

Advances in the development of micro-electro-mechanical system (MEMS) technology

have leveraged the success of navigation systems based on the so-called strapdown architec-

ture, and diminished the relative importance of the mechanically more complex gimbaled

solutions. The core of a strapdown architecture is an inertial measurement unit (IMU)

comprising rate gyros and accelerometers. Strapdown systems have, however, the disad-

vantage of the inevitable errors associated with the integration of inertial measurements

(dead reckoning). To mitigate those errors, some complementary onboard sensors, such

as magnetometers, star-trackers, Sun sensors, among others, are often used [Far08]. On

the other hand, strapdown systems have the advantage of being mechanically more ro-

bust, more compact, and less expensive when compared with the gimbaled solutions. In

addition, it is simpler to add redundant sensors in strapdown systems, which makes this

architecture very attractive for high-reliability navigation systems.

The FDI schemes for navigation systems available in the literature exploit two types of

redundancy, namely, the hardware redundancy and the analytical or dynamic redundancy.

The former takes advantage of the existing redundant measurements to detect incoherences

among them. In [GM72], an FDI solution is proposed that is based on algebraic invariants.

Parity-based methods are proposed in [Stu88, JZ99], and the work in [SY04] proposes a

geometric method based on the singular value decomposition of the measurement matrix.

A comparison between several FDI techniques using hardware redundancy is presented in

[Vod99]. The analytical redundancy emerges from the dynamic relation among the sensor

data. In [Car88], distributed Kalman filters are used. A solution based on parameter

estimation, where the residuals are generated by least-squares estimation techniques, is

presented in [JKT08]. The work in [DHF09] proposes two statistical schemes based on

nonlinear autoregressive moving average, and in [DS95], a left eigenvector assignment

approach is developed and applied to an aircraft accelerometer FD filter. A comprehensive

survey on FDI methods exploiting analytical redundancy can be found in [Pat91].

12

1.3 Main contributions


This section briefly describes the main contributions of this thesis.

1.3.1 Attitude estimation using an active vision system

The recent advances in MEMS technologies have enabled the development of light-

weight, cost-effective strapdown navigation systems. On the other hand, these devices

are associated with errors induced by the integration of inertial data. Those errors can

be mitigated by resorting to external aiding sensors such as image acquisition systems.

Computer vision has long been recognized as an extremely flexible technique for sensing

the environment and acquiring valuable information for pose estimation and control. Over

the last decades, awareness of this potential has brought about a widespread interest in

the field of vision-based control and localization [CH06, CH07].

The problem of estimating the attitude of a rigid body equipped with a triad of rate

gyros and a pan and tilt camera is considered. In particular, the devised solution is

suitable to be implemented onboard UAVs. The proposed attitude nonlinear observer

fuses angular velocity measurements with the information about markers on a planar

scene provided by an active vision system. By exploiting directly sensor information, a

stabilizing feedback law is devised. This law guarantees exponential convergence to the

origin of the estimation errors in the presence of constant bias disturbances on the rate

gyro measurements (Theorem 3.1). Moreover, for time-varying bias terms, the estimation

errors are guaranteed to remain bounded, provided that the time derivative of the bias and

the error on the initial estimates are sufficiently small (Proposition 3.1). As a second goal,

an active vision system that keeps the markers inside the image plane using an image-based

control law for the camera pan and tilt angular rates is developed (Proposition 3.2).

The discrete-time implementation of the observer is specifically addressed. Using re-

cent results from numerical analysis ([CG93]), a geometric integration method is adopted

to conveniently approximate the original continuous-time observer. To exploit the high-

frequency readings of the rate gyros, a multi-rate implementation of the observer is pro-

posed, which updates the attitude estimate while the image measurements are not available

and recomputes the estimate as soon as the time-delayed image measurements are avail-

able. To assess in practice the performance of the proposed observer, an experimental

prototype comprising a MEMSense nIMU and an AXIS 215 PTZ camera was assembled.

This hardware was mounted on a Model 2103HT from Ideal Aerosmith [IA06], which is a 3-

axis motion rate table that provides precise angular position, rate, and acceleration and is

specially used for the development and test of inertial components and systems. The table

13

1. Introduction

was used to generate the desired test trajectories and provide the required ground truth

signals. The good performance demonstrated by the developed experimental prototype

motivated the implementation of the proposed estimation onboard a quadrotor. Experi-

mental results obtained in-flight further validated the high level of performance attained

by the proposed solution. Preliminary versions of this work can be found in [1, 3, 4].

1.3.2 Attitude estimation using ranges

In addition to cameras, range measurement systems are very cost-effective aiding sen-

sors for navigation systems, specially in indoor environments. This thesis proposes a

nonlinear attitude observer that fuses angular rate measurements acquired by non-ideal

rate gyros with range measurements provided by a commercially available acoustic posi-

tioning system. This positioning system comprises an ultrasonic beacon array fixed in the

inertial frame and a set of acoustic receivers installed onboard the vehicle. The proposed

solution considers the specific problem of attitude estimation based on angular rate mea-

surements and position measurements that are extracted from the range data. It is shown

that in the presence of constant bias on the rate gyros, the attitude and bias estimation

errors converge exponentially fast to the origin in any closed neighborhood of the almost

global region of attraction (Theorem 4.1). When compared with other attitude observers

proposed in the literature (see [TS03, PHS07, W07, MHP08, MS10, GFJS12] and refer-

ences therein), new stability properties are achieved. More specifically, using a Lyapunov

coordinate transformation, it is shown that worst-case bounds on the convergence rates

can be explicitly computed. Assuming that, the bias on the rate gyros is time-varying, the

estimation errors are shown to remain bounded for sufficiently small initial conditions with

ultimate bounds proportional to the bias time derivatives (Proposition 4.1). Moreover, a

method based on the Kalman-Bucy filter theory is proposed for selecting the observer

gains such that the covariance of the estimation error in the vicinity of the origin is min-

imized. Detailed experiments were conducted with a custom-built prototype comprising

a MEMSense nIMU and a Cricket localization system [PCB00]. Similarly to the exper-

imental apparatus described in Section 1.3.1, the experimental prototype was placed on

an Ideal Aerosmith Positioning Rate Table System Model 2103HT [IA06] that provides

precise ground truth data. A comparison between the experimental results obtained with

the filter and those given by a state-of-the-art estimation technique illustrates the advan-

tages of the proposed solution. In addition to be targeted at a distinct sensor suite, this

observer has the following advantages when compared with the observer in Section 1.3.1.

• An alternative description for the estimation error dynamics is obtained by applying

14


a time-varying Lyapunov transformation. This novel description takes the form of

an LTV system and significantly simplifies the stability analysis.

• Computation of explicit bounds on the estimation error in the presence of time-

varying rate gyro bias based on the new description of the error.

• An explicit method to tune the observer gains such that the covariance of the esti-

mation error in the vicinity of the desired equilibrium point is minimized.

As disadvantage, the eventual high directionality of the ranging measurement system may

limit the trajectory range and angular excursion. Also, the possibility of multi-path of

the acoustic signals may reduce the accuracy of the system. Experimental results and a

comparison with the MEKF attest the good performance of the proposed solution. This

work has resulted in some scientific publications, namely [2, 6, 10].

1.3.3 3-D rigid body motion estimation

This thesis also presents the design of an observer for estimating the configuration

(pose) and velocities of a rigid vehicle on SE(3), the Lie group of rigid body transla-

tions and rotations. The resulting configuration and velocity error dynamics are shown

to be almost globally exponentially stable. The problem of unmodeled disturbances is

also addressed. Resorting to Lyapunov analysis, the proposed feedback law is shown to

drive the estimation errors exponentially fast to the origin (Theorem 5.1). The design of

the nonlinear observer assumes that exact pose and velocity measurements are available.

Nonetheless, in the presence of sensor noise and disturbances, the use of an observer has

clear advantages over the raw measurements as the sensor information is fused with the

rigid body dynamics yielding less noisy estimates than the raw sensor data. The problem

of unmodeled disturbances is also addressed by showing that, under those circumstances,

the estimation error is uniformly bounded (Proposition 5.1). Simulations are given that

illustrate the results of the proposed solution.

New challenges to observer design arise when full state measurements are not available.

An interesting case is when the translational velocity is measured by a Doppler sensor. This

sensor measures the frequency shift of a signal that occurs when the source and the receiver

have different velocities and it is nowadays used in the operation of many underwater

vehicles and spacecrafts. This work addresses the problem of estimating the configuration

and velocities of a rigid vehicle on SE(3) taking advantage of Doppler measurements. The

configuration and velocity error dynamics is shown to be almost globally asymptotically

15

1. Introduction

stable (Theorem 6.1) and, if the position vector of the rigid body is persistently exciting,

almost global exponential stability of the observer is guaranteed (Theorem 6.2).

The use of homogeneous coordinates to formally describe the kinematics and dynamics

of a rigid body has the advantage of fully describing the coupling between rotational and

translational motion. Furthermore, these observers take advantage of the dynamic model

to improve the state estimates. Preliminary versions of this work can be found in [11, 14].

1.3.4 Robust attitude estimation

Usually, attitude estimation solutions require either the noise-free sensor measurements,

or the knowledge of a stochastic description of the exogenous disturbances and measure-

ment noise. However, if none of this information is not available a priori and only norm

bounds on the disturbances are known, it is possibly desirable to compute explicit bounds

on the attitude of the vehicle. Such bounds are suitable, for instance, in robust con-

trol designs, where worst-case guarantees are provided regarding the performance of the

closed-loop system – see, for instance, [ZDG95, SP05].

This thesis proposes an attitude estimator based on SVOs that relies on vector obser-

vations and rate gyros measurements, where the sensor measurements are assumed to be

corrupted by bounded noise. This solution considers uncertainties defined by polytopes.

Moreover, it guarantees that the true state of the system is inside the estimated set as long

as the assumptions on the bounds on the measurements are satisfied (Theorem 7.1). No

linearization is required and non-conservative estimates are obtained for the case where

there is no uncertainty in the angular velocity measurements (Corollary 7.1). The case of

single vector observation is also addressed by establishing sufficient conditions for bound-

edness of the set-valued estimate (Proposition 7.1). Preliminary versions of these results

can be found in [5, 8, 12].

The work in [SLLM08a] exploits a different approach and proposes an attitude estima-

tor whose uncertainty ellipsoids bound the sensor measurements and the filter state. This

observer has the advantage of considering also the rigid body dynamics in the filter, which

may render it more accurate. However, it has the disadvantage of constraining the mo-

ments acting on the rigid body to be generated by an attitude dependent potential, while

only relying on the linearization of the system to propagate the uncertainty ellipsoids.

1.3.5 Fault detection and isolation in inertial measurement units

The navigation system is a critical component in any aircraft or spacecraft. It provides

key information – the attitude and position of the system – and, in case of failure, there

16

1.4 Thesis outline

is serious risk of damage or even human losses. In high reliability systems, in addition to

detecting the occurrence of faults, it is also important to isolate the defective sensor. This

allows for the reconfiguration of the system and the continuous operation with minimal

perturbation using the remaining sensors. This has motivated a considerable amount of

research to devise FDI schemes for navigation systems – see for instance [HD73, Pat91,

Vod99, CFSG09] and references therein.

In this thesis, two FDI schemes are proposed for IMUs and vector observations, where

the sensor measurements are assumed to be corrupted by bounded noise. The two schemes

exploit different types of redundancy:

• hardware redundancy – by resorting to the intersection of the sets compatible with

the measurements (Proposition 8.1);

• analytical redundancy – by using SVOs to model the dynamic relation among the

sensor measurements (Proposition 8.4).

Necessary and sufficient conditions on the magnitude of the fault that ensure detection and

isolation using hardware redundancy are established (Proposition 8.2 and Proposition 8.3).

For further details on SVOs and SVO-based FDI methods, the interested reader is referred

to [MV91, ST99, RSSA09, RSSA10b] and references therein. Typically, the proposed

solutions require only a few sampling periods to detect and isolate faults. The maximum

magnitude of sensor noise and of external disturbances are properly taken into account in

the filter design. The absence of false alarms is guaranteed assuming that the pre-specified

bounds on the measurement noise are not violated. Furthermore, the computation of a

decision rule, based on a threshold to be tuned and used to declare whether or not a fault

has occurred in residual-based FDI approaches, is not needed. Preliminary versions of

these results can be found in [7, 13].

1.4 Thesis outline

This thesis is organized in chapters that are mostly self-contained. Concepts and

formalisms that are common to more than one chapter will be treated in more detail in

the first occurrence and reviewed in the following instances.

Chapter 2 presents introductory background on the motion of rigid bodies. In particu-

lar, the kinematic and dynamic equations are described and the homogeneous coordinates

are introduced. Additionally, the advantages and shortcomings of key attitude represen-

tations are highlighted. Throughout this thesis, these representations are adopted either

17

1. Introduction

as a design preference or as an analysis tool in order to achieve the desirable stability and

convergence properties.

In Chapter 3 and Chapter 4, the problem of attitude estimation in indoor or out-

door structured scenarios is considered. The proposed solutions are based on nonlinear

observers. Their convergence properties are studied resorting to Lyapunov theory and

advanced methods for analysis of LTV systems. Chapter 3 devises an attitude observer

for an autonomous aerial vehicle equipped with a pan and tilt camera and rate gyros. The

online estimation of the rate gyro bias is explicitly considered in the estimator design. A

feedback law for the observer is proposed based on the position of visual markers present

in the mission scenario and bias estimates. Exponential convergence to the desired equi-

librium point is established. A pan and tilt controller is also proposed to keep the visual

markers inside the field of view of the camera. A multi-rate discrete-time implementation

is achieved exploiting recent results in the field of Lie Groups integration methods. These

methods allow for a natural update of the rotation matrix estimates on the underlying

manifold. Simulation results illustrate the performance of the proposed observer. The al-

gorithm developed is implemented in a real-time prototype and encouraging experimental

results were obtained using a high-accuracy motion rate table. These results motivated the

implementation of the observer onboard a quadrotor providing in-flight estimates, which

are compared with the measurements of a motion capture system.

Chapter 4 proposed a nonlinear observer for attitude estimation resorting to inertial

measurements and range data. The filter is designed by resorting to rotation matrices as

attitude parametrization, which avoids singularities and unwinding phenomena present in

other parametrizations. By exploiting a judiciously selected Lyapunov transformation and

LTV systems, the estimation error is shown to converge exponentially fast to the origin.

Experimental and simulation results validate the performance of the proposed solution.

An observer design for the configuration (pose) and velocities of a rigid vehicle on SE(3)

is presented in Chapter 5. The configuration and velocity error dynamics are shown to be

almost globally exponentially stable. Disturbances in the forces and torques are considered

and, under such circumstances, ultimate boundedness is established. Simulations are

presented to illustrate the performance of the proposed solution.

Chapter 6 proposes an observer that uses configuration and Doppler measurements as

well as modeled forces and torques to estimate configuration and velocities. The estima-

tion problem is formulated directly on the special Euclidean group SE(3). The proposed

observer is almost globally asymptotically stable and the estimation errors converge expo-

nentially fast to the origin. Robustness to bounded measurement errors is shown through

18

1.4 Thesis outline

numerical simulation results.

Chapter 7 describes an attitude estimation method based on SVOs. The sensor mea-

surements are assumed to be corrupted by bounded noise and constant bias in the rate

gyros measurements is also considered. A discretization of the system and the associated

errors are taken into account explicitly in the SVO formalism. The problem of attitude

estimation using a single time-varying vector measurement and without gyro bias is also

addressed. Several implementation details are discussed and possible solutions to reduce

the computational burden are proposed. The performance of this solution is illustrated in

simulation for a 3-D trajectory.

Chapter 8 addresses the problem of FDI for IMUs. Two strategies are proposed, one

exploiting hardware redundancy and the other analytical redundancy. The former assumes

that the navigation system has more than one sensor per main axis, whereas the latter

exploits the dynamic relation among sensor measurements. Necessary and sufficient condi-

tions for the case of hardware redundancy guaranteeing adequate detection and isolation

of the damaged sensor are established. None of the solutions generate false positives. Fur-

thermore, no decision threshold in required. The performance of both solutions is shown

in simulation.

Finally, a summary of the main results in this thesis as well as a discussion on possible

directions for future work are presented in Chapter 9.

19

Chapter 2

Rigid body motion

2.1 Introduction

This thesis addresses the topic of rigid body attitude and position estimation. Rigid

bodies can be any type of vehicle – underwater, terrestrial, aerial, or even a spacecraft

without any flexible or moving parts. The main advantage of modeling vehicles as a rigid

body is that the equations that describe the rotational and translational motions are the

same for all rigid bodies, only physical parameters like mass and inertia may change.

In this chapter, key attitude representations are introduced together with the equations

that describe the rotational and translational motion of rigid bodies. The interested reader

is referred to [Cra89], where a more detailed description can be found.

2.2 Attitude representations

While the position of a rigid body is naturally represented by a 3-dimensional vector

without much debate, such consensus no longer exists regarding its attitude or orientation

representation. Since there are a variety of attitude representations to choose from, the

decision of which one to adopt offers an additional degree of freedom in the design of

estimators. This decision is of critical importance as different attitude representations can

render the same problem straightforward or intractable. In the following, some advantages

and disadvantages of several attitude representations are presented.

21

2. Rigid body motion

2.2.1 Rotation matrix

The rotation matrix is a linear transformation on an Euclidean space used to rotate

vectors and map coordinates from one reference frame to another with the same origin.

Consider two reference frames, A and B, and let the position of the rigid body

in B be denoted by Bp ∈ R3. Then, the position of the rigid body with respect to the

reference frame A is given by

Ap = A

BRBp,

where ABR denotes the rotation matrix from B to A. All rotation matrices satisfy

RTR = I and det(R) = 1, and together they form the set of special Orthogonal matrices

denoted by SO(3). This set is a Lie group and its associated theory provides useful tools

and insight into how to address attitude estimation problems. For further details see

Appendix A and references therein.

2.2.2 Euler angles

Representing attitude by a rotation matrix requires nine scalars. However, the attitude

can be defined using less parameters. An alternative that uses only three parameters is

the so called Euler angles representation. This representation is defined by the angles

that each axis of reference frame B needs to rotate, in a sequential order, to coincide

with reference frame A. There are several conventions related to Euler angles that are

associated with the axes about which the rotations are performed and the respective order.

For instance, in the Z-Y-X Euler angles convention, one starts by rotating the reference

frame B about its z-axis, zB, by an angle γ, yielding the reference frame B′. Then,

B′ is rotated about its y-axis by an angle β to obtain the reference frame B′′. Finally,B′′ is rotated about its x-axis by an angle α so that the final reference frame matches

A. This process is illustrated in Figure 2.1. The rotation matrix can be obtained from

the Euler angles using successive rotations about the main axes, i.e.,

A

BR = Rz(γ)Ry(β)Rx(α),

where

Rx(α) =

1 0 00 cα −sα0 sα cα

, Ry(β) =

cβ 0 sβ0 1 0

−sβ 0 cβ

, Rz(γ) =

cγ −sγ 0sγ cγ 00 0 1

,(2.1)

and cα = cos(α), sα = sin(α), cβ = cos(β), sβ = sin(β), cγ = cos(γ), and sγ = sin(γ).

22

2.2 Attitude representations

Bx B′

x

By

B′

y

BzB′

z

γ

B′

xB′′

x

B′

y

B′′

y

B′

zB′′

z

αB′′

xAx

B′′

y

Ay

B′′

zAz

β

Figure 2.1: Z-Y-X Euler rotations from B to A.

The Z-Y-X Euler angles can be computed from a given rotation matrix using

β = atan2

(

−r31,√

r211 + r221

)

,

γ = atan2(r21/cβ, r11/cβ),

α = atan2(r32/cβ, r33/cβ),

where rij = [ABR]ij , cβ 6= 0, and atan2(x, y) is the four-quadrant tangent inverse, which

can be defined as atan2(x, y) = 2 atan

(

x√x2+y2+y

)

. Notice that this relation becomes

ill-defined if β = ±π2 rad. The existence of singularities is one of the main disadvantages

of the Euler angles representations. To mitigate this pitfall (but without actually solving

it), the following convention can be used. Choose γ = 0 rad and, if β = π2 rad, take

α = atan2(r12, r22),

On the other hand, if β = −π2 rad, take

α = − atan2(r12, r22).

2.2.3 Euler angle-axis representation

An alternative representation for the rotation between B and A is given by the

direction normal to the rotation plane, represented by λ ∈ R3 and referred as Euler axis,

together with the rotation angle θ. The angle θ satisfies

θ = arccos

(tr(ABR)− 1

2

)

∈ [0, π] rad, (2.2)

and the Euler axis can be obtained using

λ =θ

2 sin(θ)(ABR− A

BRT )⊗. (2.3)

23


This representation is more compact than using rotation matrices. However, for small

rotations, the Euler axis becomes ill-conditioned and there are singularities if θ = 0 rad

or θ = π rad.

This description is also called exponential representation and is tightly related with

the Lie algebra associated with the Lie group SO(3) (see Appendix A). In fact, notice the

similarities of (2.2)-(2.3) with (A.2).

Given θ ∈ [0, π] rad and λ ∈ R3, ‖λ‖ = 1, the corresponding rotation matrix is given

by the Rodrigues’s formula [MLS94] (see also (A.1))

R = rot(θ,λ) = cos(θ)I3 + sin(θ)(λ)× + (1− cos(θ))λλT . (2.4)

2.2.4 Quaternions

Quaternion are R4 unit vectors introduced in 1843 by Sir William Hamilton [Ham44]

that can be used to represent a rotation. A quaternion q comprises both a vector and a

scalar part so that

q =

[qv

qs

]

.

The vector and scalar parts of the quaternion can be obtained from the Euler angle-axis

representation using

qv = sin

(θ

2

)

λ,

qs = cos

(θ

2

)

.

The rotation matrix that corresponds to a given quaternion can be computed from

R(q) = (q2s − ‖qv‖2)I+ 2qvqT

v + 2qs(qv)×,

where (.)× : R3 → so(3) is the skew-symmetric operator that satisfies

(ω)× =

0 −ωz ωy

ωz 0 −ωx

−ωy ωx 0

.

The linear space so(3) is the Lie algebra associated with the Lie group SO(3) and corre-

sponds to the set of 3 × 3 skew-symmetric matrices. This representation does not have

any singularities and can be directly used to perform rotations and coordinate mappings

without a rotation matrix. However, the use of quaternions can lead to the so-called

unwinding phenomena [BB00].

24

2.3 Rigid body kinematics

2.3 Rigid body kinematics

This section presents the general equations of the rigid body kinematics, which describe

the rigid body motion. Consider the following reference frames: the body reference frame

B that is attached to the rigid body and the inertial frame I. In terms of classical

mechanics, an inertial reference frame is a frame that is not accelerated and in which the

velocity of any object is constant unless acted upon by an external force, i.e, a frame where

Newton’s laws of motion hold. In contrast, in a rotating frame (which is not inertial) an

object appears to be acted upon by a fictitious force that draws the object away from the

rotation axis, known as centrifugal force. The rotational kinematics of B with respect

to I is described by

I

BR = I

BR(Bω)×. (2.5)

where Bω denotes the angular velocity of B with respect to I expressed in B.Let the position of the origin of B with respect to the origin of I be denoted by

p. If p is expressed in I, it is denoted by Ip and it satisfies the kinematic equation

Ip = Iv,

where Iv denotes the velocity of the origin of Bwith respect to the origin of I expressedin I. If p is expressed in B, it is denoted by Bp and satisfies

Bp = B

I RIp, (2.6)

where BI R = I

BRT . The position kinematics expressed in body-fixed coordinates is obtained

by differentiating (2.6) with respect to time yielding

Bp =d

dt(BI R

Ip)

= B

I RIp+ B

I RI p

= (−(Bω)×B

I R)Ip+ B

I RIv

= −(Bω)×Bp+ Bv

= (Bp)×Bω + Bv,

(2.7)

where Bv denotes the velocity of the origin of B with respect to the origin of Iexpressed in B, i.e.,

Bv = B

I RIv.

25


2.4 Rigid body dynamics

The rigid body dynamic equations of motion describe the change in linear and angular

velocities of a rigid body under external forces or torques. The translational dynamics in

the inertial reference frame I is given by

Iv = Ia,

where Ia is the acceleration given by Newton’s second law Iφ = mIa, m is the rigid

body mass, and Iφ the external forces expressed in I. When expressed in body-fixed

coordinates, the rigid body dynamics is described by

Bv = (Bv)×Bω + Ba, (2.8)

where Ba = BI R

Ia.

The rotational dynamics is described by

JBω = (JBω)×Bω + Bτ , (2.9)

where J is the rigid body inertia matrix and Bτ is the rotation torque expressed in B.

2.5 Rigid body motion using homogeneous coordinates

The translational and rotational rigid body motion can be very conveniently described

using homogeneous coordinates [MLS94]. This representation provides a compact form of

describing the kinematics and dynamics of any rigid body. Moreover, it fully describes the

tight relation among rigid body translational and rotational motion in body coordinates

(which is the frame of the onboard sensors).

Consider the rigid body configuration matrix G given by

G =

[IBR

Ip01×3 1

]

∈ SE(3),

where SE(3) is the special Euclidean group of rotations and translations. For further

details see Appendix A. Using this representation, the kinematic equations take the form

G = Gξ∨, (2.10)

where ξ = [BωT BvT ]T is the vector of velocities and (.)∨ : R6 → se(3) is the vector space

isomorphism given by

ξ∨ =

[(Bω)× Bv01×3 0

]

∈ se(3).

26

2.6 Chapter summary

The space se(3) is the Lie algebra associated with SE(3) and it consists of a 6-dimensional

linear space tangent to SE(3) at the identity element.

The dynamics introduced in (2.8) and (2.9) can be expressed in compact form as

Iξ = ad∗(ξ)∨Iξ +ϕ, (2.11)

where ϕ = [Bτ T BφT ]T ,

I =

[J 03×3

03×3 mI3×3

]

,

and ad∗(ξ)∨ = (ad(ξ)∨)T . The operator ad(ξ)∨ stands for the linear adjoint representation

of the Lie algebra se(3) associated with the Lie group SE(3) such that

ad(ξ)∨ =

[(Bω)× 03×3

(Bv)× (Bω)×

]

.

Alternatively, (2.11) can be written as

Iξ = Ξ(I,ω)ξ +ϕ, (2.12)

where

Ξ(I,ω) =

[(JBω)× 03×3

03×3 −m(Bω)×

]

.

2.6 Chapter summary

In this section, the equations of motion of a rigid body were described, which represent

the starting point of the work in this thesis. Additionally, alternative attitude representa-

tions were presented and their advantages and disadvantages highlighted. The adoption

of a particular attitude representation has considerable impact on the design of attitude

estimators as well as on the analysis of their stability and convergence properties. For this

reason, in this thesis, the rotation matrix or its exponential coordinates are used in the

design of the estimators to avoid singularities and unwinding phenomena. If advantageous,

quaternions are used as an analysis tool to establish convergence rates or to enable the

selection of suitable observer gains. Euler angles are used to depict results due to their

intuitive physical interpretation. Homogeneous coordinates are used in the chapters where

position is also estimated, not only due to its compactness, but also because they provide

a framework that fully describes the tight relation among rigid body translational and

rotational motion (in body coordinates, which is the frame of the onboard sensors).

Henceforth, the superscript indicating the reference frame in which the physical quan-

tities are expressed is often omitted when clear from the context to ease the notation.

27

Chapter 3

Nonlinear observer for attitude

estimation based on active vision

3.1 Introduction

Recent years have witnessed a growing interest in UAVs as a research topic but also

as a hobby and, more importantly, as a business opportunity. High maneuverability,

reduced size, and low-cost are characteristics that make UAVs an attractive solution for

several applications both indoors and outdoors, such as disaster recovery, search and rescue,

inspection of inaccessible infrastructures, greenhouse pulverization, aerial photography,

terrain mapping, situation awareness for law enforcement and emergency responders, fish

tracking, among others. This has increased the need for navigation solutions that are

robust and meet the desired levels of performance. In particular, an accurate attitude

determination system is key for the UAVs operation. In this chapter, a solution is proposed

based on an active vision system (a steerable camera and visual markers) and inertial

measurements. Since the visual markers need to be placed at known locations, this solution

is especially suitable for applications where one has some control over the mission scenario,

an example of which being surveillance and inspection of infrastructures.

Computer vision has long been recognized as an extremely flexible technique for sensing

the environment and acquiring valuable information for pose estimation and control. Over

the last decades, awareness of this potential has brought about a widespread interest in the

field of vision-based control and localization [CH06, CH07]. Vision-based techniques are a

rich and reliable source of information regarding the surrounding environment making it

29

3. Nonlinear observer for attitude estimation based on active vision

very suitable to be used together with inertial data in navigation systems for UAVs. This

chapter presents an experimentally evaluated solution to the problem of estimating the

attitude motion using rate gyros and a pan and tilt camera. A nonlinear attitude observer

combines angular velocity measurements obtained from rate gyros with image coordinates

of fiducial markers located on a flat surface.

By directly exploiting the sensor information, a stabilizing feedback law is introduced

and exponential convergence to the origin of the estimation errors is shown. Additionally,

an active vision system is proposed that relies on an image-based exponentially input–

to-state stable (ISS) control law for the pan and tilt angular rates of the camera to keep

the fiducial markers in the image plane. This controller drives the center of the image to

the centroid of visual markers. Using recent results in geometric numerical integration a

multi-rate implementation of the observer is proposed, which exploits the complementary

bandwidth of the sensors. Practical considerations, such as the lens distortion compen-

sation and the computation of suitable observer feedback gains are considered. Experi-

mental results obtained with a high accuracy motion rate table and onboard a quadrotor

demonstrate the high level of performance attained by the proposed solution. Preliminary

versions of this work can be found in [1, 3, 4].

The chapter is structured as follows. In Section 3.2, the attitude estimation and the

camera pan and tilt control problems are introduced. The sensor suite is described and the

reference frames adopted in this chapter are detailed. The attitude observer is presented

in Section 3.3, and its properties are highlighted. In Section 3.4, a camera pan and tilt

ISS controller is proposed, which centers the visual markers in the image plane. A low

complexity discrete-time implementation of the observer is presented in Section 3.5. In

Section 3.6, simulations illustrate the performance of the observer and the pan and tilt

controller. A prototype working in real time is described in Section 3.7. This experimental

setup comprises an inertial measurement unit and a commercially available pan and tilt

camera. A motion rate table is used to provide precise angular position and rate mea-

surements, which are compared with the observer estimates. The results obtained with a

reduced-size prototype installed onboard a commercially available quadrotor are reported

in Section 3.8 and compared with the information obtained using a motion capture system.

Finally, a summary of the chapter is presented in Section 3.9.

3.2 Problem formulation

Consider a vehicle equipped with a triad of rate gyros and a pan and tilt camera. Let

B be the frame attached to the vehicle, L the local frame attached to the markers

30


plane, and C the camera frame with origin at the camera’s center of projection with

the z-axis aligned with the optical axis. The observed scene consists of N markers whose

coordinates in L are denoted by Lxi ∈ R3, i = 1, . . . , N , N ≥ 4. Without loss of

generality, the origin of L is assumed to coincide with the centroid of the marker points

so thatN∑

i=1

Lxi = 0.

The visual markers can be of any shape or color provided that their positions in the

mission scenario are known and constant. They must be identifiable, i.e., a correspondence

between the image coordinates of each marker and its position on the mission scenario can

be established. See [Fia10] for further details on the design of suitable markers. The image

based estimation problem illustrated in Figure 3.1 can be summarized as the problem of

estimating the attitude of a rigid body1 given by the rotation matrix from B to L,denoted by L

BR, using visual markers and angular velocity readings. An image-based

controller for the camera pan and tilt angular rates is also considered to keep the markers

in the image plane.

3.2.1 Sensor suite

The triad of rate gyros is assumed to be aligned with B so that it provides mea-

surements of the body angular velocity ωB corrupted by a constant unknown bias term,

b,

ωr = ωB + b, b = 0.

As shown in Figure 3.1, the camera can describe pan and tilt motions corresponding

to the angles ψ and φ, respectively. Thus, the rotation matrix from C to B is given

by

B

CR = RpanRtilt, (3.1)

Rpan = Rz(π/2 + ψ), Rtilt = Rx(π/2 + φ),

where Rz(·) and Rx(·) denote rotation matrices about the z-axis and x-axis, respectively,

which are given in (2.1).

For simplicity of notation, let R = LCR be the rotation matrix from C to L and p

the position of the origin of L with respect to C. Then, the 3-D coordinates of the

1Notice that the rigid body is the vehicle in which B is fixed and does not include the pan and tiltcamera.

31


marker i

Figure 3.1: Diagram of the experimental setup.

markers points expressed in C can be written as

qi = RT Lxi + p, i = 1, . . . , N.

Using the perspective camera model [RG03], the 2-D image coordinates of those points

yi ∈ R2 can be written as

[yi

1

]

= δiAqi, (3.2)

where A ∈ R3×3 is the camera calibration matrix, assumed to be known, and δi is an

unknown scalar encoding depth information and given by

δi = (eT

3qi)−1, e3 = [0 0 1]T . (3.3)

3.2.2 Attitude kinematics

The camera frame attitude kinematics is described by

R = R(ω)×, (3.4)

where once again for the sake of simplicity of notation the superscript in ω ∈ R3 is omitted.

Notice that, in Chapter 2, (2.5) describes the rotation kinematics of the body reference

frame with respect to the inertial frame, whereas (3.4) describes the rotation kinematics of

the camera reference frame with respect to the inertial frame. Nonetheless, the kinematics

equation remain valid, since ω denotes the angular velocity of C with respect to L(inertial) expressed in C. Observer design solely based on the motion kinematics has

the advantage of using an exact description of the physical quantities involved, and thus,

32


Figure 3.2: Block diagram of the attitude observer and camera controller.

avoiding the difficulties associated with system identification. Taking the time derivative

of (3.1), straightforward computations show that ω can be written as

ω = C

BRωB +RT

tilt[φ 0 ψ]T , (3.5)

where ψ and φ are the time derivatives of the camera pan and tilt angles, respectively.

Assuming that the camera pan and tilt angles are known, one can readily obtain the

attitude of the vehicle from LBR = RC

BR.

3.2.3 Problem summary

The estimation problem addressed in this chapter can be stated as follows.

Problem 3.1. Consider the attitude kinematic model described by (3.4) and let R and b

denote the estimates of R and b, respectively. Design a dynamic observer for R based on

ωr and yi, i = 1, . . . , N , such that R and b converge to R and b with the largest possible

basin of attraction.

To develop an active vision system using the camera pan and tilt degrees of freedom,

the following problem is considered.

Problem 3.2. Let y be the image of four selected markers’ centroid given by

[y1

]

= δAr, δ = (eT

3 r)−1, (3.6)

where r denotes the position of the four markers’ centroid expressed in the camera frame

C. Design a control law for ψ and φ based on ωr and yi, i = 1, . . . , 4, such that y

approaches the center of the image plane.

Figure 3.3 and Figure 3.4 illustrate the image coordinates and the markers’ centroid

that is obtained as the geometric center of the visual markers on the image plane.

Figure 3.2 depicts the cascaded composition of the system, where the angular rate bias

estimate is fed into the pan and tilt controller.

33


fCg

x1

x2

x3

x4

y1

y2

y3

y4

¹y

fLg

Figure 3.3: Projection of the visual markers on the image plane.

y1

y2

y3

y4

y

Figure 3.4: Illustration of the image coordinates of four markers and their centroid as seenin the image plane.

3.3 Attitude observer

In this section, a solution to Problem 3.1 that builds on results presented in [VCSO10]

is proposed, where a nonlinear position and attitude observer based on landmark measure-

ments and biased velocity measurements is shown to provide exponential convergence to

the origin for the position, velocity, attitude, and bias errors. In this chapter, the pro-

posed observer is based on visual markers instead of landmarks. Additionally, a pan and

tilt controller is designed to keep the visual markers at the center of the image plane and

a discrete-time implementation, based on a geometrical numerical integration method, is

developed and experimentally tested resorting to a custom-build prototype. The proposed

34


observer is designed to match the attitude kinematics by taking the form

˙R = R(ω)×, (3.7)

where ω is the feedback term designed to compensate for the estimation errors. The

attitude and bias estimation errors are defined as R = RRT and b = b− b, respectively.

Using (3.4) and (3.7), the rotation error dynamics can be written as

˙R = R(R(ω − ω))×. (3.8)

Some rotational degrees of freedom are unobservable in case all markers are collinear

as discussed in [VCSO10] and references therein. The following necessary conditions for

attitude estimation based on image measurements is assumed.

Assumption 3.1. There are at least four markers of which no three are collinear.

We will consider a feedback law for ω that uses measurements of the form

U = RT [Lu1 . . . LuN+1] ∈ R3×(N+1), (3.9)

where Lui ∈ R3 are time-invariant in the local frame L. To obtain these vector readings

from the image coordinates yi, we explore the geometry of planar scenes. For that purpose,

we introduce the matrices

X =[Lx1 · · · LxN

], Y =

[y1 · · · yN

1 · · · 1

]

,

where Lxi are the 3-D coordinates of the marker points expressed in L and yi the

corresponding 2-D image coordinates.

Assumption 3.2. All markers lay in the same plane.

We can now state the following lemma.

Lemma 3.1. Let σ1, . . . ,σN−3 be a set of linearly independent vectors satisfying Yσi =

0, i = 1, . . . , N − 3, i.e., ker(Y) = span(σ1, . . . ,σN−3). Also, let n be a unit vector

perpendicular to the markers plane, such that nTn = 1, XTn = 0, and 1 = 1N×1. Suppose

the markers satisfy Assumption 3.1 and Assumption 3.2, and that the camera configuration

is such that the image is not degenerate (that is, neither a point nor a line). Then, the

vector δ = [δ1 . . . δN ]T , where δi is defined in (3.3), satisfies Hδ = 0 where

H =

(X+ n1T )Dσ1

...(X+ n1T )DσN−3

,

and where Dσi= diag(σi). Moreover, the null space of H has dimension one, i.e., all

depth variables δi can be determined up to a scaling factor.

35


Proof. From (3.2), Y and X are related by the expression Y = A(RTX+p1T )Dδ, where

Dδ = diag(δ). Then,

Y = A(RTX+ p1T )Dδ

= A(RT (nnT − ((n)×)2)X+ pnTn1T )Dδ

= A(−RT ((n)×)2X+ pnTn1T −RT ((n)×)

2n1T + pnTX)Dδ

= AM(X+ n1T )Dδ, (3.10)

where M = (−RT ((n)×)2 + pnT ) is non-singular for camera configurations that do not

lead to degenerate images (straight lines and points). Using (3.10), we can write

Yσi = 0 ⇔ (X+ n1T )Dδσi = 0

⇔ (X+ n1T )Dσiδ = 0, i = 1, . . . , N − 3.

Thus, δ is the solution of the equation

Hδ = 0.

In addition, if there are three non-collinear points, it follows that for any vector

σi =

σi,1...

σi,N

,

such that Yσi = 0, the inequality∏N

j=1 σi,j 6= 0 holds (see [Cun07, Lemma D.2.1]).

Finally, to see that the null space of H has dimension one, let us assume than δ and δ∗

are both possible solutions. Then, we have

ker(X+ n1T ) = span(Dδσ1, . . . DδσN−3) = span(D∗δσ1, . . . D

∗δσN−3),

where Dδ = diag(δ) and D∗δ = diag(δ∗). However, since for any σi, we have

∏Nj=1 σi,j 6= 0,

the basis Dδσ1, . . . ,DδσN−3 cannot generate the same space as the basis

D∗δσ1, . . . ,D

∗δσN−3 unless δ∗ = cδ, where c 6= 0, which is a contradiction. This con-

cludes the proof.

Using Lemma 3.1, we obtain the depth variables up to a scaling factor which we denote

by αδα = δ. Writing (3.2) in matrix form we have

Y = A(RTX− p1T )αDδα ,

36


where Dδα = diag(αδ) = diag(δα). From the marker centroid constraint X1 = 0, it

follows that

αRTX = A−1YD−1δα

(I− 1

N11T ),

which takes the form of (3.9) up to a scale factor. We can use the properties of the rotation

matrix and the positive depth constraint δi > 0 to obtain the normalized vector readings

Cxi = RT Lxi = sign(α)αRT Lxi

‖αRT Lxi‖, (3.11)

where Lxi = Lxi/‖Lxi‖, i = 1, . . . , N . Finally, we define the matrix U using linear

combinations of (3.11) so that U = CXAX , where AX ∈ R(N+1)×(N+1) is nonsingular and

CX = [Cx1 . . . CxN (Cxi × Cxj)] for any linearly independent Cxi andCxj.

The directionality associated with the markers positions is made uniform by defining

the transformation AX such that UUT = I. The desired AX exists if Assumption 3.1 is

satisfied [VCSO10].

3.3.1 Observer design

In this section, a nonlinear observer that exploits the sensor measurements is devised. A

candidate Lyapunov function is firstly proposed. Then, a stability condition on the initial

estimation error is offered and a feedback law is designed such that the time derivative of

the Lyapunov function is negative definite. Finally, from the results on the stability of LTV

systems reported in [LP02], the exponential convergence of the observer is established.

Let the bias in angular velocity measurements be constant, i.e., b = 0, and consider

the Lyapunov function

V =‖R − I‖2F

2+

1

2kb‖b‖2, (3.12)

where kb > 0. Since R is a rotation matrix, V can also be written as

V = tr(I− R) +1

2kb‖b‖2.

Using (3.8) and noting that tr(A(b)×) = −((A−AT )⊗)Tb, we obtain the time derivative

of V

V = sTω(ω − ω) +1

kb

˙bT b, (3.13)

where sω = RT (R− RT )⊗. The feedback term sω can be expressed as an explicit function

37


of the sensor readings as

sω = RT (R − RT )⊗

= ((RT LXAX)UT −U(RT LXAX)T )⊗

= (

N+1∑

i=1

RT LXAXei(Uei)T − (Uei)(RT LXAXei)

T )⊗

=N+1∑

i=1

(RT LXAXei)× (Uei),

where LX = [Lx1, . . . ,LxN (Lxi × Lxj)], for i and j that satisfy LX = RCX and where ei

is the unit vector such that ei = 1. Consider the following attitude feedback law

ω = C

BR(ωr − b+RT

pan[φ 0 ψ]T )− kωsω

= C

BR(ωB − b+RT

pan[φ 0 ψ]T )− kωsω,(3.14)

where kω > 0. Replacing (3.14) in (3.13) and defining the bias estimates time derivative

as˙b = kb

B

CRs, (3.15)

the Lyapunov function time derivative becomes V = −kω‖s‖2. Considering the feedback

law (3.14) and the update law (3.15), the closed-loop attitude error dynamics can be

written as

˙R = −kωR(R − RT )− R(RC

BRb)×,

˙b = kb

B

CRRT (R − RT )⊗.(3.16)

Consider the Euler angle-axis parameterization of R described in Section 2.2.3, and let

the rotation vector λ ∈ R3, with ‖λ‖ = 1 and the rotation angle θ ∈ [0, π] rad, be such that

R = rot(θ,λ). The rotation matrices R satisfying θ = π rad are undesirable equilibrium

points of the system (3.16). The following lemma provides sufficient conditions on the

initial estimates for the boundedness of the estimation errors, which excludes the set of

points such that R = rot(π,λ), λ ∈ R3 and guarantees that, for any initial estimate such

that θ 6= π rad, no system trajectory contains the points R = rot(π,λ), ‖λ‖ = 1. Global

asymptotic stability of the origin is precluded by topological limitations associated with

those points [BB00].

Lemma 3.2. For any initial condition that satisfies

kb >‖b(to)‖2

4(1 + cos(θ(to))), (3.17)

the estimation error x = (R, b) is bounded and there exists θmax such that θ(t) ≤ θmax <

π rad for all t ≥ to.

38


Proof. Let Ωρ = x ∈ D : V ≤ ρ. As the Lyapunov function (3.12) is a weighted

distance from the origin, there exists γ > 0 such that ‖x‖2 ≤ γV and Ωρ is a compact

set. V ≤ 0 implies that any trajectory that starts in Ωρ remains in Ωρ. So, for all t ≥ to,

‖x(t)‖2 ≤ γV (x(to)) and the state is bounded.

The Lyapunov function (3.12) can be rewritten as

V =‖R − I‖2F

2+

1

2kb‖b‖2 = 2(1− cos(θ) +

1

2kb‖b‖2.

The gain condition (3.17) is equivalent to V (x(to)) < 4. The invariance of Ωρ implies that

V (x(t)) ≤ V (x(to)). Thus, 2(1 − cos(θ(t))) ≤ V (x(to)) < 4 and consequently there exists

θmax such that θ(t) ≤ θmax < π rad for all t ≥ to.

In practice, Lemma 3.2 establishes that, even for large initial estimation errors, a large

enough gain kb is sufficient to guarantee that the undesirable equilibrium points do not

belong to any of the system trajectories.

Consider the form of parameterized systems given by x = f(t, λ,x), i.e., systems

characterized by a constant parameter λ (note that the parameter λ is distinct from the

Euler-angle λ in Lemma 3.2). The exponential stability of these systems is introduced in

the following definition.

Definition 3.1. (λ-parameterized uniformly globally exponentially stable (system)

(λ-UGES) and (λ-parameterized uniformly locally exponentially stable (system) (λ-ULES))

[LP02] The origin of the system x = f(t, λ,x) is said to be λ-ULES if there exist r > 0

and two constants kλ and γλ > 0 such that, for all t > to, λ ∈ D,

‖xo‖ ≤ r ⇒ ‖x(t, λ, to,xo)‖ ≤ kλ‖xo‖e−γλ(t−to).

Furthermore, the system is said to be λ-UGES if the exponential bound holds for all

(to,xo) ∈ R+0 × R

n.

Exploiting the results for LTV systems in [LP02], the next theorem shows that the

trajectories of the system (3.16) converge to the desired equilibrium point.

Theorem 3.1. Assume that ω is bounded and b = 0. Then the attitude and bias esti-

mation errors converge exponentially fast to the equilibrium point (R, b) = (I, 0) for any

initial condition satisfying (3.17).

39


Proof. Let the attitude error be given in the quaternion form q =[qTq qs

]T, where qq =

(R−RT )⊗

2√

1+tr(R)and qs =

12

√

1 + tr(R). The closed-loop dynamics are

˙qq =1

2Q(q)(−RC

BRb− 4kωqq qs),

˙b = 4kbB

CRRTQT (q)qq,

(3.18)

where Q(q) := qsI+ (qq)× and ˙qs = −2kωqTq qq qs − 1

2 qTqCBRb. Using ‖qq‖2 = 1

8‖R − I‖2F ,the Lyapunov function in quaternion form is given by V = 4‖qq‖2 + 1

2kb‖b‖2.

Consider a coordinate transformation like the one proposed in [TS03]. Let xq :=

(qq, b), xq ∈ Dq and Dq := B(3) × R3, and define the system (3.18) in the domain

Dq = xq ∈ Dq : V < 4. The set Dq corresponds to the interior of a Lyapunov surface,

so it is well defined and positively invariant. The condition (3.17) implies that the initial

state is contained in Dq.

Let x⋆ := (qq⋆, bω⋆) and Dq := R3 × R

3, and define the LTV system

x⋆ =

[A(t, λ) BT (t, λ)−C(t, λ) 03

]

x⋆, (3.19)

where λ ∈ R+0 × Dq, the submatrices are described by A(t, λ) = −2kω qs(t, λ)Q(q(t, λ)),

B(t, λ) = −12BCRRTQT (q(t, λ)), C(t, λ) = −4B

CRRTQT (q(t, λ)), and q(t, λ) represents the

solution of (3.18) with initial condition λ = (to, q(to), b(to)). The matrices A(t, λ), B(t, λ),and C(t, λ) are bounded and the system is well defined.

If the parametrized system is λ-UGES, then the system (3.18) is uniformly exponen-

tially stable in Dq [LP02, pp.15-16]. The parameterized system satisfies the assumptions

of [LP02]:

1. For bounded ω, the elements of B(t, λ) and ∂B(t,λ)∂t are bounded.

2. The positive definite matrices P (t, λ) = 8kbI and Q(t, λ) = 32kbq2s(t, λ)kωI, sat-

isfy P (t, λ)BT (t, λ) = CT (t, λ), −Q(t, λ) = AT (t, λ)P (t, λ) + P (t, λ)A(t, λ) + P (t, λ)

and are bounded, namely qmI ≤ Q(t, λ) ≤ qMI, where qM = 32kωkb, and qm =

qM minxq∈Dq(1− ‖qq‖2).

The system (3.19) is λ-UGES if and only if B(t, λ) is λ-uniformly persistently exciting

[LP02, pp.15-16]. To guarantee this, it is sufficient to show that B(τ, λ)BT (τ, λ) ≥ αBI

holds for αB > 0 independent of τ and λ. Note that

4yB(τ, λ)BT (τ, λ)yT = ‖y‖2 − (yT B

CRRqq)2 ≥ ‖y‖2

(1− ‖qq‖2

)≥ ‖y‖2 min

xq∈Dq

(1− ‖qq‖2).

40


Then, B(τ, λ)BT (τ, λ) is lower bounded and the persistence of excitation condition is satis-

fied. Consequently, the parameterized system (3.19) is λ-UGES, and the nonlinear system

(3.18) is exponentially stable in the domain Dq.

If the constant bias assumption is not satisfied, the convergence result of Theorem 1

no longer holds. However, we can show that for ‖b‖ < γ, the estimation errors are also

bounded with ultimate bound proportional to γ.

Proposition 3.1. Consider the attitude observer defined in (3.16) and assume that ω and

b are bounded, with ‖b‖ < γ. Then, for a sufficiently small γ, there is a sufficiently small

error on the initial estimates such that the estimation errors are bounded with ultimate

bound proportional to γ.

Proof. Let xq = (2qv ,1√2kb

b), V (xq) = 4‖qv‖2 + 12kb

‖b‖2, and xq0 = xq(to) and define

the domain U = xq : V (xq) ≤ 4 − ǫ, where 0 < ǫ < 4. From Theorem 3.1, we know

that if kb satisfies (3.17) and b = 0, xq converges exponentially fast to zero inside the

domain U . Then, by the converse Lyapunov theorem for systems with an exponentially

stable equilibrium point [Kha02], there is a Lyapunov function Vq such that c1‖xq‖2 ≤Vq(t,xq) ≤ c2‖xq‖2, Vq(t,xq) ≤ −c3‖xq‖2, and

∣∣∣

∣∣∣∂Vq

∂xq

∣∣∣

∣∣∣ ≤ c4‖xq‖, for xq ∈ U . It follows

that if b 6= 0 the time derivative of Vq satisfies

Vq ≤ −c3‖xq‖2 + c4κ‖xq‖‖b‖, (3.20)

where κ = 1/√2kb. Assuming that ‖b‖ < γ, (3.20) implies that Vq < 0 for ‖xq‖ > c4

c3κγ

and ‖xq‖ <√4− ǫ. If γ is sufficiently small, we can guarantee that c4

c3κγ <

√c1c2(4− ǫ) <

√4− ǫ. Then for every initial condition ‖xq(to)‖ <

√c1c2(4− ǫ), xq(t) remains in U for all

t ≥ to. Since Vq < 0 for c4c3κγ < ‖xq‖ <

√4− ǫ, we can conclude that xq is ultimately

bounded by√

c2c1

c4c3κγ.

Remark 3.1. Although Proposition 3.1 does not provide an expression for the bound on

the bias time derivative, it shows that this bound exists and the estimation errors will

remain bounded for time-varying rate gyro bias. In Section 3.6, we present simulation

results that corroborate this behavior and indicate that the bounds on ‖b‖ and on the

initial conditions are adequate for practical purposes. Also note that Proposition 3.1 can

be adapted to account for band-limited noise, since the time derivative of the bias terms can

also be interpreted as the time derivative of additive noise on the rate gyros measurements.

41


3.4 Camera pan and tilt controller

In this section, we address the problem of keeping the markers inside the image plane,

exploiting the camera’s ability to describe pan and tilt angular motions. As stated in

Problem 3.2, the strategy adopted to achieve this goal amounts to controlling the camera

pan and tilt angular velocities ψ and φ using directly the image measurements yi and the

angular velocity readings ωr, so as to keep the image of the centroid of four of the markers

at a close distance from the center of the image plane.

We resort to Lyapunov theory and consider the following candidate Lyapunov function

W =1

2rTΠr =

1

2(r2x + r2y), (3.21)

where r = [rx ry rz]T is the position of L expressed in C and Π ∈ R

3×3 is the x–y

plane projection matrix. Using the expression for ω given in (3.5) and the linear motion

kinematics described in (2.7), the camera position kinematics can be written as

r = (r)×ω − v

= (r)×(RT

tiltRT

panωB +RT

tilt[φ 0 ψ]T )− v, (3.22)

where v is the camera linear velocity with respect to r. Recall that, by definition, r

coincides with the position of the centroid of the selected four markers and that according

to (3.6) its image is given by y. Therefore, by guaranteeing that the Lyapunov function

W is decreasing, or equivalently, [rx ry] is approaching the origin, we can ensure that y is

approaching the center of the image plane. To simplify the notation and without loss of

generality, assume from now on that A = I so that yx = rx/rz and yy = ry/rz.

Before proceeding to the definition of the pan and tilt control laws, we highlight the

fact that y can be easily obtained from the image measurements yi. By noting that the

marker centroid lies at the intersection between the vectors x3 − x1 and x4 − x2, and

using the fact that the intersection between lines is clearly an image invariant, we can

immediately conclude that y coincides with the point at the intersection between y3 − y1

and y4 − y2 (see Figure 3.3).

Proposition 3.2. Let the camera position kinematics be described by (3.22) and assume

that the vehicle and camera motions are such that rz > 0 and cosφ 6= 0. Consider the

control law for the camera pan and tilt angular velocities given by

[φ

ψ

]

= kc

[0 −11

cos φ 0

]

y −[1 0 00 − tanφ 1

]

RT

panωB, (3.23)

42

3.4 Camera pan and tilt controller

where ωB = ωr − b and kc > 0. Then, the time derivative of the Lyapunov function W

along the system trajectories satisfies

W ≤ −(kc − ǫ)W, ∀ ‖Πr‖ ≥ 1

ǫ

(

‖Πv‖ + rz‖b‖)

, (3.24)

and 0 < ǫ < kc.

Proof. Taking the time derivative of (3.21) and using the expressions for r given in (3.22),

we obtain

W = rTΠ((e3)×rzω − v)

= rz[ry −rx 0]RT

tilt(RT

panωB + [φ 0 ψ]T )− rTΠv.

Choosing φ and ψ such that

RT

tilt

RT

panωB +

φ0

ψ

= kc

−yyyxκ

, (3.25)

for some κ and noting that ωB = ωB − b, yields

W = rz[ry −rx 0]RT

tilt(RT

panωB + [φ 0 ψ]T )− rTΠv

= rz[ry −rx 0](kc[−yy yx κ]T )− rTΠv

= kcrz[ry −rx 0][−ryrz rxrz κ]T − rTΠv

= −kcW − rTΠ(v + rz(e3)×C

BRb),

and consequently (3.24) holds. Solving (3.25) for φ, ψ, and κ we obtain the control law

(3.23). Notice that

Rtilt =

1 0 00 − sin(φ) − cos(φ)0 cos(φ) − sin(φ)

.

Then, let us rewrite (3.25) in the form

RT

panωB +

φ0

ψ

= kcRtilt

−yyyxκ

.

and solve each row individually. From the first row one gets

[1 0 0]RT

panωB + φ = kc[1 0 0]Rtilt[−yy yx κ]T ⇔φ = −kcyy − [1 0 0]RT

panωB.

43


Solving the second row, it yields

[0 1 0]RT

panωB + 0 = kc[0 1 0]Rtilt[−yy yx κ]T ⇔0 = kc(− sin(φ)− cos(φ)κ) − [0 1 0]RT

panωB ⇔

κ = − tan(φ)yx −1

kc cos(φ)[0 1 0]RT

panωB.

And finally, from the last row, we have that

[0 0 1]RT

panωB + ψ = kc[0 0 1]Rtilt[−yy yx κ]T ⇔ψ = kc[0 0 1]Rtilt[−yy yx κ]T − [0 0 1]RT

panωB ⇔ψ = kc cos(φ)yx − [0 0 1]RT

panωB − kc sin(φ)k ⇔

Consequently,

ψ = kc cos(φ)yx − [0 0 1]RT

panωB − kc sin(φ)

(

− tan(φ)yx −1

kc cos(φ)[0 1 0]RT

panωB

)

⇔

ψ = kc(cos(φ) + sin(φ) tan(φ))yx + [0 tan(φ) −1]RT

panωB ⇔

ψ =kc

cos(φ)yx + [0 tan(φ) −1]RT

panωB.

Remark 3.2. If we apply the control law (3.23) to the system with state Πr = [rx ry]T

and interpret v and rzb as inputs, it follows from (3.24) that the system is exponentially

ISS. As such, the distance between the image coordinates of the markers’ centroid y and

the origin is ultimately bounded by ‖Πv/rz‖ and ‖b‖ and converges exponentially fast to

that bound. Moreover, if Πv/rz and b converge to zero so does y.

The proposed camera pan and tilt controller presents some noteworthy differences

with respect to classical visual-servoing schemes, such as, the image-based visual servoing

(IBVS) solution that uses the image coordinates of four coplanar points [CH06].

Comparing the control law (3.23) and an IBVS scheme with actuation on the pan and

tilt angular rates, both approaches guarantee exponential decay of the error, provided

that v and b converge to zero. However, the traditional IBVS solution uses the inverse of

the error Jacobian matrix, which introduces singularities and problems with local minima

[CH06] not present in (3.23). Moreover, (3.23) relies solely on the image coordinates y,

whereas using the inverse of the Jacobian matrix would require reconstruction of the depth

coordinate rz.

44

3.5 Implementation

3.5 Implementation

In this section, we describe the computational implementation of the attitude observer

proposed in Section 3.3 and the camera’s pan and tilt controller proposed in Section 3.4.

3.5.1 Discrete-time algorithm

Cameras typically have much lower sampling rates than inertial sensors. To accom-

modate the different sampling rates, we adopt a multi-rate strategy. The complementary

bandwidth characteristics of the camera and the rate gyros can be explored to obtain

attitude estimates that are high-bandwidth due to the inertial sensors and converge to

zero due to the correction term provided by the vision system. This implementation can

be seen as a prediction-correction algorithm. As illustrated in the flowchart shown in

Figure 3.5, while an image is being processed and the attitude feedback cannot be ap-

plied, the attitude estimate R is propagated using solely the angular velocity readings ωr.

As soon as the image data is available, R is recomputed using both ωr and the vector

readings CX. Note that, with this approach, the lag associated with the acquisition and

processing of the visual information does not degrade the estimates accuracy. Moreover,

this algorithm increases the inter-sampling accuracy of the estimates, which may be of

critical importance for control purposes.

Several techniques can be adopted for the discretization of differential equations. The

choice of the algorithm depends on the specific problem and stability and convergence

are seldom guaranteed. The integration method should guarantee that the discrete-time

implementation conveniently approximates the original continuous-time observer. Classic

Runge-Kutta methods cannot be accurately applied to rotation matrix dynamics since

polynomial invariants like the determinant of degree three or greater are not preserved in

general [HLW06, Theorem IV.3.3]. An alternative is to apply a method that intrinsically

preserves orthogonality like a Lie group integrator.

The attitude observer comprises the differential equations (3.7) and (3.15), with ω

given by (3.14), and evolve on SO(3) and R3, respectively. Equation (3.7) is integrated

resorting to a geometric numerical integration method, such as the Crouch-Grossman

method (CG) [CG93], the Munthe-Kaas method (MK) [MK98], or the commutator-free

Lie group method (CF) [CMO03]. Equation (3.15) is implemented in discrete-time using

a classic numerical integration technique.

The geometric numerical integration algorithm may require the knowledge of the func-

tion ω(t) at instants between sampling times. Alternative sampling and computation

strategies can be adopted to obtain an approximation of this function, such as, polynomial

45


yesno

Start

j = 0

j = j + 1

Read and store ωrj

Open-loopupdate of Rj

j = µ?

Read pan and tilt

Acquire image

Detect markers

Undistort

Compute CX

Computecentroid y

Restore Ro

Closed-loopobserver R1

Open-loopupdate of R2 to Rn

Compute ψ and φ

Figure 3.5: Algorithm flowchart, where µ is the ratio between the sampling rates of therate gyros and of the image acquisition.

interpolation of the sampled data. Such techniques can significantly increase the computa-

tional cost undermining implementations in real-time. Moreover, the unit is equipped with

low-cost sensors which do not justify the use of high order methods and the computational

resources are limited. Hence, we chose to implement a second order geometric numerical

method because it eliminates the need for inter-sampling interpolation and therefore it

reduces the computational burden.

Let the sampling period of the rate gyros be T ∈ R+ and assume for convenience that

the sampling period of the camera measurements is µT , where µ is an integer greater

than or equal to one. The discrete-time implementation of the bias estimator (3.15) is ob-

tained by using a second order Adams-Bashford Method which is a multi-step method and

hence suitable for sampled data (see [BF93] for further details). The resulting numerical

46

3.5 Implementation

integration algorithm can be summarized as

bµk =bµ(k−1) + µTkb(3/2B

CRµ(k−1)sω,µ(k−1) − 1/2B

CRµ(k−2)sω,µ(k−2)).

This part of the algorithm is performed at the lower sampling frequency of 1/µT , since sω

is a function of the camera measurements.

To choose between the CF, CG, and MK methods to numerically integrate (3.7), we

took into account the computational cost of each method. The CF technique is discarded,

since its second order method is similar to the second order CG method and higher order

algorithms are very computationally intensive. As for the second order CG and MK

methods, their coefficients can be obtained from [PC05] and [HLW06]. These numerical

integration techniques make use of the following operations: exponential map expmSO(3)(.)

given in (A.1) and inverse of the differential of the exponential map dexpm-1(.), which on

SO(3) can be implemented using the explicit form (B.3). The number of elementary

operations required to implement each step of these methods is summarized in Table 3.1

for the following operations: addition (+), multiplication (×), division (/), square root (√)

and trigonometric function (trig). From this table, we can conclude that both methods

Table 3.1: Number of elementary operations in each step for second order CG and MKmethods.

operation + × /√

trig

CG 2nd order 732 948 4 2 2MK 2nd order 732 946 5 3 3

have similar computational costs. The implementation of the attitude observer resorting

to the CG method leads to

s(1)ω =

N+1∑

i=1

(RT

k−1LXAXei)× (Uk−1ei),

ω(1) = C

BRT

k−1(ωr k−1 − bk−1 +Rpan,k−1[φ 0 ψ]T )− kωs(1)ω ,

Y = Rk−1 expmSO(3)

(

T (ω(1))×)

,

s(2)ω =

N+1∑

i=1

(YT LXAXei)× (Ukei),

ω(2) = C

BRT

k (ωr k − bk +Rpan,k[φ 0 ψ]T )− kωs(2)ω ,

Rk = Rk−1 expmSO(3)

(T

2(ω(1))×

)

expmSO(3)

(T

2(ω(2))×

)

,

where the feedback terms s(1)ω and s

(2)ω are set to zero when the camera measurements are

not available.

47


3.5.2 Lens distortion

Camera lens often introduce distortion in the captured images. The most common

distortion effects are the so called barrel distortion and pincushion distortion [TBW03].

Real lens usually display a combination of these two effects.

When there is distortion, the mapping function between the 3-D world coordinates

and the image coordinates provided by the perspective camera model is no longer valid.

Usually, the radial distortion is the main source of errors and therefore a nonlinear inverse

radial function is applied. This function maps the image coordinates to those that would

have been obtained with ideal lens.

Let the distorted image coordinates and undistorted image coordinates be denoted as

(ydx, ydy) and (y′x, y

′y), respectively. These coordinates are expressed in a reference frame

with origin in the image distortion center. The inverse radial distortion function can be

written as a Taylor expansion [TBW03], that yields

y′x = ydx + ydx

∞∑

i=0

kiri−1d , y′y = ydy + ydy

∞∑

i=0

kiri−1d ,

where rd =√

(ydx)2+(ydy)2.

In this work, we only take into account the parameters k3 and k5 that, as stated in

[TBW03], are enough to obtain good practical results. These constants are obtained by

solving an optimization problem as proposed in [GO09]. This method is based on the

principle that straight lines in 3-D space should ideally remain straight lines in the 2-D

image. A cost function is then defined as the distance between a set of lines in the image

and a set of ideal straight lines, and it is minimized with respect to the constants of the

inverse radial distortion function [TBW03]. This method is independent of the calibra-

tion process, which is responsible for the computation of the camera intrinsic parameters.

Figure 3.6(a) and Figure 3.6(b) show the results obtained with the distortion correction

method.

3.6 Simulation results

To demonstrate the effectiveness of the proposed ensemble this section illustrates, in

simulation, the dynamic behavior of the active camera pan and tilt controller and the

discrete-time implementation of the attitude observer during a typical vehicle maneuver.

The tuning capabilities of the observer and the controller are also displayed for two sets

of feedback gains. To test the performance of the observer, we evaluate the convergence

of the closed-loop system when the constant bias assumption is not satisfied. We also

48


(a) Original image. (b) Image with distortion compensation.

Figure 3.6: Experimental results for the image distortion compensation.

Figure 3.7: Simulated trajectory and position of the markers (where vB denotes thevelocity of the vehicle with respect to the local frame).

present a comparison between a single-rate implementation at the lower sampling rate of

the camera and the proposed multi-rate implementation, which explores the high sampling

frequency of the rate gyros.

The positions of the markers are

Lx1 =

0−1−1

m, Lx2 =

01−1

m, Lx3 =

0−11

m, Lx4 =

011

m,

which satisfy∑4

i=1Lxi = 0 and Assumption 3.1. The maneuver consists in a circular

trajectory with a radius of 2 m, parallel to the Lx,L y, plane and at a distance of 6 m.

49


0 2 4 6 8 10 12 14 16 18 200

0.2

0.4

0 2 4 6 8 10 12 14 16 18 200

2

4

6

8

10

kω = kb = 0.5kω = kb = 1

kω = kb = 0.5kω = kb = 1

Attitude error

Rate gyro bias error

time (s)

‖R−

I‖F

‖b‖(deg/s)

Figure 3.8: Attitude observer estimates for two different sets of gains.

The body velocity is aligned with the By axis and during 10 s takes the constant value

−4π/12.5 m/s (≈ 1 m/s). From then on, it decreases linearly until the starting point of the

maneuver is reached with zero velocity. The position of the markers and the trajectory of

the vehicle are depicted in Figure 3.7. The sampling rate of the vision system and inertial

measurements are set to 10 Hz and 150 Hz, respectively.

The initial estimation errors in the simulations are ‖R(0) − I‖F = 0.4129, b(0) =

[0.5 0.5 0.5]T 180π deg /s, and the initial pan and tilt camera angles are both set to 10 deg,

thus (3.17) is satisfied by the initial conditions.

Figure 3.8 illustrates the stability and convergence of the estimation errors. The time

evolution of the norm of the four markers’ centroid, ‖y‖, and the actuation imposed by

the camera pan and tilt controller are shown in Figure 3.9. The overshoot on ‖y‖ is due

to the initial bias estimation error. Notice that, as expected, when the camera linear

velocity is nonzero, the centroid of the markers in the image plane differs from the center

of the image. The figures also show that the feedback gains can be used for tuning the

convergence characteristics of the observer and the controller. Finally, Figure 3.10 depicts

the evolution of y in the image plane.

50


0 2 4 6 8 10 12 14 16 18 200

50

100

150

kω = kb = 0.5 kc = 2kω = kb = 1 kc = 4

0 2 4 6 8 10 12 14 16 18 20−100

−50

0

50

ψ k = kb = 0.5 kc = 2

φ kω = kb = 0.5 kc = 2

ψ kω = kb = 1 kc = 4

φ kω = kb = 1 kc = 4

Distance of markers centroid to the center of the image

Pan and tilt actuation

time (s)

‖y‖(pixels)

(deg/s)

Figure 3.9: Distance between the markers center and the image centroid and the pan andtilt actuation for two different sets of gains.

−100 −50 0 50 100

−150

−100

−50

0

50

100

150

kω = kb = 0.5 kc = 2kω = kb = 1 kc = 4Starting point

Path of markers’ centroid in the image

y y(pixels)

yx (pixels)

Figure 3.10: Path followed by the centroid of the markers in the image plane for twodifferent sets of gains.

51


0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

kω = kb = 0.5kω = kb = 1

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

b(t) = 0.5 sin(0.5 · 2πt)[1 1 1]T (deg /s)

b(t) = 0.5 sin(0.05 · 2πt)[1 1 1]T (deg /s)

time (s)

‖R−

I‖F

‖R−

I‖F

Figure 3.11: Simulations with a time varying rate gyro bias.

3.6.1 Time-varying rate gyro bias

The proposed attitude observer is obtained under the assumption that the rate gyro

bias is constant. If this is not the case, the estimator will not be able to estimate exactly

the time-varying offset in the rate gyro measurements. Figure 3.11 and Figure 3.12 show

simulation results for two sinusoidal bias signals that vary at different rates. These figures

show that the attitude and rate gyro bias estimation errors no longer converge to zero,

but remain bounded. Moreover, the simulations also show that the bound increases with

the bias time derivative, which is the theoretically predicted behavior.

3.6.2 Multi-rate algorithm

The inertial sensors have a much larger bandwidth than computer vision systems. In

our discrete-time algorithm we exploit this fact to predict the attitude using solely the

angular velocity information when the visual information is not available. This is illus-

trated in Figure 3.13, which shows the time evolution of the attitude estimation error for

the continuous-time, multi-rate, and single-rate implementations of the observer. Notice

that the latter does not update the attitude estimate using the angular velocity readings

when the visual information is not available. Figure 3.13 clearly shows that the multi-rate

implementation provides a much better approximation of the continuous-time observer

than the single-rate version and thereby leads to smaller estimation errors.

52

3.7 Experimental prototype on a calibration motion rate table

0 2 4 6 8 10 12 14 16 18 200

0.02

0.04

0.06

0.08

0 2 4 6 8 10 12 14 16 18 200

0.02

0.04

0.06

0.08

kω = kb = 0.5kω = kb = 1

b(t) = 0.5 sin(0.5 · 2πt)[1 1 1]T (deg /s)

b(t) = 0.5 sin(0.05 · 2πt)[1 1 1]T (deg /s)

time (s)

‖b‖(deg/s)

‖b‖(deg/s)

Figure 3.12: Simulations with a time varying rate gyro bias.

0 1 2 3 4 5 6 7 8 9 100

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

continuous−time observermulti−rate observersingle−rate observer

time (s)

‖R−

I‖F

Figure 3.13: Attitude estimation error using the continuous-time observer, the single-rateobserver and the multi-rate observer.

53


Figure 3.14: Experimental setup.

(a) MEMSense nIMU. (b) AXIS 215 PTZ Camera.

Figure 3.15: Hardware used in the real time prototype.

3.7 Experimental prototype on a calibration motion rate

table

3.7.1 Experimental setup

To assess the performance of the proposed solution, an experimental setup comprising a

MEMSense nIMU and an AXIS 215 PTZ camera was mounted on a Model 2103HT motion

rate table, which enabled the acquisition of high accuracy ground truth data. Figure 3.14

shows the experimental setup together with the set of four colored circles that were used

as visual markers in the experiments. The circles are placed in the vertices of a rectangle

54


with 0.5 m of height and 0.75 m of width.

The Model 2103HT from Ideal Aerosmith [IA06] is a 3-axis motion rate table that

provides precise angular position and rate (position accuracy ±0.0083 deg, rate accuracy

0.01% ± 0.0005 deg /s).

The MEMSense nIMU shown in Figure 3.15(a) is an inertial unit that contains a triad

of rate gyros, a triad of accelerometers, and a triad of magnetometers. The data is received

via a RS422 communication link with sampling frequency of 150 Hz. The rate gyros have

a dynamic range of ±150 deg /s, a typical noise of 0.36 deg /s (1σ) and a maximum noise

of 0.95 deg /s (1σ).

The AXIS 215 PTZ shown in Figure 3.15(b) is a network camera that can be controlled

in pan and tilt using stepper motors (±170 deg pan range, 180 deg /s pan speed, 180 deg

tilt range, and 140 deg /s tilt speed). The angular positions and speeds can be set with a

resolution of 1 deg and 1 deg /s, respectively. The interface with the camera is done via a

local network and using the HTTP protocol. The acquired JPEG images have 352 × 288

pixels, which reflects the compromise reached between image resolution, acquisition time

and processing time. The sampling frequency of the image measurements, which accounts

for acquisition and marker extraction, is 10 Hz.

This section describes the results obtained in two experiments that attest the feasibility

of the attitude observer and of the ensemble comprising the observer and the camera pan

and tilt controller. The camera acquires images at 10 Hz and the rate gyros sampling rate

is 150 Hz. In both experiments, the motion rate table described a combination of angular

motions in roll, pitch, and yaw and the distance between the camera and the markers

ranged between 1.40 m and 1.60 m. The camera calibration matrix is given by

A =

f

sx−f cot θ

sxox

0f

sy sin θoz

0 0 1

,

where the focal length is f = 3.8 mm, the image center is (ox, oy) = (168.87, 143.42) pixels,

the effective pixel sizes in x and y is given by the pair (sx, sy) = (0.011, 0.01)mm, and

the angle between axes is θ = 90 deg.

3.7.2 Using the measurement noise to tune the observer gains

Figure 3.16 shows the angular rate measurements obtained by the nIMU MEMSense

during the first experiment (which will be described later) in comparison with the values

provided by the calibration table. The measurement noise is characterized by a standard

55


0 50 100 150 200 250 300 350

−2

0

2

measuredground truth

0 50 100 150 200 250 300 350

−2

0

2

0 50 100 150 200 250 300 350

−2

0

2

time (s)

ωrx(deg/s)

ωry(deg/s)

ωrz(deg/s)

Figure 3.16: Angular rate measurements of the first experiment.

deviation of 0.5443 deg /s, 0.4756 deg /s, and 0.5386 deg /s for the x-, y-, and z-axis,

respectively. Regarding the visual markers, the measurement noise exhibited a standard

deviation of 0.1 pixels.

The technique used for obtaining the attitude observer is naturally suited for tackling

uncertainties like the presence of a constant bias in the rate gyros. On the other hand,

it renders the handling of others like unbiased noise not straightforward. Nonetheless, we

can exploit the information about the measurement noise characteristics in the design of

the observer gains. This is accomplished by running the observer in simulation using the

experimentally acquired measurement noise and searching for the minimum error over a

discrete array of values for the gains kω, kb (see Figure 3.17). The minimum quadratic

error was obtained for the pair kω = 10−0.5 ≈ 0.316, kb = 10−1.75 ≈ 0.018.

3.7.3 Wahba’s problem and the multiplicative extended Kalman filter

The results were compared with the solution to the classical Wahba’s problem [Wah65]

and with estimates produced with the multiplicative extended Kalman filter (MEKF)

[LM82, Mar03]. The Wahba’s solution is algebraic and gives the optimal rotation matrix

to the problem of attitude reconstruction using vector measurements, while the MEKF is

a dynamic estimator that builds on the Kalman filtering theory for linear systems but is

specially tailored to take into account the norm constrain of quaternions. This estimator

56


1 n

∑n k=1‖R

(kT)−

I‖F

kωkb

minimum

Figure 3.17: Average quadratic error for several sets of gains k and kb.

is adopted to estimate attitude of many high-end platforms such as satellites and UAVs.

Wahba’s problem

The Wahba’s problem consists in finding the proper orthogonal matrix R ∈ SO(3)

such that the loss function

J(R) = ‖V −RW‖, V,W ∈ R3×m,

is minimized for a given matrix norm. The solution to this problem is given, for instance,

by the Davenport’s q-method, the QUEST algorithm, the ESOQ algorithms and the SVD

algorithm [MM00].

Multiplicative extended Kalman filter (MEKF)

As in the case of the nonlinear observer proposed in this chapter, when rate gyro mea-

surements are available, one can exploit the rigid body kinematics to filter and improve

the attitude estimates. The unit quaternion is the globally nonsingular attitude param-

eterization with the lowest dimension. However, its norm constrain leads to a singular

state covariance matrix which hamper the application of the well-known EKF. This issue

is addressed by the MEKF [LM82] by representing the attitude as the quaternion product

q = δq(α)⊠ q, (3.26)

57


where q is the actual attitude, q is the estimated attitude, and δq(α) is a quaternion

representing the estimation error parameterized by α ∈ R3. The quaternion product can

be computed by

δq(α)⊠ q = Ψ(δq(α))q,

where Ψ(.) is such that

Ψ(q)q =

[qsI3 + (qv)× qv

−qTv qs

] [qv

qs

]

,

where qv and qs denote the vector and scalar components of q, respectively, and qv and qs

denote the vector and scalar components of q, respectively. The MEKF estimates α using

an unconstrained approach while preserving the norm of q. Additionally, the MEKF also

estimates the drift-rate bias using a simple measurement model for the rate gyros, where

its bias are assumed to be governed by a Gaussian white-noise process.

The MEKF state dynamic model can be obtained by differentiating and rearranging

(3.26)

d

dtδq(α) = q⊠ q−1 − q⊠ q−1

⊠ ˙q⊠ q−1,

where the identity ddt q

−1 = −q−1⊠ ˙q⊠ q−1 is used,

q =1

2

[ω

0

]

⊠ q,

˙q =1

2

[ω

0

]

⊠ q.

After some algebraic manipulations, we obtain

d

dtδq(α) =

1

2

([ω

0

]

⊠ δq(α)− δq(α)⊠

[ω

0

])

. (3.27)

The rate gyros measurements are described by

ωr = ω + b+ nω,

where ω is the actual angular velocity, b is the gyro bias, and nω denotes the measurement

noise, which is a white noise random process whose covariance matrix satisfies

Cov(nω(t)) = σ2ωI3δ(t− t′),

where Cov(nω(t)) denotes the covariance matrix of nω(t), δ(t) denotes the Dirac delta

function, and σ2ω denotes the variance of each measurement component. The bias error is

defined as

δb = b− b,

58


where b denotes the bias estimates. The estimated angular velocity is obtained by sub-

tracting the estimated bias to the angular velocity measurements

ω = ωr − b = ω + δb+ nω.

The MEKF assumes the bias time evolution is governed by a white noise random process

b = nb,

where nb is a white noise random process satisfying

Cov(nb(t)) = σ2b I3δ(t− t′),

and σ2b denotes the variance of each bias component. This implies that

˙b = 0,

and, consequently, that

δb = nb.

Rearranging (3.27) and neglecting second order terms yields

d

dtδqv(α) = −(ω)×δqv(α)− 1

2(δb+ nω),

d

dtδqs(α) = 0,

where δqv(α) and δqs(α) denotes the vectorial and scalar parts of the quaternion δq(α).

As mentioned before, the MEKF addresses the singularity of the state covariance matrix by

representing the estimation error by a 3-dimensional attitude parameterization. Different

attitude errors representations α are analyzed in [Mar03]. As illustrative example, let us

use the following representation

α = θλ,

where θ is the rotation angle and λ is the Euler axis. By considering a small angle

approximation such that α ≈ 12δqv , the MEKF system model is given by

α = −(ω)×α− (δb+ nω),

δb = 0.

The attitude measurements are in the form of vector observations satisfying

Bvi = A(q)Ivi, i = 1, . . . , N,

59


0 50 100 150 200 250 300 350

−2

0

2

4

Wahba’s solutionground truth

0 50 100 150 200 250 300 350

0

5

10

0 50 100 150 200 250 300 350−10

0

10

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.18: Attitude determination using the solution to Wahba’s problem and groundtruth data.

where Bvi denotes the vector observation in body frame coordinates, Ivi denotes the

constant vector in inertial coordinates, and A(q) is the rotation matrix describing the

same attitude as the quaternion q. Then, the small angle approach leads to the following

measurement model

δy ≈

(A(q−)Iv1)× 03...

...(A(q−)IvN )× 03

δx,

where q− denotes the estimated quaternion before the update step and

δx =

[α

δb

]

.

More details on the derivation and implementation of the MEKF can be found in

[LM82] and [Mar03].

3.7.4 Experiment 1

In the first experiment the camera controller is idle and the camera stands still. The

trajectory is composed of a sequence of angular motions and is such that the visual markers

remain visible. The initial attitude estimate has an error of 6 deg in roll, pitch and yaw.

Firstly, the advantages of the proposed algorithm when compared with an algebraic

method are illustrated. As opposed to the dynamic observer proposed in this chapter, the

60


0 50 100 150 200 250 300 350

−2

0

2

4

NL observer estimatesground truth

0 50 100 150 200 250 300 350

0

5

10

0 50 100 150 200 250 300 350−10

0

10

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.19: Attitude estimates using the proposed algorithm and ground truth data.

estimation method based on Wahba’s problem neglects all knowledge of previous estimates

and relies solely on the vector readings extracted from the current image measurements.

The resulting attitude estimates are shown in Figure 3.18. Figure 3.19 shows the atti-

tude estimation results provided by the method proposed in this chapter that exhibit an

increased accuracy and smoothness.

Figure 3.20 shows the errors of the two methods. The algorithm proposed in this

chapter produces smaller errors and, in addition, can provide attitude estimates at a

much higher sampling rate. The improvement in accuracy is also evidenced by the

root-mean-square (RMS) estimation errors. The RMS errors obtained with the observer

are 0.1094 deg for roll, 0.3598 deg for pitch, and 0.4712 deg for yaw. Whereas the RMS

errors obtained with the Wahba’s solution are 0.0945 deg, 0.5251 deg, and 0.5509 deg for

roll, pitch, and yaw, respectively. This clearly illustrated the advantages of fusing the vi-

sual information of the markers with inertial measurements. In fact, as mentioned before,

the measurements from these two sources have complementary frequency characteristics.

The rate gyros provides high bandwidth angular velocity measurements that drive the rigid

body kinematic model, whereas the active vision system provide low bandwidth stable at-

titude information. By fusing these two complementary measurements, high working rates

of the estimator can be achieved, while preserving long term stability. Figure 3.21 shows

the time evolution of the angular rate bias estimates. Although, due to the existence of

61


0 50 100 150 200 250 300 350−1

0

1

2

3

Wahba´s solution errorNL observer estimation error

0 50 100 150 200 250 300 350−2

0

2

0 50 100 150 200 250 300 350−2

0

2

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.20: Comparison of the estimation errors brought about by the use of the Wahba’ssolution and by the use of the proposed observer.

0 50 100 150 200 250 300 350

−0.4−0.2

00.20.4

0 50 100 150 200 250 300 3500

0.5

1

0 50 100 150 200 250 300 3500

0.5

1

time (s)

b x(deg/s)

b y(deg/s)

b z(deg/s)

Figure 3.21: Rate gyros bias estimates.

measurement errors, the bias estimates do not reach a constant value, they remain within

tight intervals.

62


0 50 100 150 200 250 300 350

−2

0

2

4

0 50 100 150 200 250 300 350

0

5

10

0 50 100 150 200 250 300 350−10

0

10

MEKF estimatesground truth

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.22: Attitude estimates using an MEKF and ground truth data.

0 50 100 150 200 250 300 350−1

0

1

2

3

0 50 100 150 200 250 300 350−2

0

2

0 50 100 150 200 250 300 350−2

0

2

MEKF estimation errorNL observer estimation error

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.23: Comparison of the estimation errors brought about by the use of the MEKFand by the use of the proposed observer.

Figure 3.22 shows the attitude estimates obtained by a carefully tuned MEKF. In order

to apply this method to the problem described in this chapter, it was necessary to express

63


0 50 100 150 200 250 300 350

−0.4−0.2

00.20.4

0 50 100 150 200 250 300 3500

0.5

1

0 50 100 150 200 250 300 3500

0.5

1

time (s)

b x(deg/s)

b y(deg/s)

b z(deg/s)

Figure 3.24: MEKF rate gyros bias estimates.

the vectors xi, i = 1, . . . , N , in the body reference frame B. This is accomplished by

multiplying the vector Cxi, i = 1, . . . , N by BCR given in (3.1). It was also necessary to

judiciously tune the MEKF dynamics and measurements covariance matrices, which was

not a straightforward exercise. The initial conditions of the MEKF are similar to the

ones of the nonlinear observer devised in this chapter. Figure 3.23 compares the attitude

estimation errors produced by the MEKF and by the proposed method. Both estimation

errors are indeed very similar, almost overlapping each other. This is also confirmed by

the RMS errors of each of the methods, which are presented in Table 3.2 together with the

RMS error of the Wahba’s solution. This fact, however, should not be a surprise, since

the MEKF relies on the linearization of the actual kinematics. Thus, in the close vicinity

of the equilibrium point, it has characteristics analogous to ones of the Kalman filter,

which is an optimal estimator for linear systems. The main advantages of the nonlinear

observer are the very large basin of attraction and simpler tuning process. The MEKF

bias estimates are depicted in Figure 3.24, which are also very similar to the nonlinear

observer bias estimates depicted in Figure 3.21.

64


Table 3.2: RMS of the estimation errors of the nonlinear (NL) observer and of the Wahba’ssolution and of the MEKF.

rollRMS (deg) pitchRMS (deg) yawRMS (deg)

Wahba’s solution 0.0945 0.5251 0.5509MEKF 0.0909 0.3795 0.4706

NL observer 0.1094 0.3598 0.4712

3.7.5 Experiment 2

In the second experiment, the performance of the ensemble comprising the attitude

observer and the camera pan and tilt controller is shown. In contrast to the first experi-

ment, the motion range of the second one requires the control of the camera pan and tilt

angles in order to keep the markers visible. This experiment is characterized by a set of

motions at constant angular velocity. The observer gains are the same used in the first

experiment.

Figure 3.25 shows the time evolution of the camera pan and tilt position and velocity,

and the time evolution of the markers’ centroid in the image plane. Despite the reasonable

range of movements of the trajectory, the markers remain visible throughout the experi-

ment due to the compensation provided by the camera pan and tilt controller. Since the

body yaw angle is aligned with the camera pan rotation axis, when a rotation is performed

around this axis, the camera has no linear velocity and the centroid of the four selected

markers, y, remains close to the center of the image. On the other hand, when there

exists a pitch angle rotation, the linear velocity is different from zero and, as theoretically

expected, y is not in the origin.

Figure 3.26 and Figure 3.27 show the attitude estimates obtained using the solution to

the Wahba’s problem and the proposed method. The errors of the two methods are shown

in Figure 3.28. As in the previous experience, the errors are smaller using the proposed

attitude observer. The RMS errors obtained with the proposed observer are 0.3431 deg

for roll, 0.4872 deg for pitch, and 0.8367 deg for yaw. Whereas the RMS errors obtained

with the solution to the Wahba’s problem are 0.4762 deg, 0.5816 deg, and 0.8622 deg for

roll, pitch, and yaw, respectively. These results confirm the advantages of using an inertial

fusing algorithm instead of an algebraic method.

Figure 3.29 shows the time evolution of the angular rate bias estimates. The attitude

estimation errors and the variability of the bias estimate are greater in the second experi-

ment than in the first. This is due to the errors introduced by the camera motion and the

low accuracy of the pan and tilt angular position and velocity measurements.

Figure 3.30 shows the attitude estimates obtained with the MEKF and Figure 3.31

65


0 50 100 150 200 250−100

0

100

ψφ

0 50 100 150 200 250−10

0

10

ψ

φ

0 50 100 150 200 250−20

−10

0

10

yxyy

Camera pan and tilt position

Camera pan and tilt velocity

Position of the markers’ centroid in the image

time (s)

(deg)

(deg/s)

(pixels)

Figure 3.25: Time evolution of the camera pan and tilt position and velocity and of themarkers’ centroid in the image plane.

0 50 100 150 200 250

−2

0

2

4

Wahba’s solutionground truth

0 50 100 150 200 250

−40

−20

0

20

0 50 100 150 200 250−100

0

100

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.26: Attitude determination using the solution to Wahba’s problem and groundtruth data.

depicts the attitude estimation errors produced by the MEKF and by the proposed method.

Similarly to the Experiment 1 estimation errors in Figure 3.23, Figure 3.31 shows that

the observer estimates are almost coincident with the MEKF. This indicates that in the

66


0 50 100 150 200 250

−2

0

2

4

NL observer estimatesground truth

0 50 100 150 200 250

−40

−20

0

20

0 50 100 150 200 250−100

0

100

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.27: Attitude estimation using the proposed algorithm and ground truth data.

0 50 100 150 200 250−2

0

2

4

Wahba’s solution errorNL observer estimation error

0 50 100 150 200 250

−2

0

2

0 50 100 150 200 250

−2

0

2

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.28: Comparison of the estimation errors brought about by the use of the Wahba’ssolution and by use of the observer.

vicinity of the desirable equilibrium point both estimation methods have performances

close the optimum. The higher noise levels observed in Figure 3.31 and discontinuities

are due to the pan and tilt stepper motors, which are not ideal and exhibit hysteresis

67


0 50 100 150 200 250

−0.4−0.2

00.20.4

0 50 100 150 200 2500

0.5

1

0 50 100 150 200 2500

0.5

1

1.5

time (s)

b x(deg/s)

b y(deg/s)

b z(deg/s)

Figure 3.29: Angular rate bias estimation.

0 50 100 150 200 250

−2

0

2

4

MEKF estimatesground truth

0 50 100 150 200 250

−40

−20

0

20

0 50 100 150 200 250−100

0

100

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.30: Attitude estimates using an MEKF and ground truth data.

with magnitude up to 2 deg. The RMS errors of the estimates obtained with the three

considered estimation methods are given in Table 3.3. These results are coherent with the

ones obtained during Experiment 1, which show that once the estimators have converged,

the MEKF and the proposed observer have similar performances, which are better than

68

3.8 Experimental implementation onboard a quadrotor

0 50 100 150 200 250−1

0

1

2

3

MEKF estimation errorNL observer estimation error

0 50 100 150 200 250−2

0

2

0 50 100 150 200 250−2

0

2

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)


Table 3.3: RMS of the estimation errors of the nonlinear (NL) observer and of the Wahba’ssolution and of the MEKF.

rollRMS (deg) pitchRMS (deg) yawRMS (deg)

Wahba’s solution 0.4762 0.5816 0.8622MEKF 0.3727 0.4903 0.8502

NL observer 0.3431 0.4872 0.8367

the performance of the Wahba’s solution. The rate gyro bias estimates of the MEKF are

depicted in Figure 3.32. These estimates are similar to the ones of the observer shown in

Figure 3.29.


The promising results obtained by the prototype presented in Section 3.7 motivated its

implementation in a more practical scenario. To that end, a new experimental prototype

was built and integrated with an AscTec Pelican quadrotor made by Ascending Technolo-

gies GmbH. This new prototype was necessary in order to be compatible with the payload

capabilities of the AscTec Pelican.

Quadrotors, also called quadcopters, are UAVs equipped with four propellers, which

provide lift and steering capabilities. This type of rotorcraft has a relatively simple con-

69


0 50 100 150 200 250

−0.4−0.2

00.20.4

0 50 100 150 200 2500

0.5

1

0 50 100 150 200 2500

0.5

1

time (s)

b x(deg/s)

b y(deg/s)

b z(deg/s)

Figure 3.32: MEKF rate gyros bias estimates.

struction, but on the other hand, poses a very interesting control problem, due to its

underactuated nature (horizontal motion is not possible without tilting the platform) and

challenging human piloting by direct control of the propellers rotation. These reasons have

motivated considerable research interest during the past years. Moreover, quadrotors are

nowadays used in several applications due to its simple construction, reduced-size, high

maneuverability, and low-cost when compared, for instance, with helicopters. Capture of

aerial images of sport or cultural events, real-time situation awareness (civil and military),

and infrastructure inspection are examples of applications where the quadrotors particular

characteristics are most valuable.

The experiment results described in this section were obtained at the SCORE lab

flying arena at the University of Macau, where a motion capture system is installed pro-

viding ground truth attitude and position data and enabling an accurate evaluation of the

performance of the proposed nonlinear observer as well as the onboard controllers.

The purpose of this experiment is to evaluate the performance of the nonlinear observer

proposed in Section 3.3, and of the camera pan and tilt controller devised in Section 3.4, but

also of the closed-loop consisting of the quadrotor stabilizer proposed in [9] together with

the observer and camera controller. The stability of the ensemble has been theoretically

analyzed and established in [9].

The quadrotor is equipped with a low-level AscTec autopilot and an Atom computer

70


Figure 3.33: Experimental apparatus at SCORE Lab – University of Macau: AscTecPelican quadrotor, fiducial markers, and motion capture system.

which obtains angular velocity measurements from the autopilot at 100Hz and acquires

and processes camera images at 30Hz. These measurements are transmitted to a ground

station where the nonlinear attitude observer and control loops are implemented. A light-

weight pan and tilt platform was developed comprising a camera with an M12 mount

lens with focal length of 2.1mm and the calibrated camera optical center is given by

(ox, oy) = (368.6, 220.0) pixels. The sensor area is 752 × 480 pixels and the pan and tilt

angles are controlled by two servo motors. The feedback actuation is computed on the

ground station and transmitted back to the onboard computer which relays them to the

AscTec autopilot and to the pan and tilt servos. The onboard computer also performs the

detection of fiducial markers which are illustrated in Figure 3.33. The SCORE lab flying

arena, where the tests were realized, is equipped with a motion capture system that relies

on reflective markers installed onboard to provide ground truth data.

The adopted quadrotor controller is proposed in [9] following the approach of [ISM03]

and [MN07], and is characterized by different time-scale dynamics in the vertical and

lateral-longitudinal motion. The vertical controller can be viewed as a time-varying

proportional derivative controller, whereas the lateral-longitudinal motion is stabilized

71


Quadrotor

Controller

Camera

Controller

Attitude

Observer

ωr

yi, φ, ψ

bφ, ψ

φ, ψ

R, b

Propellers’thrust

Figure 3.34: Block diagram of the overall system comprising the nonlinear observer, thecamera controller, the quadrotor controller, the pan and tilt steering platform and thequadrotor itself.

by a nested saturations control scheme with a feedforward structure. For the lateral-

longitudinal controller, an inner-outer loop strategy is adopted. The inner-loop stabilizes

the attitude of the quadrotor such that the thrust vector points in the desired direction

and the outer-loop provides the desired thrust direction based on the linear position and

velocity of the vehicle. The block diagram of the closed-loop system comprising the non-

linear attitude observer, the pan and tilt camera controller and the quadrotor controller

is depicted in Figure 3.34.

The nonlinear observer relies on the image coordinates of the markers and on the

angular velocity measurements to compute attitude and gyro bias estimates, which are

exploited by the camera controller together with the markers coordinates to compute

pan and tilt rates that maintain the camera in the direction of the markers. Then, the

quadrotor controller uses the camera information, markers coordinates and pan and tilt

angles, as well as the attitude and gyro bias estimates to compute the actuation that

stabilizes the quadrotor motion.

In the sequel, a representative experiment of the proposed observer and controllers at

the SCORE flying arena is analyzed. Initially, the quadrotor is at rest, and the camera

is pointing towards the markers center, and at t = 0 s, the attitude observer, the camera

controller and the quadrotor controller are activated. In Figure 3.35, the camera pan

and tilt angles, their angular velocities and the image coordinates of the marker’s center

are shown. The initial transient observed in the pan and tilt angles (upper panel of

Figure 3.35) is due to the quadrotor motion. In particular, due to the quadrotor tilting

to be able to move longitudinally, i.e. horizontally. It was verified that the servos had a

72


0 2 4 6 8 10 12 14 16−20

0

20

40

0 2 4 6 8 10 12 14 16−100

0

100

200

0 2 4 6 8 10 12 14 16−100

0

100

200

ψφ

ψ

φ

yxyy

Camera pan and tilt position

Camera pan and tilt velocity

Position of the markers in the image

time (s)

(deg)

(deg/s)

(pixels)

Figure 3.35: Time evolution of the camera pan and tilt position and velocity and of themarkers’ centroid in the image plane.

0 100 200 300 400 500 600 7000

50

100

150

200

250

300

350

400

450

Markers’ centroidMarker 1Marker 2Marker 3Marker 4

y y(pixels)

yx (pixels)

Figure 3.36: Trajectory of the markers in the image plane.

low response time and were not able to totally mitigate this transient. Thus, the image

coordinates of the marker’s center (bottom panel of Figure 3.35) also display an initial

transient associated with the quadrotor motion. However, after the initial transient the

pan and tilt controller was able to keep the camera pointing at the center of the markers.

This experiment highlights the importance of having a steerable camera onboard and an

73


0 2 4 6 8 10 12 14 16−30−20−10

010

0 2 4 6 8 10 12 14 16−20

0

20

0 2 4 6 8 10 12 14 16

160

180

ground thruthestimated

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)

Figure 3.37: Attitude estimation using the proposed nonlinear observer and the attitudeground truth.

adequate controller. Without controlling the pan and tilt angles, the makers would leave

the field of view of the camera due to the quadrotor motion and the controller would no

longer have the required data to stabilize the platform.

Initially, the quadrotor performs a tilting maneuver to be able to move laterally and

reduce the initial position error. This maneuver is evident in the roll and pitch angles

shown in Figure 3.37 between t = 0 s and t = 1 s. The initial roll, pitch and yaw estimation

errors were set to −10 deg, −6 deg, and 6 deg, respectively. Figure 3.37 shows the attitude

estimates and the attitude provided by the motion capture system, considered as ground

truth. For comparison proposes, attitude estimates were obtained using the Wahba’s

solution and the MEKF, which are depicted in Figure 3.38 and Figure 3.39, respectively.

The Wahba’s solution is very noisy during the first second of the experiment. This is due

to initial movement of the quadrotor and of the camera pan and tilt platform. Since the

pan and tilt mechanism is not well dumped, undesirable vibrations of the camera sensor

occur. The MEKF and observer are able to filter this initial camera measurements noise

and produce much smoother estimates. However, approximately constant pan and tilt

measurement bias also introduce errors that cannot be fully compensated.

The time evolution of the attitude estimation error of the proposed observer and those

of the MEKF are depicted in Fig 3.40. This figure shows that, after the initial transient,

which was about 2.5 s, the estimation error is small despite the non-negligible inaccuracy

74


0 2 4 6 8 10 12 14 16−30

−20

−10

0

10

ground thruthWahba’s solution

0 2 4 6 8 10 12 14 16−20

0

20

0 2 4 6 8 10 12 14 16

160

180

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)


0 2 4 6 8 10 12 14 16−30

−20

−10

0

10

ground thruthMEKF estimates

0 2 4 6 8 10 12 14 16−20

0

20

0 2 4 6 8 10 12 14 16

160

180

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)


of the pan and tilt mechanism. Although very similar, the estimates of the proposed

observer and those of the MEKF are not as close as in the previous experiments, which is

justified by a different gain selection. The rate gyro bias estimates obtained during the

75


0 2 4 6 8 10 12 14 16−15−10−5

05

NL observer estimation errorMEKF estimation error

0 2 4 6 8 10 12 14 16−10

−5

0

5

0 2 4 6 8 10 12 14 16−5

0

5

10

time (s)

roll(deg)

pitch

(deg)

yaw

(deg)


0 2 4 6 8 10 12 14 16−2

0

2

NL observer bias estimatesMEKF bias estimates

0 2 4 6 8 10 12 14 16−2

0

2

0 2 4 6 8 10 12 14 16−2

0

2

time (s)

b x(deg/s)

b y(deg/s)

b z(deg/s)

Figure 3.41: Proposed observer and MEKF rate gyros bias estimates.

experiment using the proposed observer and the MEKF are shown in Fig 3.41. Interesting

enough, these estimates are of the same order of magnitude of the bias estimated in the

previous experiments (see Figure 3.21 and Figure 3.29).

76

3.9 Chapter summary

0 2 4 6 8 10 12 14 16

−1

−0.5

0

0.5

1

xyz

time (s)

(m)

Figure 3.42: Position error.

In this experiment, the performance of the quadrotor controller proposed in [9] was

also evaluated. The position error is depicted in Figure 3.42, showing that after the

initial transient, the position of the quadrotor converges rapidly to the desired location.

This figure shows that after the initial 2 s the position error remains below 0.2m, which

attests the performance of the closed-loop system comprising the attitude observer, and

the camera and quadrotor controllers.

3.9 Chapter summary

This chapter addressed the problem of estimating the attitude of a vehicle equipped

with a triad of rate gyros and a pan and tilt camera. An exponential ISS pan and tilt

control law that enforces marker visibility was introduced. A multi-rate implementation of

the nonlinear attitude observer was proposed, which takes advantage of the complementary

frequency bands of the sensors and uses recent results in geometric numerical integration.

An optimal method for the compensation of the lens distortion was implemented and

a griding technique was used for obtaining suitable observer feedback gains. The high

level of performance attained by the proposed solution was experimentally demonstrated

resorting to a 3-axis motion rate table. Another prototype was developed and installed

onboard a quadrotor. This new prototype was necessary in order to be compatible with

the payload capabilities of the AscTec Pelican quadrotor. The promising results which

were achieved have demonstrated the relevance of the proposed solution for the operation

of autonomous platforms, such as UAVs, in GPS denied environments.

77

Chapter 4

A nonlinear attitude observer

based on range and inertial

measurements

4.1 Introduction

The previous chapter was dedicated to the development of an attitude estimation

solution for UAVs based on vision and inertial sensors. External aiding measurements, as

the ones provided by cameras, are important to correct the estimation errors associated

with dead reckoning. However, environmental characteristics, such as poor luminosity

and the existence obstacles that block the markers visibility may hinder its adoption.

A cost effective alternative to the vision system is the use of ranges between ultrasonic

beacons installed on the mission scenario and receivers on board. Such alternative is

mainly targeted at indoor environments, since, in general, there is greater control over the

mission scenario and the distances covered by the vehicle, for instance a UAVs, are more

compatible with the operational range of ultrasonic beacons. This range-based solution

can be exploited in applications such as surveillance and green house crop spraying (for

e.g. fertilization).

Range measurements are obtained from beacons installed in the mission scenario (in-

ertial frame) and acoustic sensors fixed in the moving body reference frame. A nonlinear

observer is proposed and its stability and performance properties are studied resorting to

Lyapunov techniques. In the presence of constant bias on the rate gyro measurements,

the attitude and bias estimates are shown to converge exponentially fast to the desired

79

4. A nonlinear attitude observer based on range and inertial measurements

values inside an almost global region of attraction. Additionally to the different sensor

suite, this chapter also present some theoretical innovations with respect to the previous

chapter. By using an attitude error definition and a feedback law distinct from the ones in

Chapter 3 together with a judicious selected Lyapunov transformation, exponential con-

vergence bounds can be easily computed and for time-varying bias sufficient small bounds

on the rate gyro bias can be obtained that guarantee boundedness of the overall estimation

error. Moreover, the tuning method developed for this observer is considerably more effi-

cient than the one presented in Chapter 3 and is rooted on solid theoretical background,

namely, on Kalman-Bucy filtering theory. The performance of the proposed nonlinear

observer is validated experimentally using a custom built prototype and a high accuracy

calibration table, which provides ground truth signals for assessing the performance of

the resulting estimator. Preliminary versions of the work in this chapter can be found in

[2, 6, 10].

The remainder of this chapter is structured as follows. In Section 4.2, the attitude

estimation problem is introduced and the sensor suite is described. In Section 4.3, the

nonlinear attitude observer is presented and its properties highlighted. A method to tune

the observer gains based on the Kalman-Bucy filter is proposed in Section 4.4. Simulation

results are outlined in Section 4.5, illustrating the observer properties for time-varying

angular velocities. The experimental setup is described in Section 4.6, and in Section 4.7,

experimental results validating the performance of the proposed solution are presented.

Finally, Section 4.8 presents a summary of the results in this chapter.


In this section, the attitude estimation problem is introduced and the sensor suite is

characterized. Consider the two reference frames depicted in Figure 4.1, the local reference

frame L, which is placed in the mission scenario, and the reference frame B, attachedto the rigid body. For the sake of simplicity, L is assumed to be an inertial frame.

Let R ∈ SO(3) be the rotation matrix that transforms the vectors expressed in Bto L and ω ∈ R

3 the rigid body angular velocity expressed in B. As introduced

in Chapter 2 in (2.5), the rigid body attitude kinematics is described by the differential

equation

R = R(ω)×.

The sensor suite comprises a triad of rate gyros and an acoustic positioning system.

Like in the previous chapter, the angular velocity measurements ωr ∈ R3 are assumed to

80


B

x4

B

x3

B

x1

B

x2

L

x2

L

x1

L

x3

L

x4

B

L

Figure 4.1: Frames and navigation system configuration.

be corrupted by a constant unknown bias term b ∈ R3 so that

ωr = ω + b.

The basic elements of the acoustic positioning system are an array of beacons placed

in the mission scenario and an array of receivers installed on the vehicle. The beacons

cannot be all in the same position nor in a line and should be placed so as to minimize

the multi-path effects and occlusions, which can lead to the existence of outliers and

incorrect measurements. Additionally, a pre-processing algorithm should be implemented

in order to detect such measurement faults. However, this algorithm is not addressed

in this work, which is focused on the attitude estimation problem. The receivers have

the capability of distinguishing the beacons which have emitted the received signal. This

can be accomplish, for instance, by modulating the ultrasonic signal or, like the system

used later in this chapter, by emitting a RF signal simultaneously with the sound pulse

containing the identification of the emitting beacon. The range from each beacon to all

acoustic receivers can then be measured.

Let the position of the ith beacon in L and in B be denoted by Lxi ∈ R3 and

by Bxi ∈ R3, respectively, where i = 1, . . . , Nb, and Nb is the number of beacons. We

resort to the spherical interpolation method proposed in [SA87] to obtain the positions

of the beacons in B, thereby Bxi and Lxi, i = 1, . . . , Nb, are available in the filter.

The spherical interpolation method requires the existence of at least four non-coplanar

receivers, i.e., Nb ≥ 4.

81


Let BX = [Bx1 . . . BxNb] ∈ R

3×Nb and LX = [Lx1 . . . LxNb] ∈ R

3×Nb . Without loss

of generality, the origin of the local frame L is defined at the centroid of the beacons.

Therefore, the vectors Lxi, i = 1, . . . , Nb, satisfy

Nb∑

i=1

Lxi = 0. (4.1)

The observer uses measurements in the form Bui, i = 1, . . . Nb, which are obtained

using a conveniently defined coordinate transformation. This transformation takes the

form

BU = BXDXAX ,

where BU ∈ R3×Nb−1,

DX =

[01×Nb−1

INb−1

]

−[

INb−1

01×Nb−1

]

,

and AX ∈ RNb−1×Nb−1. Matrix DX serves the purpose of computing the difference be-

tween beacon positions so that BU is independent of the position of the vehicle. The

representation of the position measurements in the local reference frame L is given by

LU = RBU = LXDXAX . Since the beacons are placed in known locations of the mission

scenario, the matrix LU is known a priori.

The measurements directionality is made uniform by the non-singular linear trans-

formation AX , i.e., BUBUT = I. Such linear transformation can be obtained resorting

to the singular valued decomposition (SVD) of LXDX = WSVT , where W ∈ O(3),

V ∈ O(Nb − 1), S = [diag(s1, s2, s3) 03×(Nb−4)]. The following assumption is necessary for

computing the transformation AX .

Assumption 4.1. The positions of the beacons are not collinear, i.e., rank(LX) ≥ 2.

If LXDX is full rank, i.e., rank(LX) = 3, AX can be computed using

AX = V

[diag(s1, s2, s3)

−1 03×(Nb−4)

0(Nb−4)×3 INb−4

]

.

Then, due to the orthogonality of W and V it follows that

LULUT = LXDXAX(LXDXAX)T = I,

and since LU = RBU, we have that BUBUT = I. If [Lx1 . . . LxNb] is rank 2, LX needs to

be redefined as LX = [Lx1 . . .LxNb

(Lxi×Lxj)], for some i and j such that Lxi×Lxj 6= 0,

in order to obtain a full rank matrix.

82

4.3 Observer design

Note that the matrix AX is constant along the system trajectories since it is computed

using the position of the beacons in L which are fixed. It is convenient to orthogonalize

the measured directions, in order to simplify the design of observer feedback law.

In this chapter, we rely on range measurements to obtain the positions Bx1, . . . ,BxNb

.

Other alternatives are available that provide such measurements. Nevertheless, from the

application point of view, the use of ultrasonic beacons is still very relevant, since range

measurements are widely used in many robotic applications, in great part due to its flexibil-

ity and the availability of compact and low-power sensors. With the proper modifications

to the proposed algorithm, other sensors, such as magnetometers and gravimeters, can

also be used. These sensors have, however, their own limitations.

The objective of the present work is to exploit the information provided by the sensor

suite, by designing an attitude observer that combines the inertial measurements with

ranges between a beacon array and a receiver array.

4.3 Observer design

In this section, the attitude observer, which relies on the sensors measurements to

estimate simultaneously the rigid body attitude and rate gyro bias, is presented and its

properties are highlighted. It is shown that the proposed feedback law yields exponential

convergence of the estimation error for almost all initial conditions, in the sense that only

a set of measure zero lies outside the region of attraction.

The proposed observer takes the form

˙R = R(ω)×, (4.2a)

ω = BUBUT (ωr − b)− kωsω, (4.2b)

˙b = kbsω, (4.2c)

sω = (BUBUT − BUBUT )⊗ (4.2d)

where R ∈ SO(3) is the estimated attitude, b ∈ R3 is the estimated rate gyro bias,

kω, kb ∈ R are positive gains, the term BU = RT LU is a function of the observer estimates

and known quantities, and the term BU = BXDXAX is an explicit function of the sensor

measurements.

Let the attitude and bias estimation errors be given by R = RTR and b = b − b,

respectively, and note that, since LULUT = I and BU = RTLU, the term sω satisfies

sω = (RT −R)⊗. Notice that the attitude error and the feedback law in (4.2d) are distinct

from those proposed in the previous chapter. The main advantage of this alternative

design is that the error system is autonomous, i.e., it does not depend on time or other

83


variables other than the state, which in turn have enabled other results present in this

chapter. The autonomous error dynamics are given by

˙R = kωR(RT − R) + R(b)×,

˙b = kb(RT − R)⊗.

(4.3)

Consider the Euler angle-axis parameterization of R described in Section 2.2.3 and let

the rotation vector λ ∈ R3 and the rotation angle θ ∈ [0, π] rad, be such that R = rot(θ,λ).

Similarly to Chapter 3, a condition on the initial estimation error and observer gains

is sufficient to guarantee that no undesirable equilibrium points belong to the system

trajectories. The following lemma provides such condition, which excludes the set of

points such that R = rot(π,λ), λ ∈ R3.

Lemma 4.1. For any initial condition that satisfies

kb >‖b(to)‖2

4(1 + cos(θ(to))), (4.4)

the estimation errors x = (R, b) are bounded, and the attitude error satisfies 0 ≤ θ(t) ≤θmax < π rad for all t ≥ to.

Proof. Consider the following Lyapunov function

V =‖R − I‖2F

2+

1

2kb‖b‖2 = 2(1− cos(θ)) +

1

2kb‖b‖2. (4.5)

The time derivative of V is given by

V = − tr( ˙R) +1

kbbT ˙

b

= − tr(R(−RT ω + ω)×) +1

kbbT ˙

b = sTω(RT ω − ω) +1

kbbT ˙

b.

Since (4.2b) can be rewritten in the form ω = R(ω − b)− kωsω, along the trajectories of

the closed-loop system (4.3), V takes the form V = −kω‖sω‖2.Since V is identical to the one used in Chapter 3, the remaining of the proof follows the

same steps of the proof of Lemma 3.2. Let Ωρ = x ∈ Db : V ≤ ρ, where ρ ∈ R+. As

the Lyapunov function (4.5) is a weighted squared distance from the desired equilibrium

point, the set Ωρ is a compact set. The time derivative of the Lyapunov function satisfies

V ≤ 0, which means that any trajectory starting in Ωρ remains in Ωρ. Then, for all t ≥ to

the state is bounded.

The gain condition (4.4) is equivalent to V (x(to)) < 4. The invariance of Ωρ implies

that V (x(t)) ≤ V (x(to)) < 4, then the exists θmax such that θ(t) ≤ θmax < π rad for all

t ≥ to.

84

4.3 Observer design

Lemma 4.1 implies that by selecting a large enough gain kb, it is guaranteed that, even

for large initial estimation errors, the system trajectories include no equilibrium points

other than R = I. In general, one does not know the values of b(to) and cos(θ(to))

required in (4.4). However, in practice, one can use the information provided by the

manufacturer of the sensors to obtain worst-case values for both quantities.

The proposed observer is designed on SO(3) in order to overcome the problems associ-

ated with unwinding phenomena and singularities introduced by other attitude represen-

tations. However, to gain further insight into the properties of the error system (4.3), it is

convenient to rewrite it using the attitude vector qv = sin(θ/2)λ and apply an adequately

defined Lyapunov transformation to obtain a more tractable system. We can analyze the

properties of the error of the observer using this representation since ‖R − I‖2F = 8‖qv‖2.Using qv instead of R in the state vector yields

˙qv = −2kω q2s qv +1

2(qsI+ (qv)×)b,

˙b = −4kbqsqv,

(4.6)

where qs = cos(θ/2). Consider the following coordinate transformation η1 = 2T(t)qv,

η2 = 1√2kb

T(t)b , where T(t) ∈ SO(3) is a rotation matrix that integrates the rate gyro

bias

T(t) =1

2T(t)(b)×. (4.7)

The time derivative of the transformed state vector η = [ηT1η

T2 ]

T is given by

η = A(η)η, (4.8)

where

A(η) =

[−2kω q2sI

√2kbqsI

−√2kbqsI 0

]

.

Since T(t) is a rotation matrix, it satisfies ‖T(t)‖2 = det(T(t)) = 1 for all t ≥ to. Hence,

T(t) is a Lyapunov transformation [Rug96] and the stability properties of (4.8) also hold

for (4.6).

The following theorem characterizes the stability of the error dynamics (4.8), and

consequently (4.3), showing that the origin is exponentially stable given bounded initial

estimation errors.

Theorem 4.1. For any initial condition satisfying (4.4) the error dynamics converge

exponentially fast to the stable equilibrium point (R, b) = (I,0).

85


Proof. In order to prove the exponential convergence of the system (4.8), it is sufficient

to show that there exists positive constants a, k1, k2, k3 and a continuously differentiable

function VT such that [Kha02, Theorem 4.10]

k1‖η‖a ≤ VT (t,η) ≤ k2‖η‖a,∂VT∂t

+∂VT∂η

A(η)η ≤ −k3‖η‖a.

In particular, these conditions are satisfied if the function VT takes the form

VT (η) = ηTPη > 0, (4.9)

VT (η, t) = −ηTQ(η)η < 0, (4.10)

where P ∈ R6×6, Q(η) ∈ R

6×6 are symmetric positive definite matrices and the minimum

singular value of Q(η), denoted by σmin(Q(η)), is uniformly lower bounded by a nonzero

positive constant. In what follows, the dependency of A and Q on η is omitted for reasons

of simplicity. Let

P =

[p11I3 p12I3p12I3 p22I3

]

,

Q =

[q11I3 q12I3q12I3 q22I3

]

,

where p11, p12, p22, q11, q12, q22 ∈ R and Q = −(PA +ATP). In order to satisfy (4.9) and

(4.10), it is sufficient to show that the leading principal minors of P and Q are strictly

positive

p11 > 0, (4.11)

p11p22 − p212 > 0, (4.12)

q11 > 0, (4.13)

q11q22 − q212 > 0. (4.14)

Using some algebraic manipulations on the inequalities (4.11)-(4.14), we obtain suffi-

cient conditions for p11, p12 and p22 so that (4.9) and (4.10) hold. The condition (4.14)

can be expanded in the form

(4kω q2sp11 + 2

√

2kbqsp12)(−2√

2kbqsp12)

− (2kω q2sp12 −

√

2kbqsp11 +√

2kbqsp22)2 > 0.

(4.15)

Using qs = cos(θ/2) and assuming that Lemma 4.1 holds, it is easy to show that qsmin ≤q ≤ 1, where qsmin = cos(θmax/2) > 0. Consequently, qs can be factored out of (4.15).

Let p11 = p22, then if

− 2kω qs√2kb +

k2ω q2s√

2kb

p11 < p12 < 0, (4.16)

86

4.3 Observer design

the inequality (4.15) is satisfied. The condition (4.13) is equivalent to

−2kω qs√2kb

p11 < p12 < 0. (4.17)

Note that, any p12 that satisfies (4.16) also satisfies (4.17). Since qs ≥ qsmin> 0, we can

conclude that for any positive p11 = p22, there exists a p12 such that (4.12), (4.13) and

(4.14) are satisfied. Also note that for any choice of p12 satisfying (4.14), |det(Q)| doesnot converge to zero along time and all elements of Q are finite. Together, these two

properties guarantee that σmin(Q) is lower bounded by a positive constant, and hence the

conditions that guarantee exponential convergence of the errors to zero are satisfied.

This proof exploits a new Lyapunov transformation given by (4.7) and resorts to the

positive definite matrices P and Q to show that the errors converge exponentially fast

to the desired equilibrium point. Using the constraints of positive definiteness, P and

Q can be explicitly computed to provide exponential convergence bounds and worst-case

convergence rates as specified in

‖η(t)‖ ≤(σmax(P)

σmin(P)

)1/2

‖η(to)‖e−σmin(Q)

2σmax(P))(t−to). (4.18)

Note that, for purposes of analysis, different values for the parameter p12 can be selected

so as to reach different conclusions about the system. If p12 is close to zero the value ofσmax(P)σmin(P) is close to one, so we obtain a larger basin of attraction. Choosing a smaller value

of p12 we obtain a higher σmin(Q) and consequently a higher value of the lower bound on

the convergence rate.

Corollary 4.1. Assume that the initial estimation errors are bounded according to

θ(to) ≤ θmax, ‖b‖ ≤ bomax , (4.19)

where θmax is any satisfying (1− cos(θmax)) < 2, and select kb such that

kb >bomax

4(1 + cos(θ(to))).

Then, the origin (R, b) = (I,0) is uniformly exponentially stable in the set defined by

(4.19).

In Theorem 4.1 and Corollary 4.1, we have assumed that the rate gyro bias is con-

stant. In practice, the bias suffers fluctuations that depend on several parameters, such

as, temperature, power supply or g-sensitivities. If the constant bias assumption is lifted,

the convergence result of Theorem 4.1 no longer holds. However, we can show that for

87


bias values with bounded time derivative ‖b‖ < γ, the estimation errors are also bounded

with ultimate bound proportional to γ. A similar result has been presented previously in

Chapter 3. However, in this chapter, due to the Lyapunov transformation (4.7), explicit

matrices P and Q in (4.9) and (4.10), respectively, can be computed and, consequently,

the ultimate bound on ‖η‖ can be explicitly obtained.

Proposition 4.1. Consider the attitude observer defined in (4.2) and assume that b is

bounded, with ‖b‖ < γ. Then for a sufficiently small γ, there is a sufficiently small error

on the initial estimates such that, the estimation errors are bounded with ultimate bound

proportional to γ.

Proof. Define the domain U = η : VT (η) ≤ 4 − ǫ, 0 < ǫ < 4. From Theorem 4.1, it is

known that, if kb satisfies (4.4), there exist positive constants k1, k2, k3, and k4 such that

k1‖η‖2 ≤ VT (t,η) ≤ k2‖η‖2,VT (t,η) ≤ −k3‖η‖2,∥∥∥∥

∂VT∂η

∥∥∥∥≤ k4‖η‖,

In particular, we can choose k1 = σmin (P), k2 = σmax (P), k3 = σmin (Q), and k4 =

2σmax (P). It follows that if b 6= 0 the time derivative of VT is given by

VT ≤ −k3‖η‖2 + k4κ‖η‖‖b‖,

where κ = 1/√2kb. Assuming that ‖b‖ ≤ γ, implies that VT < 0 for ‖η‖ > k4

k3κγ and

‖η‖ <√4− ǫ. To ensure that ‖η(t)‖ does not exceed the upper bound

√4− ǫ, the initial

conditions must satisfy ‖η(to)‖ <√

k1k2(4− ǫ). Then, if γ is sufficiently small we can

guarantee that k4k3κγ <

√k1k2(4− ǫ) <

√4− ǫ, and η(t) remains in U for all t ≥ to. Since

VT < 0 for k4k3κγ < ‖η‖ <

√4− ǫ, we can conclude that ‖η‖ is ultimately bounded by

√k2k1

k4k3κγ.

4.4 Tuning the observer

The technique adopted to obtain the attitude observer is naturally suited for tackling

uncertainties like the presence of a constant bias in the rate gyros, but renders the handling

of others, like random noise, not straightforward. However, it is possible to exploit the

observer structure and consider the characteristics of the sensor noise to tune the feedback

gains kω and kb.

88

4.4 Tuning the observer

−0.05 0 0.05 0.10

0.05

0.1

0.15

0.2

0.25

0.3

0.35

−0.05 0 0.05 0.10

0.05

0.1

0.15

0.2

0.25

−0.1 0 0.10

0.1

0.2

0.3

0.4

−5 0 5 10

x 10−4

0

0.05

0.1

0.15

0.2

0.25

−5 0 5 10

x 10−4

0

0.05

0.1

0.15

0.2

0.25

−2 0 2

x 10−4

0

0.05

0.1

0.15

0.2

nω x

Norm.Hist.

nω y

Norm.Hist.

nω z

Norm.Hist.

nv x

Norm.Hist.

nv y

Norm.Hist.

nv z

Norm.Hist.

Figure 4.2: Normalized histogram of nω and nv using simulation data, where the rangeand rate gyro measurements are corrupted by Gaussian noise.

In equations (4.2b) and (4.2c) the terms BU, ωr and sω depend on the sensor mea-

surements. Taking into account the presence of noise, the equations (4.2b) and (4.2c) can

be approximated by

ω ≈ R(ωr − b+ nw)− kω(sω + nv), (4.20)

˙b ≈ −kb(sω + nv), (4.21)

where nw ∈ R3 and nv ∈ R

3 are zero mean Gaussian random variables with second

moments identical to the second moments of BUBUT (ωr − b) and (BUBUT − BUBUT )⊗,

respectively. It should be noted however that (4.20) and (4.21) are only approximations.

Computing the true probability density functions of ω and˙b would be a too cumbersome

process and the result would not be very useful. Thus, the terms nω and nv are taken

as Gaussian random variables which allows for their probability density functions to be

described by the two first moments, the mean and the variance. The validity of the

approximation is illustrated in Figure 4.2, where the profile of a normalized histogram of

nω and nv is clearly close to a Gaussian distribution.

These moments are computed in simulation environment using noisy measurements

for the distances and angular rates. With this approximation, the error dynamics are

89


Figure 4.3: Block diagram of the transformed system (4.22).

described by

˙qv =−2kω q2s qv +1

2(qsI+ (qv)×)(b+ nw − kωRTnv),

˙b =−4kbqsqv − kbnv.

Consider now the transformation ξ1(t) = T(t)qv(t), ξ2(t) = T(t)b(t), where T(t) is given

by (4.7). Linearizing the dynamics of the transformed system about the equilibrium point

(R,b) = (I,0), the following linear system is obtained

ξ = Aξ + KCξ − Kv +w, (4.22)

where

A =

[0 1

20 0

]

, K =

[kωI2kbI

]

, C = [2I 0], (4.23)

and

v =T

2nv, w =

T

2nw.

The block diagram of the transformed system is depicted in Figure 4.3. It turns out that

this system is in the Kalman-Bucy filter form (see [KB61] for more details) and therefore

one can use the associated theory to compute suitable constant gains kω and kb. This is

accomplished by solving the well known algebraic Riccati equation

0 = AP+ PAT − PCTR−1CP+ Q,

where R and Q are the covariance matrices of v and w, respectively, and P is the steady

state covariance matrix of the state ξ. Notice that, since T is a rotation matrix, it does

not affect the covariance of v and w, hence R and Q can be obtained directly from the

covariance of nv/2 and nw/2, respectively. The matrix of gains K that minimizes P is

given by K = PCR−1. Finally, the gains kω and kb are obtained directly from (4.23).

90



In this section, simulation results that illustrate the convergence properties of the

system (4.3) are shown. Five beacons are placed in the mission scenario located at Lx1 =

[2 2 2]T m, Lx2 = [−2 −2 2]T m, Lx3 = [2 −2 −2]T m, Lx4 = [−2 2 −2]T m, and Lx5 =

[0 0 0]T m. The acoustic receivers are placed onboard the vehicle at Br1 = [0 0 0]T m,

Br2 = [0.5 0 0]T m, Br3 = [0 0.5 0]T m, and Br4 = [0 0 0.5]T m, where Bri denotes the

position of the ith acoustic receiver in the body reference frame. The vehicle trajectory

is characterized by oscillatory acceleration and angular velocity with frequency 0.5 Hz.

The acceleration and angular velocity amplitudes are 1 m/s2, and 180π deg /s in all axes,

respectively. This trajectory is such that the vehicle rotates around the same axis but in

different directions along time.

The system is simulated using two different sets of feedback gains kω = 1, kb = 0.2

and kω = 3, kb = 1, respectively. The initial errors are θ(to) = 10 deg and b(to) =

[−1 −1 −1]T deg /s and the gain kb satisfies condition (4.4). For the two simulations, the

following positive definite matrices P

Psim1 =

[I3 −0.1810I3

−0.1810I3 I3

]

,

Psim2 =

[I3 −0.1547I3

−0.1547I3 I3

]

,

are considered, which satisfy conditions (4.16) and (4.17).

Figure 4.4 and Figure 4.5, depict the time evolution of the attitude estimation error and

angular rate bias estimation error, respectively, showing the convergence of the error to the

origin. The time evolution of the Lyapunov function (4.9) is displayed in Figure 4.6, where

the logarithmic scale shows clearly its exponential decay. In Figure 4.7 the strictly negative

characteristic of the time derivative of the Lyapunov function is displayed. According to

(4.18), the theoretical conservative upper bound on the estimation error ‖η(t)‖ for the

first set of gains is given by

‖η(t)‖ ≤ 1.2008‖η(to)‖e−0.0812(t−to),

and for the second set of gains is given by

‖η(t)‖ ≤ 1.1687‖η(to)‖e−0.1557(t−to),

as depicted in Figure 4.8. As expected, the gains kω and kb can be used to tune the

observer convergence rate.

91


0 1 2 3 4 5 6 7 8 9 100

0.1

0.2

0.3

0.4

kω = 1 kb = 0.2kω = 3 kb = 1

time (s)

‖R−

I‖F

Figure 4.4: Attitude estimation error.

0 1 2 3 4 5 6 7 8 9 100

0.5

1

1.5

2

2.5

3

kω = 1 kb = 0.2kω = 3 kb = 1

time (s)

‖b‖(deg/s)

Figure 4.5: Angular velocity bias estimation error.

0 1 2 3 4 5 6 7 8 9 1010

−6

10−4

10−2

100

kω = 1 kb = 0.2kω = 3 kb = 1

time (s)

Figure 4.6: Lyapunov function VT = ηTPη.

4.6 Experimental apparatus

A real-time prototype was developed comprising an inertial unit and a range measure-

ment system, which was installed on a motion rate table, that provides ground truth data.

In this section, we describe the experimental setup and the experiment carried out to

validate the proposed technique.

92


0 1 2 3 4 5 6 7 8 9 10−0.4

−0.3

−0.2

−0.1

0

kω = 1 kb = 0.2kω = 3 kb = 1

time (s)

Figure 4.7: Time derivative of the Lyapunov function.

0 1 2 3 4 5 6 7 8 9 100

0.05

0.1

0.15

0.2

0.25

‖η(t)‖ kω = 1 kb = 0.2bound kω = 1 kb = 0.2‖η(t)‖ kω = 3 kb = 1bound kω = 3 kb = 1

time (s)

Figure 4.8: Time evolution of the estimation error ‖η(t)‖ and the theoretical upper bound.

4.6.1 Discrete-time implementation

An advantage of the continuous-time approach adopted in this work is the large region

of attraction of the desired equilibrium point. However, the nonlinear differential equations

that constitute the observer are not in a suitable form for computer implementation, hence

it is necessary to obtain a discrete time approximation of the original system. The key idea

governing the selection of the numerical integration method is the fact that the rotation

matrix belongs to the Lie group SO(3) and the intrinsic geometric properties of this group

have to be preserved by the numerical integration algorithms. This is not guaranteed by

the classical integration techniques like Runge-Kutta methods, since these do not preserve

polynomials invariants, like the determinant, with degree greater than three [HLW06, The-

orem IV-3.3]. Therefore, we have implemented the proposed attitude observer resorting to

the CG [CG93], which is a geometrical integration method and preserves the geometrical

properties of the rotation group. This method leads to a singularity-free integration and

does not require projection after the update because the integration already evolves on

the underlying manifold.

93


(a) (b)

Figure 4.9: Inertial unit MEMSense nIMU (a) and Cricket mote (b).

The sampling rate of the range measurements is 2 Hz, whereas the sampling rate of

the rate gyros is 150 Hz. Instead of discarding the angular rate measurements in excess,

we took advantage of these measurements to update the attitude estimate in open loop,

so that the observer output rate is 150 Hz. This strategy has increased relevance in fast

real time applications such as landing or aggressive maneuvers of unmanned air vehicles.

4.6.2 MEMSense nIMU

The angular rate measurements are acquired by a MEMSense nIMU (Figure 4.9(a))

that contains a triad of rate gyros, a triad of accelerometers, and a triad of magnetome-

ters. The data is received via an RS422 communication link. The rate gyros have a

dynamic range of ±150 deg/s, and typical and maximum noise of 0.36 deg/s (1σ) and

0.95 deg/s (1σ), respectively.

4.6.3 Cricket localization system

The range measurements are obtained using the Cricket localization system [PCB00],

which is composed of a set of Cricket motes (Figure 4.9(b)). Each mote can be set as a

beacon or as a receiver. In this experimental setup, we used four motes as beacons and

four as receivers. This system relies on the fact that speed of sound in air (about 343 m/s)

is much lower than the speed of the RF signal (speed of light). Each beacon emits an

ultrasonic pulse simultaneously with an RF signal containing the respective identification.

When the RF signal is detected by a receiver, it uses the time difference of arrival with

respect to the ultrasonic pulse to compute the propagation time and subsequently the

distance between the emitter and the receiver. For this experiment, the Cricket motes

firmware was slightly modified so that the beacons only emit a pulse when requested by

94


Beacon 2

Beacon 3Beacon 4

Beacon 1

Receiver 1Receiver 2

Receiver 4Receiver 3

Figure 4.10: Experimental setup.

an external source via the RS422 data link. With this configuration, the beacons can be

commanded to emit sequentially, avoiding the collisions usually observed with this system.

At each sampling cycle, all distances between the beacons and the receivers are computed,

neglecting the time difference between each beacon transmission, which is a relatively good

approximation, given the proximity of the receivers with respect to the beacons. The range

measurements have a resolution of 0.01 m. This system allows for a sampling frequency

of 2 Hz.

4.6.4 Motion rate table

Similarly to the experimental prototype developed in the previous chapter, the system

is installed on an Ideal Aerosmith Positioning and Rate Table Model 2103HT [IA06], see

Figure 4.10. This table provides precise tri-axial angular position, rate, and acceleration

measurements for development and testing of inertial components and systems. It has

±0.0083 deg of position accuracy and 0.01% ± 0.0005 deg/s of rate accuracy.

95


4.6.5 Calibration of the position of the Cricket motes

The calibration of the position of the motes is a crucial step to the success of the

experiment. The high accuracy of the motion rate table can be exploited to obtain these

positions. Using the table, the prototype is placed in N = 150 different orientations given

by the rotation matricesRi, and for each we acquire the distances between the beacons and

receivers, denoted as di,nb,nr , where i = 1, . . . , N identifies the orientation, nb = 1, . . . , 4

identifies the beacon, and nr = 1, . . . , 4 identifies the receiver. The approach followed

consists in minimizing a cost function that is the sum of the quadratic differences between

the distances measured by the Cricket motes and the theoretical distance in terms of the

beacons positions Lxnbin L, and receivers positions Lrnr in L. To that end, let the

cost function be defined as

J(di,nb,nr ,xnb, rnr) =

1

N

N∑

i=1

4∑

nb=1

4∑

nr=1

(di,nb,nr − ‖Lxnb−Ri

Lrnr‖)2.

Then, the minimization problem

minLx1,...,4

Lr1,...,4

J(di,nb,nr ,xnb, rnr)

is addressed by resorting to the quasi-Newton line search method. The value of the

functional J after the minimization procedure is 7.6861×10−4 m2. After the optimization

the following positions were obtained

Lx1 =

−0.333−0.722−1.251

m, Lx2 =

0.538−0.660−1.266

m, Lx3 =

−0.3670.432

−1.246

m, Lx4 =

0.5490.513

−1.242

m,

Lr1 =

−0.1780.010

−0.568

m, Lr2 =

0.187−0.016−0.570

m, Lr3 =

0.0070.191

−0.392

m, Lr4 =

−0.014−0.175−0.396

m.

Finally, a constant needs to be subtracted to these positions such that (4.1). The 3-D

positions of the beacons and receivers are depicted in Figure 4.11.

4.6.6 Observer gains

The measurement noise of the angular rate measurements is characterized by biased

Gaussian distribution with standard deviation given by 0.5443 deg/s, 0.4756 deg/s, and

0.5386 deg/s (1σ) for the x, y, and z axis, respectively. Regarding the range measurements

we consider a standard deviation of the measurements of 0.1 m. This value was slightly

inflated to accommodate effects like delays in the communications.

96


−1−0.500.51−1

0

1

−0.5

0

0.5

1

1.5

y (m)

3

1

L

3

y

1

4

z

2

x (m)

4x

2

z(m

)

beaconsreceivers

Figure 4.11: Cricket motes positions.

Based on the variance of the distance and angular rate measurements, the second mo-

ments of all channels of nv and nw are 0.2843 and 1.07×10−4, respectively. Consequently,

as discussed in Section 4.4, the matrices R and Q are given by

R =

0.0702 0 00 0.0702 00 0 0.0702

,

Q = 10−4

0.2614 0 00 0.2614 00 0 0.2614

,

and the Kalman gain matrix is given by

K =

0.0982 0 00 0.0982 00 0 0.0982

0.0193 0 00 0.0193 00 0 0.0193

.

From this matrix we extract the observer gains using the definition (4.23) kω = 0.0982,

kb = 0.0096.

This set of gains is suitable in terms of local performance, since in the neighborhood of

the origin the nonlinear observer can be approximated by its linearization and the gains

are designed so as to reduce the impact of the measurement noise on the estimation error.

97


0 50 100 150 200 250 300−20

0

20

MEKFground truth

0 50 100 150 200 250 300−20

0

20

0 50 100 150 200 250 300−200

−100

0

100

roll(deg)

pitch

(deg)

yaw

(deg)

time (s)

Figure 4.12: Attitude estimates provided by the MEKF and ground truth obtained withthe calibration table.

4.7 Experimental results

This section presents experimental results that validate the applicability of the pro-

posed solution, considering the reduced accuracy and low sampling frequency of the ul-

trasonic sensors. To guarantee that range measurements are available throughout the

experiment, the performed trajectory is selected to ensure proper detection of ultrasonic

pulses by the receiver transducers. Due to the high directionality of the cricket ultrasound

emitter [PCB00], the trajectory is characterized by small angular displacements in roll

and pitch. This restriction does not apply to the yaw angle, which can perform larger

excursions. The results of the proposed nonlinear observer (NL observer) are compared

with the common solution based on the MEKF [Mar03] using the same initial conditions

for both estimators.

Figure 4.12 shows the results provided by the MEKF. Figure 4.13 shows the time

evolution of the attitude estimates obtained using the proposed solution. A large error

on the initial estimates was purposely introduced to highlight the convergence rate of

the estimates. The time evolution of the estimation errors for both the MEKF and the

proposed attitude observer is shown in Figure 4.14. From this figure, we conclude that both

solution successfully estimate the attitude, however the MEKF exhibits more sensitivity

to outliers and large angular velocities. Finally, Figure 4.15 depicts the time evolution of

98

4.7 Experimental results

0 50 100 150 200 250 300 350−20

0

20

NL observerground truth

0 50 100 150 200 250 300 350−20

0

20

0 50 100 150 200 250 300 350−200

−100

0

100

roll(deg)

pitch

(deg)

yaw

(deg)

time (s)

Figure 4.13: Attitude estimates provided by the proposed observer and ground truthobtained with the calibration table.

0 50 100 150 200 250 300−20

0

20

NL observerMEKF

0 50 100 150 200 250 300−20

0

20

0 50 100 150 200 250 300−10

0

10

20

roll(deg)

pitch

(deg)

yaw

(deg)

time (s)

Figure 4.14: Comparison of the estimation errors brought about by the use of the MEKFand the proposed attitude observer.

the rate gyro bias estimates of the MEKF and the nonlinear observer. This figure shows

that the estimates converge to approximately constant values of bias. Some variations

99


0 50 100 150 200 250 300−2

0

2

NL observerMEKF

0 50 100 150 200 250 300−2

0

2

0 50 100 150 200 250 300−2

0

2

b x(deg/s)

b y(deg/s)

b z(deg/s)

time (s)

Figure 4.15: Angular rate bias estimates from the MEKF and from the proposed nonlinearobserver.

after the initial transient can be attributed to measurement errors, thermal fluctuations,

and small changes in the real value of the rate gyro bias. Even in these conditions, the

MEKF shows more oscillations on its estimates, which is undesirable.

Bearing in mind the reduced accuracy of the ultrasonic range measurements, the stan-

dard deviations obtained with the observer after the initial transient are 2.4856 deg for

roll, 1.5381 deg for pitch, and 0.6928 deg for yaw. By comparison, the standard deviations

obtained with the MEKF are 1.7569 deg, 2.6918 deg, and 1.2412 deg for roll, pitch and

yaw, respectively. From these results, we can conclude that the proposed nonlinear ob-

server has better accuracy than the MEKF in two of the Euler angles. In spite of this, the

main advantage of the nonlinear observer when compared with the MEKF is the very large

region of attraction. For our data set, with a considerable tuning effort, it was possible to

tune the MEKF so that stability is achieved even for large initial errors. However, different

operating conditions may compromise stability, which might not be acceptable for critical

scenarios. In contrast, the nonlinear observer is easily tuned and has a straightforward

implementation.

100

4.8 Chapter summary

4.8 Chapter summary

This chapter addressed the problem of attitude estimation using angular velocity and

range measurements. The range measurements are obtained between on-board receivers

and beacons fixed in the mission scenario. Ranging sensors are relatively low cost, have

low power consumption and can have, in general, compact dimensions. An almost globally

asymptotically stable nonlinear attitude observer defined on the SO(3) manifold that relies

on was devised. The presence of static bias in angular velocity measurements was consid-

ered and its value estimated by the observer. Exponential convergence of the estimates to

the desired equilibrium points was established. Taking advantage o Kalman-Bucy filtering

theory, a methodology to compute suitable observer gains was developed and applied. Fi-

nally, an experimental prototype validated the good performance of the proposed solution

resorting to a 3-axis motion rate table.

101

Chapter 5

Nonlinear observer for 3-D rigid

body motion using full state

feedback

5.1 Introduction

The previous two chapters have addressed the problem of attitude estimation using

a camera and an ultrasonic range system as aiding sensors. Determination of the rota-

tional motion is very important for aerial and maritime autonomous vehicles as well as

of spacecrafts. However, in many scenarios, the estimation of linear motion is of no less

importance. The accuracy of the position and attitude estimation system is then a key

factor in the operation of many modern vehicles. Due to its importance, this problem has

been the focus of intense research, which resulted in more accurate and efficient algorithms.

Nevertheless, there are still challenges and open problems, some of which are related to

the geometry of the state space. This chapter proposes a nonlinear observer for estimating

the configuration (position and attitude) and velocity states of a rigid body. The primarily

focus is on the theoretical development of the observer. Consequently, in contrast to the

previous two chapters, implementation issues are not explicitly addressed. The sensors

are considered to be ideal, providing configuration and velocity measurements, and later

in the chapter, the consequences of unmodeled forces and torques are analyzed. Since

most unmanned and manned vehicles can be modeled as rigid bodies, the applications

of this observer extend to both classes. When operated in uncertain or poorly known

environments, these vehicles can be subject to unknown forces and moments. Therefore,

103

5. Nonlinear observer for 3-D rigid body motion using full state feedback

robustness of the observer to unknown disturbances is essential for applications of vehicles

in scenarios such as space or underwater exploration. The dynamical coupling between the

rotational and translational dynamics, which occurs both due to the natural dynamics as

well as control forces and torques, is treated directly in the framework used in the observer

design.

This chapter proposes an observer design for pose and velocity estimation for 3-di-

mensional rigid body motion in the framework of geometric mechanics. Resorting to a

conveniently defined Lyapunov function, a nonlinear observer on the special Euclidean

group (SE(3)) is devised. This observer is based on the exponential coordinates, which

are used to represent the group of rigid body motions. Exponential convergence of the

estimation errors is shown and boundedness of the estimation error under bounded un-

modeled torques and forces is established. Since exponential coordinates can describe

uniquely almost the entire group of rigid body motions, the resulting observer design is

almost globally exponentially convergent. Numerical simulation results are presented to

illustrate the performance of this observer, both in the absence and with unmodeled forces

and torques. A preliminary version of this work can be found in [11].

The remainder of this chapter is organized as follows. The rigid body kinematics and

dynamics are presented in Section 5.2, and the homogeneous representation of the motion

equations in SE(3) are described. The nonlinear observer for the rigid body motion based

on configuration and velocities measurements as well as on modeled forces and torques is

proposed in Section 5.3. Its convergence and stability properties are analyzed in Section 5.4.

Robustness to unmodeled forces and torques is also shown. Simulations results presented

in Section 5.5 illustrate the performance of the proposed solution. A summary of the

results in the chapter is given in Section 5.6.


Consider a body-fixed coordinate frame with origin at the center of mass of a rigid body

denoted by B, and an inertially-fixed reference frame denoted by I. Let the rotation

matrix from B to the inertial fixed frame I be given by R and the coordinates of the

origin of B with respect to I be denoted by p. Taking advantage of the homogeneous

coordinates introduced in Chapter 2, denote the rigid body configuration by G, such that

G =

[R p0 1

]

∈ SE(3).

Recall the kinematic equation (2.10)

G = Gξ∨,

104

5.3 Observer synthesis

where ξ = [ωT vT ]T is the vector of body velocities and (.)∨ : R6 → se(3) can be written

as

ξ∨ =

[(ω)× v0 0

]

∈ se(3).

Using (2.11), the rigid body dynamics in homogeneous coordinates is given by

Iξ = ad∗(ξ)∨Iξ +ϕ,

where

I =

[J 03×3

03×3 mI3×3

]

,

with m ∈ R+ and J ∈ R

3×3 being the rigid body scalar mass and inertia matrix, respec-

tively. The vector of external inputs is ϕ = [τ T φT ]T , where φ denotes the force applied

to the rigid body and τ the external torque, both expressed in the body-fixed coordinate

frame, and ad∗(ξ)∨ = (ad(ξ)∨)T with ad denoting the adjoint action of se(3) on itself.

The sensor suite and models available provide information regarding the configuration,

velocities, forces and torques applied to the vehicle. The aim of this chapter is to design

a dynamic observer that exploits the sensors measurements to estimate the configuration

(pose) and the velocities, such that the estimated states converge to their true values in

the absence of measurement errors. The use of an observer has clear advantages over the

raw measurements as the sensor information is fused with the rigid body kinematics and

dynamics. The resulting estimates are less noisy and the errors due to sensor bias have

smaller magnitude than the raw sensor data. Robustness to bounded measurement errors

is obtained consequently, and is illustrated through numerical simulation results.


In this section, we propose an observer for the configuration and velocity. The config-

uration observer takes the form

˙G = Gξ∨, G =

[R p0 1

]

, (5.1)

where ξ = [ωT vT ]T ∈ R6 ≃ se(3). We define the configuration error as

G = G−1G =

[R −RT p0 1

]

∈ SE(3), (5.2)

where R = RTR, and p = p−p. The configuration error can be expressed in exponential

coordinates using

η∨ = logmSE(3)(G), (5.3)

105


where logmSE(3)(.) : SE(3) → se(3) denotes the logarithmic map on SE(3) (see Ap-

pendix A). The time derivative of the configuration error (5.2) is given by

˙G =d

dt(G−1)G + G−1G (5.4)

= −G−1 ˙GG−1G + G−1G= −G−1Gξ∨G−1G + G−1Gξ∨

= −ξ∨G + Gξ∨

= G(ξ∨ −AdG−1 ξ∨),

where the adjoint action of G ∈ SE(3) on ζ∨ ∈ se(3) (see (A.4) in Appendix A) satisfies

AdGζ∨ =

([R 0

(p)×R R

]

ζ

)∨.

In this work, we assume that full state measurement is provided by the sensor suite on

board the vehicle. Thus, we pose the following assumption.

Assumption 5.1. The sensor suite and models available provide data about the configu-

ration, velocity, forces and torques applied to the vehicle.

Note that, even with full state measurements, the existence of an observer is valuable

for any navigation and control system as, like the EKF, it can mitigate the effects of sensor

uncertainties such as noise and bias.

Let us now define the following quantities

ξ∨= AdG−1 ξ

∨, ξ = ξ − ξ, (5.5)

Using (5.5) to update (5.4), it yields

˙G = Gξ∨. (5.6)

We express the exponential coordinate vector η for the pose estimate error as

η =

[Θ

β

]

∈ R6 ≃ se(3),

where Θ ∈ R3 ≃ so(3) is the exponential coordinate vector (principal rotation vector)

for the attitude estimation error and β ∈ R3 is the exponential coordinate vector for

the position estimate error. The time derivative of the exponential coordinates of the

configuration error are obtained by resorting to (5.6) and [BM95] resulting in

˙η = G(η)ξ, (5.7)

106


where

G(η) =

[A(Θ) 0

T (Θ, β) A(Θ)

]

, (5.8)

where

A(Θ) = I+1

2(Θ)× +

(1

θ2− 1 + cos θ

2θ sin θ

)((Θ)×

)2,

S(Θ) = I+1− cos θ

θ2(Θ)× +

θ − sin θ

θ3((Θ)×

)2, and

T (Θ, β) =1

2(S(Θ)β)×A(Θ) +

(1

θ2− 1 + cos θ

2θ sin θ

)[Θβ

T

+ (ΘT

β)A(Θ)]

(5.9)

− (1 + cos θ)(θ − sin θ)

2θ sin2 θS(Θ)βΘ

T

+

((1 + cos θ)(θ + sin θ)

2θ3 sin2 θ− 2

θ4

)

ΘT

βΘΘT

,

and θ = ‖Θ‖. The exponential coordinate vector Θ for the rotational motion and its time

derivative are obtained from Rodrigues’ formula (equivalent to (2.4))

R(Θ) = I+sin θ

θ(Θ)× +

1− cos θ

θ2((Θ)×

)2,

which is a well-known expression for the rotation matrix in terms of the exponential

coordinates on SO(3), the Lie group of special orthogonal matrices. In the context of

(5.7)-(5.8), we have that R(Θ) = R.

The following lemmas are useful to establish the convergence and stability properties

of the nonlinear observer proposed later in this section.

Lemma 5.1. The matrix G(η), in the kinematic equations (5.7)-(5.8), satisfies the relation

G(η)η = η.

Proof. Beginning with the expression for G(η) given by (5.8), we evaluate

G(η)η =

[A(Θ)Θ

T (Θ, β)Θ+A(Θ)β

]

.

From the expression for A(Θ), it is clear that

A(Θ)Θ = Θ.

On the evaluation of the other component, after some algebra, we obtain

T (Θ, β)Θ = β −A(Θ)β.

Therefore, we have established that

T (Θ, β)Θ+A(Θ)β = β,

which gives the desired result.

107


Remark 5.1. The exponential coordinate Θ for SO(3) cannot be uniquely obtained when

θ = ‖Θ‖ = π rad, since Θ and −Θ correspond to the same rotation matrix according to

Rodrigues’ formula. In this case, the matrix G(η) also becomes singular.

Remark 5.2. In [BM95], an expansion for G(η) is given in terms of matrix powers of

ad(η)∨ , from which the above result can be easily concluded given that adχχ = 0 for any

χ ∈ se(3) ≃ R6.

Lemma 5.2. The matrix G(η) satisfies the following equality

K−1GT (η)K = GT (Kη), (5.10)

where K =[

I3 03×3

03×3 k2I3

]

, k2 > 0.

Proof. The left-hand side of (5.10) fulfils

K−1GT (η)K =

[I 03×3

03×3 k−12 I

] [AT (Θ) T T (Θ, β)

03×3 AT (Θ)

] [I 03×3

03×3 k2I

]

=

[AT (Θ) k2T

T (Θ, β)

03×3 AT (Θ)

]

.

On the other hand, we have

GT (Kη) =

[AT (Θ) T T (Θ, k2β)

03×3 AT (Θ)

]

.

Notice that, in (5.9), β multiply each term of T (Θ, β). Thus, T (Θ, β) is proportional to

the norm of β, and consequently we have k2T (Θ, β) = T (Θ, k2β).

Consider the following candidate Lyapunov function

V =1

2ηTKη +

1

2(k1η + ξ)TKI(k1η + ξ), (5.11)

where k1, k2 > 0, which motivates the development of the velocity observer. By defining

u = k1η + ξ and taking the time derivative, we obtain

V =ηTK ˙η + uTK(k1I ˙η + Iξ − I˙ξ)

=ηTKG(η)ξ + uTK(k1IG(η)ξ + ad∗(ξ)∨Iξ +ϕ− I˙ξ).

Using Lemma 5.2, the time derivative of the Lyapunov candidate function is written as

V =−k1ηTKη + uTK(GT (Kη)η + k1IG(η)ξ + ad∗(ξ)∨Iξ +ϕ− I˙ξ).

Then, resorting to the simple structure of K and some algebraic manipulations, by defining

I˙ξ = ad∗−(K(k1η−ξ))∨

Iξ +ϕ+ k1IG(η)ξ +GT (Kη)η + k3u, (5.12)

108


where k3 > 0, the time derivative of (5.11) takes the negative definite form

V = −k1ηTKη − k3(k1η + ξ)TK(k1η + ξ).

Thus, the point (η, ξ) = (0,0) is asymptotically stable in sense of Lyapunov [Kha02].

Topological limitations preclude global asymptotic stability of the origin [BB00]. In fact,

if θ = π rad, the exponential coordinates of the configuration error η cannot be computed

without ambiguity from the sensor data. The next result provides sufficient conditions

which ensure that for all t > to, θ(t) < π rad.

Lemma 5.3. For any initial condition satisfying

‖Θo‖2 + c1‖po‖2 + c2

(

σJ‖Θo‖+ k2µm‖po‖)

+ c3 < π2, (5.13)

where µ =√

1 + π2/2,

c1 =k2µ(1 + k21m)

1 + k21σJ, c2 =

2k1‖ξo‖1 + k21σJ

, c3 =ξT

o Iξo1 + k21σJ

,

and σJ is the maximum singular value of J, Θo = Θ(to), ‖po‖ = p(to), and ‖ξo‖ = ξ(to),

there is θ < π rad such that the exponential representation of the attitude error satisfies

‖Θ(t)‖ ≤ θ for all t > to. Moreover, there is a one-to-one mapping between the Lie

group SE(3) and its Lie algebra (exponential) representation along all the trajectories of

the system.

Proof. The exponential coordinates of the configuration error (see Appendix A) are related

to R and p by

Θ =θ

2 sin θ(R− RT ), cos θ =

tr(R)− 1

2, θ ∈ (0, π) rad,

β = S−1(Θ)p,

where tr(R) 6= −1, and

S−1(Θ) = I− 1

2(Θ)× +

2 sin θ − θ(1 + cos θ)

2θ2 sin θ((Θ)×)

2.

From the relation among matrix norms [GVL96], we have

‖S−1(Θ)p‖ ≤ ‖S−1(Θ)‖2‖p‖ ≤ ‖S−1(Θ)‖F ‖p‖,

where ‖.‖2 and ‖.‖F denote the Euclidean and Forbenius norms of matrices, respec-

tively. Through some algebraic manipulations one obtains ‖S−1(Θ)‖F ≤ µ, where µ =

109


√

1 + π2/2. Hence, ‖η‖2 ≤ ‖Θ‖2 + µ2‖p‖2. Consider the level set CV of the Lyapunov

function (5.11) defined as

CV =

x : V (x, t) <π2

2

(1 + k21σJ

)

,

where x = (η, ξ). The condition (5.13) guarantees that x(t = to) ∈ CV . Since CV

is a positive invariant set, we have that x(t) ∈ CV . To conclude the proof note that

x(t) ∈ CV ⇒ ‖Θ(t)‖ ≤ π rad for all t ≥ to.

Note that, for all initial conditions such that ‖Θo‖ = π − ǫ rad, ǫ > 0, the tuning

parameters of the proposed observer, k1 and k2, can always be selected such that (5.13) is

satisfied. To summarize, the observer design is given by equations (5.1) and (5.12) . This

observer directly estimates the configuration (pose) on SE(3) and the velocities on se(3)

using the exponential coordinates for the pose estimate error, given by η in (5.3).

5.4 Observer properties

This section evidences important characteristics of the observer. Namely, it is shown

that the observer is almost globally exponentially stable and that, when there is bounded

perturbations in the forces and torques measurements, the estimation errors are uniformly

ultimately bounded. Explicit convergence bounds for the estimation error are also pre-

sented.

Lemma 5.4. The Lyapunov function (5.11) and the corresponding time derivative satisfy

the following bounds

α1‖x(t)‖2 ≤V (x, t) ≤ α2‖x(t)‖2,V (x, t) ≤ −α3‖x(t)‖2,

where α1 = σmin(P), α2 = σmax(P), α3 = σmin(Q), with

P =1

2

[K(I6 + k21I) k1KI

k1KI KI

]

,

Q =

[(k1 + k21k3)K k1k3K

k1k3K k3K

]

,

where σmin(A) and σmax(A) denote the minimum and maximum singular values of a

generic matrix A, respectively. Furthermore, we have that

α1 ≥1

2min1, k2(−k1σI +min1 + k21σI, σI),

α2 ≤1

2max1, k2(k1σI +max1 + k21σI, σI),

α3 ≥ min1, k2(−k1k3 +mink1 + k21k3, k3),

110


where σI = σmin(I) and σI = σmax(I).

Proof. Let ρ = min1, k2 and ρ = max1, k2. To compute α1 start by noting that

V (x, t) = xTPx, P =1

2

[K(I6 + k21I) k1KI

k1KI KI

]

,

Following [TU11, Theorem 6], we have that

σmin

([A 00 C

])

− σmin(B) ≤ σmin

([A BB C

])

, (5.14)

for any square matrices A, B and C. Exploiting this result, we conclude that

2σmin(P) ≥ σmin

([K(I6 + k21I) 0

0 KI

])

− σmin(k1KI)

≥ k1ρ(−k1σI +min1 + k21σI, σI).

By resorting to some algebraic manipulations, it can be concluded that the Lyapunov

function (5.11) satisfies

V (x, t) =1

2ηTKη +

1

2(k1η + ξ)TKI(k1η + ξ)

=1

2ηTK(I6 + k21I)η + ηT (k1KI)ξ +

1

2ξT

KIξ

= xTPx ≤ σmax(P).

From [TU11, Theorem 6], we have that

σmax

([A BB C

])

≤ σmax

([A 00 C

])

+ σmax(B).

Thus,

σmax(P) ≤ 1

2‖x‖2ρ(k1σI +max1 + k21σI, σI).

Finally, α3 is obtained resorting to a reasoning similar to the one used to obtain α1.

Note that

−V (x, t) = xTQx, Q =

[(k1 + k21k3)K k1k3K

k1k3K k3K

]

.

Thus, −V (x, t) ≥ σmin(Q) and using (5.14), we conclude that

σmin(Q) ≥ σmin

([(k1 + k21k3)K 0

0 k3KI

])

− σmin(k1k3K)

≥ ρ(−k1k3 +mink1 + k21k3, k3).

111


5.4.1 Almost global exponential stability

The following theorem characterizes the stability of the estimation errors and provides

explicit convergence bounds.

Theorem 5.1. Under Assumption 5.1, let the configuration and velocities observer be

given by (5.1) and (5.12), and k1, k2, k3 > 0 be such that the conditions of Lemma 5.3 are

satisfied. Then, the estimation errors x = (η, ξ) are almost globally exponentially stable

with convergence rate upper bounded by

‖x(t)‖ ≤ κe−γ(t−to)‖x(to)‖,

where κ =√

α2/α1 and γ = α3/(2α2).

Proof. Lemma 5.4 shows that V satisfies

V ≤ −α3

α2V.

Thus, we conclude that

V (x, t) ≤ e−α3

α2(t−to)V (x(to), t).

Using once more Lemma 5.4, we have that

V (x(to), t) ≤ α2‖x(to)‖2, V (x, t) ≥ α1‖x(t)‖2.

Then,

α1‖x(t)‖2 ≤ e−α3

α2(t−to)α2‖x(to)‖2,

and consequently,

‖x(t)‖ ≤√

α2/α1e− α3

2α2(t−to)‖x(to)‖.

Note that the conditions of Lemma 5.3 ensure that unwinding does not occur. In other

words, the norm of the attitude estimate error expressed in exponential coordinates always

remains bounded above by π rad.

112


5.4.2 Unmodeled torques and forces

In real mission scenarios, the rigid body can be affected by external unmodeled distur-

bances.

Assumption 5.2. The forces and torques applied to the rigid body are composed of modeled

and unmodeled components, such that,

ϕ = ϕr +ϕd,

where ϕr ∈ R6 and ϕd ∈ R

6 denote the modeled and unmodeled torques and forces, respec-

tively. Moreover, ϕd(t) is uniformly bounded, i.e., there exists ϕd > 0 such that, for all

t > to, ‖ϕd(t)‖ ≤ ϕd.

In the presence of unmodeled forces and torques, Lemma 5.3 is no longer valid and an

additional condition is needed to guarantee that the exponential coordinates of G can be

computed uniquely.

Lemma 5.5. Under Assumption 5.2, let the velocities observer be given by

I˙ξ =ad∗−(K(k1η−ξ))∨

Iξ +ϕr + k1IG(η)ξ +GT (Kη)η + k3u. (5.18)

Then, if the condition (5.13) and

mink21 , k23/σ2I (1 + k21σJ) >ρ2

π2ρσIϕ2d

are satisfied, there is θ < π rad such that ‖Θ(t)‖ ≤ θ for all t ≥ to and there is a one-

to-one mapping between G ∈ SE(3) and its exponential representation along all the system

trajectories.

Proof. From Lemma 5.3, we have that in order to the attitude error satisfy ‖Θ(t)‖ ≤ θ,

where θ < π rad, and to exist a one-to-one mapping between G ∈ SE(3) and its exponential

representation for all t > to, the following condition is necessary

V (x(t)) <π2

2(1 + k21σJ). (5.19)

This inequality defines a level set with boundary given by

Ω = η, ξ ∈ R6 : ηTKη + (k1η + ξ)TKI(k1η + ξ) = π2(1 + k21σJ).

The goal is to provide sufficient conditions such that V (η, ξ) < 0 for any (η, ξ) ∈ Ω, which

guarantees that (5.19) holds.

113


The time derivative of the Lyapunov function, V , taking into account the existence of

unmodeled forces and torques, satisfies

V =−k1ηTKη − k3uTKu+ uTKϕd

≤−k1ηTKη − k3σI

uTKIu+ ‖u‖ρϕd

≤−mink1, k3/σI(ηTKη + uTKIu) +√

ηTKη + uTKIu√

ρ2/(ρσI)ϕd.

Evaluating V at Ω, it can be concluded that

V ((η, ξ) ∈ Ω) ≤ −mink1, k3/σIπ2(1 + k21σJ) +√

π2(1 + k21σJ)√

ρ2/(ρσI)ϕd. (5.20)

Then, from (5.20), we conclude that V ((η, ξ) ∈ Ω) < 0 holds for

mink21 , k23/σ2I (1 + k21σJ) >ρ2

π2ρσIϕ2d.

If there are unmodeled forces and torques, the estimation errors are no longer assimp-

totically stable. However, it can be shown that, for ‖ϕd‖ ≤ ϕd there is a sufficiently small

error on the initial estimates such that the estimation errors are uniformly ultimately

bounded with ultimate bound proportional to ϕd.

Proposition 5.1. Under the conditions of Lemma 5.5, the estimation errors are uniformly

ultimately bounded, with ultimate bound given by√

α2α1

α4α3ϕd, where α4 = 2maxk1, k2, k1k2.

Proof. Under Assumption 5.2 and with a velocities observer given by (5.18), we have that

V < −α3‖x‖2 + ‖x‖2ϕd maxk1, k2, k1k2≤ −‖x‖(α3‖x‖ − ϕdα4),

where α4 = 2maxk1, k2, k1k2. Thus, V < 0 for ‖x‖ > α4α3ϕd and the set

I =

x : V (x) <

(α4

α3ϕd

)2

is positive invariant. Finally, from the results in Lemma 5.4, we conclude that

‖x(t)‖ ∈ I ⇒ ‖x(t)‖ ≤√α2

α1

α4

α3ϕd. (5.21)

114

5.5 Simulations

5.5 Simulations

In this section, simulation results for the proposed nonlinear observer are presented.

It is assumed that the rigid body has an onboard sensor suite and models providing data

of configuration and velocities as well as aforces and torques. The performance of the

solution is studied for a typical trajectory, which is depicted in Figure 5.1. This trajectory

is generated by torques and forces with an oscillatory profile with frequency of 12πHz and

amplitude of τ = 0.1[1.5 −2 1]T rad/s2 ≈ [8.6 −11.5 5.7]Tdeg/s2 and φ = [−3 2 1]T m/s2.

The inertia of the rigid is given by

J =

1.1 0 00 1 00 0 0.9

kg.m2,

and m = 2 kg.

We performed simulations under two different conditions: i) perfect sensor measure-

ments and ii) with unmodeled torques and forces. The exponential convergence of the esti-

mation errors is studied first. The observer parameters are given by k1 = 1, k2 = 1, k3 = 4,

Θ(to) = [−0.4−0.2−0.1]T rad ≈ [−22.9−11.5−5.7]Tdeg, and the initial estimation errors are

set to β = [−1.073 −0.349 0.488]T m, ω(to) = 10−3[7 4 10]T rad/s ≈ [0.40 0.23 0.57] deg/s

and v(to) = 10−3[10 0 −5]T m/s. Note that these parameters satisfy the condition (5.13).

The configuration and velocity estimation results are depicted in Figure 5.2 in both

linear and logarithmic scales. The exponential upper bound obtained in Section 5.4.1 is

also shown.

The robustness properties of the observer are illustrated by considering the presence

of unmodeled torques and forces with an oscillatory profile with frequency 1 Hz and am-

plitude 0.01 rad/s2 (≈ 0.57 deg/s2) and 0.01 m/s2 in each axis of the torques and forces,

respectively. The same observer parameters as defined previously are considered. The

norm of the unmodeled torques and forces is depicted in Figure 5.3. The estimation error

is shown in logarithmic scale in Figure 5.4, which emphasizes that the estimation error

converges in finite time to the region defined by (5.21).

5.6 Chapter summary

In this chapter, a nonlinear observer for arbitrary rigid body motion with full state

measurements was devised. This observer was obtained and expressed in terms of the ex-

ponential coordinates on the group of rigid body motions in the 3-dimensional Euclidean

space. This observer was shown to be exponentially stable whenever the exponential coor-

dinates are defined, which includes all attitude estimate errors except those corresponding

115


−6

−4

−2

0

2

−10

12

30

0.5

1

1.5

2

2.5

t=0 s

t=5 s

t=10 s

t=15 s

t=20 s

x (m)y (m)

z(m

)

Figure 5.1: Rigid body trajectory.

to a principal rotation angle of 180 deg or π rad. Therefore, the convergence of esti-

mates given by this observer was almost global over the state space of rigid body motion.

Boundedness of the estimation error under bounded unmodeled torques and forces was es-

tablished. Numerical simulation results confirmed the convergence and stability properties

of this observer.

116

5.6 Chapter summary

0 2 4 6 8 10 12 14 16 18 200

1

2

3

4

5

6

estimation errorerror upper bound

0 2 4 6 8 10 12 14 16 18 2010

−30

10−20

10−10

100

1010

‖x‖

time (s)

‖x‖

Figure 5.2: Norm of the estimation error in linear and logarithmic scale.

0 2 4 6 8 10 12 14 16 18 200

0.01

0.02

0.03

0.04

norm of the unmodeled forces and torques

time (s)

‖ϕd‖

Figure 5.3: Norm of the unmodeled forces and torques, ϕd.

0 2 4 6 8 10 12 14 16 18 2010

−10

10−5

100

105

estimation errorultimate bound

time (s)

‖x‖

Figure 5.4: Norm of the estimation error in the presence of unmodeled forces and torques.

117

Chapter 6

Nonlinear observer for 3-D rigid

body motion using Doppler

measurements

6.1 Introduction

In the previous chapter, a nonlinear observer for rigid body motion was proposed

resorting to full state measurements. However, in many practical application scenarios,

full state measurements are not available. A particularly interesting case is when the

translational velocity information is given by a Doppler sensor, which provides range rate

with respect to the source of a receive signal. In contrast to linear velocity measurements,

which are, in general, difficult to obtain, the Doppler effect of received signals can in many

cases be measured very accurately. Acoustic and electromagnetic Doppler sensors can be

found for instance in underwater autonomous vehicles and in spacecrafts, respectively.

This chapter addresses the design of a nonlinear observer for pose and velocity esti-

mation for 3-dimensional motion of a rigid body equipped with a single direction Doppler

sensor. Using a conveniently defined Lyapunov function, a nonlinear observer on SE(3) is

proposed. The resulting observer design is almost globally exponentially convergent. For

rich enough trajectories, exponential convergence of the estimation errors to the origin is

established. Numerical simulation results are presented that illustrate the performance of

the proposed solution.

The proposed nonlinear observer can be used, for instance, in a mission scenario where

a mother-ship needs to monitor and track the position and attitude of a UAV or a remotely

119

6. Nonlinear observer for 3-D rigid body motion using Doppler measurements

operated underwater vehicle (ROV). An ultra-short baseline (USBL) system installed on-

board the ship can compute the UAV/ROV position and the Doppler shift, and using

an acoustic communication system (in the case of a UAV) or the umbilical cable connec-

tion (in the case of a ROV), the ship can receive the attitude and the angular velocity of

the underwater vehicle (from an attitude and heading reference system (AHRS) installed

onboard) as well as other information such as the pressure and status of the actuators.

Another scenario of interest is when the underwater vehicle needs to estimate its own po-

sition and attitude. In this case, the mother-ship transmits the position of the underwater

vehicle (in the inertial frame) measured using, for instance, the USBL, and the vehicle can

exploit the Doppler and AHRS measurements to estimate its configuration and velocity.

A preliminary version of this work can be found in [14].

The remainder of this chapter is organized as follows. Section 6.2 recalls the rigid body

equations of motion formulated explicitly on SE(3) and introduces the state (configuration

and velocity) estimation problem. In Section 6.3, the nonlinear observer that estimates

pose and velocities is proposed, and its convergence and stability properties are analyzed.

In Section 6.4, simulation results illustrating the performance of the proposed observer are

presented. Finally, a summary of main conclusions of this chapter is given in Section 6.5.

6.2 Problem formulation and measurement model

Similarly to the previous chapter, consider a body-fixed coordinate frame with origin

at the center of mass of a rigid body denoted by B and an inertially-fixed reference

frame denoted by I. Let the rotation matrix from B to I be given by R and the

coordinates of the origin of B with respect to I be denoted by p.

Let G be the rigid body configuration, such that

G =

[R p

01×3 1

]

∈ SE(3).

As seen in Section 2.5, using this representation, the kinematic equations take the form

G = Gξ∨,

where ξ = [ωT vT ]T is the vector of body velocities and (.)∨ : R6 → se(3) satisfies

ξ∨ =

[(ω)× v01×3 0

]

∈ se(3).

In this chapter, dynamics description (2.12) is adopted, which is distinct from (2.11)

adopted in Chapter 5. The main advantage of the dynamic equation (2.12), when com-

pared with (2.11), is that the term Ξ(I,ω) in the former does not depend on the linear

120

6.2 Problem formulation and measurement model

velocity, (that is not available in the scenario addressed in this chapter) whereas the term

ad∗ξ in the latter does. Recall that the dynamics in (2.12) is given by

Iξ = Ξ(I,ω)ξ +ϕ,

where ϕ = [τ T φT ]T , φ denotes the external force applied to the rigid body and τ the

external torque, both expressed in the body-fixed coordinate frame, and

I =

[J 03×3

03×3 mI3

]

,

Ξ(I,ω) =

[(Jω)× 03×3

03×3 −m(ω)×

]

,

and where m ∈ R+ and J ∈ R

3×3 denote the rigid body scalar mass and inertia matrix,

respectively.

A sensor based on the Doppler effect measures the frequency shift between an emitted

signal (electromagnetic or acoustic) and the same signal when is received. This difference

is proportional to the time-derivative of the distance between emitter and receiver and is

given by

∆f =ddt(‖pr − ps‖)

cfo, (6.1)

where fo denotes the emitted frequency, c is the propagation velocity of the signal, and

pr and ps denote the positions of the receiver and of the source, respectively. Thus, the

Doppler measurements does not provide full velocity information but rather only radial

velocity information. Without loss of generality, consider ps = 0 and consider a receiver

installed onboard the vehicle so that pr = p. Then, (6.1) can be rewritten as

∆f = αD

pT

‖p‖ p = αD

pT

‖p‖Rv,

where αD = foc . Since the p is known, one can compute the direction of the rigid body

with respect to the signal emitter, which is given by p(t)/‖p(t)‖. Thus, the projection

of the velocity vector on the direction from the origin of I to the origin of B can

be computed from the measurements. Without loss of generality, let the emitter and the

receiver be located at the origin of I and B, respectively. Then, the projected velocity

vector is given by

vD(t) = RTp(t)

‖p(t)‖∆f

αD

= d(t)dT (t)v(t),

where d(t) = RT p(t)‖p(t)‖ .

121


Finally, let us now introduce ξD, which comprises the angular velocity and the pro-

jected translational velocity

ξD =

[ω

vD

]

= D(t)ξ, D(t) =

[I3 03×3

03×3 d(t)dT (t)

]

. (6.2)

The aim of this chapter is to design a dynamic observer which uses configuration and

Doppler measurements, and modeled forces and torques to estimate configuration (pose)

and velocities.

6.3 Observer design with Doppler measurements

In this section, we propose an observer for the configuration and velocity. Let us denote

the configuration estimates by

G =

[R p

01×3 1

]

,

where R and p are the estimated attitude and position, respectively. The estimated

velocity is denoted by

ξ =

[ω

v

]

,

where ω and v are the angular and translation velocity estimates, respectively. The

configuration error is defined as

G = G−1G =

[

R −RT p01×3 1

]

∈ SE(3), (6.3)

where R = RTR and p = p− p, and the velocity estimation errors is defined as

ξ = ξ − ξ =

[ω

v

]

∈ R6,

where ω = ω−ω, and v = v−v. The configuration error can be expressed in exponential

coordinates using

η∨ = logmSE(3)(G),

where logmSE(3)(.) : SE(3) → se(3) denotes the logarithmic map on SE(3) (see Ap-

pendix A). We express the exponential coordinate vector of the pose estimate error η

as

η =

[Θ

β

]

∈ R6 ≃ se(3),

where Θ ∈ R3 ≃ so(3) is the exponential coordinate vector (principal rotation vector)

for the attitude estimation error and β ∈ R3 is the exponential coordinate vector for the

position estimate error.

122


Similarly to Chapter 5, the time derivative of the configuration error (6.3) is given by

˙G =d

dt(G−1)G + G−1G

= −G−1 ˙GG−1G + G−1G= −G−1 ˙GG + G−1Gξ∨

= G(

ξ∨ −AdG−1G−1 ˙G)

,

where the adjoin action Ad on SE(3) defined in (A.3) satisfies

AdGζ∨ =

([R 0

(p)×R R

]

ζ

)∨, G =

[R p

01×3 1

]

∈ SE(3), ζ∨ ∈ se(3),

and the velocity error dynamics satisfies

I˙ξ = I

˙ξ − Iξ

= I˙ξ −Ξ(I,ω)ξ −ϕ.

The time derivative of the exponential representation of G is given by [BL04]

˙η = G(η)(

ξ − (AdG−1 G−1 ˙G)∧)

,

where (.)∧ is such that ((a)∨)∧ = a, a ∈ R6 and G(η) is given by (5.8).

The following theorem establishes the stability and convergence of the proposed ob-

server.

Theorem 6.1. Let the configuration and velocity observer be described by

˙G = G(

AdG(ξ + k1η)∨)

, (6.4)

I˙ξ = Ξ(I,ω)ξ +ϕ+

1

k3GT (Kη)η +

k4k3

(ξD −D(t)ξ), (6.5)

where K =[

I3 03×3

03×3 k2I3

]

, and where k1, k2, k3, k4 > 0 are such that the initial conditions

satisfy the inequality

‖Θo‖2 + k2µ‖po‖2 + k3ξT

oKIξo < π2, (6.6)

where Θo = Θ(to), po = p(to), ξo = ξ(to). Then, there exists θ < π rad such that for all

t > to, the attitude error satisfies ‖Θ(t)‖ < θ and the estimation error x(t) = [ηT ξT

]T is

almost globally asymptotically stable.

Proof. As in the observers designed in the previous chapters, topological limitations pre-

clude global asymptotic stability of the origin [BB00]. In fact, if θ = π rad, the exponen-

tial coordinates of the configuration error η cannot be computed without ambiguity. This

123


proof is divided in two parts: i) using a judiciously selected Lyapunov function establish

that (6.6) is a sufficient condition to ensure that for all t > to, θ(t) < π rad and thus, the

critical points where η is not well defined do not belong to the error system trajectories,

and ii) show that all system trajectories converge to the origin. To that end, consider the

Lyapunov function candidate

V =1

2ηTKη +

k32ξT

KIξ.

Taking its time derivative, one obtains

V =ηTG(η)(ξ − (AdG−1 G−1 ˙G)∧) + k3ξK(

I˙ξ −Ξ(I,ω)ξ −ϕ

)

.

By replacing˙G and I

˙ξ by (6.4) and (6.5), respectively, one gets

V =ηTG(η)(ξ − (AdG−1 G−1GAdG(ξ + k1η)∨)∧) + k3ξK

(

Ξ(I,ω)ξ +ϕ+1

k3GT (Kη)η

+k4k3

(ξD −D(t)ξ)−Ξ(I,ω)ξ −ϕ)

=ηTG(η)(−ξ − k1η) + k3ξK(

Ξ(I,ω)ξ +1

k3GT (Kη)η +

k4k3

(ξD −D(t)ξ))

.

From (6.2), we have ξD = D(t)ξ and by noticing that Ξ(I,ω)ξ is perpendicular to ξ, the

time derivative of V satisfies

V = ηTG(η)(−ξ − k1η) + ξK(GT (Kη)η + k4D(t)ξ).

Finally, using the results in Lemma 5.1 and Lemma 5.2 to update the time derivative of

the V , one obtains

V = −k1ηTKη − k4ξT

KD(t)ξ,

which is negative semi-definite, since D(t) is positive semi-definite.

Recall that, the exponential coordinates (see Appendix A) of the configuration error

are related to R and p by

Θ =θ

2 sin θ(R− RT ), cos θ =

tr(R)− 1

2, θ ∈ (0, π) rad,

β = S−1(Θ)p,

where (.)⊗ is the inverse of the skew operator (.)×, tr(.) denotes the trace operator, tr(R) 6=−1, and

S−1(Θ) = I3 −1

2(Θ)× +

2 sin θ − θ(1 + cos θ)

2θ2 sin θ((Θ)×)

2.

124


From the relation among matrix norms [GVL96], one concludes

‖S−1(Θ)p‖ ≤ ‖S−1(Θ)‖2‖p‖ ≤ ‖S−1(Θ)‖F ‖p‖,

where ‖.‖2 and ‖.‖F denote the Euclidean and Frobenius norms of matrices, respec-

tively. Through some algebraic manipulations, one obtains ‖S−1(Θ)‖F ≤ µ, where

µ =√

1 + π2/2. Hence,

‖η‖2 ≤ ‖Θ‖2 + µ2‖p‖2. (6.7)

Consider the level set described by

CV =

x : V (x(t), t) ≤ π2

2

.

Then, using (6.7), one can infer that the condition (6.6) guarantees that x(to) ∈ CV . As

CV is a positive invariant set, one concludes that, for all t ≥ to, x(t) ∈ CV , which in

turn yields x(t) ∈ CV ⇒ ‖Θ(t)‖ ≤ π rad. Thus, we have concluded the first part of the

proof by establishing that condition (6.6) guarantees that the configuration error η can

be retrieved from the sensor measurements and current estimates.

To show that all system trajectories converge to the origin notice that V is negative

semi-definite. On the other hand, by assumption, the velocity is bounded, and conse-

quently, V is finite. Then, according to Barbalat’s Lemma [Kha02], V → 0, from which

one concludes that ‖η‖ → 0. Since the configuration coordinates are bounded, this result

implies that ddt‖η‖ → 0. Thus, ‖ξ‖ → 0 and consequently ‖x‖ → 0.

In case of a rich enough trajectory, a stronger result is obtained. To that end, consider

the following definition.

Definition 6.1 (Persistence of excitation - see [Sas99]). A vector z(.) is said to be persis-

tently exciting (PE) if there exist T > 0, u1 ≥ 0, and u2 ≥ 0 such that

u1I ≤∫ t+T

tz(τ)zT (τ)dτ ≤ u2I, ∀t ≥ to.

This definition states that a trajectory is rich if its position wobbles around enough

in all time intervals of duration T . In fact, if the position of the rigid body is PE, the

following result can be established.

Theorem 6.2. Let (6.4) and (6.5) describe the configuration and velocities observer, let

k1, k2, k3, k4 > 0, be such that (6.6) holds. Moreover, assume that the position of the rigid

body expressed in B is PE. Then, the estimation error x(t) converges exponentially fast

to the origin.

125


Proof. Without loss of generality let to = 0. Under condition (6.6), the configuration

error η can be retrieved from the sensor measurements and the current estimates. The

Lyapunov function V fulfills

γ1‖x(t)‖2 ≤ V (xD, t) ≤ γ2‖x(t)‖2, (6.8)

where γ1 = min1, k2,mk2k3, k3σJ, γ2 = max1, k2,mk2k3, σJk3, and σJ and σJ denote

the maximum and minimum singular values of J, respectively. According to the premises

of the theorem, the position of the rigid body expressed in the body fixed frame B is

PE. Thus, by Definition 6.1, there exist T ≥ 0 and u > 0 such that

∫ t+T

tD(τ)dτ ≥ uI, ∀t ≥ 0.

Hence, the following inequality holds

∫ t+T

tV (x, τ)dτ ≤ −γ3‖x(t)‖2, ∀t ≥ 0, (6.9)

where γ3 = mink1, k1k2, uk3, uk2k3. Then, we have

V (t+ T ) = V (t) +

∫ t+T

tV (x, τ)dτ. (6.10)

Using (6.8) and (6.9), we obtain

V (t+ T ) ≤ V (t)− γ3‖x(t)‖2 ≤ V (t)

(

1− γ3γ2

)

.

Let 1 − γ3γ2

= λ and t = NT + tr, where N ∈ N0 and tr ∈ [0, T ). Notice that, λ < 1,

since by definition γ2 and γ3 are positive real numbers. Moreover, λ > 0, as shown in the

following. By definition, if ‖x(0)‖ 6= 0, V (t+ T ) > 0 for a finite t. Thus, from (6.10), we

have

0 < V (t) +

∫ t+T

tV (x, τ)dτ.

From (6.8) and (6.9), we have

0 < γ2‖x(t)‖2 − γ3‖x(t)‖,

for all 0 < t <∞. Then, γ2 > γ3 and consequently λ > 0.

Since V (t) is a time decreasing function

V (t) ≤ V (NT ) ≤ V ((N − 1)T )λ

≤ V ((N − 2)T )λ2 ≤ V (0)λN

≤ V (0)λt/T ,

126

6.4 Simulations

12

34

5−2

−1

0

1−2

−1.5

−1

−0.5

0

0.5

1

1.5

t=30 st=25 s

t=15 s

t=20 s

t=10 s

t=0 s

t=5 s

t=35 s

x (m)

y (m)

z(m

)

Figure 6.1: Rigid body trajectory.

and from (6.8), we get

γ1‖x(t)‖2 ≤ γ2‖x(0)‖2λt/T .

After some algebraic manipulations, we finally obtain

‖x(t)‖ ≤√γ2γ1e−(t/2T ) log(1/λ)‖x(0)‖.

6.4 Simulations

In this section, simulation results that illustrate the robustness and convergence prop-

erties of the proposed solution are presented. The sensor suite onboard the rigid body

provides configuration and Doppler measurements. Moreover, it is considered that forces

and torques are accurately modeled. The rigid body performs a typical trajectory, illus-

trated in Figure 6.1. The inertia of the rigid body is set to

J =

1.1 0 00 1 00 0 0.9

kg.m2,

and its mass is set to m = 2 kg. The simulation parameters are set to k1 = 1, k2 =

1, k3 = 1, k4 = 4, Θ(to) = [−0.4 −0.2 −0.1]T rad ≈ [−22.9−11.5−5.7]T deg, β =

127


[−1.073 −0.349 0.488]T m, ω(to) = 10−3[7 4 10]T rad/s ≈ [0.40 0.23 0.57] deg/s, v(to) =

10−3[10 0 −5]T m/s. Note that, with these parameters, the condition (6.6) is satisfied.

Figure 6.2 depicts the estimation error of the observer which exploits Doppler velocity

measurements. This figure illustrates the convergence of the estimation error. In the lower

plot, a logarithmic scale is adopted to highlight the exponentially fast convergence of the

estimation error to the origin. Since the proposed observer is exponentially stable, the

estimation error in presence of disturbances in the torques and forces is ultimately bounded

[Kha02, Theorem 4.16 and Theorem 4.18]. This robustness property is also illustrated in

Figure 6.2, where the disturbances in the torques and forces are modeled as random

variables with uniform distribution and amplitude of 0.01 Nm and 0.1 N in each axis,

respectively. Additionally to the disturbances, in Figure 6.2 we also present simulation

results with also noise in all the sensor measurements. The noise characteristics are given

by std(Θ) = 0.01 rad ≈ 0.57 deg, std(p) = 0.01 m, std(ω) = 0.01 rad/s ≈ 0.57 deg/s

and std(dTv) = 0.01 m/s, where std(.) denotes the standard deviation of each axis of the

sensor.

0 5 10 15 20 25 30 350

0.5

1

1.5

0 5 10 15 20 25 30 35

1e−4

1e−3

1e−2

1e−1

1e−0

without disturbanceswith disturbanceswith disturbances and noise

‖x‖

time (s)

‖x‖

Figure 6.2: Norm of the estimation error of the observer under nominal conditions and inthe presence of disturbances.

6.5 Chapter summary

In this chapter, a nonlinear observer for arbitrary rigid body motion was devised.

An observer taking advantage of Doppler data was designed and expressed in terms of

the exponential coordinates on the group of rigid body motions in the 3-dimensional

128

6.5 Chapter summary

Euclidean space. The estimation error was shown to be asymptotically stable whenever

the exponential coordinates are defined, which includes all attitude estimate errors except

for those corresponding to a principal rotation angle of 180 deg or π rad. Therefore, the

convergence of the estimates was shown to be almost global over the state space of rigid

body motions. Given persistence of excitation of the position of the rigid body, almost

global exponential stability of the observer is guaranteed. Numerical simulation results

confirmed the convergence and stability properties of the observer.

129

Chapter 7

Global attitude and gyro bias

estimation based on set-valued

observers

7.1 Introduction

The previous chapters addressed the problem of position and attitude estimation re-

sorting to nonlinear observers. Those solutions bear important properties such as very

large basin of attraction, fast convergence rates and robustness to bounded disturbances.

However, in the design of nonlinear observers there is no systematic methodology to ac-

count for uncertainties (bias and noise) in all sensors. This chapter revisits the problem of

attitude and rate gyro bias estimation resorting to a different approach based on set-valued

observers (SVOs). This method belong to a broader class of set-membership estimation

techniques which are based on the assumption that the sensor noise is bounded. Some

advantages of using set-membership estimation methods are i) a set of feasible states con-

sistent with the dynamic model, measurements and bounded uncertainty characterization

is obtained, ii) no assumption is made on the noise and uncertainties statistical character-

istics other than they have a known bound, which may be the only available information,

iii) it is suitable to be integrated in the design of robust control solutions where worst-case

guarantees are provided regarding the properties of the closed-loop system. This chap-

ter addresses the challenges associated with the application of the set-based methodology

to the attitude estimation problem, such as, the underlying compact manifold where the

attitude is defined.

131

7. Global attitude and gyro bias estimation based on set-valued observers

It is assumed that the available sensor suite comprises biased rate gyros and vector ob-

servations, such as, magnetometers, star-trackers, Sun and horizon sensors, and accelerom-

eters. An SVO is proposed that has no singularities and that, for any initial conditions,

provides a bounding set with guarantees of containing the actual (unknown) rotation ma-

trix. The sensor readings are assumed to be corrupted by bounded measurement noise

and constant gyro bias, and are fused directly by the SVO without intermediate attitude

computation. Sufficient conditions for boundedness of the estimated sets for the cases

of multi- and singlevector observations are established and implementation details are

discussed. The feasibility of the technique is demonstrated in simulation. Preliminary

versions of these results can be found in [5, 8, 12].

The remainder of this chapter is organized as follows. In Section 7.2, the attitude

estimation problem is introduced, the available sensor information is described. A dis-

cretization of the system dynamics distinct from the one proposed in the previous chapter

is also proposed and the associated errors are studied. Section 7.3 introduces the SVOs

formalism. The proposed attitude estimation method is devised in Section 7.4 and several

implementation issues are discussed in Section 7.5. In Section 7.6, the performance of the

proposed solution is illustrated in simulation for a typical trajectory. Finally, Section 7.7

presents a summary of the main results in the chapter.


In this section, the attitude estimation problem using vector observations and biased an-

gular velocity measurements is introduced. The vector observations provide instantaneous

information regarding attitude, while the angular velocity characterizes its time-evolution.

The objective of the present work is to estimate the smallest set that contains the

attitude of a rigid body and the rate gyro bias using the available sensor suite, i.e., to

obtain the set-valued attitude and bias estimates with the smallest possible uncertainty.

7.2.1 System description

The rotation matrix is adopted as attitude representation for it does not suffer from

the problems associated with the many alternatives, namely, unwinding phenomena and

singularities [MLS94, BB00]. Recall the rotation matrix kinematics introduced previously

in Chapter 2

R = R(ω)×. (7.1)

132


where R ∈ SO(3) denotes the rotation matrix from the inertial reference frame I to

the body-fixed reference frame B and ω ∈ R3 denotes the angular velocity of B with

respect to I measured in B.A triad of rate gyros, fixed in reference frame B, measures ωr(k) ∈ R

3, which denotes

the angular velocity measured at instant k corrupted by the bias term b ∈ R3 and bounded

noise n(k) ∈ R3 so that

ωr(k) = ω(k) + b+ n(k), ‖n(k)‖∞ ≤ n. (7.2)

Similarly to Chapter 3 and Chapter 4, the unknown rate gyro bias is modeled by a constant

vector, i.e.,

b = 0. (7.3)

7.2.2 Measurements

Onboard sensors such as magnetometers, star trackers, among others, provide vector

measurements expressed in body frame coordinates, i.e.,

RBvi =Ivi, (7.4)

where i = 1, . . . , Nv, and Nv is the number of vector measurements; or, in the matrix form,

(7.4) becomes

RBV = IV, (7.5)

where BV = [Bv1 . . .BvNv ] and

IV = [Iv1 . . .IvNv ]. Notice that, if the linear acceleration

can be considered negligible when compared with gravity, the accelerometers measure-

ments can also be suitable to be used as vector observations. In addition, in many sce-

narios, the rigid body acceleration can be modelled as a disturbance with a known upper

bound and this information included in the vector observation measurement model.

It is assumed that the sensor measurements are corrupted by noise contained inside

compact polytopes. Thus, the measurements Bvi ∈ R3, i = 1, . . . , Nv belong to the convex

polytope defined by some real matrix Mvi ∈ Rm×3 and some vector mvi ∈ R

m, i.e.,

Bvi ∈ Set(Mvi , mvi), (7.6)

where Set(M,m) is the short-form notation that denotes the set x ∈ Rn : Mx ≤ m,

where the vector inequalities are taken element-wise. The measurements are thus provided

by means of a set, rather than as a singleton.

133


−1 0 1 2 3 4 5 6 7 8 9 10−1

0

1

2

3

4

5

6

7

8

9

10

x

yy = 6

y + 2x = 7 −y + x = 2−y + x = 2

S

Figure 7.1: Example of a polytope.

As illustrative example, consider the polytope in Figure 7.1. The boundaries of this

polytope are defined by

y = 6,

−y + x = 2,

y + 2x = 7.

Hence, using the notation introduced previously, the set S can be expressed by

S = Set(M,m) = M

[xy

]

≤ m,

where

M =

0 11 −1−2 −1

, m =

62−7

.

7.3 Set-valued observers

This section presents a methodology for estimating the set of states that evolve accord-

ing to a given dynamic system and are compatible with state measurements affected by

bounded noise.

The seminal work in [Sch68] discusses the state estimation problem for systems with

bounded inputs, while in [BR71, MV91] a similar problem, but using a set-membership de-

scription for model uncertainty, is addressed. Recent advances in the framework of these

134


estimators, referred to as SVOs [Aub01], are presented in [ST99, RSSA09, RSSA10b].

Later, this work was extended and the SVOs used in different problems [LZA03, RSSA09].

The results presented in this chapter stem from the set-valued observers, or SVOs, intro-

duced in [ST97, ST99, RSSA09].

7.3.1 SVO formulation

In the sequel, the framework of SVOs is presented for the case with no inputs and

uncertain dynamic matrix and sensor measurements. The interested reader is referred

to [MV91, ST97, ST99, RSSA09, RSSA10b] for further details on SVOs. Consider the

following time-varying uncertain discrete linear system

x(k + 1) = Ao(k)x(k) +A∆(k)x(k)

y(k) = C(k)x(k) +D(k)n(k), (7.7)

where x(k) ∈ Rm is the state of the system, y(k) ∈ R

l is the measurement vector,

Ao ∈ Rm×m is the nominal system dynamic matrix, A∆ ∈ R

m×m is a perturbation

matrix, C ∈ Rl×m is the measurement matrix, D(k) is a l × l real matrix, and n(k) =

[n1(k) . . . nl(k))]T is the sensor noise , which is assumed bounded, i.e., |ni(k)| ≤ ni,

i = 1, . . . , l. Furthermore, define n = [n1 . . . nl]T , xo = x(to), xo ∈ X(to), and

X(to) := Set(M(to),m(to)). In many practical situations, we have that D(k) = I. In

this case, the states of the system at instant k compatible with the measurements satisfy

[C(k)−C(k)

]

x(k) ≤[y(k) + n−y(k) + n

]

,

that is x(k) ∈ Set(M(k), m(k)) where

M(k) =

[C(k)−C(k)

]

, m(k) =

[y(k) + n−y(k) + n

]

.

If D(k) is not the identity matrix, a set of states compatible with the measurements can

still be obtained resorting to the projection of an augmented set. Using (7.7), the following

inequality can be obtained

C(k) D(k)−C(k) −D(k)

0 I0 −I

︸︷︷︸

Λ(k)

[x(k)n(k)

]

≤

y(k)y(k)n(k)n(k)

︸︷︷︸

γ(k)

.

Then, M(k) and m(k) such that M(k)x(k) ≤ m(k) can be computed by resorting to the

Fourier-Motzkin elimination method [KG87], which can be used to eliminates variables

135


from a system of linear inequalities, i.e.,

(M(k), m(k)) = FM(Λ(k),γ(k), 9),

where for any x ∈ Rnx , x ∈ Set(A,b), (A, b) := FM(A,b, n) stands for the Fourier-

Motzkin projection operator, where n = nx − nx > 0, and A and b satisfy, for all x ∈ Rn,

Ax ≤ b ⇔ ∃x∈Rn : A[xT xT ]T ≤ b.

The objective of the SVOs is to find the smallest set X(k + 1) containing the state

x(k + 1), based on (7.7) and on the knowledge that x(k) ∈ X(k), where X(k) :=

Set(M(k),m(k)). Without loss of generality, let

A∆(k) =N∑

i=1

Ai(k)∆i(k), i = 1, . . . , N, (7.8)

where |∆i(k)| ≤ 1 are unknown variables and define ∆(k) = [∆1(k) . . . ∆N (k)]T . The

matrices Ai(k) ∈ Rm×m encode the uncertainties in the system dynamic matrix. A par-

ticular case occurs when all the elements have uncorrelated uncertainties. Under such

circumstances, the matrices Ai have only one non-zero element whose value denotes the

magnitude of the uncertainty in the corresponding element of A. Since ∆i(k) ranges from

−1 to 1, the actual element of A(k) can be less or greater than the value of the nominal

Ao(k). If ∆(k) was exactly known, one could compute A(k) and there was no uncertainty

in the system model. Since this is not the case, it is only possible to guarantee that A(k)

is contained in a polytope whose boundaries are −Ai(k) and Ai(k), i = 1, . . . , N .

Due to the presence of uncertainty in the system dynamics, which is reflected in the

parameter∆, the set of feasible states at time k+1 is, in general, non-convex. Nevertheless,

it will be shown next that, by considering specific realizations of (7.8) and using an SVO to

obtain the polytope that contains the state for each particular realization, a set containing

the state x(k+1) can be computed. As such, consider a particular realization of (7.7)-(7.8)

with ∆i(k) = ∆∗i , |∆∗

i | ≤ 1, i = 1, . . . , N and denote the corresponding uncertainty matrix

by A∆∗ , i.e., A∆∗ = A1∗∆1∗ + · · ·+AN∗∆N∗ . For each realization, the method described

in [ASS09] can be used to design an SVO that provides a set-valued estimate of the state

of the system. Indeed, if the matrix Ao(k)+A∆∗(k) is invertible, the following inequality

can be written

M(k)(Ao(k) +A∆∗)−1

︸︷︷︸

M∗(k+1)

x(k + 1) ≤ m(k)︸︷︷︸

m∗(k+1)

. (7.9)

In other words, for ∆i(k) = ∆i∗ , i = 1, . . . , N , the state satisfies x(k+1) ∈ Set(M∗(k+

1),m∗(k + 1)).

136


Let γi, i = 1, . . . , 2N , denote the coordinates of each vertex of the hyper-cube

H := δ ∈ RN : |δ| ≤ 1, (7.10)

where γi = γj ⇔ i = j. Then, denote by Xγi(k+1) the set of points x(k+1) that satisfies

(7.9) where A∆∗ = Aγi and with x(k) ∈ Set(M(k),m(k)). Notice, that Xγi(k+1) can be

obtained by resorting to (7.9). Further define

X(k + 1) := co

Xγ1(k + 1), . . . , Xγ2N

(k + 1)

, (7.11)

where cop1, . . . , pm is the smallest convex set containing the points p1, . . . , pm, also

known as convex hull of p1, . . . , pm. Since X(k+1) is, in general, non-convex even if X(k)

is convex, we are going to use X(k + 1) to overbound the set X(k + 1). Since the set of

all possible feasible states at time k + 1 is, in general, non-convex, X(k + 1) will be used

to overbound it. Moreover, as X(k+1) is the convex hull of a finite number of polytopes,

it can be written in the form Set(M(k + 1), m(k + 1)).

X(k)

Ao(k) +Aγ1(k)

Ao(k) +Aγ2(k)

Ao(k) +Aγ3(k)

Xγ2

Xγ1

Xγ3

X(k + 1)

Figure 7.2: Convex hull of the different set propagations.

The process just described is illustrated in Figure 7.2. The set X(k) is propagated

using the different dynamic matrices that are obtained with the extreme disturbances.

The convex hull of all propagated sets is then guaranteed to contain the actual state at

time k + 1,i.e., x(k + 1).

Then, by intersecting this set with the points compatible with the measurements, it

can be concluded that x(k + 1) ∈ X(k + 1), where

X(k + 1) = Set(M(k + 1),m(k + 1)),

and

M(k + 1) =

[M(k + 1)

M(k + 1)

]

, m(k + 1) =

[m(k + 1)m(k + 1)

]

.

137


X(k)X(k + 1) X(k + 1)

x(k) x(k + 1)

Ao(k) +A∆(k)X(k + 1)

Figure 7.3: Illustration of an SVO iteration.

Figure 7.3 illustrates the overall operations performed by the SVO in one iteration. Using

the system dynamics, the set containing the state at instant k, X(k), is propagated to

obtain a set compatible with the system dynamics at time k + 1, i.e, X(k + 1). The set

X(k + 1) is then intersected with the set of states compatible with the measurements,

X(k + 1), to obtain X(k + 1), which will be propagated in the following iteration.

Remark 7.1. If Ao(k)+A∆∗(k) is singular or ill-conditioned, one can no longer obtain a

set X∗(k+1) from the construction in (7.9). However, a similar strategy used to address the

case where D(k) is not the identity matrix can be adopted. Indeed, consider the following

inequality

[I −Ao(k)−A∆∗ (k)−I Ao(k)+A∆∗(k)0 M(k)

]

︸︷︷︸

Λ(k)

[x(k+1)x(k)

]

≤[ 0

0m(k)

]

︸︷︷︸

γ(k)

. (7.12)

Then resorting to the Fourier-Motzkin elimination method [KG87] is used to project the set

described in (7.12) and compute M∗(k+1) and m∗(k+1) such that M∗(k+1)x(k+1) ≤m∗(k + 1), i.e.,

(M∗(k + 1),m∗(k + 1)) = FM(Λ(k),γ(k), 9).

Remark 7.2. Either using (7.9) or the Fourier-Motzkin algorithm in (7.12), the sizes of

M(k) and m(k) may be increasing with time. To overcome this issue, one can eliminate

the linearly dependent constraints. This can be accomplished by solving several small linear

programming problems at each sampling time, which, however, increases the complexity of

the practical implementation of this type of observers.

138

7.4 Attitude and rate gyro bias estimation using SVOs

7.3.2 Observability

Let us define Ψo(k + n, k) = Ao(k + n− 1)Ao(k + n− 2) . . .Ao(k), C(k) = IVT ⊗ I3,

and

W(k, k +N) =

C(k)C(k + 1)Ao(k)

C(k + 2)Ψo(k + 2, k)...

C(k +N)Ψo(k +N, k)

.

In [ST97, Proposition 3.1 and 3.2], it is shown that, if the following observability-like

assumption holds, the SVO described in Section 7.3.1 is non-divergent.

Assumption 7.1. Matrices A(k) and C(k) are uniformly bounded and there exist con-

stants µ1, µ2 > 0 and N ∈ N such that for all k

µ1I ≤ WT (k, k +N − 1)W(k, k +N − 1) ≤ µ2I, (7.13)

where

W(k, k +N) =

C(k)C(k + 1)Ao(k)

C(k + 2)Ψo(k + 2, k)...

C(k +N)Ψo(k +N, k)

.

and Ψo(k + n, k) = Ao(k + n− 1)Ao(k + n− 2) . . .Ao(k).


In this section, a methodology is proposed for the attitude estimation problem with

bounded sensor noise and biased angular velocity measurements.

7.4.1 Discretization of the system

The solution of continuous-time attitude kinematics (7.1) can be expressed as

R(t) = R(to)Φ(t, to), t > to,

where Φ(t, to) is the transition matrix [Rug96] from to to t. However, this solution is not

suitable to be implemented on a digital system. Consequently, we are rather interested

in computing R and ω at discrete time instants kT , k ∈ N, where T ∈ R+ denotes the

sampling period. For the sake of clarity, the time dependence of the variables will be

139


simply denoted by k, rather than kT . Then, the solution of the differential equation (7.1)

at k + 1 is given by

R(k + 1) = R(k)Φ(k + 1, k), (7.14)

where Φ(k + 1, k) ≡ Φ(T (k + 1), Tk).

The state transition matrix can be computed by the Peano-Baker series. However, this

requires the sum of an infinite series and the knowledge of the angular velocity between

sampling times. Thus, an alternative approach is herein pursued. For the discretization

errors to remain bounded, the angular velocity requires bounded rate of change.

Assumption 7.2. The magnitude of the angular acceleration is bounded by a known (but

possibly conservative) positive scalar α, i.e.,

‖ω‖∞ ≤ α, α ∈ R+.

For many practical purposes, this assumption is not restrictive since the angular ac-

celeration of any rigid body is limited by the external torque provided by the actuators

which cannot be unbounded. In the following lemma, the discretization of Φ(k+1, k) and

the associated errors are characterized.

Lemma 7.1. Under Assumption 7.2, the state transition matrix Φ(k + 1, k) satisfies

Φ(k + 1, k) = Φo(k + 1, k) +Φ∆(k + 1, k),

where Φo(k + 1, k) = expmSO(3) (T (ω(k))×) and ‖Φ∆(k + 1, k)‖max ≤ eT2α/2 − 1.

Proof. Let ω(t) = ω(to) + ω∆(t) and, as in [Coo99], define

ϕ(t, to) = expmSO(3)(−t(ωo)×)Φ(t, to) expmSO(3)(to(ωo)×), ωo = ω(to),

which fulfils

ϕ(t, to) = expmSO(3)(−t(ωo)×)(ω∆(t))× expmSO(3)(t(ωo)×)ϕ(t, to).

Then, a solution of ϕ(t, to) can be obtained by resorting to the Peano-Baker series [Rug96]:

ϕ(t, to) =

∞∑

i=0

ϕi(t, to), ϕo(t, to) = I3,

ϕi(t, to) =

∫ t

to

expmSO(3)(−τ(ωo)×)(ω∆(τ))× expmSO(3)(τ(ωo)×)ϕi−1(τ, to)dτ.

140


Define Φi(t, to) = expmSO(3)(t(ωo)×)ϕi(t, to) expmSO(3)(−to(ωo)×). Then, the state tran-

sition matrix satisfies

Φ(t, to) = Φo(t, to) +Φ∆(t, to),

where

Φo(t, to) = expmSO(3)((t− to)(ωo)×), Φ∆(t, to) =∞∑

i=1

Φi(t, to),

Φi(t, to) =

∫ t

to

expmSO(3) ((t− τ)(ωo)×)ω∆(τ)Φi−1(τ, to)dτ.

Algebraic manipulations allow us to obtain the relation

‖Φi(t, to)‖2 ≤∫ t

to

α(τ − to)‖Φi−1(τ, to)‖2dτ

≤ αi (t− to)2i

2ii!.

Thus, ‖Φ∆(t, to)‖2 ≤ eα(t−to)2/2 − 1. To conclude the proof let to = kT , t = (k + 1)T and

recall the relation among matrix norms ‖A‖max ≤ ‖A‖2.

Since the bias is assumed to be constant, the discretization of (7.3) is simply given by

b(k + 1) = b(k).

7.4.2 Set of states compatible with the measurements

Let us define the state vector and the measurement vector, respectively, as

x = [xT

R bT ]T , y = [yT

v bT ]T ,

where xR = vec(RT ), yv = vec(BV), and vec(.) denotes the operator which stacks the

columns of a matrix with m columns and n rows into a mn × 1 vector, from left to

right. By applying the property vec(AXB) = vec(BT ⊗A) vec(X) to (7.5), the complete

measurement model is obtained

y(k) = C(k)x(k), C(k) =

[IVT (k)⊗ I3 03

03 I3

]

. (7.15)

From (7.6), one directly concludes that yv ∈ Set(Mv, mv), where Mv =

diag(Mv1 , . . . ,MvNb) and mv = [mT

v1 . . . mTvNb

]T .

A set containing the value of the rate gyro bias, b, is obtained from the sensors resorting

to (7.2), from which we have that b satisfies

b = −ωr(k)− n(k)− ω(k), ‖n(k)‖∞ ≤ n. (7.16)

141


The corrupted angular velocity measurements, ωr(k), is given by the rate gyros. The

absolute value of the noise is known to be upper bounded by n. The computation of a set

containing ω(k) can be more involved. However, this can be accomplished by inverting

the attitude kinematics (7.1), which yields

ω(k) = (logmSO(3)(RT (k)R(k + 1)))⊗,

where logmSO(3)(.) : SO(3) → so(3) can be computed using (A.2)

logmSO(3)(R) =

03 if θ = 0θ

2 sin(θ)(R−RT ) if θ ∈ (0, π),

where R ∈ SO(3) and θ = arccos ((tr(R)− 1)/2) ∈ [0, π) rad. The actual values of R(k)

and R(k + 1) are not available, since only the sets which contain them are known. A set

containing R(k) is known directly from the state vector as vec(RT (k)) = xR, while a set

containing R(k + 1) is obtained from the vector measurements, since

R(k + 1) = (BV(k + 1)IV†(k + 1))T ,

where IV(k + 1) is known and vec(BV(k + 1)) ∈ Set(Mv(k + 1), mv(k + 1)). The set

containing R−RT , where R = RT (k)R(k+1) is computed using the Minkowski difference

of two polytopes [AF90]. The set containing ω(k) is then computed resorting to (A.2)

together with interval analysis computations, which are applied to the set containing R1.

Since the set containing ω(k) is a polytope, there exist Mω and mω such that

ω(k) ∈ Set(Mω,mω).

Notice that from (7.16), we have that

I I I−I −I −I0 Mω 00 0 I0 0 −I

bω(k)n(k)

≤

ωr(k)ωr(k)mω(k)

nn

.

Then, by resorting to the Fourier-Motzkin projection operator [KG87], the set containing

the value of the bias, i.e.,

b ∈ Set(Mb, mb),

can be obtained. Alternatively, the Minkowski addition [AF90] can be also used.

1As the quotient θsin(θ)

is a monotonically increasing function in the domain θ ∈ [0, π) rad, the corre-sponding bounds can be easily computed by evaluating its value at the extremes of the interval of θ.

142


Definition 7.1. Given IV and ωr, x is said to be compatible with the set of observations,

S = Set([MTv MT

b ]T , [mT

v mT

b ]T ), if there exists y ∈ S such that (7.2) and (7.5) are satisfied.

Remark 7.3. Definition 7.1 states that a given rotation matrix, R, and rate gyro bias,

b, are compatible with the (uncertain) measurements, if the set of measurements contains

the inertial vector observations, IV, rotated by RT , and the angular velocity measurements

contains the angular velocity plus b.

The next lemma shows the relation among the output of the system and the state, i.e.,

the rate gyro bias and the time-varying rotation matrix.

Lemma 7.2. Assume that the measurement vector y(k), at each time k satisfies y(k) ∈Set([MT

v MT

b ]T , [mT

v mT

b ]T ). Then, R(k) and b are compatible with the measurements

if and only if x ∈ Set(M(k), m(k)), where M(k) =

[Mv(k)(

IV ⊗ I3)T

Mb(k)

]

and m(k) =[mv(k)mb(k)

]

.

Proof. By resorting to the property vec(AXB) = vec(BT ⊗ A) vec(X), it is concluded

that yv = (IV ⊗ I3)TxR. Then, we have that Mv(k)yv(k) ≤ mv(k) is equivalent to

Mv(k)(IV ⊗ I3)

TxR ≤ mv(k). Thus, M(k)x ≤ m(k).

The goal of this chapter is to devise an estimator that reduces the volume of the set

containing the current state of the system described by (7.1)-(7.3).

7.4.3 SVO design

Let X(k) be a polytope containing x at time k and denote B(k) as the polytope

obtained by projecting X(k) into the rate gyro bias coordinates. Define bo(k) as the center

of B(k) and δb(k) as a measure of the uncertainty on the bias, so that δb(k) = b− bo(k).

The so-called Chebyshev center of a polyhedron is adopted, which is defined as the point

inside the polyhedron that is furthest from its limits. This center can be obtained by

solving a linear programming (LP) problem (see [BV04, Section 8.5.1]).

Consider the vec(.) operator applied to the traspose of (7.14). This leads to

vec(RT (k + 1)) = vec(ΦT (k + 1, k)RT (k)).

Using once more the property vec(AXB) = vec(BT ⊗A) vec(X), we obtain

vec(RT (k + 1)) = (I3 ⊗ΦT (k + 1, k)) vec(RT (k),

which is using to

xR(k + 1) = (I3 ⊗ΦT (k + 1, k))xR(k).

143


Then, the system dynamics in (7.14) can be rewritten as

xR(k + 1) = A(k)xR(k),

where A(k) = I3 ⊗ΦT (k + 1, k). Due to sensor uncertainty, the transition matrix Φ(k +

1, k), and consequently A(k), cannot be determined exactly. However, by resorting to

Lemma 7.1, A(k) can be decomposed into the sum of a known component, Ao(k), and

unknown components, Ai(k), i = 1, . . . , 9, which encode the uncertainty in each element

of A(k). Thus, A(k) satisfies

A(k) = Ao(k) +

9∑

i=1

Ai(k)∆i(k), ,

for some |∆i(k)| ≤ 1, where

Ao(k) = I3 ⊗ΦT (k + 1, k)

= I3 ⊗ expmSO(3)(T (−ωr(k) + bo(k))×),

Ai(k) = I3 ⊗ ǫEmn,

m,n = 1, 2, 3, i = 3(n−1)+m, Emn denotes the matrix whose elements are zero except

the element of row m and column n, which is one, and

ǫ =1

2

(e2Tǫ1 − e2Tǫ2

)+ e

T2α2 − 1,

ǫ1 = ωo + bδ + n,

ǫ2 = ωo,

where ωo = ‖ωr − bo‖∞, bδ = ‖bδ‖∞. The value of ǫ is obtained by exploiting the results

in Lemma 7.1 and in Appendix C.

The full state dynamics is then described by

x(k + 1) = A(k)x(k), (7.17)

where

A(k) = Ao(k) +

9∑

i=1

Ai(k)∆i(k), |∆i(k)| ≤ 1, (7.18)

and

Ao(k) =

[Ao(k) 0303 I3

]

, Ai(k) =

[Ai(k) 0303 03

]

.

Assumption 7.1 is required for the existence of non-divergent set valued observer. Note

that, this assumption is always satisfied if span(Iv1, . . . ,IvNv) = R

3.

As the system (7.15)-(7.17)-(7.18) enjoys the same properties of (7.7), an SVO can be

designed. The following theorem describes the proposed attitude SVO.

144


Theorem 7.1. Assume that there exist matrix M(k) and vector m(k), such that

x(k) ∈ Set(M(k),m(k)) ∩ vec(SO(3)) × R3,

where Set(M(k),m(k)) is a compact set. Then, under Assumptions 7.1 and 7.2, the set

Set(M(k+1),m(k+1))∩vec(SO(3))×R3, as defined previously, is compact and contains

all the states x(k+1) (i.e., matrices R(k+1) and rate gyro bias b) that satisfy (7.1)-(7.2)

and that are compatible with the observations at time k + 1.

Proof. Define X(k+1) as the set of all possible states of the system at time k+1. Under

Assumption 7.2, one can compute the matrices Ai(k) in (7.18). In addition, (7.9) defines

the set of states at time k+1 that satisfy (7.18) and are compatible with the measurements.

By evaluating (7.9) for ∆ = [∆1 . . . ∆9] in the vertices of H, defined in (7.10), one obtains

Xγ1(k+ 1), . . . , Xγ29(k+1). Uniform boundedness of the sets Xγ1(k+1), . . . , Xγ29

(k+ 1)

is ensured by the observability-like condition in Assumption 7.1. It is shown in [RSSA10b]

that, since ∆(k) can be obtained by convex combinations of the vertices of H, the state

of the system, x(k + 1), is inside the set generated by the convex hull in (7.11), which

is compact since it is the convex hull of compact sets. The polytope X(k + 1) is the

intersection of the X(k + 1) with the set of points compatible with the measurements.

Thus, X(k + 1), is an overbound of X(k + 1) and an overbound of the space of possible

solutions of R(k+1) and b, and is given by Set(M(k+1),m(k+1))∩vec(SO(3))×R3.

Remark 7.4. As C(k) fulfils (7.13), the estimates of the SVO proposed in this section

converge to a bounded set and the observer is global for it converges for any initial condi-

tions.

7.4.4 Attitude estimation with exact angular velocity measurements

For some particular cases, the proposed SVO exhibits interesting properties. The case

with exact angular velocity measurements is relevant since, in these ideal circumstances,

the SVO is optimal, in the sense that no conservatism is added to the solution, i.e., the

volume of set-valued estimate is minimal. This result is highlighted in Corollary 7.1.

Corollary 7.1. Under the conditions of Theorem 7.1 and assuming that there is a triad

of rate gyros that measure the rigid body angular velocity without uncertainty, i.e., n = 0

and bδ = 0, the estimator (without the rate gyro bias component), at each time k + 1,

provides the smallest set compatible with (7.1) and the vector observations.

Proof. Under the conditions of Corollary 7.1, we have that,

xR(k + 1) = Ao(k)xR(k), (7.19)

145


where Ao(k) is fully known. Let us now define the set X(k+1) = x ∈ R9 : M(k+1)x ≤

m(k + 1) ∩ vec(SO(3)), where

M(k + 1) =

[M(k)A−1

o (k)

MR(k + 1)

]

, m(k + 1) =

[m(k)

mR(k + 1)

]

.

By Theorem 7.1, the set X(k + 1) contains all the solutions of (7.1) at k + 1 that are

compatible with the vector observation.

To show that the smallest set is obtained, we also need to demonstrate that all the

states xR(k + 1) ∈ X(k + 1) are compatible with (7.1) and with

xR(k) ∈ Set(M(k),m(k)) ∩ vec(SO(3)).

The state at time k + 1 satisfies

M(k)A−1o (k)xR(k + 1) ≤ m(k),

and, by using (7.19), which, under the conditions of the corollary, is equivalent to (7.1),

in the previous equation, we recover

M(k)xR(k) ≤ m(k).

Finally, since for any matrix A ∈ SO(3), AB ∈ SO(3) if and only if B ∈ SO(3), we have

that, no conservatism is added by considering that xR(k) ∈ R9 and xR(k+1) ∈ R

9 instead

of R(k) ∈ SO(3) and R(k + 1) ∈ SO(3), respectively.

7.4.5 Attitude estimation using a single vector observation

Assumption 7.1 clearly holds if C(k) is invertible, that is, if span(Iv1, . . . ,IvNv) = R

3.

However, it is also possible to satisfy this assumption for the problem of attitude estimation

without rate gyro bias if only a single time-varying vector observation is available. For that

purpose, let Iv(k) be the only available vector observation and redefine the observation

matrix such that

C(k) = IvT (k)⊗ I3,

and

W(k, k +N) =

C(k)C(k + 1)Ao(k)

C(k + 2)Ψ(k + 2, k)...

C(k +N)Ψ(k +N, k)

. (7.20)

146

7.5 Implementation issues

Proposition 7.1. Assume that only a single time-varying vector observation, Iv(·), is

available. Then, there exist constants µ1, µ2 > 0 and N ∈ N such that, for all k, we have

µ1I ≤ WT (k, k +N − 1)W(k, k +N − 1) ≤ µ2I, (7.21)

if and only if the following holds

span(Iv(k), . . . , Iv(k +N)) = R3. (7.22)

Proof. Since for any k, Ao(k) is a block diagonal matrix of rotation matrices it is uni-

formly bounded. For condition (7.21) to hold, it is necessary and sufficient for WT (k, k +

N)W(k, k+N) be full rank. From the properties of the Kronecker product, we have that

Ψ(k + n, k) = Ao(k + n− 1) . . .Ao(k) = I3 ⊗ R(k, n), where

R(k, n) = expmSO(3)(T (ωr(k + n− 1))×) . . . expmSO(3)(T (ωr(k))×),

and W(k, k +N), as described in (7.20), satisfies

WT (k, k +N)W(k, k +N) =

= ([Iv(k) . . . Iv(k +N)]⊗ I3) ([Iv(k) . . . Iv(k +N)]⊗ I3)

T

On the other hand, for any matrices P and Q, we have rank(P⊗Q) = rank(P) rank(Q).

Hence, WT (k, k +N)W(k, k +N) is full rank and (7.21) is satisfied if and only if (7.22)

holds.

Then, according to [ST97, Proposition 3.1 and 3.2] the SVO designed with only one

vector observation, with no gyro bias and satisfying (7.22) is non-divergent.

7.5 Implementation issues

In this section, several details on the implementation of the proposed solution are

discussed, namely, memory consumption, computational complexity, and strategies to

increase performance.

Either using the inversion of matrices in (7.9) or using the Fourier-Motzkin algorithm in

(7.12), the sizes of M(k) and m(k) may be increasing with time, which can be problematic

from the implementation point-of-view. To reduce number of rows of M(k) and the length

of m(k), one should eliminate the linearly dependent constraints. An algorithm to iden-

tify redundant constraints can be found in [CMP89]. This method consists in optimizing

the left-hand side of each constraint subject to the remaining constraints. The resulting

optimal value is then compared with the right-hand of the tested constraint to decide

147


whether the constraint is redundant or not. For alternative methods, see [TL89, VS01]

and references therein. Even if the linearly dependent constraints are removed, the num-

ber of constraints may grow, in worst-case scenario, linearly with the number of iterations.

The number of constraints can be limited by enclosing the estimated polytope within a

polyhedron with fixed number of vertices. This introduces conservatism in the set-valued

estimates. However, note that this operation does not need to be executed on every it-

eration. Therefore, to reduce the computational burden, one can decide to carry out

the enclosing every fixed number of iterations, or when the number of linear indepen-

dent constraints exceeds a pre-specified limit. This strategy guarantees that the memory

consumption remains within reasonable values.

The computational complexity and the accuracy of the estimates depend on the algo-

rithms that perform the operations over the set-valued estimates, namely, the LP problems,

the polyhedron enclosure and the convex hull. There is a trade-off between accuracy and

complexity, and the best selection of algorithms depends on the available computational

resources and whether the target application requires real-time estimates.

The main shortcoming of this approach is its high computational cost when compared

with other estimation methods such as the EKF. This cost is due to the several small LP

problems that need to be solved. On the other hand, this approach provides robust uncer-

tainty bounds that take explicitly into account linearization errors and sensor noise. Such

bounds are suitable, for instance, in robust control designs, where worst-case guarantees

are provided regarding the performance of the closed-loop system. In addition, the SVOs

take explicitly into account the bounds of external disturbances and errors due to model

discretization.

Since the rate gyro bias can be seen as a parameter of the rotation kinematics, if

enough computational resources are available, one can increase the convergence rate of

the estimator by using a divide-and-conquer strategy. The uncertainty set of the rate gyro

bias can be divided and each sub-set used to initialize a different SVO. Since the size of

each uncertainty is smaller, so is the uncertainty in the kinematic model. In addition,

only one SVO contains the true value of the bias. Thus, as time goes by, all SVOs

eventually degenerate into empty sets for the estimates. At that point, we divide again

the uncertainty space of the bias. This approach has the additional advantage of being

easily parallelizable.

148



This section presents simulation results illustrating the performance of the proposed

solution. The results of the SVO estimator are compared with the MEKF [Mar03]. Re-

call that, the MEKF is a modified EKF with the advantage of preserving the geometric

properties of the underlying attitude representation.

The simulated trajectory is characterized by angular velocity with the following os-

cillatory profile ωx(k) = 4.01 sin(2π0.05kT ) deg /s, ωy(k) = −2.86 sin(2π0.04kT ) deg /s,

ωz(k) = 3.44 sin(2π0.02kT ) deg /s, ω(k) = [ωx(k) ωy(k) ωz(k)]T . The sampling period, T ,

and the maximum rate gyros noise ‖n‖∞ = n, are set to 0.1 s, and 0.115 deg/s, respec-

tively. The initial uncertainty on the rate gyro bias is ±5.73 deg/s in each channel, while

the true rate gyro bias is b = [0.03 −0.01 0.02]T deg/s. These simulation parameters are

selected so that they are compatible with the accuracy and trajectory of a typical UAV

used in research applications, such as the AscTec Pelican described in Section 3.8. The

directions for the vector observations in the inertial frame are given by

Iv1 = [1 4 0]T , Iv2 = [3 0 0]T , Iv3 = [0 0 6]T ,

and each channel of measurements is corrupted by noise bounded by ±0.01. The MEKF

is initialized with the true initial attitude and state covariance matrix Po = 6 × 10−7I6.

The system covariance matrix is set to Q = 14σ

2ωI6, and the measurements covariance

matrix is set to R = σ2vI9, where σ2ω and σ2v are the variance of each axis of the angular

velocity measurements and of the vector measurements, respectively. Figure 7.4 depicts

the trajectory performed by the rigid body expressed in exponential coordinates [MLS94],

which are computed using η = (logmSO(3)(R))⊗.

The main goal of the SVO is to provide a set-valued estimate for the state. Nevertheless,

one can obtain a single estimate by considering, for instance, the center of the estimated

set. The estimation errors of the central point of the set given by the SVO and the

MEKF are shown in Figure 7.5. The estimates are expressed in exponential coordinates,

computed by resorting to (A.2). Both strategies have similar accuracies, in terms of the

RMS value of the errors of the SVO and MEKF estimates (Table 7.1).

Table 7.1: RMS of the estimation errors of the SVO and of the MEKF.

xRMS (deg) yRMS (deg) zRMS (deg)

SVO 0.0414 0.0318 0.0494MEKF 0.0346 0.0350 0.0456

Figure 7.6 depicts the maximum uncertainty of the set-valued estimate as well as the

3σ bound of the MEKF state (based on the state covariance matrix). Most of the time,

149


0 2 4 6 8 10 12 14 16 18 20

−50

0

50

time (s)

η (d

eg)

ηx

ηy

ηz

Figure 7.4: Attitude trajectory of the rigid body.

0 2 4 6 8 10 12 14 16 18 20−0.2

−0.1

0

0.1

0.2

η x (de

g)

central SVO attitude error MEKF attitude error

0 2 4 6 8 10 12 14 16 18 20−0.2

−0.1

0

0.1

0.2

η y (de

g)

0 2 4 6 8 10 12 14 16 18 20−0.2

−0.1

0

0.1

0.2

η z (de

g)

time (s)

Figure 7.5: Error of the central point of the rotation vector set.

particularly for the x and y axes, the bounds provided by the SVO are smaller, i.e., less

conservative than the 3σ bounds of the MEKF. More exactly, the bounds given by the

SVO are smaller 87% of the time in the x-axis , 61% of the time in the y-axis, and 35%

of the time in the z-axis. The estimates and the state covariance matrix of the MEKF

are expressed in quaternions [Mar03]. Thus, a transformation to exponential coordinates

is necessary. The bias estimates provided by the SVO converge faster than those of the

MEKF, as shown in Figure 7.7.

When compared with the MEKF, the proposed technique presents some advantages: it

is global, it has faster convergence rates (at least in the simulations performed), it is robust

against different sensor noise characteristics as no other information but worst-case bounds

on sensor noise and external disturbances is required, and, finally, it provides a set-valued

state estimate that is guaranteed to contain the true attitude. The computational demands

150

7.7 Chapter summary

0 2 4 6 8 10 12 14 16 18 200

0.2

0.4

0.6

η x (de

g)

estimates uncertainty SVO estimates uncertainty MEKF

0 2 4 6 8 10 12 14 16 18 200

0.1

0.2

0.3

0.4

η y (de

g)

0 2 4 6 8 10 12 14 16 18 200

0.2

0.4

0.6

η z (de

g)

time (s)

Figure 7.6: Uncertainty limits along the main axes.

Figure 7.7: Error of rate gyro bias estimates.

of the operations over the set-valued estimates can, however, hinder its implementation

in real-time systems with low computational power. The MEKF is less computational

expensive. However, it may lead to degraded performance and, ultimately, to divergent

solutions, due to the approximation of the dynamics of the system by a linear model.

7.7 Chapter summary

In this chapter, a solution to the problem of attitude and rate gyro bias estimation was

proposed based on vector observations and angular velocity measurements corrupted by

bounded noise. An SVO that considers uncertainties defined by polytopes was developed.

For any initial conditions, the actual state of the system is guaranteed to be inside the

set-valued estimate. Sufficient conditions for boundedness of the estimated set in the case

151


of single vector observation are established. The obtained solution is conservative due to

the uncertainty in the measurements provided by the rate gyros. However, no linearization

is required and the attitude of the rigid body is parametrized by a rotation matrix yielding

estimates that are free of singularities. Simulation results illustrate the performance of

the proposed technique.

Comparing this solution with the ones proposed in the previous chapters, the following

considerations can be made. This set-based approach has the advantage of providing a sys-

tematic methodology for considering bounded sensor noise in all sensors. The convergence

is global whereas, in the previous chapters, it was almost global, and the SVO does not

have gains that require tuning. On the other hand, the solution proposed is this chapter

requires more computational resources than the nonlinear observers previously designed.

Moreover, the assumption of boundedness of sensor noise may be too restrictive in some

cases.

152

Chapter 8

Fault detection and isolation in

inertial measurement units based

on bounding sets

8.1 Introduction

The objective of this chapter is distinct from the ones of the previous chapters, since is

dedicated to the detection and isolation of faults in inertial measurement units (IMUs) and

it does not involve the direct computation of the position or the attitude of a rigid body.

However, this topic is not of minor importance for position and attitude estimation systems,

as IMUs provide key measurements in many navigation systems. The fault detection

problem consists in noticing that a sensor measurement is not compatible with the expected

model, while isolating the fault involves the identification of its typology and location on

the overall system.

Two strategies are proposed for the problem of FDI in navigation systems equipped

with sensors providing inertial measurements and vector observations: i) the first one takes

advantage of existing hardware redundancy; ii) the second approach exploits the analyt-

ical redundancy between the angular velocity measurements and the vector observations

by resorting to SVOs. Necessary and sufficient conditions on the magnitude of the faults

are established, in order to guarantee successful detection and isolation when hardware

redundancy is available. Due to the set-based construction of the methods, none of the

solutions generate false detections and no decision threshold is required. Using a simula-

tion scenario, the proposed strategies are compared with two alternatives available in the

153

8. Fault detection and isolation in IMUs based on bounding sets

literature. Preliminary versions of these results can be found in [7, 13].

The remainder of this chapter is organized as follows. In Section 8.2, the problem of

FDI in IMU measurements and vector observations is introduced. In Section 8.3, the FDI

method that exploits sensor redundancy is described. The FDI filter that takes advantage

of the dynamic relation among the sensor measurements using the SVOs is developed in

Section 8.4. In Section 8.5, simulation results are presented illustrating the performance of

the proposed strategies when compared with alternatives present in the literature. Finally,

a summary of the main results in this chapter is given in Section 8.6.


Consider that a craft is equipped with a strapdown navigation system comprising

an IMU fixed in the body reference frame B. Vector observations, that are fixed in

the inertial reference frame I, are also available to mitigate the errors associated with

dead reckoning. Without loss of generality, throughout the remainder of this chapter,

assume that I shares the origin with B. Since the same attitude kinematics are

shared by all rigid bodies, the method proposed in this chapter can be used by any vehicle

(underwater, terrestrial, aerial or spacecraft) whose sensor suite comprise rate gyros and

vector observations. Vector observations are, for instance, measurements provided by

magnetometers, Sun sensors, Earth sensors, star-trackers, and, under low acceleration

conditions, also accelerometers.

8.2.1 Measurement model

Let the angular velocity of B with respect to I and expressed in B be denoted

as ω ∈ R3. Also, denote by aSF ∈ R

3 the specific force, which is the time-rate-of-change

of the velocity of B, with respect to I relative to a local gravitational space and

expressed in B, and satisfies [Bek07, p. 77]

aSF = Ba− Bg,

where Ba ∈ R3 is the linear acceleration of the rigid body and Bg ∈ R

3 corresponds to the

gravity, both expressed in the body-fixed coordinates B.The IMU is composed of a set of rate gyros and a set of accelerometers. The ideal ith

rate gyro measures the projection of ω on its measurement axis, denoted by h(i)Ω ∈ R

3.

Since the rate gyro is fixed in the body reference frame, h(i)Ω is constant in B. Thus, the

ideal ith rate gyro measurement is given by

Ωi = hΩiTω, i = 1, . . . , NΩ. (8.1)

154


However, the actual rate gyro measurement, Ωri, is corrupted by bias and noise, which

are assumed to be bounded, i.e.,

Ωri = Ωi + bi + δbi + nΩi, (8.2)

where bi ∈ R and nΩi ∈ R denote the measurement bias and noise, respectively, and

δbi satisfies |δbi| ≤ δbi, where δbi is a positive scalar. The bound δbi can be seen as a

bias tolerance, which reflects the confidence that one has on bi remaining constant. The

measurement noise is also assumed to be bounded by a positive constant, i.e., |nΩi| ≤ nΩi.

Remark 8.1. In this chapter, no assumption on the sensor noise is made besides its

boundedness. In the proposed methodology, the acceptable sensor uncertainty is defined

explicitly by its bound, and outliers and high levels of sensor noise are considered as faults.

Let the measurement axis of the ith accelerometer be given by hαi ∈ R3, i = 1, . . . , Nα,

which is constant when expressed in B. This sensor ideally measures the projection of

the specific force on its measurement axis

αi = hT

αiaSF.

However, by assumption, the sensed data are corrupted by bounded sensor noise

αri = αi + nαi,

where nαi denotes the measurement noise, which satisfies |nαi| ≤ nαi, nαi ∈ R+.

The following assumption guarantees that one can recover ω and aSF from Ω =

[Ω1 . . . ΩNΩ]T and α = [α1 . . . αNα ]

T , respectively.

Assumption 8.1. The measurement axes of the rate gyros and accelerometers form a

basis for R3, i.e.,

span(hΩ1, . . . ,hΩNΩ) = span(hα1, . . . ,hαNα) = R

3.

Note that, under this constraint, the following expression can be used to compute the

angular velocity

ω = (HT

ΩHΩ)−1HT

ΩΩ,

where HΩ = [hΩ1 . . . hΩNΩ]T , and

aSF = (HT

αHα)−1HT

αα,

where Hα = [hα1 . . . hαNα ]T , can be used to compute the specific force. The matrices HΩ

and Hα are called measurement matrices of the angular velocity and angular acceleration,

respectively.

155


8.2.2 Dynamic model

Integration of the measurements is necessary to obtain estimates of position and atti-

tude from the inertial data. This process introduces cumulative errors in the estimates.

To correct them, it is typical to use aiding sensors such as magnetometers, star trackers,

and Sun sensors [Far08]. These sensors measure directions expressed in B, which, formost practical purposes, can be considered constant in the inertial coordinates I. Thesevectors satisfy the kinematic equation

v = −(ω)×v, (8.3)

where v ∈ R3 denotes a generic vector observation expressed in B. Let us assume that

the sensors provide measurements of Nv vector observations in the form

v(i)r = H

(i)v v(i) + n

(i)v , i = 1, . . . , Nv, (8.4)

where H(i)v is the measurement matrix of vector v(i), and n

(i)v is the measurement noise

vector. Each component of vector i, denoted by n(i)vj = eT

j n(i)v , satisfies

|n(i)vj | ≤ n(i)vj , (8.5)

where n(i)vj ∈ R

+, i = 1, . . . , Nv.

The time derivative of the specific force, aSF, is given by

aSF = Ba− d

dt(BI R

Ig) , (8.6)

where BI R ∈ SO(3) is the rotation matrix from the inertial coordinates I to the body

coordinates B, and Ig ∈ R3 is the acceleration due to the gravity force expressed in

I. By transposing both sides of (2.5), the time-rate-of-change of the rotation matrix

describing the rigid body attitude can be rewritten in the form

B

I R = −(ω)×B

I R.

On the other hand, in many practical applications, the external accelerations can be

neglected when compared with gravity. Under this assumption, (8.6) can be rewritten as

aSF = −(ω)×(−Bg) ≈ −(ω)×aSF,

where Bg ∈ R3 is the gravity vector expressed in B. Thus, the specific force has a

behavior similar to a vector observation as described in (8.3), i.e., the measurements of

the accelerometers can also be seen as a vector observation.

156

8.3 FDI using hardware redundancy

8.2.3 Faults description

The characterization of faults as described in [MS91] is adopted, classifying them into

hard and soft faults. The hard faults include step-type failures, such as zero output and

stuck at faults. Changes in noise level and variation of measurement bias are typical ex-

amples of soft faults. A comprehensive study of the faults affecting mechanical rate gyros

is present in [ZJ06].

This chapter proposes a novel technique based on set (polytope) operations and SVOs

(described in Section 7.3) to detect and isolate faults in sensors of navigation systems,

namely, rate gyros, accelerometers, and sensors providing vector observations, such as

magnetometers, Sun sensors, and star trackers. As described in the sequel, this is done by

building upon recent results that extend the applicability of SVOs to FDI [RSSA10b].


In this section, a technique to detect faults on sensors by using hardware redundancy

is designed. The method is illustrated resorting to the rate gyros measurements. Never-

theless, it is equally fitted to exploit redundancy in other sensors, such as accelerometers

and magnetometers.

Even in situations where payload size and weight are constrained, due to the compact-

ness of MEMS devices, it is an acceptable option to have some hardware redundancy to

create a fault tolerant systems. If the sensor redundancy is achieved by using multiple

sensors in the same axis, six sensors are required to detect faults and nine are required for

the isolation of non-simultaneous faults. Another alternative is to place them in different

axes, thereby allowing the detection of faults using only four sensors, while fault isolation

is possible using five sensors [Stu88]. The optimal sensor configuration depends on the

quantity of available sensors. In [SY10], the optimal configuration using different number

of redundant sensors is studied assuming equal probabilistic properties for the noise of

each sensor. It is shown that the optimal configuration is obtained when the measurement

matrix satisfies HTH = N3 I3, where N is the number of available sensor measurements.

8.3.1 Fault detection

Recall the rate gyro measurement model in (8.2). Taking into account the bounds of

δbi and nΩi, we have that Ωi satisfies

Ωi ≤ Ωri − bi + δbi + nΩi,

157


and

Ωi ≥ Ωri − bi − δbi − nΩi.

Rewriting these inequalities for all rate gyros in matrix format yields

[INΩ

−INΩ

]

Ω ≤[Ωr − b+ δΩ−Ωr + b+ δΩ

]

, (8.7)

where the vector inequalities are taken element-wise, INΩis the NΩ ×NΩ identity matrix,

Ωr = [Ωr1 . . . ΩrNΩ]T ∈ R

NΩ , b = [b1 . . . bNΩ]T ∈ R

NΩ , δΩ = [δΩ1 . . . δΩNΩ]T ∈ R

NΩ and

δΩi = δbi + nΩi. (8.8)

Therefore,

Ω ∈ Set(MΩ,mΩ),

where

MΩ =

[INΩ

−INΩ

]

, mΩ =

[Ωr − b+ δΩ−Ωr + b+ δΩ

]

.

The notation Set(M,m) was introduced in the previous chapter to denote the polytope

defined by matrix M and vector m such that

Set(M,m) = z ∈ Rn : Mz ≤ m.

Writing (8.1) in matrix form yields

Ω = HΩω. (8.9)

Then, from (8.7) and (8.9) is possible to write

[INΩ

−INΩ

]

HΩω ≤[Ωr − b+ δΩ−Ωr + b+ δΩ

]

.

Consequently, using the previously introduced notation, ω satisfies

ω ∈ Set(MΩHΩ,mΩ). (8.10)

Definition 8.1. A rate gyro is faulty if its measurements do not satisfy the relations

(8.1)-(8.2).

The following proposition characterizes the proposed FD method, illustrated in Fig-

ure 8.1 and in Figure 8.2, which exploits the existence of redundant sensor measurements.

158


ω(k)

Figure 8.1: Illustration of the proposed FD method to exploit hardware redundancy: nonfaulty rate gyros.

ω(k)

Figure 8.2: Illustration of the proposed FD method to exploit hardware redundancy: faultyrate gyros.

Proposition 8.1. Consider the rate gyros model (8.2) and the linear transformation be-

tween the ideal sensor measurements Ω and the angular velocity ω given in (8.9). Then,

under Assumption 8.1, if

Set(MΩHΩ,mΩ) = ∅,

there exists at least one faulty rate gyro.

Proof. Assume that all rate gyros are healthy and that Set(MΩHΩ,mΩ) = ∅. Since all rategyros are healthy, the rate gyros model (8.2) holds and Ω ∈ Set(MΩ,mΩ). Then, from the

linear transformation (8.9), we have that ω ∈ Set(MΩHΩ,mΩ). But this contradicts the

initial assumption that Set(MΩHΩ,mΩ) = ∅. Thus, there is at least one faulty sensor.

From a theoretical point of view, this strategy has the advantages of guaranteeing that

no false alarms are issued and of no threshold tuning being required. Although the pro-

posed solution requires fixing an upper bound on the sensor noise magnitude and a decision

159


rule based on a limit threshold also depends directly on the sensor noise characteristics

[BSK+06], the upper bounds to be devised are fixed directly on the sensor space rather

than on a more intangible residuals space. Thus, they can be set in a much more straight-

forward manner, when compared with classical residual-based approaches. Furthermore,

the proposed methods are suitable for time-varying bounds, taking into account, for in-

stance, variations in temperature. Hence, in the approach followed in this chapter, sensor

noise with a magnitude greater than an established value is interpreted as a fault.

8.3.2 Fault isolation

The proposed scheme for fault isolation consists in evaluating the emptiness of Si =

Set(MΩ\iHΩ\i,mΩ\i), where

MΩ\i =

[INΩ−1

−INΩ−1

]

,

HΩ\i = E\iHΩ, E\i = [e1 . . . ei−1 ei+1 . . . eNΩ]T ∈ R

NΩ−1×NΩ ,

mΩ\i = diag(E\i, E\i)mΩ ∈ R2NΩ−2.

Note that HΩ\i is the matrix which contains all the measurement vectors except for

the one corresponding to the fault measurement i, and mΩ\i is the vector mΩ with-

out the elements i and 2i, which are associated with the fault measurement. Thus,

Set(MΩ\iHΩ\i,mΩ\i) is the set of angular velocities that may be incompatible with

the faulty measurement but are compatible with all remaining measurements. If only

for one i, Si is non-empty, the faulty measurement is Ωri. If more than one Si is non-

empty, it is not possible to isolate the fault. The proposed FDI procedure is described in

Algorithm 1.

Let the model of the faulty rate gyro, f ∈ 1, . . . , NΩ, be given by

Ωrf = Ωf + bf + δbf + nΩf + ε, (8.11)

where bf ∈ R, |δbf | ≤ δbf , |nΩf | ≤ nΩf , and ε ∈ R denotes the measurement error resulting

from a fault. Moreover, consider all the minimum singular values of the square matrices

whose columns are three measurement axes

σs = σmin (Hijk) , Hijk =[hi hj hk

]T,

where i 6= j 6= k, i 6= k, j 6= k, σmin(.) denotes the minimum singular value of its argument,

and s = 1, . . . , Nσ, with Nσ =(NΩ3

). Additionally, let ρ ∈ N be such that σρ is the

minimum non-zero σs.

The following proposition provides sufficient conditions on the magnitude of a fault

that ensure its detection and isolation.

160


Algorithm 1 FDI using hardware redundancy.

k = 0fault detected=falsefault isolated=falsewhile fault isolated =false do

if Set(MΩ(k),mΩ(k)) = ∅ thenfault detected=true

end ifif fault detected=true then

number of empty Si=0for i = 1 to NΩ do

Si = Set(MΩ\iHΩ\i(k),mΩ\i(k))if Si 6= ∅ then

store inumber of empty Si=number of empty Si + 1

end ifend forif number of empty Si = 1 then

the faulty sensor identified by the stored i!fault isolated=true

elseit is not possible to isolate the faultfault isolated=false

end ifend ifk = k + 1

end while

Proposition 8.2. Assume that there are at least five non-coplanar measurements. Then,

if the error associated with a fault satisfies

|ε| > 2σ−1ρ δΩ + 2max

iδΩi, (8.12)

where

δΩ = maxi 6=ji 6=kk 6=j

∥∥δΩ(ijk)

∥∥, δΩ(ijk) =

[δΩi δΩj δΩk

]T,

the proposed FDI scheme is guaranteed to detect and isolate non-simultaneous faults.

Proof. The proposed scheme is guaranteed to detect faults if the set compatible with the

faulty measurement model does not intersect the set compatible with more than two non-

faulty independent measurements. The idea of the proof is to obtain an overbounding ball

for the set of points that can be compatible with more than two non-faulty independent

measurements, and then provide a condition on the magnitude of the fault that guarantees

that the set of points compatible with the faulty measurements do not intersect that ball.

161


For the sake of comprehension, and without loss of generality, let ω = 0. Then, the

measurement model (8.1)-(8.2) satisfies

hT

i ω ≤ Ωri − bi + δΩi

−hT

i ω ≤ −Ωri + bi + δΩi,

where i = 1, . . . , NΩ. With ω = 0, the following inequality is obtained

|Ωri − bi| ≤ δΩi.

Thus, all the points compatible with the ith measurement belong to the set

Λi =

z ∈ R3 :

[hT

i

−hT

i

]

z ≤ 2

[δΩi

δΩi

]

.

The points where three hyperplanes limiting the sets Λi, Λj and Λk intersect satisfy

Hijkχ = 2δΩ(ijk),

and also

‖χ‖ ≤ 2

σmin(Hijk)‖δΩ(ijk)‖ ≤ 2δΩ

σmin(Hijk).

If one takes the minimum non-zero σmin(Hijk), i.e., σρ, an overbound on the norm of the

farthest intersection point, χρ, is obtained, i.e., ‖χρ‖ ≤ 2δΩσρ

. This overbound is the radius

of the ball Bρ, whose complementary space does not contain any point compatible with

more than two non-coplanar measurements.

Now, consider the set compatible with the faulty measurement, which is described by

Λf =

χf ∈ R3 :

[hT

f

−hT

f

]

χf ≤[2δΩf + ε2δΩf − ε

]

, (8.13)

where f ∈ 1, . . . , NΩ denotes the index of the faulty gyro. Since there are at least five

non-coplanar measurements, if for all χf ∈ Λf one has ‖χf‖ > ‖χρ‖, the fault is detectedand isolated. From (8.13) it can be concluded that

‖χf‖ ≥ −2δΩf − ε

‖χf‖ ≥ −2δΩf + ε⇔ ‖χf‖ ≥ −2δΩf + |ε|. (8.14)

Then, by using (8.12) in (8.14), it yields

‖χf‖ ≥ −2δΩf + |ε| > 2σ−1ρ δΩ,

and thus ‖χf‖ > ‖χρ‖.

162

8.4 FDI using analytical redundancy and SVOs

Proposition 8.2 provides sufficient conditions that ensure detection and isolation. In

addition, a necessary condition on the magnitude of the fault for detection and isolation

is described in the following proposition.

Proposition 8.3. Detection (and isolation) of a fault is only possible if the corresponding

fault magnitude satisfies

|δbf + nΩf + ε| > δΩf .

Proof. To prove this proposition it suffices to show that if

|δbf + nΩf + ε| ≤ δΩf , (8.15)

then, there exists ω such that the inequality

[INΩ

−INΩ

]

HΩω ≤[Ωr − b+ δΩ−Ωr + b+ δΩ

]

, (8.16)

holds. Without loss of generality let the faulty sensor be the first and let ε = [ε 0 . . . 0]T .

Then, from (8.2) and (8.11), (8.16) takes the form

[INΩ

−INΩ

]

HΩω ≤[Ω+ δb + nΩ + ε+ δΩ−Ω− δb − nΩ − ε+ δΩ

]

.

From (8.8) and (8.15), we have that

[INΩ

−INΩ

]

HΩω ≤[Ω+ λ+

−Ω+ λ−

]

, (8.17)

where λ+ = δb + nΩ + ε + δΩ > 0 and λ− = −δb − nΩ − ε + δΩ > 0. By taking

ω = (HT

ΩHΩ)−1HT

ΩΩ, it can be concluded that (8.17) holds, which contradicts the initial

assumption, thereby proving the results.

Remark 8.2. The described method is suitable to detect and isolate non-simultaneous

faults. To that end, at least five sensors are required. With more sensors and the appropri-

ate modifications, the proposed method may also be suitable to isolate simultaneous faults

in two or more sensors.


In this section, an FDI algorithm based on analytical redundancy is proposed for the

case where measurements from rate gyros and vector observations are available.

163


8.4.1 Fault detection

As most physical phenomena, the kinematic model described in (8.3) is continuous in

time and, hence, not in the desired discrete-time framework of SVOs. In the following,

a discrete-time approximation of the model, based on the knowledge of upper bounds on

the magnitude of the angular acceleration, is devised.

In this chapter, the discretization error due to the angular acceleration is computed

by resorting to the Mean Value Theorem [Apo67] instead of Lemma 7.1 as in the previous

chapter. The exact solution of the differential equation (8.3) is given by

v(t) = expmSO(3)

(

−∫ t

to

(ω(τ))×dτ

)

v(to), (8.18)

where to < t. By using the Mean Value Theorem twice and (8.18), it can be conclude that

there exists ξ ∈ [to, t] such that

v(t) = expmSO(3)

(

−∫ t

to

(ω(to) + (τ − to)ω(ξ))×dτ

)

v(to). (8.19)

Here, the goal is to obtain the discrete-time description of v(t) at time kT , k ∈ N, where T

denotes the sampling period of the sensor data. Noting that ω(to) and ω(ξ) are constant

between sampling times, we can rewrite (8.19) at the sampling times as

v((k + 1)T ) = expmSO(3)

(

−T2

2(ω(ξ))× − T (ω(kT ))×

)

v(kT ), (8.20)

for some ξ ∈ [kT, (k + 1)T ].

In (8.20), the angular velocity and the angular acceleration are not completely known

and hence are not suitable to be used by the SVO. On the other hand, from (8.10) the

angular velocity satisfies

ω(kT ) ∈ Set(Mω(kT ),mω(kT )), (8.21)

where Mω(kT ) = MΩ(kT )HΩ and mω(kT ) = mΩ(kT ). Since (8.21) defines a convex

polytope, the center of the polytope, denoted by ωr(kT ), can be computed by resorting

to a LP problem. As seen in the previous chapter, the so-called Chebyshev center of

a polyhedron [BV04, Section 8.5.1] is defined as the point contained by the polyhedron

which is furthest from its limits.

Define ωr = ‖ωr(kT )‖∞ as the maximum distance between the center and the border

of the polytope, and define the uncertainty in the angular velocity as δω(kT ), such that

δω(kT ) = ω(kT )− ωr(kT ). (8.22)

164


Note that the uncertainty δω(kT ) satisfies

δω(kT ) ∈ Set(Mω(kT ),mω(kT )−Mω(kT )ωr(kT )),

which denotes a polytope centered at the origin and let the maximum distance along any

major axis to the boundary of this polytope be given by

δω = ‖δω(kT )‖∞.

The angular acceleration is inherently bounded due to the limitations in terms of the

energy that can be provided to any physical system. Moreover, in many applications,

either due to constraints on the thrusters, or due to the action of friction, it is in fact

possible to obtain an upper bound on the magnitude of the angular acceleration. Hence,

consider the following assumption.

Assumption 8.2. Assume that the magnitude of the angular acceleration is bounded by

a known (but possibly conservative) positive scalar φω, i.e.,

‖ω‖∞ ≤ φω, φω ∈ R+. (8.23)

For simplicity of notation, in the remainder of this chapter, the time dependence of

the variables will be simply denoted by k, k ∈ N.

Using (C.1) from Appendix C and the magnitude bounds on the angular acceleration

(8.22) and on the uncertainty of the angular velocity measurements (8.23), the following

relation for each element of the dynamic matrix can be established[

expmSO(3)

(

−T (ωr(k) +T

2ω(ξ) + δω(k))×

)]

ij

=

= [expmSO(3)(−T (ωr(k))×)]ij + ǫ∆(i,j)(k),

for some |∆(i,j)(k)| ≤ 1, where

ǫ =1

2

(

e2T (ωr+δω+T2φω) − e2T ωr

)

.

This construction provided a discrete-time approximate system that depends solely on

sensor data and is in the framework of SVOs in (7.7). The upper bound on the error

of the approximation, ǫ, can be handled in the same framework. Hence, an SVO can be

designed to the system

x(k + 1) = Ao(k)x(k) +A∆(k)x(k)

y(k) = C(k)x(k) + n(k), (8.24)

165


(model)Predict Update

(measurements)

∩

Set(M,m)=Ø

SVO

PhysicalSystem

Sensor noise

n (k)v

FD Filter

ωr(k) v(k)i

n (k)ω

Fault Detected

yes no

No Fault Detected

Figure 8.3: Proposed fault detection filter for navigation systems.

where x(k + 1) = [v(1)T . . . v(Nv)T ]T , n = [n(1)v

T . . . n(Nv)v

T ]T and

Ao(k) = INv ⊗ expmSO(3)(−T (ωr(k))×)

A∆(k) =

N∑

i=1

Ai(k)∆(m,n)(k), i = 1, . . . , N,

Ai(k) = INv ⊗ ǫEm,n,

where m = 1, . . . , 3, n = 1, . . . , 3, i = m+ 3(n− 1) and

C(k) = diag(H(1)v , . . . ,H

(Nv)v ).

If, at some point, the set containing the state, i.e., Set(M(k),m(k)), degenerates into

the empty set, it follows that the model no longer describes the system and sensor data,

and hence a fault has occurred. The proposed FD architecture is illustrated in Figure 8.3

and its main property is formally stated in the following proposition.

Proposition 8.4. Consider the model of the rate gyros (8.2) and the model of the vector

observations (8.4), which are dynamically related by the model (8.3), and the corresponding

SVO described in (8.24). Then, if Set(M(k),m(k)) = ∅, for some k ≥ 0, a fault has

occurred at some time instant kf ≤ k.

The proof of this proposition is omitted as it follows directly from the construction in

this section.

166


FD Filter for theNominal System

SVO Robustto Faults

FD Filter Robustto Fault #1

Physical System

FD obustto Fault #2Filter R

FD obustto Fault #NFilter R

.

.

.

.

.

.

Sensor noise

n (k)vn (k)ω

Sensordata

ConservativeState Bounds

FD Signal

Fault #1invalidated / not

invalidated

.

.

.

Isotation

FDI Filter

Fault #2invalidated / not

invalidated

Fault #Ninvalidated / not

invalidated

Figure 8.4: Proposed fault detection and isolation filter for navigation systems.

Remark 8.3. The proposed FD filter guarantees that there will be no false alarms. How-

ever, it may not be able to detect some sensor faults. This may be due to severe sensor

noise or to the conservatism added to the model in (8.24). This problem is related to the

concept of indistinguishability. The interested reader is referred to [RS11].

Remark 8.4. This method may lead to some implementation issues, since it might not

be suitable for systems with very low computational power. However, for instance, many

aircrafts are nowadays equipped with powerful computers. In addition, the proposed so-

lution is highly parallelizable, and thus can take advantage of the recent multi-core and

multi-processor systems.

8.4.2 Fault isolation

Fault-tolerant systems require not only the detection of faults but also the identifi-

cation of its exact location. Therefore, if redundant sensors exist, the system should be

reconfigured so that the normal operation can be resumed using the remaining healthy

sensors. In this section, the isolation strategy proposed in [RSSA10b] and illustrated in

Figure 8.4 is adopted. This strategy relies on the concept of model invalidation. A bank of

SVOs is designed modeling each different fault, and another SVO is synthesized modeling

the nominal (non-faulty) system. Under certain distinguishability conditions, only one

model is consistent with the sensor data, and thus all the others SVOs will be invalidated,

i.e., their set-valued state estimates will degenerate into the empty set. The remaining

SVO, if any, isolates the fault. To spare unnecessary computational burden, and since

faults can occur at any time, the following scheme is used. Firstly, only the nominal FD

filter and one SVO robust to all faults are active. The set estimated by the robust SVO

167



SVO Robustto Faults


Physical System



.

.

.

.

.

.


FD Signal

.

.

.

Isotation

FDI Filter


SVO Robustto Faults


Physical System



.

.

.

.

.

.


FD Signal

.

.

.

Isotation

FDI Filter


SVO Robustto Faults


Physical System



.

.

.

.

.

.


FD Signal

.

.

.

Isotation

FDI Filter

Stage 1 Stage 2 Stage 3

Inactive SVO Active SVO SVO estimate degenerated into empty set

Figure 8.5: Different stages of the FDI filter based on SVOs.

is designed to always include the true state of the system, even if a fault in the sensors

has occurred. If, at some point, the FD filter for the nominal system is invalidated, it

indicates that a fault has occurred. Hence, the bank of FD filters modeling the faults is

initialized with the set estimated by the robust SVO. Once all the filters describing faults

that did not occur have been invalidated, the fault has been isolated. The FDI procedure

using analytically redundancy is described in Algorithm 2.

Figure 8.5 illustrates the three stages of the FDI filter. The first stage is characterized

by the nominal and the robust SVOs active and the FD filters robust to each of the faults

inactive. A fault is detected if the set-valued estimate of the nominal SVO degenerates into

the empty set. Then, the set-valued estimate of the robust SVO is used in the initialization

of each of the SVOs robust to each of the faults, which is illustrated in stage two. Finally,

the third and final stage depicts the moment when all but one SVO (the one robust to

that specific fault) have degenerated, i.e., the fault has been isolated.

After this general description of the proposed FDI filter, the particularities of its use

to detect and isolate inertial measurements and vector observations faults are presented

next.

Faults in the vector observations

The faults in the vector observations can be modeled directly by an SVO. The hard

faults considered are the zero output and the stuck at types of faults as described in

Section 8.2.3. The zero output fault is modeled by zeroing the row in the measurement

matrix corresponding to the faulty sensor, whereas a stuck at type of fault is modeled by

considering that the sensor model is given by

v(i)rj (k + 1) = v

(i)rj (k),

168


Algorithm 2 FDI using analytically redundancy.

k = 0initialize X(k = 0)initialize Xrobust(k = 0)fault detected=falsefault isolated=falsewhile fault isolated=false do

if fault detected=false thenfor i = 1 to 23Nv do

compute Xγi using (7.12) (or (7.9))compute Xγi robust using (7.12) (or (7.9))

end forX(k + 1) := co

Xγ1(k + 1), . . . , Xγ23Nv

(k + 1)

Xrobust(k + 1) := co

Xγ1 robust(k + 1), . . . , Xγ23Nv robust(k + 1)

if X(k + 1) = ∅ thenfault detected=trueXfault#1(k + 1) = Xrobust(k + 1)

...Xfault#N (k + 1) = Xrobust(k + 1)

end ifelse

number of possible faults= Nfaults

for j = 1 to Nfaults dofor i = 1 to 23Nv do

compute Xγi fault#j using (7.12) (or (7.9))end forXfault#j(k + 1) := co

Xγ1 fault#j(k + 1), . . . , Xγ23Nv fault#j(k + 1)

if Xfault#j(k + 1) = ∅ thennumber of possible faults=number of possible faults−1

elsestore j

end ifend for

end ifif number of possible faults= 1 then

fault isolated=truethe fault is identified by the stored j

end ifk = k + 1

end while

169


where v(i)rj = eT

j v(i)r , i ∈ N and j = 1, . . . , Nv. Thus, the SVO for this fault performs the

intersection of the set obtained from the measurements that contains v(i)rj at successive

time instants, neglecting the dynamics of the system (for this sensor). The soft faults are

modeled by an unexpected increase in the magnitude of the sensor noise, i.e., a larger

value n(i)vj in (8.5).

Faults in the rate gyros

The kinematics of the rigid body attitude depends directly on the angular velocity.

For that reason, this method can accurately determine that a fault has occurred in the

rate gyros. However, it may not be able to isolate the faulty rate gyro. A higher noise

magnitude in the rate gyros is modeled by using an SVO with larger value of nΩi in (8.8).

On the other hand, a bias variation greater than the anticipated can be modeled by a larger

value of δbi also in (8.8). Since these two sources of uncertainty influence the dynamics in

a similar way, they are indistinguishable in the sense of [RS11]. As a consequence, only

an SVO that is tolerant to both faults simultaneously can be designed. The hard faults

– zeroing the measurements and the stuck at type of faults – invalidate any information

regarding the model. Hence, to model hard faults in the rate gyros, it is necessary to

design a particular SVO for the faulty rate gyro measurement. For stuck at faults the

following model is adopted

Ωi(k + 1) = Ωi(k).

For zero output faults the adopted model is

Ωi(k + 1) = 0.

8.5 Simulations results

In this section, Monte-Carlo simulations have been carried out to assess the perfor-

mance of the two proposed FDI schemes. The number of iterations necessary for detection

and isolation of faults are evaluated for 100 runs. We consider two different system config-

urations: i) five rate gyros, and two vector observations, each of which with five sensors,

with installation matrices described by

HΩ = H(1)v = H

(2)v =

[ 1 0 00 1 0

0.47 0.47 0.75−0.64 0.17 0.750.17 −0.64 0.75

]

,

and satisfying HTH = N3 I; ii) three rate gyros and two vector observations, each of which

with three sensors, with installation matrices given by HΩ = H(1)v = H

(2)v = I. In both

170


cases, it is assumed that the sensors are installed onboard a vehicle describing oscillatory

angular movements characterized by the following angular velocity vector

ω(t) =

40 sin(2π0.05t)−29 sin(2π0.04t)52 sin(2π0.02t)

deg/s.

Under normal operation, the rate gyros measurements are corrupted by noise with uniform

distribution over the interval [−0.573, 0.573] deg /s. The tolerance of the bias calibrated

at the beginning of the mission is set to |δbi| = 0.0115 deg/s, i = 1, . . . , 5. Each vector has

unit norm, and each sensor measurement is corrupted by (normalized) noise with uniform

distribution over the interval [−0.05, 0.05]. The sampling period of all sensors is set to

T = 0.1 s.

It is assumed that one of the following six faults can occur:

1. a stuck at type of fault in rate gyro #1;

2. rate gyro #3 badly damaged generating a null measurement;

3. the maximum amplitude of the noise in the rate gyro #3 increases fifteen times;

4. a stuck at type of fault in first sensor of vector #1;

5. the second component of vector #2 is null;

6. the maximum amplitude of the noise in the third sensor of vector #3 increases five

times.

Table 8.1 provides the mean and the standard deviation of number of iterations, i.e., the

number of sampling periods, required to detect and isolate each fault using the proposed

methods exploiting hardware redundancy (HW) and analytical redundancy (An.), as well

as the ratio of correct fault isolations for the case where there are five rate gyros and

five sensors for each vector. The results obtained with two alternative FDI methods are

included. One exploits the hardware redundancy and is based on the projection of the

measurement vector on the orthogonal complement of the range space of the system matrix

(Proj.) [SL06], whereas the second one exploits the analytical redundancy using a bank

of Kalman filters (KFs), as described in [May99]. In this table, kd and ki stand for the

number of iterations necessary for detection and isolation, respectively. The mean and the

standard deviation of the number of iterations required to detect and isolate each fault

for the case where there are three rate gyros and five sensors for each vector is presented

in Table 8.2. Note that, since there are no redundant sensors, it is not possible to detect

or isolate faults by using the method that takes advantage of the hardware redundancy.

171


Table 8.1: Mean (µ) and standard deviation (σ) of the number of iterations necessary todetect and isolate the faults and percentage of correct isolations (% Corr. Isol.) with fivegyros and five sensors per vector.

Fault kd HW ki HW % kd An. ki An. %# µ (σ) µ (σ) Corr. Isol. µ (σ) µ (σ) Corr. Isol.1 0.88 (0.54) 4.23 (1.20) 100 1.88 (0.54) 2.86 (0.78) 1002 0.00 (0.00) 0.00 (0.00) 100 1.00 (0.00) 1.49 (0.76) 1003 0.34 (0.64) 0.46 (0.72) 100 1.34 (0.64) 1.93 (1.12) 1004 16.26 (2.62) 23.71 (2.18) 100 13.09 (4.05) 15.06 (3.31) 1005 1.72 (1.05) 9.96 (2.83) 100 0.00 (0.00) 1.28 (0.67) 1006 1.63 (2.04) 6.18 (5.53) 100 0.47 (0.73) 1.30 (1.18) 100

kd Proj. ki Proj. % kd KF ki KF %# µ (σ) µ (σ) Corr. Isol. µ (σ) µ (σ) Corr. Isol.1 13.16 (0.99) 14.29 (1.17) 100 0.49 (1.97) 0.49 (1.97) 952 0.00 (0.00) 0.13 (0.34) 100 0.57 (1.98) 0.57 (1.98) 953 - - 0 26.30 (8.59) 26.30 (8.59) 04 20.37 (1.06) 20.81 (1.17) 100 3.20 (2.85) 3.20 (2.85) 955 5.31 (0.94) 6.12 (1.32) 100 0.01 (1.91) 0.01 (1.91) 956 29.14 (30.13) 51.98 (51.08) 100 3.46 (4.34) 3.46 (4.34) 77

Table 8.2: Mean (µ) and standard deviation (σ) of the number of iterations necessary todetect and isolate the faults and isolate the faults and percentage of correct isolations (%Corr. Isol.) with three gyros and three sensors per vector.

Fault Td An. Ti An. %# µ (σ) µ (σ) Corr. Isol.1 7.65 (1.68) 113.14 (1.28) 1002 1.98 (0.72) 68.70 (1.26) 1003 88.62 (76.65) 126.82 (81.43) 1004 13.44 (4.34) 33.06 (1.53) 1005 0.00 (0.00) 18.25 (1.22) 1006 0.39 (0.65) 2.84 (1.92) 100

Fault Td KF Ti KF %# µ (σ) µ (σ) Corr. Isol.1 1.84 (2.92) 1.84 (2.92) 95.002 0.68 (1.59) 0.68 (1.59) 96.003 - - -4 3.60 (2.53) 3.60 (2.53) 97.005 0.31 (1.60) 0.31 (1.60) 96.006 3.96 (4.49) 3.96 (4.49) 12.00

172


The results presented in Table 8.1 show that the proposed methods are able to detect

and isolate all considered faults. However, it should be noted that, even with a sampling

time of T = 0.1 s, when there are five sensors per gyro and vector observation, the two

proposed methods are, on average, able to detect and isolate all the tested faults in less

that 2.5 s (25 iterations). The results indicate that none of the proposed methods is

superior to the other in detecting every fault. For this scenario, the proposed methods

(HW and An.), in most cases, detect and isolate faults in less iterations than the Proj.

and KF-based methods, even after considerable tuning effort of these methods. In the

KF-based method (proposed in [May99]), a fault is detected and isolated simultaneously.

Thus, the values kd and ki obtained with this method are similar. Fault #4 takes longer

to be detected and isolated by the proposed methods (HW and Analytical) than the other

faults, while also requiring a significant time to be detected and isolated by the Proj. and

KF-based methods. When this fault occurs, the time derivative of the sensor measurement

is almost zero. Since the fault is characterized by the sensor measurement being stuck at

a constant value, the faulty measurements and the corresponding value if a fault has not

occurred are initially very similar to one another, which hinders its detection. The KF

method is the fastest of the considered approaches to detect and isolate this fault, due to

its probabilistic nature, as the model with zero time derivative (the model of the faulty

system) has higher probability than that of the nominal system. It is also remarked that

the KF-based method is not able to correctly isolate fault #3, whereas the Proj. method

cannot detect it. Moreover, the KF-based method sometimes isolated the remaining faults

incorrectly.

The proposed method based on hardware redundancy has the advantage of requiring

less computational power than the one based on SVOs, while the latter, by exploiting

the dynamic relation among sensor measurements, has the advantage of not requiring

redundant sensors. Table 8.2 shows that, as expected, with less sensors the detection and

isolation of faults requires more iterations. In this scenario, the KF-based method requires

less time to detect and isolate fault #2. However, this alternative method is not capable

of isolating fault #3. Moreover, it requires more iterations to detect and isolate all the

remaining faults and also these are not always correctly isolated.

Figure 8.6 and Figure 8.7 illustrate the time-evolution of the proposed SVO-based FDI

process for a particular fault for the case with three gyros and three sensors per vector. In

Figure 8.6, the upper and lower bounds of the nominal and robust SVO’s x-axis component

of v1 are depicted. At t = 1 s fault #4 occurs and the nominal SVO degenerates into the

empty set at t = 2.5 s. At this moment, the six SVOs tolerant to each of the six possible

173


0 0.5 1 1.5 2 2.5−0.4

−0.2

0

0.2

0.4

v1x

time (s)

non−faulty period faulty period

true state

upperbound nominal SVO

lowerbound nominal SVO

upperbound robust SVO

lowerbound robust SVO

Figure 8.6: Nominal state v1x, estimate of the nominal SVO and estimate of the robustSVO.

faults are initialized with the bounding polytope provided by the robust SVOs. The upper

and lower bounds of the x-axis component of v1 of each SVOs are shown in Figure 8.7.

The SVOs tolerant to faults #1, #2 and #5 degenerate after the first iteration, while the

SVO tolerant to fault #6 degenerates at t = 2.9 s and the SVOs tolerant to faults #3

degenerates at t = 4.3 s, leaving active only the SVO tolerant to fault #4, which thus

isolates the fault.

8.6 Chapter summary

In this chapter, two novel FDI methodologies for IMUs and vector observations were

proposed. The first scheme takes advantage of hardware redundancy in the sensor mea-

surements to detect incoherences between them. Necessary and sufficient conditions have

been provided that depend on the measurement matrix and on the measurements uncer-

tainty and that guarantee detection and isolation of non-simultaneous faults. To exploit

the dynamic relation among the angular velocity and the vector measurements, a second

methodology was proposed based on set-valued state estimates provided by SVOs, which

can be used to validate or falsify different models of faults. This method has, however, the

disadvantage of requiring greater computational power. Neither solution generates false

detections, as long as the non-faulty model of the system remains valid. In addition, due

to the set-valued construction of the methods, the tuning of a decision threshold – hard

to set and typically required by classical FDI techniques – is not necessary. Resorting

to the Monte-Carlo simulations of a realistic scenario presented in the chapter, it can be

concluded that, at least for the studied case, the proposed methods perform better than

two alternatives presented in the literature.

174

8.6 Chapter summary

Figure 8.7: Estimates of the SVOs tolerant to faults #1 to #6.

175

Chapter 9

Conclusions and future work

This thesis addressed the problem of deterministic position and attitude estimation,

providing some contributions to the fields of nonlinear observers and set-membership ob-

servers for attitude estimation and in the field of fault detection and isolation of faults in

inertial measurement units.

Chapter 3 and Chapter 4 were dedicated to the development of attitude estimation

methods for indoor environments. Two different sensor suites to be installed on a rigid

body (e.g. unmanned aerial vehicle) were considered, namely inertial measurements and

cameras in Chapter 3, and inertial measurements and range sensors in Chapter 4. These

estimation problems were addressed resorting to specially devised nonlinear observers. Ju-

dicious design of the observers feedback laws allowed for the definition of corresponding

negative semi-definite Lyapunov functions. Negative definite Lyapunov functions defined

in the full SO(3) manifold do not exist, since global asymptotic stability is precluded by

topological limitations. In Chapter 3, the available sensor suite comprised biased angu-

lar velocity measurements and a pan and tilt camera. The image coordinates of markers

located on the mission scenario were used to correct the observer estimates. A stabiliz-

ing feedback law for the attitude observer was introduced such that the estimation errors

converge exponentially fast to the desired equilibrium point. Recent geometric numerical

integration methods were used for devising a multi-rate implementation of the observer

exploiting the complementary bandwidth of the sensors. In order to maintain the visual

markers inside the image field-of-view, a controller was designed to keep the image center

aligned with the centroid of four selected markers. The devised controller is exponentially

input-to-state stable with respect to the camera linear velocity and rate gyro bias. More-

177

9. Conclusions and future work

over, it relies only on the images coordinates, whereas the traditional image-based visual

servoing solutions require the computation of the inverse of the Jacobian matrix and of

the depth coordinate.

In Chapter 4, a new nonlinear attitude observer is proposed. However, in this chap-

ter, range measurements are used as aiding sensors instead of visual data. Range sensors

have the advantages of being affordable, having compact size and having low power con-

sumption. Geometric methods guarantee almost global asymptotic stability of the desired

equilibrium point. A novel description of the estimation error dynamics was proposed

which takes the form of a linear time-varying system and significantly simplifies the sta-

bility and convergence analysis. The estimation error in the presence of time-varying rate

gyro bias was shown to be bounded and an explicit bound was provided. A method pro-

viding observer gains such that the covariance of the estimation error is minimized was

also proposed. Custom-built low-cost prototypes implementing both estimation methods

were developed. Their performance was evaluated resorting to a motion rate table which

provides ground truth data regarding the position, attitude and rate of a programmed

trajectory. The experimental results validated the good performance of the proposed solu-

tions. Nevertheless, a few considerations can be made. The computer vision is a very rich

source of information. However, it consumes more computational resources, and the ex-

perimental prototype is mechanically more complex since it requires control of the camera

pan and tilt axes in order to keep the markers inside the field of view of the camera. On the

other hand, the range based solution is mechanically simpler with no moving parts, and

the processing of the sensor data is more straightforward. However, the angular excursion

of the rigid body is limited by the sound propagation pattern of the ultrasonic transmitter

and the estimates can be degraded by multi-path phenomena. Thus, a pre-processing

algorithm may be required in order to discard outliers due to multi-path.

In some scenarios, the dynamics of the autonomous vehicles can be accurately modeled.

In such cases, one can use the dynamic model together with the kinematic equations

of motion to obtain more accurate position and attitude estimates. In Chapter 5 and

Chapter 6, this idea was explored and two nonlinear observers for full 3-D rigid body

motion were proposed. In Chapter 5, the case with full state measurements was considered.

The observer was obtained and expressed in terms of exponential coordinates on the group

of rigid body motions in 3-dimensional Euclidean space. The estimation error was shown

to be almost globally asymptotically stable and exponentially convergent to the origin.

The robustness to uncertainties in the model was also addressed by establishing that, in

the case of bounded unmodeled torques and forces, the estimation error remains bounded.

178

The case of full state measurements, although very interesting, is perhaps difficult to

encounter in practice, specially because the full state measurements are often not available.

In particular, translational velocity is not available in many practical situations. A more

realistic scenario is the one where a Doppler effect sensor is installed onboard or in the

inertial frame. This case was addressed in Chapter 6. Convergence of the pose and velocity

estimates given by the devised observer was shown to be almost global over the state

space of rigid body motions. Given persistence of excitation of the rigid body position,

exponential convergence of the observer is guaranteed. Numerical simulations results

confirmed the convergence and stability properties of both observers.

The nonlinear observers framework seldom considers sensor noise. In Chapter 7, a

solution to the problem of attitude and rate gyro bias estimation was proposed based on

generic vector observations and angular velocity measurements corrupted by noise, which

is considered to be bounded. A set-valued observer which describes the uncertainties

in the estimates and in the sensor measurements by polytopes was designed to address

this problem. The rotation matrix was adopted as attitude representation since it is

free of singularities and unwinding phenomena, and was embedded into the 9-dimensional

Euclidean space in which the polytope is defined. The set-valued estimates are shown to

always contain the state corresponding to the true attitude of the rigid body. Sufficient

conditions for boundedness of the estimated set were established in the case of single

vector observation. Compared with other set-membership attitude estimators proposed in

the literature, the devised solution has the advantage of not requiring linearization of the

system. Additionally, since it is only based on the rigid body kinematics, which are exact,

it does not suffer from an eventual poor identification of the rigid body dynamics. Several

implementation details were presented and discussed. The performance of the proposed

technique was illustrated in simulation.

Finally, Chapter 8 was dedicated to the problem of fault detection and isolation in

inertial measurement units. Two solutions based on bounding sets addressing this topic

were proposed. The former exploited the existence of redundant measurements, whereas

the latter was based on the set-valued observer framework introduced in Chapter 7 and

on the dynamic relation among inertial sensors. Sufficient conditions which guarantee

detection and isolation of faults were provided. The performance of both methods were

compared with the one of two methods described in the literature resorting to Monte-Carlo

simulations of a realistic scenario.

179

9. Conclusions and future work

9.1 Future directions of research

The navigation systems are key for many modern platforms and every day they become

more present in our daily life. Examples of this are the new applications of small aircrafts

such as surveillance, infrastructure inspection, geophysical exploration, extreme weather

assessment, disaster relief, aerial photography and video recording. There are signs that

this trend is to continue and the forthcoming novel applications will certainly demand

more robust, flexible and accurate systems, which will call for new improved estimation

algorithms. The solutions proposed in this thesis aim to address some of these new chal-

lenges, such as the navigation in indoor environments and the need for robust attitude

estimation methods.

Nonlinear observers can be very efficient state estimators for complex systems. Their

design and analysis tools are inherited from the formally well established theory devised

to study nonlinear systems. In general, the observers are tightly linked with the underling

physical process providing deep insight on the system properties, such as the geometric

constraints, and can be very computational efficient when compared with other solutions

such as the extended Kalman filter, which requires the computation of the system and

state covariance matrices at each iteration and, in general, does not provide convergence

or stability guarantees. Consequently, relevant contributions to position and attitude

estimation problems can arise by designing nonlinear observers to specific cases. For

instance, contributions to i) the position and velocity estimation using directly pseudo-

ranges and Doppler velocity from GPS signals, ii) to positioning problems resorting to

direction and velocity or acceleration measurements, and iii) to the estimation of attitude

resorting to radar or lidar and inertial measurements. In Chapter 4, a method was devised

to obtain constant feedback gains that minimize the estimates covariance in steady-state.

A possible future research direction would be the generalization and development of this

method to a broader class of nonlinear observers and the comparison of the steady-state

error covariance matrix with that resulting from using extended Kalman filters.

The nonlinear observers proposed in Chapter 3 and Chapter 4 evolve directly in the non-

Euclidean manifold SO(3). Since classical integration techniques, such as the Runge-Kutta

methods, do not preserve the underlying geometrical constraints, their computational

implementation calls for geometrically exact numerical integration technique such as the

Crouch-Grossman method, the Munthe-Kaas method and the commutator-free Lie group

method. This approach may be further developed by determining the maximum estimation

error due to numerical integration and by comparing the results with the those obtained

with observers designed directly in the discrete-time domain.

180

9.1 Future directions of research

The development of computational systems with better performances has enabled the

use of set-valued based tools in many applications where before was not possible. Nonethe-

less, the set-valued approach can greatly benefit from a judicious selection of the set

description (such as ellipsoids, hyperplane and vertices representation of polytopes and

zonotopes), such that the computational cost of critical operations such as set propagation,

intersection, and computation of the convex hull are minimized. In set-based approaches,

such as the one adopted in Chapter 7 and Chapter 8, the sensor noise is assumed to be

bounded. In some cases, this assumption may be too restrictive. Thus, further research

will be necessary to study the relaxation of this constrain and how it would affect the

estimator accuracy and stability.

181

Appendix A

Lie groups and Lie algebras

In this appendix, some definitions and results of Lie groups and Lie algebras are intro-

duced. The goal is to briefly present some elementary concepts to the reader less familiar

with these topics. The interested reader can find much more detailed and formally com-

plete introductions to Lie groups and Lie algebras in the books available on the topic, e.g.

[Bak02] and [II07].

A.1 Lie groups

Let us start by some definitions necessary to characterize a Lie group, namely, the

notion of group and of manifold.

Definition A.1. A group consists of a set of elements together with a binary operation

that originates a third element satisfying the conditions of closure, associativity, identity

and invertibility.

Definition A.2. A manifold is an n-dimensional space such that any neighbourhood of a

point is homomorphic to the Euclidean space of dimension n [MLS94, p. 403].

The notions of group and manifold can be combined to define the so called Lie groups.

Definition A.3. A Lie group is a set G that is a group and a differential manifold with

operations of multiplication and inversion that are smooth maps.

A Lie group is then a set that locally resembles an Euclidean space with the appropriate

dimensions and with an operation that satisfies the usual multiplication axioms. There

183

A. Lie groups and Lie algebras

are some Lie groups tightly related to mechanical systems. Consider a simply 2-D gravity

pendulum composed of a weight suspended on a fixed point by a rod of fixed length that,

for sake of simplicity, is assumed to be unitary. Clearly, the weight cannot occupy any

position, as it is restricted to be at a fixed distance to the point where the rod is fixed. In

fact, the set of positions where the weight can be form a circle, which is a Lie group and

is defined by

S(1) = x ∈ R2 : xTx = 1.

The set of 3× 3 invertible real matrices whose transpose is equal to its inverse is also

a Lie group. This group is the so called orthogonal group, and is denoted as O(3). The

O(3) group contains all matrices representing rotations and reflections.

O(3) = M ∈ R3×3 : MTM = I.

A very important sub-set of the orthogonal group is the group containing only rotations.

This group is called special orthogonal group and is defined by

SO(3) = R ∈ R3×3 : RTR = I,det(R) = 1.

The special orthogonal group can be used to model the attitude of rigid bodies and also

to characterize the transformations from a coordinate frame into another. Consequently,

there are a wide variety of significant applications of this Lie group in many fields of

physics and engineering such as robotics, aerospace, mechanical systems, computer vision,

among many others.

Lie groups can also represent the full rigid body configuration (rotational and trans-

lational) by combining the special orthogonal group SO(3) with R3. The configuration

of the rigid body is then described by the pair (p,R), where p ∈ R3 is the rigid body

position and R ∈ SO(3) is its attitude. The group formed by the pair (p,R) is the special

Euclidean group, which is the product space of R3 with SO(3). This group is defined by

SE(3) = (p,R) : p ∈ R3,R ∈ R

3×3,R ∈ SO(3) = R3 × SO(3).

Similarly to SO(3), SE(3) can define a rigid body motion but also a transformation of a

point from one set of coordinates into a different one. A very convenient representation

of the pair (p,R) is the homogeneous representation, which is given by

G =

[R p0 1

]

.

184

A.2 Lie algebras

A.2 Lie algebras

Deeply related with the structure of a Lie group is the associated Lie algebra, which

is the tangent space of the Lie group at the identity and is linear. Formally, it has the

following definition.

Definition A.4. Let F be a field. Then, Lie algebra is a vector space g with an operation

g× g → g : (X,Y ) → [X,Y ] called a Lie bracket, such that:

• Antisymmetry: [Y,X] = −[X,Y ]

• Bilinearity: for all a, b ∈ F and for all X,Y,Z ∈ g we have [X, aY + bZ] = a[X,Y ]+

b[X,Z].

• Satisfy the Jacobi identity: for all X,Y,Z ∈ g, [X, [Y,Z]]+ [Y, [Z,X]]+ [Z, [X,Y ]] =

0.

The Lie algebra associated with SO(3) is denoted as so(3) and is the set of skew-

symmetric real matrices, i.e.,

so(3) = K ∈ R3×3 : K = −KT.

It is easy to verify that the Lie bracket associated with so(3) defined by

[K1,K2] = K1K2 −K2K1, K1,K2 ∈ so(3),

satisfies the properties in (A.4). The Lie algebra so(3) is isomorphic to R3 and the function

(.)× : R3 → so(3) such that

(ω)× =

0 −ωz ωy

ωz 0 −ωx

−ωy ωx 0

, ω =[ωx ωy ωz

]T,

is an isomorphism. Hence, (.)× : R3 → so(3) has inverse, denoted in this thesis by

(.)⊗ : so(3) → R3 and that satisfies ((ω)×)⊗ = ω, ω ∈ R

3.

The Lie algebra associated with SE(3) is denoted by se(3) and is characterized by

se(3) = (v,K) : v ∈ R3,K ∈ so(3).

In homogeneous coordinates, the pair of (v,K) ∈ se(3) is written in the matrix form

H =

[K v0 0

]

∈ R4×4.

185


As (v,K) and H represent the same geometrical element, in this thesis, the notation

H ∈ se(3) is used as equivalent to (v,K) ∈ se(3). Moreover, let us define the isomorphism

(.)∨ : R6 → se(3) as

ξ∨ =

[(ω)× v0 0

]

∈ R4×4, ξ = [ωT vT ]T ,

where ω,v ∈ R3. The inverse of (.)∨ is denoted by (.)∧ : se(3) → R

6 and it satisfies

(ξ∨)∧ = ξ, ξ ∈ R6.

A.3 Exponential map and logarithmic map

For the sake of clarity, the following definitions are given with respect to matrix Lie

groups, i.e., groups which elements are invertible n× n matrices. Note, however, that the

concepts introduced in this section can be generalized for non-matrix Lie groups.

A crucial tool that relates the Lie group with its underlying Lie algebra is the so called

exponential map, denoted by expm(.). This function maps a Lie algebra to the associated

Lie group and, for matrix Lie groups, it coincides with the usual matrix exponential.

Hence, for matrix Lie groups, we have

expm(X) = eX.

The matrix exponential can be defined in terms of power series.

Definition A.5. Let X be an n× n matrix (real or complex). The matrix exponential of

X is given by

eX =

∞∑

n=1

Xn

n!.

Fortunately, in some cases, the exponential map can be computed using a more con-

venient alternative. In particular on SO(3) and SE(3), closed-form solutions are available.

On SO(3), we have [Sel05]

expmSO(3)((ω)×) = e(ω)× = I+sin(‖ω‖)

‖ω‖ (ω)× +(1− cos(‖ω‖))

‖ω‖2 (ω)2×, (A.1)

if ‖ω‖ 6= 0, and expmSO(3)((ω)×) = I, otherwise. The closed-form solution of the expo-

nential map on SE(3) is given by [MLS94]

expmSE(3)(ξ∨) =

[R (I−R)(ω × v) + ‖ω‖ωωTv0 1

]

,

where ξ = [ωTvT ]T and R = expmSO(3)((ω)×).

186

A.4 Actions

In general, the exponential map is neither injective or surjective [Sel05, p. 62]. However,

in a neighbourhood of the identity it is bijective. Thus, naturally, the inverse of the

exponential map, the so called logarithmic map, is only defined in a vicinity of the origin.

Similarly, to the exponential map, the logarithmic map can be defined resorting to a power

series [MLS94, p. 413]. Let g be the Lie algebra associated with the Lie group G and

assume A ∈ G. Then, the power series of the logarithm map is given by

logm(A) =

∞∑

n=1

(−1)n+1 (A− I)n

n,

where ‖A−I‖ ≤ 1. The logarithmic map also has closed-form solutions on SO(3) and SE(3).

The closed-form solution of the logarithmic map on SO(3), logmSO(3) : SO(3) → so(3), is

given by [MLS94, p. 414]

logmSO(3)(R) =

03 if θ = 0θ

2 sin(θ)(R−RT ) if θ ∈ (0, π), (A.2)

where θ = arccos ((tr(R) − 1)/2) ∈ [0, π) rad. On SE(3), the logarithmic map logmSE(3) :

SE(3) → se(3) can be computed using [MLS94, p. 414]

logmSE(3)(G) =

[(ω)× A−1p03×1 1

]

, G =

[R p0 1

]

,

where (ω)× = logmSO(3)(R) and

A−1 = I− 1

2(ω)× +

2 sin(‖ω‖)− ‖ω‖(1 + cos(‖ω‖))2‖ω‖2 sin(‖ω‖) (ω)2×,

if ω 6= 0, and A = I, otherwise.

A.4 Actions

Lie groups are very important to study symmetries of objects. In this context, the

group actions are bijective transformations of those objects.

Definition A.6. The action of a Lie group G on a manifold M is a function

α : G→M,

such that

α(E)M = M,

α(G1G2)M = α(G1)α(G2)M,

where G1,G2 ∈ G, E is the identity (neutral) element of G and M ∈M .

187


Two actions are of particular interest: i) the adjoint action of a Lie group on its Lie

algebra and ii) the adjoint action of the Lie algebra on itself. The former has the following

definition.

Definition A.7. Let G be a Lie group and g the associated Lie algebra. Then adjoint

action of a Lie group G on its Lie algebra is given by

AdgX = gXg−1, g ∈ G, X ∈ g. (A.3)

The adjoint representation on SO(3) satisfies

AdR((ω)×) = R(ω)×RT = (Rω)×,

and the adjoint representation on SE(3) can be given by

AdG((ξ)∨) =

([R 0

(p)×R R

]

ξ

)∨∈ R

3×3. (A.4)

To see that (A.4) satisfies the definition (A.3), consider the following.

AdG((ξ)∨) =

([R 0

(p)×R R

] [ω

v

])∨

=

[(Rω)× Rv − (Rω)×p

0 0

]

=

[R p0 1

] [(ω)× v0 0

] [RT −RTp0 1

]

=

[R p0 1

] [(ω)× v0 0

] [R p0 1

]−1

= G(ξ)∨G−1.

Consider now a distinct action. The adjoint action of a Lie algebra on itself is defined as

follows.

Definition A.8. Let g be the Lie algebra associated with the Lie group G. Then, the

adjoint action of the Lie algebra on itself is given by

adXY = XY −YX = [X,Y].

The adjoint action of so(3) on itself is given by

ad(ω1)×(ω2)× = (ω1)×(ω2)× − (ω2)×(ω1)×

= ((ω1)×ω2)×.

188

A.4 Actions

On the special Euclidean group, the adjoint action of se(3) on itself is computed using

ad(ξ1)∨((ξ2)∨) = (ξ1)

∨(ξ2)∨ − (ξ2)

∨(ξ1)∨

=

[(ω1)× v1

0 0

] [(ω2)× v2

0 0

]

−[(ω2)× v2

0 0

] [(ω1)× v1

0 0

]

=

([(ω1)× 0(v1)× (ω1)×

] [ω2

v2

])∨,

where ξ1 = [ωT1 vT

1 ]T , and ξ2 = [ωT

2 vT2 ]

T .

189

Appendix B

Numerical integration on Lie

groups

The topic of numerical integration is a mathematical field of research with much liter-

ature specially dedicated to it (see e.g. [HLW06]). Thus, this appendix is not intended to

present an in-deep coverage of such methods. Its objective is rather to present some details

about integration methods specially tailored to preserve the geometric properties of the

underlying Lie group. In particular, three Lie group integrators will be described, namely,

the Crouch–Grossman method (CG), the Munthe–Kaas method (MK), and commutator-

free method (CF).

B.1 Crouch-Grossman method

The tangent space of a Lie group G (with Lie algebra g) at Y ∈ G, denoted as TYG,

is given by [HLW06]

TYG = AY|A ∈ g.

Thus, a differential equation on a Lie group G can be written as

Y = A(t)Y, Y(to) = Yo, (B.1)

where A(t) ∈ g.

Consider the differential equation (B.1). If tr(A) = 0 (as in the case of so(3) and

se(3)), the determinant of Y is invariant [HLW06, Lemma 3.1]. On the other hand, it

is known that no Runge-Kutta method can preserve all polynomial invariants of degree

191

B. Numerical integration on Lie groups

greater than n, where n ≥ 3 [HLW06, Theorem 3.3], which renders the standard Runge-

Kutta methods unfit to numerically integrate differential equations evolving on SO(3) and

SE(3). One possibility to address this issue is to resort to local coordinates. However, this

can be problematic if the system trajectory is not contained in the subspace where the

local representation is valid. An alternative is to embed the system in some higher-order

manifold and perform there the integration. The difficulty associated with this approach

is the necessity to project the numerical solution back into the original manifold. Hence,

the numerical integration of (B.1) calls of specially tailored methods that preserve the

geometric properties of the underlying Lie group.

In 1993, Crouch and Grossman proposed a numerical solution to integrate ordinary

differential equations on manifolds based on flows of vector fields [CG93]. Consider the

following differential equation

Y = F (Y), with Y(to) = Yo.

Let TYM be the tangent space of manifold M at point Y and assume that the vector

fields E1(Y), . . . ,EN (Y) form a basis of TYM such that

TYM = span(E1, . . . ,EN ).

At point Y ∈M , we have that, for some functions fn :M → R

F (Y) =

N∑

n=1

fn(Y)En(Y).

Crouch and Grossman introduced the notion of frozen coefficients at X ∈ M , where

fn(X) = an, that are given by

FX(Y) =N∑

n=1

anEn(Y). (B.2)

The main assumption of Crouch and Grossman in the development of numerical integration

algorithms on manifolds is the possibility of computing the flow (t,Y) of any vector field of

the form (B.2) either exactly or with any desired accuracy. It turns out that, for equations

evolving on Lie groups in the form of (B.1), such flow is computed using the exponential

map. Hence, the algorithm proposed in [CG93] with s stages takes the form [HLW06,

p. 124]

Y(i) = expm(hai,i−1Ki−1) . . . expm(hai,1K1−1)Yn, Ki = A(tn + hci,Y(i)),

Yn+1 = expm(hbsKs) . . . expm(hb1K1)Yn,

192

B.2 Munthe-Kaas method

c1 a11 . . . a1sc2 a21 . . . a2s...

......

cs b1 . . . bs

Table B.1: Butcher tableau.

11 1

12

12

Table B.2: Coefficients of a second-order CG method.

034

34

1724

119216

17108

1351 −2

3247

Table B.3: Coefficients of a third-order CG method.

where i, j = 1, . . . , s, and ai,j and bi are real numbers and constitute the algorithm co-

efficients. The accuracy of a numerical integration method is indicated by its order or

degree of accuracy which depends on the method coefficients ai,j , ci, bi, i, j = 1, . . . , s. A

method has order p if the difference between the true integral and the method result is

O(hp+1) [HLW06]. The coefficients of an s-stage method can be represented in a Butcher

tableau in the form of Table B.1. Coefficients of third-order CG methods are given in

[CG93]. Note that first- and second-order algorithms have coefficients identical to the

first- and second-order standard Runge-Kutta method, respectively. Table B.2 and ta-

ble B.3 present coefficients for second- and third- order methods. Posterior works were

able to obtain coefficients for fourth- and higher-order methods [OM99, JMO00].

B.2 Munthe-Kaas method

After the development of the CG methods, Munthe-Kaas proposed a solution that

directly integrate (B.1) resorting to its solution on the Lie algebra [MK99]. Let

Y(t) = expm(Ω(t))Yo,

for some appropriate Ω(t) be the solution of (B.1). The idea of the MK methods is to

solve numerically the differential equation for Ω(t). The time evolution of Ω(t) can be

characterized by the differential equation

Ω = dexpm-1Ω(A(Y(t))), Ω(to) = 0,

193

B. Numerical integration on Lie groups

11 1

12

12

Table B.4: Coefficients of a second-order MK method.

013

13

23 0 2

314 0 3

4

Table B.5: Coefficients of a third-order MK method.

where dexpm-1Ω(A) denotes the inverse of the derivative of the exponential map which

can be computed in terms of the expansion series [HLW06]

dexpm-1Ω(A) =

∞∑

n=1

bnn!

adΩn(A),

where bn are the Bernoulli numbers. Interesting enough, the inverse of the differential of

the exponential map has a closed-form solution on so(3) given by [IMKZ00]

dexpm-1ω = I− 1

2(ω)× −

‖ω‖ cot(‖ω‖2

)

− 2

2‖ω‖2 (ω)2×. (B.3)

Since Ω belongs to so(3), which is a linear space, the conventional Runge-Kutta methods

can be used with the corresponding standard coefficients. This results in the following

algorithm. [PC05]

Θ(i) = h

i−1∑

j=1

aijF(j),

Y(i) = expm(

Θ(i))

Yk,

F(i) = dexpm-1(

Θ(i))

A(

tk + hci,Y(i))

,

Yk+1 = expm

(

h

s∑

i=1

biF(i)

)

.

Coefficients for second- and third-order methods are given in Table B.4 and Table B.5,

respectively.

B.3 Commutator-free method

The MK methods are very attractive as they solve the numerical integration problem

in the Lie algebra which is a linear space. Hence, standard Runge-Kutta methods can be

194

B.3 Commutator-free method

11 1

12

12

Table B.6: Coefficients of a second-order CF method.

013

13

23 0 2

313 0 0- 112 0 3

4

Table B.7: Coefficients of a third-order CF method.

used to integrate the linear part. However, to use the Lie algebra representation, the MK

methods evolves the computation of the adjoint action of the Lie algebra on itself which

uses commutators. A third alternative is proposed in [CMO03], which avoids the use of

commutators and, simultaneously, has fewer flow computations than the CG method. In

fact, the CG methods are part of the bigger class of CF methods. The CF methods have

the following algorithm

K(i) = A(

tk + hci,Y(i))

, (B.4)

Y(i) = expm

∑

j

ha[k]i,kK

j

. . . expm

∑

j

ha[1]i,jK

(j)

,

Yk+1 = expm

∑

j

hβ[k]j Kj

. . . expm

∑

j

hβ[1]j K(j)Yk

,

for i = 1, . . . , s where a[k]i,k, βj ,ci are the method coefficients.

Similarly to the CG methods, the coefficients of the standard Runge-Kutta methods

are not valid for CF methods and specially tailored order conditions had been developed

to obtain suitable ones [CMO03]. Coefficients of second- and third-order CF methods are

given in Table B.6 and Table B.7, respectively. Note that Table B.7 is not exactly in

the format of Table B.1, since there is a double row of bi coefficients. This is usual in CF

methods. In (B.4), the row of the coefficient is indicated by j in b[j]i .

195

Appendix C

Bound on the exponential map of

the sum of two skew-symmetric

matrices

In this appendix, a bound on the exponential map of the sum of two skew-symmetric

matrices is derived, when only bounds are known for one term of the product.

Let k1 ∈ R3 and k2 ∈ R

3 be two generic vectors, and let us define the skew-symmetric

matrices K1 = (k1)×, K2 = (k2)×. From the definition of matrix multiplication, we have

that [C]ij =∑3

k=1[K1]ik[K2]kj , where C = K1K2 ∈ R3×3. Using the fact that, for skew-

symmetric matrices, at least one of the elements of each row and each column is zero, the

following inequalities are obtained

‖Kk1‖max ≤ (2k1)

k

2, ‖Kk

1Kl2‖max ≤ (2k1)

k(2k2)l

2,

where k1 = ‖k1‖∞ and k2 = ‖k2‖∞. These inequalities yield an upper bound for each

element of the power of the sum of two matrices

[

(K1 +K2)k]

ij≤[

Kk1 +

1

213×3((k1 + k2)

k − kk1 )

]

ij

.

Consequently, the exponential map of the sum of the matrices K1 and K2 satisfies

[expmSO(3)(K1 +K2)

]

ij=

∞∑

k=0

[(K1 +K2)

k]

ij

k!(C.1)

≤[expmSO(3)(K1)

]

ij+

1

2(e2k1+2k2 − e2k1).

197

References

[ABA12] M.Z. Ali Bhotto and A. Antoniou. Robust set-membership affine-projectionadaptive-filtering algorithm. IEEE Transactions on Signal Processing,60(1):73–81, 2012.

[AD03] A. Ansar and K. Daniilidis. Linear pose estimation from points or lines. IEEETransactions on Pattern Analysis and Machine Intelligence, 25(5):578–589,May 2003.

[AF90] J.-P. Aubin and H. Frankowska. Set-Valued Analysis. Birkhauser, Boston,1990.

[AH06] A. Aguiar and J. Hespanha. Minimum-energy state estimation for sys-tems with perspective outputs. IEEE Transactions on Automatic Control,51(2):226–241, 2006.

[AHB87] K.S. Arun, T.S. Huang, and S.D. Blostein. Least-squares fitting of two 3-Dpoint sets. IEEE Transactions on Pattern Analysis and Machine Intelligence,9(5):698–700, Sep. 1987.

[AM79] B. Anderson and J. Moore. Optimal Filtering. Dover Publications, 1979.

[AP11] M. Abdelrahman and Sang-Young Park. Simultaneous spacecraft attitudeand orbit estimation using magnetic field vector measurements. AerospaceScience and Technology, 15(8):653–669, 2011.

[Apo67] T.M. Apostol. Calculus - One-Variable Calculus, with an Introduction toLinear Algebra, volume I. John Wiley & Sons, Inc., 2nd edition, 1967.

[Arn13] D. Arnold. The Age of Discovery, 1400-1600. Lancaster Pamphlets. Taylor& Francis, 2013.

[ASS09] I. Al-Shyoukh and J.S. Shamma. Switching supervisory control using cali-brated forecasts. IEEE Transactions on Automatic Control, 54(4):705–716,2009.

[ASZ06] M. Akella, D. Seo, , and R. Zanetti. Attracting manifolds for attitude esti-mation in flatland and otherlands. The Journal of the Astronautical Sciences,54((3,4)):635––655, Dec. 2006.

[Aub01] J.-P. Aubin. Viability Theory. Birkhauser Boston, 2nd edition, 2001.

199

References

[Bak02] A. Baker. Matrix Groups: An Introduction to Lie Group Theory. SpringerUndergraduate Mathematics Series. Springer-Verlag London, 2002.

[BB00] S.P. Bhat and D.S. Bernstein. A topological obstruction to continuous globalstabilization of rotational motion and the unwinding phenomenon. Systems& Control Letters, 39(1):63–70, 2000.

[BB04] J. Bokor and G. Balas. Detection filter design for LPV systems – a geometricapproach. Automatica, 40:511–518, 2004.

[BB13] A. Barrau and S. Bonnabel. Intrinsic filtering on SO(3) with discrete-timeobservations. In IEEE 52nd Conference on Decision and Control (CDC),pages 3255–3260, Florence, Italy, Dec. 2013.

[BB15] A. Barrau and S. Bonnabel. Intrinsic filtering on Lie groups with applicationsto attitude estimation. IEEE Transactions on Automatic Control, 60(2):436–449, Feb. 2015.

[BD09] W.F. Bruyns and R. Dunn. Sextants at Greenwich: A Catalogue of theMariner’s Quadrants, Mariner’s Astrabes, Cross-staffs, Backstaffs, Octants,Sextants, Quintants, Reflecting Circles and Artificial Horizons in the NationalMaritime Museum, Greenwich. OUP Oxford, 2009.

[Bek07] E. Bekir. Introduction to Modern Navigation Systems. World Scientific, 2007.

[BF93] R.L. Burden and J.D. Faires. Numerical Analysis. PWS-KENT PublishingCompany, Boston, 1993.

[BIZBL97] M. Blanke, R. Izadi-Zamanabadi, S.A. Bogh, and C. Lunau. Fault tolerantcontrol systems – a holistic view. Control Engineering Practice, 5(5):693–702,1997.

[BKL+06] M. Blanke, M. Kinnaert, J. Lunze, M. Staroswiecki, and J. Schroder. Diag-nosis and Fault-Tolerant Control. Springer, 2006.

[BL04] F. Bullo and A.D. Lewis. Geometric Control of Mechanical Systems, vol-ume 49 of Texts in Applied Mathematics. Springer Verlag, New York-Heidelberg-Berlin, 2004.

[BM95] F. Bullo and R.M. Murray. Proportional derivative (PD) control on theEuclidean group. In European Control Conference, volume 2, pages 1091–1097, Rome, Italy, Jun. 1995.

[BM09] A. Ben-Menahem. Historical Encyclopedia of Natural and MathematicalSciences. Historical Encyclopedia of Natural and Mathematical Sciences.Springer, 2009.

[BMR08] S. Bonnabel, P. Martin, and P. Rouchon. Symmetry-preserving observers.IEEE Transactions on Automatic Control, 53(11):2514–2526, Dec. 2008.

[BMR09] S. Bonnabel, P. Martin, and P. Rouchon. Non-linear symmetry-preserving ob-servers on Lie groups. IEEE Transactions on Automatic Control, 54(7):1709–1713, Jul. 2009.

200

References

[BMS09] S. Bonnabel, P. Martin, and E. Salaun. Invariant extended Kalman filter: the-ory and application to a velocity-aided attitude estimation problem. In 48thIEEE Conference Decision and Control, 2009 held jointly with the 2009 28thChinese Control Conference. CDC/CCC 2009, pages 1297–1304, Shanghai,China, Dec. 2009.

[BP03] C.J. Budd and M.D. Piggott. Geometric Integration and its Applications,volume 11 of Handbook of Numerical Analysis. Elsevier, 2003.

[BR71] D.P. Bertsekas and I.B. Rhodes. Recursive state estimation for a set-membership description of uncertainty. IEEE Transactions on AutomaticControl, AC-16(2):117–128, 1971.

[BS04] M. Bryson and S. Sukkarieh. Vehicle model aided inertial navigation for aUAV using low-cost sensors. In Australasian Conference on Robotics andAutomation (ACRA), Canberra, Australia, 2004.

[BSK+06] M. Blanke, J. Schroder, M. Kinnaert, J. Lunze, and M. Staroswiecki. Diag-nosis and Fault-Tolerant Control. Springer, 2006.

[BSO10] P. Batista, C. Silvestre, and P. Oliveira. Optimal position and velocity navi-gation filters for autonomous vehicles. Automatica, 46(4):767–774, 2010.

[BSO11a] P. Batista, C. Silvestre, and P. Oliveira. Single range aided navigation andsource localization: Observability and filter design. Systems & Control Let-ters, 60(8):665–673, 2011.

[BSO11b] P. Batista, C. Silvestre, and P. Oliveira. Vector-based attitude filter for spacenavigation. Journal of Intelligent & Robotic Systems, 64(2):221–243, 2011.

[BSO12a] P. Batista, C. Silvestre, and P. Oliveira. A GES attitude observer with singlevector observations. Automatica, 48(2):388–395, 2012.

[BSO12b] P. Batista, C. Silvestre, and P. Oliveira. Globally exponentially stable cascadeobservers for attitude estimation. Control Engineering Practice, 20(2):148–155, 2012.

[BSO12c] P. Batista, C. Silvestre, and P. Oliveira. Sensor-based globally asymptoti-cally stable filters for attitude estimation: Analysis, design, and performanceevaluation. IEEE Transactions on Automatic Control, 57(8):2095–2100, Aug.2012.

[BSW01] M. Blanke, M. Staroswiecki, and N.E. Wu. Concepts and methods in fault-tolerant control. In 2001 American Control Conference, Jun. 2001.

[BV04] S. Boyd and L. Vandenberghe. Convex Optimization. Cambridge UniversityPress, Mar. 2004.

[Cal01] G. Calafiore. A set-valued non-linear filter for robust localization. In EuropeanControl Conference (ECC’01), Porto, Portugal, 2001.

[Car88] N.A. Carlson. Federated filter for fault-tolerant integrated navigation systems.In Position Location and Navigation Symposium, 1988. Record. Navigationinto the 21st Century. IEEE PLANS ’88., IEEE, pages 110–119, Nov. 1988.

[CBIO06] D. Choukroun, I.Y. Bar-Itzhack, and Y. Oshman. Novel quaternion Kalmanfilter. IEEE Transactions on Aerospace and Electronic Systems, 42(1):174–190, Jan. 2006.

201

References

[CC05] N. Cowan and D. Chang. Geometric visual servoing. IEEE Transactions onRobotics, 21(6):1128–1138, Dec. 2005.

[CFG+97] H. Comisel, M. Forster, E. Georgescu, M. Ciobanu, V. Truhlik, and J. Vojta.Attitude estimation of a near-Earth satellite using magnetometer data. ActaAstronautica, 40(11):781–788, 1997.

[CFSG09] M. Carminati, G. Ferrari, M. Sampietro, and R. Grassetti. Fault detectionand isolation enhancement of an aircraft attitude and heading reference sys-tem based on MEMS inertial sensors. Procedia Chemistry, 1(1):509–512, 2009.

[CG93] P.E. Crouch and R. Grossman. Numerical integration of ordinary differentialequations on manifolds. Journal of Nonlinear Science, 3:1–33, 1993.

[CGLP05] A. Caiti, A. Garulli, F. Livide, and D. Prattichizzo. Localization of autono-mous underwater vehicles by floating acoustic buoys: a set-membership ap-proach. IEEE Journal of Oceanic Engineering, 30(1):140–152, 2005.

[CH06] F. Chaumette and S. Hutchinson. Visual servo control, part I: Basic ap-proaches. IEEE Robotics and Automation Magazine, 13(4):82–90, Dec. 2006.

[CH07] F. Chaumette and S. Hutchinson. Visual servo control, part II: Advancedapproaches. IEEE Robotics and Automation Magazine, 14(1):109–118, Mar.2007.

[CM06] N. Chaturvedi and N. McClamroch. Almost global attitude stabilization ofan orbiting satellite including gravity gradient and control saturation effects.In 2006 American Control Conference, Minneapolis, MN, USA, Jun. 2006.

[CMO03] E. Celledoni, A. Marthinsen, and B. Owren. Commutator-free Lie groupmethods. Future Generation Computer Systems, 19(3):341–352, Apr. 2003.

[CMP89] R.J. Caron, J.F. McDonald, and C.M. Ponic. A degenerate extreme pointstrategy for the classification of linear constraints as redundant or necessary.Journal of Optimization Theory and Applications, 62:225–237, 1989.

[Coo99] P.A. Cook. Stability of linear systems with slowly changing parameters. C.S. C. Report No. 881, Control Systems Centre. Department of ElectricalEngineering and Electronics, UMIST, Manchester, U.K., Jun. 1999.

[Cra89] J.J. Craig. Introduction to Robotics: Mechanics and Control. Addison-Wesley,2nd edition, 1989.

[Cra06] J. Crassidis. Sigma-point Kalman filtering for integrated GPS and iner-tial navigation. IEEE Transactions on Aerospace and Electronic Systems,2(42):750–756, Apr. 2006.

[CT01] E.G. Jr. Collins and S. Tinglun. Robust l1 estimation using the Popov–Tsypkin multiplier with application to robust fault detection. InternationalJournal of Control, 74(3):303–313, 2001.

[Cun07] R. Cunha. Advanced motion control for autonomous air vehicles. PhD thesis,Instituto Superior Tecnico, Lisbon, 2007.

[CV04] G. Chesi and A. Vicino. Visual servoing for large camera displacements. IEEETransactions on Robotics, 20(4):724–735, 2004.

202

References

[CWK02] N. Cowan, J. Weingarten, and D. Koditschek. Visual servoing via navigationfunctions. IEEE Transactions on Robotics and Automation, 18(4):521–533,2002.

[Dav07] A.J. Davison. MonoSLAM: Real-time single camera SLAM. IEEE Trans-actions on Pattern Analysis and Machine Intelligence, 29(6):1052–1067, Jun.2007.

[DH87] S. Dasgupta and Yih-Fang Huang. Asymptotically convergent modified re-cursive least-squares with data-dependent updating and forgetting factor forsystems with bounded noise. IEEE Transactions on Information Theory,33(3):383–392, 1987.

[DHF09] D.G. Dimogianopoulos, J.D. Hios, and S.D. Fassois. FDI for aircraft systemsusing stochastic pooled-NARMAX representations: Design and assessment.IEEE Transactions on Control Systems Technology, 17(6):1385–1397, Nov.2009.

[DPI01] C. De Persis and A. Isidori. A geometric approach to nonlinear fault detectionand isolation. IEEE Transactions on Automatic Control, 46(6):853–865, 2001.

[DS95] R.K. Douglas and J.l. Speyer. Robust fault detection filter design. In 1995American Control Conference, volume 1, pages 91–96, Seattle, WA, USA, Jun.1995.

[Duc09] G. Ducard. Fault-tolerant Flight Control and Guidance Systems: PracticalMethods for Small Unmanned Aerial Vehicles. Advances in Industrial Control.Springer, 2009.

[ES04] J.A.A. Engelbrecht and W.H. Steyn. In-orbit identification of unmodelled dis-turbance torques acting on a spacecraft body. In 7th AFRICON Conferencein Africa, volume 1, pages 3–8, Sep. 2004.

[Est04] A.M. Esteban. Aircraft Applications of Fault Detection and Isolation Tech-niques. PhD thesis, University of Minnesota, 2004.

[Far70] J.L. Farrell. Attitude determination by Kalman filtering. Automatica,6(3):419–430, 1970.

[Far08] J. Farrell. Aided Navigation: GPS with High Rate Sensors. McGraw-Hill,Inc., New York, NY, USA, 1st edition, 2008.

[FD94] P.M Frank and X. Ding. Frequency domain approach to optimally robustresidual generation. Automatica, 30(5):789–804, 1994.

[FD97] P.M. Frank and X. Ding. Survey of robust residual generation and evalua-tion methods in observer-based fault detection systems. Journal of ProcessControl, 7(6):403–424, 1997.

[FH82] E. Fogel and Y.F. Huang. On the value of information in systemidentification–bounded noise case. Automatica, 18(2):229–238, 1982.

[FI04] D. Fragopoulos and M. Innocenti. Stability considerations in quaternion at-titude control using discontinuous Lyapunov functions. IEEE Proceedings onControl Theory and Applications, 151(3):253–258, May 2004.

203

References

[Fia10] M. Fiala. Designing highly reliable fiducial markers. IEEE Transactions onPattern Analysis and Machine Intelligence, 32(7):1317–1324, Jul. 2010.

[GFJS12] H.F. Grip, T.I. Fossen, T.A. Johansen, and A. Saberi. Attitude estimation us-ing biased gyro and vector measurements with time-varying reference vectors.IEEE Transactions on Automatic Control, 57(5):1332–1338, May 2012.

[GM72] J.P. Gilmore and R.A. McKern. A redundant strapdown inertial unit (SIRU).J. Spacecraft, 9(1):39–47, 1972.

[GO09] T. Gaspar and P. Oliveira. A single pan and tilt camera architecture forindoor positioning and tracking. In Proc. VISIGRAPP, Lisbon, Portugal,2009.

[GSJ12] H.F. Grip, A. Saberi, and T.A. Johansen. Observers for interconnected non-linear and linear systems. Automatica, 48(7):1339–1346, 2012.

[GTJS15] H.F. Grip, Fossen. T.I., T.A. Johansen, and A. Saberi. Globally exponentiallystable attitude and gyro bias estimation with application to GNSS/INS inte-gration. Automatica, 51(0):158–166, 2015.

[GVL96] G.H. Golub and C.F. Van Loan. Matrix Computations. Johns Hopkins Uni-versity Press, Baltimore, MD, USA, 3rd edition, 1996.

[Ham44] W. Hamilton. On a new species of imaginary quantities connected with atheory of quaternions. Proceedings of the Royal Irish Academy, 2:424–434,Nov. 1844.

[HD73] J.C. Hung and B.J. Doran. High-reliability strapdown platforms using two-degree-of-freedom gyros. IEEE Transactions on Aerospace and ElectronicSystems, AES-9(2):253–259, Mar. 1973.

[HH11] O. Hegrenaes and O. Hallingstad. Model-aided INS with sea current estima-tion for robust underwater navigation. IEEE Journal of Oceanic Engineering,36(2):316–337, Apr. 2011.

[HKEY99] H. Hammouri, M. Kinnaert, and E.-H. El Yaagoubi. Observer-based approachto fault detection and isolation for nonlinear systems. IEEE Transactions onAutomatic Control, 44(10):1879–1884, 1999.

[HKKES10] I. Hwang, S. Kim, Y. Kim, and Chze Eng Seah. A survey of fault detec-tion, isolation and reconfiguration methods. IEEE Transactions on ControlSystems Technology, 18(3):636–653, 2010.

[HLW06] E. Hairer, C. Lubich, and G. Wanner. Geometric Numerical Integration,Structure-Preserving Algorithms for Ordinary Differential Equations, vol-ume 31 of Springer Series in Computational Mathematics. Springer, 2ndedition, 2006.

[HMS07] T. Harada, T. Mori, and T. Sato. Control for localization of targets us-ing range-only sensors. The International Journal of Robotics Research,26(6):547–559, Jun. 2007.

[Hua10] Minh-Duc Hua. Attitude estimation for accelerated vehicles using GPS/INSmeasurements. Control Engineering Practice, 18(7):723–732, 2010. SpecialIssue on Aerial Robotics.

204

References

[HZT+11] Minh-Duc Hua, M. Zamani, J. Trumpf, R. Mahony, and T. Hamel. Observerdesign on the special Euclidean group SE(3). In IEEE 50th Conference onDecision and Control and 2011 European Control Conference (ECC), pages8169–8175, Dec. 2011.

[IA06] Inc. Ideal Aerosmith. 2103HT Multi-Axis Table Data Sheet, Rev C, 2006.http://www.ideal-aerosmith.com/.

[II07] V.G. Ivancevic and T.T. Ivancevic. Applied Differential Geometry: a ModernIntroduction. World Scientific, 2007.

[IMKZ00] A. Iserles, H. Munthe-Kaas, and A. Zanna. Lie-group methods. Acta Numer-ica, 9:1–148, 2000.

[Ise97] R. Isermann. Supervision, fault-detection and fault-diagnosis methods - anintroduction. Control Engineering Practice, 5(5):639–652, 1997.

[ISM03] A. Isidori, A. Serrani, and L. Marconi. Robust autonomous guidance: aninternal model approach. Advances in industrial control. Springer, Berlin,2003.

[JKT08] T. Jiang, K. Khorasani, and S. Tafazoli. Parameter estimation-based faultdetection, isolation and recovery for nonlinear satellite models. IEEE Trans-actions on Control Systems Technology, 16(4):799–808, Jul. 2008.

[JMO00] Z. Jackiewicz, A. Marthinsen, and B. Owren. Construction of Runge–Kuttamethods of Crouch–Grossman type of high order. Advances in ComputationalMathematics, 13(4):405–415, 2000.

[JZ99] Hong Jin and Hong Yue Zhang. Optimal parity vector sensitive to desig-nated sensor fault. IEEE Transactions on Aerospace and Electronic Systems,35(4):1122–1128, Oct. 1999.

[KB61] R.E. Kalman and R.S Bucy. New results in linear filtering and predictiontheory. Transactions of the ASME - Journal of Basic Engineering, 83:95–108, 1961.

[KG87] S. Keerthi and E. Gilbert. Computation of minimum-time feedback controllaws for discrete-time systems with state-control constraints. IEEE Transac-tions on Automatic Control, AC-32(5):432–435, 1987.

[Kha02] H.K. Khalil. Nonlinear Systems. Prentice Hall, New Jersey, 3rd edition, 2002.

[Kod89] D.E. Koditschek. The Application of Total Energy as a Lyapunov Functionfor Mechanical Control Systems. Control Theory and Multibody Systems,97:131–151, 1989.

[LD03] J. Lobo and J. Dias. Vision and inertial sensor cooperation using gravity asa vertical reference. IEEE Transactions on Pattern Analysis and MachineIntelligence, 25(12):1597–1608, Dec. 2003.

[LLMS07] T. Lee, M. Leok, M. McClamroch, and A.K. Sanyal. Global attitude es-timation using single direction measurements. In 2007 American ControlConference, New York, USA, Jul. 2007.

[LM82] E.J. Lefferts and M.D. Markley, F.L. Shuster. Kalman filtering for spacecraftattitude estimation. Journal of Guidance, Control, and Dynamics, 5(5):417–429, Sep.-Oct. 1982.

205

References

[LM09] S. Longhi and A. Moteriu. Fault detection for linear periodic systems using ageometric approach. IEEE Transactions on Automatic Control, 54(7):1637–1643, Jul. 2009.

[LO10] Yangming Li and E.B. Olson. Extracting general-purpose features from LI-DAR data. In IEEE International Conference on Robotics and Automation,pages 1388–1393, Anchorage, AK, USA, May 2010.

[LP02] A. Lorıa and E. Panteley. Uniform exponential stability of linear time-varyingsystems: revisited. Systems and Control Letters, 47(1):13–24, Sep. 2002.

[LTM10] C. Lageman, J. Trumpf, and R. Mahony. Gradient-like observers for invari-ant dynamics on a Lie group. IEEE Transactions on Automatic Control,55(2):367–377, Feb. 2010.

[LW99] W.J. Larson and J.R. Wertz. Spacecraft Mission Analysis and Design. Mi-crocosm, Inc., New York, USA, 3rd edition, 1999.

[LZA03] H. Lin, G. Zhai, and P.J. Antsaklis. Set-valued observer design for a classof uncertain linear systems with persistent disturbance. In 2003 AmericanControl Conference, volume 3, pages 1902–1907, Denver, CO, USA, Jun. 2003.

[Maj13] F. Major. Quo Vadis: Evolution of Modern Navigation: The Rise of QuantumTechniques. Springer New York, 2013.

[Mar03] F.L. Markley. Attitude error representations for Kalman filtering. Journal ofGuidance Control and Dynamics, 26(2):311–317, 2003.

[May99] P.S. Maybeck. Multiple model adaptive algorithms for detecting and com-pensating sensor and actuator/surface failures in aircraft flight control sys-tems. International Journal of Robust and Nonlinear Control, 9(14):1051–1070, 1999.

[MB82] M. Milanese and G. Belforte. Estimation theory and uncertainty intervalsevaluation in presence of unknown but bounded errors: Linear families ofmodels and estimators. IEEE Transactions on Automatic Control, 27(2):408–414, 1982.

[MBD04] D.H.S. Maithripala, J.M. Berg, and W.P. Dayawansa. An intrinsic observerfor a class of simple mechanical systems on a Lie group. In 2004 AmericanControl Conference, pages 1546–1551, Boston, MA, USA, Jun. 2004.

[MBOS11] M. Morgado, P. Batista, P. Oliveira, and C. Silvestre. Position USBL/DVLsensor-based navigation filter in the presence of unknown ocean currents. Au-tomatica, 47(12):2604–2614, 2011.

[MC02] E. Malis and F. Chaumette. Theoretical improvements in the stability analy-sis of a new class of model-free visual servoing methods. IEEE Transactionson Robotics and Automation, 18(2):176–186, 2002.

[MDL06] R. Mattone and A. De Luca. Relaxed fault detection and isolation: Anapplication to a nonlinear case study. Automatica, 42:109–116, 2006.

[MHP08] R. Mahony, T. Hamel, and J.-M. Pflimlin. Nonlinear complementary filterson the special orthogonal group. IEEE Transactions on Automatic Control,53(5):1203–1218, Jun. 2008.

206

References

[MK98] H. Munthe-Kaas. Runge-Kutta methods on Lie groups. BIT NumericalMathematics, 38(1):92–11, 1998.

[MK99] H. Munthe-Kaas. High order Runge-Kutta methods on manifolds. AppliedNumerical Mathematics, 29(1):115–127, 1999.

[MKS06] M. Malisoff, M. Krichman, and E. Sontag. Global stabilization for systemsevolving on manifolds. Journal of Dynamical and Control Systems, 12(2):161–184, Apr. 2006.

[MLS94] R.M. Murray, Z. Li, and S.S. Sastry. A Mathematical Introduction to RoboticManipulation. CRC, 1994.

[MM00] F.L. Markley and M. Mortari. Quaternion attitude estimation using vectormeasurements. The Journal of the Astronautical Sciences, 48(2):359–380,Apr.-Sep. 2000.

[MN04] M. Milanese and C Novara. Set membership identification of nonlinear sys-tems. Automatica, 40(6):957–975, 2004.

[MN07] L. Marconi and R. Naldi. Robust full degree-of-freedom tracking control of ahelicopter. Automatica, 43(11):1909–1920, 2007.

[MS91] P.S. Maybeck and R.D. Stevens. Reconfigurable flight control via multiplemodel adaptive control methods. IEEE Transactions on Aerospace and Elec-tronic Systems, 27(3):470–480, May 1991.

[MS10] P. Martin and E. Salaun. Design and implementation of a low-cost observer-based attitude and heading reference system. Control Engineering Practice,18(7):712–722, 2010.

[MTR+09] A.I. Mourikis, N. Trawny, S.I. Roumeliotis, A.E. Johnson, A. Ansar, andL. Matthies. Vision-aided inertial navigation for spacecraft entry, descent,and landing. IEEE Transaction on Robotics, 25(2):264–280, 2009.

[MV91] M. Milanese and A. Vicino. Optimal estimation theory for dynamic systemswith set membership uncertainty: an overview. Automatica, 27(6), 1991.

[NBL+09] H.V. Nguyen, C. Berbra, S. Lesecq, S. Gentil, A. Barraud, and C. Godin.Diagnosis of an inertial measurement unit based on set membership estima-tion. In 17th Mediterranean Conference on Control and Automation, pages211–216, Thessaloniki, Greece, 2009.

[NF99] H. Nijmeijer and T. Fossen. New Directions in Nonlinear Observer Design.Springer-Verlag, 1999.

[NS13] M. Namvar and F. Safaei. Adaptive compensation of gyro bias in rigid-bodyattitude estimation using a single vector measurement. IEEE Transactionson Automatic Control, 58(7):1816–1822, Jul. 2013.

[NVR08] S. Narasimhan, P. Vachhani, and R. Rengaswamy. New nonlinear resid-ual feedback observer for fault diagnosis in nonlinear systems. Automatica,44:2222–2229, 2008.

[OM99] B. Owren and A. Marthinsen. Runge-Kutta methods adapted to manifoldsand based on rigid frames. BIT Numerical Mathematics, 39(1):116–142, 1999.

207

References

[Owr06] B. Owren. Order conditions for commutator-free Lie group methods. Journalof Physics A: Mathematical and General, 39:5585–5599, 2006.

[Pat91] R.J. Patton. Fault detection and diagnosis in aerospace systems using ana-lytical redundancy. Computing & Control Engineering Journal, 2(3):127–136,1991.

[PBLR02] F. Prieto, P. Boulanger, R. Lepage, and T. Redarce. Automated inspectionsystem using range data. In IEEE International Conference on Robotics &Automation, Washington, DC, USA, May 2002.

[PC97] R.J. Patton and J. Chen. Observer-based fault detection and isolation: Ro-bustness and applications. Control Engineering Practice, 5(5):671–682, 1997.

[PC05] Jonghoon Park and Wan-Kyun Chung. Geometric integration on Euclideangroup with application to articulated multibody systems. IEEE Transactionson Robotics, 21(5):850–863, Oct. 2005.

[PCB00] N. Priyantha, A. Chakraborty, and H. Balakrishnan. The Cricket location-support system. In Sixth Annual ACM International Conference on MobileComputing and Networking, Aug. 2000.

[PHS07] J. Pflimlin, T. Hamel, and P. Soueres. Nonlinear attitude and gyroscope’sbias estimation for a VTOL UAV. International Journal of Systems Science,38(3):197–210, Mar. 2007.

[PWA10] L.D.L. Perera, W.S. Wijesoma, and M.D. Adams. SLAM with joint sen-sor bias estimation: Closed form solutions on observability, error boundsand convergence rates. IEEE Transactions on Control Systems Technology,18(3):732–740, May 2010.

[RCAL11] S. Rodrigues, N. Crasta, A. Aguiar, and F. Leite. An exponential observer forsystems on SE(3) with implicit outputs. In Dynamics, Games and Science II,volume 2 of Springer Proceedings in Mathematics, pages 611–633. SpringerBerlin Heidelberg, 2011.

[RG03] H. Rehbinder and B. Ghosh. Pose estimation using line-based dynamic visionand inertial sensors. IEEE Transactions on Automatic Control, 48(2):186–199, Feb. 2003.

[RH04] H. Rehbinder and Xiaoming Hu. Drift-free attitude estimation for acceleratedrigid bodies. Automatica, 40(4):653–659, 2004.

[RS11] P. Rosa and C. Silvestre. On the distinguishability of discrete linear time-invariant dynamic systems. In 50th IEEE Conference on Decision and Con-trol and European Control Conference (CDC-ECC), Orlando, FL, USA, Dec.2011.

[RSA11] P. Rosa, C. Silvestre, and M. Athans. Model falsification of LPV systemsusing set-valued observers. In 18th IFAC World Congress, Milano, Italy,Aug.–Sep. 2011.

[RSP07] J. Refsnes, A.J. Sorensen, and K.Y. Pettersen. A 6 DOF nonlinear observerfor AUVs with experimental results. In 15th IEEE Mediterranean Conferenceon Control and Automation, pages 1–7, Athens, Greece, Jun. 2007.

208

References

[RSSA09] P. Rosa, C. Silvestre, J.S. Shamma, and M. Athans. Multiple-model adaptivecontrol with set-valued observers. In 48th IEEE Conference Decision andControl, 2009 held jointly with the 2009 28th Chinese Control Conference.CDC/CCC 2009, pages 2441–2447, Shanghai, China, Dec. 2009.

[RSSA10a] P. Rosa, C. Silvestre, J.S. Shamma, and M. Athans. Fault detection andisolation of an aircraft using set-valued observers. In 18th IFAC Symposiumon Automatic Control in Aerospace, Sep. 2010.

[RSSA10b] P. Rosa, C. Silvestre, J.S. Shamma, and M. Athans. Fault detection andisolation of LTV systems using set-valued observers. In 49th IEEE Conferenceon Decision and Control, Atlanta, GA, USA, Dec. 2010.

[Rug96] W.J. Rugh. Linear system theory. Prentice Hall,, Upper Saddle River, N.J.,2nd edition, 1996.

[SA87] J.O. Smith and J.S. Abel. The spherical interpolation method of sourcelocalization. IEEE Journal of Oceanic Engineering, 12(1):246–252, Jan. 1987.

[Sal91] S. Salcudean. A globally convergent angular velocity observer for rigid bodymotion. IEEE Transactions on Automatic Control, 36(12):1493–1497, Dec.1991.

[San06] A.K. Sanyal. Optimal attitude estimation and filtering without using localcoordinates, part I: Uncontrolled and deterministic attitude dynamics. In2006 American Control Conference, pages 5734–5739, Minneapolis, MN, USA,Jun. 2006.

[Sas99] S. Sastry. Nonlinear Systems: Analysis, Stability and Control. Interdisci-plinary applied mathematics: Systems and control. Springer, 1999.

[Sch68] F.C. Schweppe. Recursive state estimation: unknown but bounded errors andsystem inputs. IEEE Transactions on Automatic Control, AC-13(1):22–28,1968.

[Sel05] J.M. Selig. Geometric Fundamentals of Robotics. Monographs in ComputerScience. Springer, 2nd edition, 2005.

[SL06] D. Skoogh and A. Lennartsson. A method for multiple fault detection andisolation of redundant inertial navigation sensor configurations. In Position,Location, And Navigation Symposium, 2006 IEEE/ION, pages 415–425, SanDiego, CA, USA, 2006.

[SLLM08a] A.K. Sanyal, T. Lee, M. Leok, and N. McClamroch. Global optimal attitudeestimation using uncertainty ellipsoids. Systems & Control Letters, 57(3):236–245, 2008.

[SLLM08b] A.K. Sanyal, T. Lee, M. Leok, and N.H. McClamroch. Global optimal at-titude estimation using uncertainty ellipsoids. Systems & Control Letters,57(3):236–245, 2008.

[SN12] A.K. Sanyal and N. Nordkvist. Attitude state estimation with multi-ratemeasurements for almost global attitude feedback tracking. AIAA Journalof Guidance, Control, and Dynamics, 35(3):868–880, 2012.

[SO81] M.D. Shuster and S.D. Oh. Three-axis attitude determination from vectorobservations. Journal of Guidance, Control, and Dynamics, 4(1):70–77, 1981.

209

References

[Sol10] V. Solo. Attitude estimation and brownian motion on SO(3). In 49th IEEEConference on Decision and Control (CDC), pages 4857–4862, Atlanta, Geor-gia USA, Dec. 2010.

[Son07] E. Sontag. Nonlinear and Optimal Control Theory. Springer-Verlag, Berlin,2007.

[SP05] S. Skogestad and I. Postlethwaite. Multivariable Feedback Control: Analysisand Design. Wiley, New York, NY, USA, 2nd edition, Sep. 2005.

[ST97] J.S. Shamma and K.-Y. Tu. Approximate set-valued observers for nonlinearsystems. IEEE Transactions on Automatic Control, 42(5):648–658, May 1997.

[ST99] J.S. Shamma and K.-Y. Tu. Set-valued observers and optimal disturbancerejection. IEEE Transactions on Automatic Control, 44(2):253–264, 1999.

[STMA13] A. Saccon, J. Trumpf, R. Mahony, and A.P. Aguiar. Second-order-optimalfilters on Lie groups. In IEEE 52nd Conference on Decision and Control(CDC), pages 4434–4441, Florence, Italy, Dec. 2013.

[Stu88] M.A. Sturza. Navigation system integrity monitoring using redundant mea-surements. Journal of the Institute of Navigation, 35(4):69–87, 1988.

[SY04] Duk-Sun Shim and Cheol-Kwan Yang. Geometric FDI based on SVD forredundant inertial sensor systems. In 5th Asian Control Conference, volume 2,pages 1094–1100, Jul. 2004.

[SY10] Duk-Sun Shim and Cheol-Kwan Yang. Optimal configuration of redundantinertial sensors for navigation and FDI performance. Sensors, 10(7):6497–6512, 2010.

[TBW03] T. Thormahlen, H. Broszio, and I. Wassermann. Robust line-based calibra-tion of lens distortion from a single view. In Mirage 2003, pages 105–112,2003.

[TL89] F. Tin-Loi. A constraint selection technique in limit analysis. Applied Math-ematical Modelling, 13(7):442–446, 1989.

[TMHL12] J. Trumpf, R. Mahony, T. Hamel, and C. Lageman. Analysis of non-linearattitude observers for time-varying reference measurements. IEEE Transac-tions on Automatic Control, 57(11):2789–2800, Nov. 2012.

[TS03] J. Thienel and R.M. Sanner. A coupled nonlinear spacecraft attitude con-troller and observer with an unknown constant gyro bias and gyro noise.IEEE Transactions on Automatic Control, 48(11):2011–2015, Nov. 2003.

[TU11] R. Turkmen and Z. Ulukok. Inequalities for singular values of positive semidef-inite block matrices. Int. Math. Forum, 6(29–32):1535–1545, 2011.

[Ume91] S. Umeyama. Least-squares estimation of transformation parameters betweentwo point patterns. IEEE Transactions on Pattern Analysis and MachineIntelligence, 13(4):376–380, Apr. 1991.

[VCS+11] J.F. Vasconcelos, B. Cardeira, C. Silvestre, P. Oliveira, and P. Batista.Discrete-time complementary filters for attitude and position estimation: De-sign, analysis and experimental validation. IEEE Transactions on ControlSystems Technology, 19(1):181–198, Jan. 2011.

210

References

[VCSO10] J.F. Vasconcelos, R. Cunha, C. Silvestre, and P. Oliveira. A nonlinear positionand attitude observer on SE(3) using landmark measurements. Systems &Control Letters, 59(3–4):155–166, 2010.

[VD09] F.S. Van Diggelen. A-GPS: Assisted GPS, GNSS, and SBAS. Artech HouseGNSS Technology and Applications Library. Artech House, Incorporated,2009.

[VF00] B. Vik and T.I. Fossen. A nonlinear observer for integration of GPS andinertial navigation systems. Modeling, Identification and Control, 21(4):192–208, 2000.

[Vod99] L.V. Vodicheva. Fault-tolerant strapdown inertial measurement unit: Fail-ure detection and isolation technique. In 6th International Conference onIntegrated Navigation Systems, Saint Petersburg, May 1999.

[VRSO11] J.F. Vasconcelos, A. Rantzer, C. Silvestre, and P. Oliveira. Combinationof Lyapunov and density functions for stability of rotational motion. IEEETransactions on Automatic Control, 56(11):2599–2607, Nov. 2011.

[VS01] Nebojsa V.S. and Predrag S.S. Two direct methods in linear programming.European Journal of Operational Research, 131(2):417–439, 2001.

[VSO11] J.F. Vasconcelos, C. Silvestre, and P. Oliveira. INS/GPS aided by frequencycontents of vector observations with application to autonomous surface crafts.IEEE Journal of Oceanic Engineering, 36(2):347–363, Apr. 2011.

[VSOG10] J.F. Vasconcelos, C. Silvestre, P. Oliveira, and B. Guerreiro. Embedded UAVmodel and LASER aiding techniques for inertial navigation systems. ControlEngineering Practice, 18(3):262–278, 2010.

[W07] J.C. and L.L. Whitcomb. Adaptive identification on the group of rigid-body rotations and its application to underwater vehicle navigation. IEEETransactions on Robotics, 23(1):124–136, Feb. 2007.

[Wah65] G. Wahba. Problem 65-1: A least squares estimate of spacecraft attitude.SIAM Review, 7(3):409, Jul. 1965.

[Wel96] R.B. Wells. Application of set-membership techniques to symbol-by-symboldecoding for binary data-transmission systems. IEEE Transactions on Infor-mation Theory, 42(4):1285–1290, 1996.

[Wil76] A.S. Willsky. A survey of design methods for failure detection in dynamicsystems. Automatica, 12:601–611, 1976.

[YF03] Yunchun Yang and J.A. Farrell. Two antennas GPS-aided INS for attitude de-termination. IEEE Transactions on Control Systems Technology, 11(6):905–918, Nov. 2003.

[YKK08] Youngrock Yoon, Akio Kosaka, and Avinash C. Kak. A new Kalman-filter-based framework for fast and accurate visual tracking of rigid objects. IEEETransactions on Robotics, 24(5):1238–1251, 2008.

[YL09] Fuwen Yang and Yongmin Li. Set-membership filtering for discrete-time sys-tems with nonlinear equality constraints. IEEE Transactions on AutomaticControl, 54(10):2480–2486, 2009.

211

References

[YSKS04] Yi Ma, S.o Soatto, J. Kosecka, and S. Sastry. An Invitation to 3-D VisionFrom Images to Geometric Models. Springer, 2004. volume 26 of Interdisci-plinary Applied Mathematics.

[ZD10] B. Zhou and X.Z. Dai. Set-membership based real-time terrain modelingof mobile robots with a laser scanner. In 2010 International Conference onMechatronics and Automation (ICMA), pages 440–445, 2010.

[ZDG95] K. Zhou, J.C. Doyle, and K. Glover. Robust and Optimal Control. PrenticeHall, Aug. 1995.

[ZDL12] Ping Zhang, S.X. Ding, and Ping Liu. A lifting based approach to observerbased fault detection of linear periodic systems. IEEE Transactions on Au-tomatic Control, 57(2):457–462, Feb. 2012.

[ZJ06] Jinhui Zhang and Jin Jiang. Modelling of rate gyroscopes with considerationof faults. In 6th IFAC Symposium on Fault Detection, Supervision and Safetyof Technical Processes, pages 168–173, Tsinghua University, China, 2006.

[ZR08] Xun Zhou and Stergios Roumeliotis. Robot-to-robot relative pose estimationfrom range measurements. IEEE Transactions on Robotics, 24(6):1379–1393,2008.

[ZTM11] M. Zamani, J. Trumpf, and R. Mahony. Near-optimal deterministic filteringon the rotation group. IEEE Transactions on Automatic Control, 56(6):1411–1414, Jun. 2011.

212

Publications

[1] S. Bras, R. Cunha, J.F. Vasconcelos, C. Silvestre, and P. Oliveira. Nonlinear attitudeestimation using active vision and inertial measurements. In 48th IEEE ConferenceDecision and Control, held jointly with the 28th Chinese Control Conference., pages6496–6501, Shangai, China, 2009.

[2] S. Bras, J.F. Vasconcelos, C. Silvestre, and P. Oliveira. Pose observers for unmannedair vehicles. In European Control Conference, pages 3989–3994, Budapest, Hungary,Aug. 2009.

[3] S. Bras, R. Cunha, J.F. Vasconcelos, C. Silvestre, and P. Oliveira. Experimentalevaluation of a nonlinear attitude observer based on image and inertial measure-ments. In 2010 American Control Conference, pages 4552–4557, Baltimore, MD,USA, 2010.

[4] S. Bras, R. Cunha, J F. Vasconcelos, C. Silvestre, and P. Oliveira. A nonlinear atti-tude observer based on active vision and inertial measurements. IEEE Transactionson Robotics, 27(4):664–677, Aug. 2011.

[5] S. Bras, P. Rosa, C. Silvestre, and P. Oliveira. Optimal attitude estimation using set-valued observers. In 50th IEEE Conference on Decision and Control and EuropeanControl Conference, pages 6007–6012, Orlando, FL, USA, Dec. 2011.

[6] S. Bras, C. Silvestre, P. Oliveira, J.F. Vasconcelos, and R. Cunha. An experimentallyevaluated attitude observer based on range and inertial measurements. In 18th IFACWorld Congress, Milan, Italy, Sep. 2011.

[7] S. Bras, P. Rosa, C. Silvestre, and P. Oliveira. Fault detection and isolation forinertial measurement units. In 51st IEEE Conference on Decision and Control,pages 600–605, Maui, HI, USA, Dec. 2012.

[8] S. Bras, P. Rosa, C. Silvestre, and P. Oliveira. Set-valued observers for attitude andrate gyro bias estimation. In 2012 American Control Conference, pages 1098–1103,Montreal, Canada, Jun. 2012.

[9] D. Cabecinhas, S. Bras, C. Silvestre, P. Oliveira, and R. Cunha. Integrated solutionto quadrotor stabilization and attitude estimation using a pan and tilt camera. In51st IEEE Conference on Decision and Control, pages 3151–3156, Maui, HI, USA,Dec. 2012.

213

Publications

[10] S. Bras, R. Cunha, C. Silvestre, and P. Oliveira. Nonlinear attitude observer basedon range and inertial measurements. IEEE Transactions on Control Systems Tech-nology, 62(5):1889–1897, Sep. 2013.

[11] S. Bras, M. Izadi, C. Silvestre, A. Sanyal, and P. Oliveira. Nonlinear observer for3D rigid body motion. In 52nd IEEE Conference on Decision and Control, Florence,Italy, Dec. 2013.

[12] S. Bras, P. Rosa, C. Silvestre, and P. Oliveira. Global attitude and gyro bias esti-mation based on set-valued observers. Systems & Control Letters, 62(10):937–942,2013.

[13] S. Bras, P. Rosa, C. Silvestre, and P. Oliveira. Fault detection and isolation in iner-tial measurement units based on bounding sets. IEEE Transactions on AutomaticControl, DOI: 10.1109/TAC.2014.2363300.

[14] S. Bras, M. Izadi, C. Silvestre, A. Sanyal, and P. Oliveira. Nonlinear observer for3D rigid body motion estimation using Doppler measurements. IEEE Transactionson Automatic Control, (submitted).

214

Documents

PhD Thesis - Deterministic Position and Attitude Estimation Methodsusers.isr.ist.utl.pt/~sbras/online_papers/sbras_thesis.pdf · where Doppler measurements are available instead of