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 single- vector 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
4.2 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
xiv
Contents
4.3 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
4.4 Tuning the observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
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.2 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
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 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
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.6 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
7.7 Chapter summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
8 Fault detection and isolation in IMUs based on bounding sets 153
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
8.2 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
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.31 Comparison of the estimation errors brought about by the use of the MEKF
and by the use of the proposed observer. . . . . . . . . . . . . . . . . . . . . . . 69
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
3.38 Attitude estimation using the proposed nonlinear observer and the attitude
ground truth. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
xx
List of figures
3.39 Attitude estimation using the proposed nonlinear observer and the attitude
ground truth. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
3.40 Comparison of the estimation errors brought about by the use of the MEKF
and by the use of the proposed observer. . . . . . . . . . . . . . . . . . . . . . . 76
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
4.14 Comparison of the estimation errors brought about by the use of the MEKF
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 Literature review
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
1.2 Literature review
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
1.2 Literature review
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 Literature review
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
1.2 Literature review
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
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
1.3 Main contributions
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
2. Rigid body motion
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. Rigid body motion
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
3.2 Problem formulation
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.2 Problem formulation
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.3 Attitude observer
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.3 Attitude observer
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.3 Attitude observer
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.3 Attitude observer
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. Nonlinear observer for attitude estimation based on active vision
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
3. Nonlinear observer for attitude estimation based on active vision
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
3. Nonlinear observer for attitude estimation based on active vision
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. Nonlinear observer for attitude estimation based on active vision
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
3.6 Simulation results
(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
3. Nonlinear observer for attitude estimation based on active vision
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
3.6 Simulation results
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
3. Nonlinear observer for attitude estimation based on active vision
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.7 Experimental prototype on a calibration motion rate table
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
3. Nonlinear observer for attitude estimation based on active vision
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)
Figure 3.31: Comparison of the estimation errors brought about by the use of the MEKFand by the use of the proposed observer.
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.
3.8 Experimental implementation onboard a quadrotor
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.8 Experimental implementation onboard a quadrotor
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.8 Experimental implementation onboard a quadrotor
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
3. Nonlinear observer for attitude estimation based on active vision
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
3.8 Experimental implementation onboard a quadrotor
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)
Figure 3.38: Attitude estimation using the proposed nonlinear observer and the attitudeground truth.
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)
Figure 3.39: Attitude estimation using the proposed nonlinear observer and the attitudeground truth.
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
3. Nonlinear observer for attitude estimation based on active vision
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)
Figure 3.40: Comparison of the estimation errors brought about by the use of the MEKFand by the use of the proposed observer.
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.
4.2 Problem formulation
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
4.2 Problem formulation
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
4. A nonlinear attitude observer based on range and inertial measurements
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
4. A nonlinear attitude observer based on range and inertial measurements
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
4. A nonlinear attitude observer based on range and inertial measurements
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
4. A nonlinear attitude observer based on range and inertial measurements
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
4. A nonlinear attitude observer based on range and inertial measurements
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
4.5 Simulation results
4.5 Simulation results
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
4. A nonlinear attitude observer based on range and inertial measurements
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
4.6 Experimental apparatus
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
4. A nonlinear attitude observer based on range and inertial measurements
(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
4.6 Experimental apparatus
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. A nonlinear attitude observer based on range and inertial measurements
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
4.6 Experimental apparatus
−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
4. A nonlinear attitude observer based on range and inertial measurements
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
4. A nonlinear attitude observer based on range and inertial measurements
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.
5.2 Problem formulation
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.
5.3 Observer synthesis
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
5. Nonlinear observer for 3-D rigid body motion using full state feedback
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
5.3 Observer synthesis
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
5. Nonlinear observer for 3-D rigid body motion using full state feedback
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
5.3 Observer synthesis
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
5. Nonlinear observer for 3-D rigid body motion using full state feedback
√
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
5.4 Observer properties
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. Nonlinear observer for 3-D rigid body motion using full state feedback
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 Observer properties
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
5. Nonlinear observer for 3-D rigid body motion using full state feedback
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
5. Nonlinear observer for 3-D rigid body motion using full state feedback
−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
6. Nonlinear observer for 3-D rigid body motion using Doppler measurements
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
6.3 Observer design with Doppler measurements
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
6. Nonlinear observer for 3-D rigid body motion using Doppler measurements
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
6.3 Observer design with Doppler measurements
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
6. Nonlinear observer for 3-D rigid body motion using Doppler measurements
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
6. Nonlinear observer for 3-D rigid body motion using Doppler measurements
[−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 single- vector 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.
7.2 Problem formulation
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
7.2 Problem formulation
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
7. Global attitude and gyro bias estimation based on set-valued observers
−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
7.3 Set-valued observers
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7.3 Set-valued observers
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
7. Global attitude and gyro bias estimation based on set-valued observers
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).
7.4 Attitude and rate gyro bias estimation using SVOs
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7.4 Attitude and rate gyro bias estimation using SVOs
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7.4 Attitude and rate gyro bias estimation using SVOs
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7.4 Attitude and rate gyro bias estimation using SVOs
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7.6 Simulation results
7.6 Simulation results
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
7. Global attitude and gyro bias estimation based on set-valued observers
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
7. Global attitude and gyro bias estimation based on set-valued observers
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.
8.2 Problem formulation
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
8.2 Problem formulation
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. Fault detection and isolation in IMUs based on bounding sets
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].
8.3 FDI using hardware redundancy
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
8. Fault detection and isolation in IMUs based on bounding sets
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
8.3 FDI using hardware redundancy
ω(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
8. Fault detection and isolation in IMUs based on bounding sets
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
8.3 FDI using hardware redundancy
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
8. Fault detection and isolation in IMUs based on bounding sets
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.
8.4 FDI using analytical redundancy and SVOs
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. Fault detection and isolation in IMUs based on bounding sets
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
8.4 FDI using analytical redundancy and SVOs
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
8. Fault detection and isolation in IMUs based on bounding sets
(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
8.4 FDI using analytical redundancy and SVOs
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
8. Fault detection and isolation in IMUs based on bounding sets
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
.
.
.
.
.
.
ConservativeState Bounds
FD Signal
.
.
.
Isotation
FDI Filter
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
.
.
.
.
.
.
ConservativeState Bounds
FD Signal
.
.
.
Isotation
FDI Filter
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
.
.
.
.
.
.
ConservativeState Bounds
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
8.4 FDI using analytical redundancy and SVOs
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
8. Fault detection and isolation in IMUs based on bounding sets
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
8.5 Simulations results
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
8. Fault detection and isolation in IMUs based on bounding sets
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
8.5 Simulations results
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
8. Fault detection and isolation in IMUs based on bounding sets
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
A. Lie groups and Lie algebras
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
A. Lie groups and Lie algebras
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