satellite navigation complemented by inertial sensors · implemented system presented a higher...
TRANSCRIPT
Satellite Navigation Complemented by Inertial Sensors
Diogo Miguel Roma Ferreira
Thesis to obtain the Master of Science Degree in
Aerospace Engineering
Supervisor(s): Prof. José Eduardo Charters Ribeiro da Cunha Sanguino
Examination Committee
Chairperson: Prof. José Fernando Alves da SilvaSupervisor: Prof. José Eduardo Charters Ribeiro da Cunha Sanguino
Member of the Committee: Prof. Pedro Joaquim Amaro Sebastião
June 2018
Declaration
I declare that this document is an original work of my own authorship and that it fulfills all the require-
ments of the Code of Conduct and Good Practices of the Universidade de Lisboa.
i
ii
Acknowledgments
The work described in this thesis was implemented in the Instituto de Telecomunicacoes (IT). I would
like to thank professor Jose Sanguino for his guidance throughout the project. Additionally, I would also
like to express my gratitude to my family and friends for their support.
iii
iv
Resumo
Existem inumeras aplicacoes que requerem um posicionamento preciso e robusto, a tal ponto que o
GPS deixa de ser adequado, como em ocasioes de multi percurso severo. Em tais situacoes, o sistema
de navegacao pode ser melhorado, recorrendo a fusao de sensores, como unidades de medicao inercial
(IMU), que tem demonstrado uma sinergia para com o GPS e tem usufruıdo de uma reducao no custo.
Esta abordagem apresenta dois obstaculos. Por um lado, as equacoes inerciais sao altamente
nao lineares. Adicionalmente, os sensores de baixo custo apresentam erros que tendem a crescer
rapidamente e sem limite. Para ultrapassar estas dificuldades, tecnicas de estimacao de erros foram
utilizadas, em contraste as tecnicas classicas. Uma integracao de acopulamento fraco foi estudada,
recorrendo a rotacoes do referencial do computador atraves de quaternioes. Varios algoritmos foram
implementados, baseados no Extended Kalman Filter (EKF) e no Unscented Kalman Filter (UKF), cada
um com uma versao simples de 10 estados e uma versao aumentada de 34 estados.
Por meio de simulacoes, foi possıvel concluir que o UKF era consistentemente mais preciso que o
EKF. Adicionalmente, a versao aumentada do UKF mostrou um melhor desempenho em relacao a sua
versao simples, enquanto que o mesmo nao acontecia consistentemente para as versoes do EKF.
Finalmente, os algoritmos foram testados num ambiente empırico, fornecendo uma prova de con-
ceito. O sistema concedido apresentou um desempenho superior ao do GPS a um custo reduzido.
Contudo, em situacoes de grandes erros de GPS, o sistema era incapaz de melhorar a estimativa.
Palavras-chave: Sistemas de Navegacao Inercial, GPS, Quaterniao, Acopulamento fraco,
Filtragem nao linear, Kalman Unscented Filter
v
vi
Abstract
Precise and robust positioning is essential for many applications, so-much so that the ubiquitous Global
Position System (GPS) may not be adequate, such as in the case of severe multipath. In such situations,
the navigation system can be improved by fusing extra sensors, such as inertial measurement units
(IMU), which has presented a synergy with the GPS and has experience a decrease in it’s cost.
There are two main obstacles to this approach, for one the inertial equations are highly non-linear,
additionally the low-cost sensors present errors which tend to grow quickly and without bounds.
To overcome these difficulties, error estimation techniques were employed, in contrast to the classical
techniques. A loosely-coupled integration, relying on quaternion rotations of the computer-frame was
studied, leading to the implementation of various algorithms, based on the Extended Kalman Filter (EKF)
and the Unscented Kalman Filter (UKF), both with a simplified 10-state version and an augmented 34
state version.
Through a simulation environment, it was able to conclude that the UKF was consistently more pre-
cise than the EKF. Moreover, the augmented version of the UKF presented an even higher performance
than it’s simpler counterpart, while the same did not consistently occur with the EKF versions.
Finally, the algorithms were tested in an empirical environment, providing a proof of concept. The
implemented system presented a higher performance relative to the GPS and at a low cost increment.
However, in situations of substantial GPS errors, the system was unable to improve the estimation.
Keywords: Inertial Navigation System, Global Positioning System, Quaternion, Loosely-Coupled
Integration, Nonlinear filtering, Unscented Kalman Filter
vii
viii
Contents
Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii
Resumo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xv
Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvii
Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xix
1 Introduction 1
1.1 Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2 Coordinate Systems 5
2.1 Conversion Between Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.1.1 Direction Cosine Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.1.2 Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2 Coordinate Systems Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.1 Earth Centered Inertial Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.2 Earth Centered Earth Fixed Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.3 Tangent Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2.4 Body Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2.5 Platform Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2.6 Computer Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3 Inertial Measurement Unit 11
3.1 Strapdown and Gimballed Architectures . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2 Individual Sensor Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2.1 Accelerometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2.2 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3 IMU Error Sources . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
ix
4 Global Positioning System 17
4.1 Basic Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4.2 GPS Error Sources . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
5 Sensor Fusion Algorithms 21
5.1 Integration Architectures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.1.1 Uncoupled Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.1.2 Loosely Coupled Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.1.3 Tightly Coupled Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
5.2 Kalman Filter Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
5.2.1 Discrete Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
5.2.2 Indirect Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
5.2.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
5.2.4 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
6 INS Kinematics 31
6.1 State Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
6.2 INS Kinematics in ECI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
6.3 INS Kinematics in ECEF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
6.4 Gravity Field Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
7 Error Propagation Model 37
7.1 Error Modeling Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
7.2 Basic Error Propagation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
7.3 Augmented States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
7.3.1 Accelerometer Error Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
7.3.2 Gyroscope Error Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.3.3 Higher Order Sensor Errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.4 Dynamics Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.5 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
8 Solution Validation Through Simulation 47
8.1 Simulation Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
8.2 Simulated Cases . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
8.2.1 Case 1 - 2D Circular Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
8.2.2 Case 2 - 3D Hybrid Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
8.3 Simulation Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
8.3.1 Case 1 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
8.3.2 Case 2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
8.3.3 Closing Remarks on Simulated Results . . . . . . . . . . . . . . . . . . . . . . . . 60
x
9 Experimental Tests 61
9.1 Equipment Specifications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
9.2 Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
9.3 Experimental Test Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
10 Summary and Conclusions 67
10.1 Achievements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
10.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
Bibliography 69
A 10 State EKF A.1
A.1 Dynamics Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.1
A.2 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.2
B 10 State UKF B.1
B.1 Dynamics Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B.1
B.2 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B.1
C 34 State EKF C.1
C.1 Dynamics Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . C.1
C.2 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . C.2
xi
xii
List of Tables
3.1 Comparison between different grades of IMU. . . . . . . . . . . . . . . . . . . . . . . . . . 16
4.1 Typical errors on the GPS observables, due to various error sources. . . . . . . . . . . . . 18
8.1 Simulated instrument noise. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
8.2 Simulated IMU calibration error. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
8.3 RMSE of each implementation for 100 Monte Carlo iterations for test case 1. . . . . . . . 52
8.4 RMSE of each implementation for 100 Monte Carlo iterations for test case 2. . . . . . . . 57
9.1 Relevant ADXL345 and ITG-3200 specifications at T = 25oC. . . . . . . . . . . . . . . . . 62
xiii
xiv
List of Figures
3.1 Pendulous accelerometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.2 Axis of vibratory gyro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.1 Effect of satellite layout in the area of uncertainty . . . . . . . . . . . . . . . . . . . . . . . 19
5.1 Structure of a Loosely Coupled Integration. . . . . . . . . . . . . . . . . . . . . . . . . . . 22
5.2 Structure of a Tightly Coupled Integration. . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
5.3 Implementation of the Indirect Kalman Filter structure to the Loosely Coupled INS/GNSS
Integration problem. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
6.1 Relationship between i-frame, b-frame and α frame. . . . . . . . . . . . . . . . . . . . . . 33
8.1 Case 1 path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
8.2 Raw IMU measurements in case 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
8.3 Case 2 path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
8.4 Raw IMU measurements in case 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
8.5 Algorithm comparison for case 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
8.6 Evolution of the position, velocity and attitude states along simulation case 1. . . . . . . . 53
8.7 Evolution of the sensor calibration error states along simulation case 1. . . . . . . . . . . 54
8.8 Behavior of algorithm in the presence of a 5s GPS outage for simulation case 1. . . . . . 55
8.9 Algorithm comparison for case 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
8.10 Evolution of the position, velocity and attitude states along simulation case 2. . . . . . . . 58
8.11 Evolution of the sensor calibration error states along simulation case 2. . . . . . . . . . . 59
8.12 Behavior of algorithm in the presence of a 5s GPS outage for simulation case 2. . . . . . 60
9.1 Block diagram of experimental measurement system. . . . . . . . . . . . . . . . . . . . . 61
9.2 Implementation of experimental measurement system. . . . . . . . . . . . . . . . . . . . . 62
9.3 Pedestrian path used for the experimental trial. . . . . . . . . . . . . . . . . . . . . . . . . 63
9.4 Algorithm comparison for experimental trajectory . . . . . . . . . . . . . . . . . . . . . . . 64
9.5 Evolution of the position, velocity and attitude states along the experimental trajectory. . . 65
9.6 Evolution of the sensor calibration error states along the experimental trajectory. . . . . . 66
xv
xvi
Nomenclature
Coordinate systems
b Body-frame
c Computer-frame
e ECEF-frame
i ECI-frame
n Tagent-frame
p Platform-frame
Greek symbols
γ Normal gravity
Ω Skew-symetric representation of a cross-product
ω Angular Velocity
Φ Discrete Transition Matrix
ψ Psi-error angle
ρ Pseudorange
σ Standard Deviation
4 Sensor axis misalignment
Latin symbols
a Acceleration
C Direction Cosine Matrix
P State Covariance
F Jacobian of state transition function
f Specific force
xvii
g Local gravity
H Observation Matrix
I Identity Matrix
K Kalman Gain
b Sensor bias
SF Sensor scale factor
Q Process Covariance
q Quaternion
R Observation Covariance
r Position
v Velocity
w Dynamic noise
x State Vector
z Filter input measurement
Operators
( ) ~ ( ) Hamilton product
( )× ( ) Cross product
δ( ) Error residue
˙( ) Derivative with respect to time
ˆ( ) Estimate
∂∂a ( ) Partial derivative with respect to a
˜( ) Measurement
Superscripts
~ Relative to the hamilton product
+ a posteriori
- a priori
T Transpose
Subscript
k At discrete time tk
xviii
Glossary
DCM Direction Cosine Matrix
DOF Degrees Of Freedom
DOP Dillution Of Precision
ECEF Earth-Centered Earth-Fixed
ECI Earth-Centered Inertial
EKF Extended Kalman Filter
ENU East-North-Down
GDOP Geometric Dilution Of Precision
GNSS Global Navigation Satellite System
GPS Global Positioning System
IMU Inertial Measurement Unit
INS Inertial Navigation System
KF Kalman Filter
MEMS Micro-Electro-Mechanical System
MSE Mean Square Error
PDOP Position Dilution Of Precision
PVT Position, Velocity and Time
RMSE Root Mean Square Error
RMS Root Mean Square
UERE User Equivalent Range Error
UKF Unscented Kalman Filter
UTC Coordinated Universal Time
UT Unscented Transform
WGS World Geodetic System
xix
xx
Chapter 1
Introduction
Throughout mankind, there’s always been a need to know where one is positioned. In the early stages
of human evolution, only the most basic methods of positioning were available, such as pilotage, i.e.
landmark recognition [1]. These early methods were prone to human error and thus inadequate for
more demanding jobs like ocean-exploration. To overcome this, new methods were developed, that
used tools which reduced human interaction. Prime examples of these tools, include the astrolabe and
the compass.
In the modern age, even better tools exist, most notably satellite navigation systems, a form of radio
navigation, which relies on a constellation of artificial satellites to compute a PVT navigation solution
(Position, Velocity and Time). A small subset of these systems can position the user around any part
of the world, these are called Global Navigation Satellite System (GNSS), which includes the Global
Positioning System (GPS).
The main advantage of this approach is it’s low long-term error, making it the most widely used
navigation system today [2]. However, this system is not without it’s flaws. For one, it’s output frequency
is relatively low, typically 1Hz. Moreover, the system is sensitive to external factors since the receiver
requires an unobstructed line-of-site to a number of satellites. For applications such as guidance of
autonomous vehicles, these issues cannot be ignored [3, 4].
For such applications, additional sensors are employed, most notably Inertial Measurement Units
(IMU), due to their synergy with satellite navigation. In fact, these two systems are almost anti-symmetric
in regards to their strengths and weaknesses. Inertial Navigation Systems (INS) present an error which
grows without bounds unless there is an aid system that will periodically correct the INS position esti-
mate. Yet, the INS provides a high output frequency of around 100Hz and is completely self-contained,
meaning that it is immune to exterior factors such as multipath, or jamming. Moreover, IMUs have ex-
perienced a decrease in it’s cost as a result of the development of Micro-Electromechanical Systems
(MEMS). Because of this, INS/GNSS integration has become an attractive approach for many applica-
tions.
1
1.1 Previous Work
Various points of research in INS/GNSS integration have been developed along the years. The first
substantial improvement was in the derivation of an error model for the INS equations in 1975, referred
to as the psi angle model [5], this model is able to integrate GPS measurements with medium-to-high
grade IMUs with success. However, this model was unsuitable for lower-grade IMUs, mainly due to
relying on small-angle approximations of the heading error angle. As a response to this, the phi angle
model was conceived which resolved this issue [6], at a cost a substantial increase on the computational
load. By today’s standards, this increase is negligible and the psi-angle model has been extended for
high heading errors [7], making both models equally viable for low-cost IMUs.
Further improvements were based on constraining the navigation algorithm with assumptions on
user motion. Examples of this approach include the Zero Velocity Update [8, 9] in pedestrian motion and
the Zero Integrated Heading Rate for ordinary vehicle motion [10]. However, to maintain the generality
of motion, these methods are unsuitable.
The computational load of the integration has been demonstrated to decrease through the application
of quaternions to express the attitude state and apply rotations [11]. This, in addition to the increase in
processing power of a standard microcontroller has opened the doors for various filtering techniques.
Currently, most INS/GNSS systems rely on either the Extended Kalman Filter (EKF) or the Unscented
Kalman Filter (UKF). The UKF has shown to perform better than the EKF to varying degrees, leading
to the debate on if the UKF is in fact appropriate, considering it’s higher computational load [12, 13].
Additional improvements on the current filter is achieved by the modeling of the IMU sensor error as
augmented state [14].
1.2 Objectives
The aim of this thesis is to implement a GNSS/INS integration algorithm that
• Makes no assumption about the user’s motion
• Is adequate to a reasonably high range of motion-dynamics
• Relies on low-grade IMU sensors
• Improves the accuracy of the GNSS system
• Is robust to the degradation of the GNSS estimate
• Is modular
To this end, various filtering techniques will be applied and tested both in a simulated and exper-
imental environment, drawing conclusions on their comparative performance and taking into account
their implementation costs in the real world.
2
1.3 Thesis Outline
Chapters 2, 3, 4 and 5 present an introduction to the various topics by which the rest of the thesis will
be based on. More specifically, chapter 2 describes the coordinate systems to be yielded and methods
to represent the relationship between them. Chapter 3 describes the IMU, including the common ar-
chitectures, methods of operations and more importantly, the errors that limit the performance of these
sensors. Chapter 4 introduces the GPS, it’s basic principles and the nature of it’s errors. Finally, chap-
ter 5 describes the common integration architectures employed in the IMU/GNSS integration problem.
Moreover, this chapter goes through the most frequently used filtering techniques.
The development of the algorithms is given in chapters 6 and 7. Chapter 6 focuses on the INS
equations, while chapter 7 expresses the error propagation model of these equations.
The implemented algorithms are validated in chapters 8 and 9. Chapter 8 describes the simulation
framework conceived and tests the viability of each algorithm. Chapter 9 provides a proof of concept of
the navigation system as a whole, by evaluating the algorithms through an experimental trial.
Finally, chapter 10 surmises the work done, describes future work that can be made of the subject
and expresses closing remarks on the thesis.
3
4
Chapter 2
Coordinate Systems
For every field in the study of navigational systems, it is important to acknowledge the available coordi-
nate systems. This is not just a matter of an output presentation, the calculations necessary for accurate
positioning can be computed in any coordinate system and each may simplify or complicate the problem
at hand.
The objective of this sections is twofold. First, the basic nomenclature to represent conversion be-
tween coordinate systems is introduced in section 2.1. Second, the coordinate system to be used
throughout the thesis are described in section 2.2.
2.1 Conversion Between Coordinate Systems
There are three different ways of representing a frame conversion: Euler angles, Direction-Cosine matrix
and quaternions.
The Euler angle representation states three different rotation which can be multiplied to create the
Direction-Cosine matrix. The only advantage to this representation is it’s low storage size of three values,
but it suffers from gimbal lock, hence it will not be used in this thesis.
2.1.1 Direction Cosine Matrix
A Direction Cosine Matrix (DCM) of Coordinate Transformation is a 3-by-3 matrix, denoted by Cβα, which
transforms one vector from one set of resolving axis, α, to a another axis, β. As a result, to describe the
attitude state as a DCM, it would be necessary to store 9 values, i.e. each entry of the DCM.
To construct the DCM from a rotation, the following formula can be applied,
Cβα =
cos(µβx,αx) cos(µβx,αy ) cos(µβx,αz )
cos(µβy,αx) cos(µβy,αy ) cos(µβy,αz )
cos(µβz,αx) cos(µβz,αy ) cos(µβz,αz )
(2.1)
where µβi,αj is the angle between axis i of frame β to axis j of frame α. This definition is not very
practical, since the angle between each and every axis is needed. Moreover, the construction of a DCM
5
is computationally heavy, by requiring the computation of 6 cosines.
To apply this rotation to an arbitrary vector v, a matrix multiplication needs to occur,
vβ = Cβαvα (2.2)
where vβ and vα are expressed in the β and α frame respectively. To chain consecutive rotations, a
matrix multiplication must be applied, such as,
Cβα = Cβ
γCγα (2.3)
where γ is an intermediate frame. The derivative of a DCM, with respect to time, is expressed by the
well known formula [15],
Cβα = ωββα ×Cβ
α = ΩββαCβ
α (2.4)
where ωββα is the angular velocity of frame α in relation to β (subscript) as expressed in the β (super-
script). Ωββα = [ωββα×] is the skew symmetric matrix representation of ωββα =
[ωx ωy ωz
]T,
Ωββα =
0 −ωz ωy
ωz 0 −ωx−ωy ωx 0
(2.5)
Due to skew symmetric nature of this matrix, the following equality can be stated,
Ωββα = −Ωβ
αβ (2.6)
Moreover, skew symmetric representations of angular velocities can also be converted from one
frame to another, by a modified version of equation 2.2,
Ωββα = Cβ
αΩαβαCα
β (2.7)
From equations 2.6 and 2.7, additional expressions of the DCM derivative can deduced, such as,
Cβα = −Ωβ
αβCβα
= CβαΩα
βα
= −CβαΩα
αβ
(2.8)
2.1.2 Quaternion
An alternative to the Direction Cosine Matrix is the Quaternion, which can be described as a 4-dimensional
vector:
6
qβα =[q0 q1 q2 q3
]T(2.9)
Any chain of rotations can be described in terms of a single rotation along a vector, which is defined
by an Euler angle and an Euler axis. Considering a rotation expressed by the Euler angle θ and Euler
axis n =[nx ny nz
], the quaternion representation of this rotation is given by,
qβα =[cos(θ2
)nx sin
(θ2
)ny sin
(θ2
)ny sin
(θ2
)]T(2.10)
To describe rotations applied consecutively, the operation is similar to the DCM,
qβα = qβγ ~ qγα. (2.11)
Where ~ represents the Hamilton multiplication, which can be defined as the following matrix multi-
plication [16],
q ~ p =
q1p0 + q2p3 + q0p1 − q3p2q3p1 + q0p2 + q2p0 − q1p3q0p3 + q3p0 + q1p2 − q2p1−q2p2 − q1p1 − q3p3 + q0p0
. (2.12)
The most efficient way to apply a rotation to a 3D vector is to convert the quaternion to a DCM using
C(qβα), Cβ
α =
q20 + q21 − q22 − q23 2q1q2 − 2q3q0 2q1q3 + 2q2q0
2q3q0 + 2q1q2 q20 − q21 + q22 − q23 2q2q3 − 2q1q0
2q1q3 − 2q2q0 2q2q3 + 2q1q0 q20 − q21 − q22 + q23
. (2.13)
For use in motion equations, it is useful to calculate the derivative of a quaternion, which is given by
[17],
qβα =1
2ωββα ~ qβα. (2.14)
It might be advantageous to express this equation as a matrix multiplication, to this end, the skew-
symmetric matrix, Ωβ~βα = [ωββα~] can be expressed as,
Ωβ~βα =
0 −ωββαT
ωββα Ωββα
=
0 −ωx −ωy −ωzωx 0 −ωz ωy
ωy ωz 0 −ωxωz −ωy ωx 0
= −Ωβ~αβ (2.15)
As a result, the quaternion derivative can be written as the following matrix multiplication,
qβα =1
2Ωβ~βαqβα (2.16)
7
Similarly to the DCM, the quaternion multiplication can be described by alternative expression,
namely,
qβα = −1
2Ωβ~αβ qβα
=1
2Ωα~βα qβα
= −1
2Ωα~αβ qβα
(2.17)
Considering ωα~βα =[ωx ωy ωz
]T, the Ω matrices are given by the following formula,
Ωα~βα =
0 −ωαβαT
ωαβα −Ωββα
=
0 −ωx −ωy −ωzωx 0 ωz −ωyωy −ωz 0 ωx
ωz ωy −ωx 0
= −Ωα~αβ (2.18)
2.2 Coordinate Systems Review
In this section, the various coordinate systems of use for INS/GNSS applications are overviewed.
2.2.1 Earth Centered Inertial Frame
The Earth Centered Inertial (ECI) frame, or i-frame, is an orthogonal coordinate system whose origin is
at the center of mass of the Earth and it’s axis has constant orientation in regards to the universe.
This coordinate system does not strictly describe a true inertial frame, which by definition must be
at rest or moving at a constant velocity in relation to the rest of the universe. This is because the Earth
itself experiences accelerations from the Sun and the Sun from the center of the Galaxy. However,
for navigational applications, the ECI Frame can be considered pseudo-inertial, meaning that Newton’s
Laws of Motion are still valid [18].
2.2.2 Earth Centered Earth Fixed Frame
The Earth Centered Earth Fixed (ECEF) frame, or e-frame, is centered on the center of mass of the
Earth but, contrary to the ECI frame, it’s axis revolves around the Earth’s axis of rotation and with the
same angular velocity. As a result, any body at rest in relation to the surface of the earth will have a
constant ECEF position, making it a very useful frame for navigation. The ECEF x-axis is always pointing
to the Greenwich meridian, while the z-axis is parallel with the Earth’s polar axis and the y-axis is defined
as to complete a right-hand orthogonal set.
The angular velocity of the ECEF frame in relation to the ECI frame is
ωiie = ωeie =
0
0
|ωie|
(2.19)
8
where |ωie| = 7.29115 × 10−5[rad/s] in WGS 84. For navigation purposes, this rotation rate is
considered constant.
2.2.3 Tangent Frame
The Tangent frame, or n-frame, is centered on the user position and it’s z-axis point towards or away
from the center of mass of the Earth, while the remaining axis are oriented arbitrarily, usually along two
cardinal points. One example of such a system is the East, North, Up frame or ENU frame.
A disadvantage of the ENU frame is that the frame rotates as the user moves. Hence the angular
velocity cannot be considered constant, complicating the navigation equations.
The conversion from the ECEF frame to the ENU frame is given as a function of the latitude φ and
longitude λ, of the origin of the ENU frame,
Cne =
− sin(λ) cos(λ) 0
− sin(φ) cos(λ) − sin(φ) sin(λ) cos(φ)
cos(φ) cos(λ) cos(φ) sin(λ) − sin(φ)
(2.20)
2.2.4 Body Frame
The Body frame is fixed with the center of mass of the vehicle with an arbitrary orientation. In the case
of an airplane, the most common axis are x-axis pointing to the nose of the airplane, y-axis pointing to
airplanes right wing and z-axis is defined as to completed the right-hand orthogonal set.
2.2.5 Platform Frame
This frame is only applicable for strapdown IMU systems. The Platform frame, or p-frame, is centered
on an arbitrary point of the sensor platform and it’s axis are aligned with accelerometer and gyro axis.
The rotation matrix between the platform and body frame, Cbp is considered constant and must be
determined at the design stage of the vehicle. Since the resolution of the IMU measurements can be
trivially converted to the body frame, during the design of the INS, this frame can be ignored, considering
that the accelerometer and gyroscope axis coincide with the b-frame.
2.2.6 Computer Frame
This last frame will only be used on the error propagation and describes the erroneous frame computed
by the INS. The coordinate system is very useful to describe the attitude errors and their consequent
effect on the velocity and the position.
9
10
Chapter 3
Inertial Measurement Unit
Without any form of external aiding, inertial navigation can be characterized as a form of dead-reckoning,
meaning that it relies on estimating the velocity vector, which is integrated in order to calculate a dis-
placement from an initial position, the fix. To accomplish this, an Inertial Navigation Unit relies on a
cluster of sensors, designated Inertial Measurement Unit.
Modern IMU incorporates at least two types of sensors, accelerometers and angular sensors, usu-
ally gyroscopes and/or magnetometers. Accelerometers measure specific forces, gyroscopes measure
angular rates and magnetometers measure the direction and strength of magnetic fields. Typically a set
of three of each sensor is present in an IMU, making possible the observation of motion in 3D space, as
long as each individual sensor is orthogonally oriented from the remainder sensors of the same type.
This chapter constitutes an introduction to the IMU and is organized in three sections, section 3.1
characterizes the two main IMU architectures, section 3.2 which overviews the main individual IMU
sensor type, and 3.3 describes the errors inherent to these sensors.
3.1 Strapdown and Gimballed Architectures
Consider a navigation system comprised only of ideal accelerometers, meaning that it’s measurements
are without error. In the case of linear motion (i.e. moving along a straight line), positioning can be
perfectly estimated just by integrating the sensor outputs twice. Now consider an angular motion, in
this case, as the user performs turning maneuvers, the accelerometer axis will rotate in relation to the
i-frame, invalidating the classic laws of motion which was used as a dogma for our INS design.
Two different solutions arise from this issue, both based on the incorporation of angular sensors:
• Use the angular information to mechanically stabilize the accelerometer cluster, forcing the cluster
frame to be angularly fixed to the pseudo-inertial frame.
• Stored the angular information in the navigation processor, which will correct the inertial estimate.
The first solution is designated as the Gimballed approach. In this version, the IMU is mounted in the
innermost platform of a gimbal system. The stabilization is performed by use of a control loop between
the angular sensors and the gimbal torque motors, which will rotate the sensor platform in such a way
11
that the gyroscope outputs is zero, meaning that the platform frame is fixed.
The second solution is designated as the Strapdown approach. In contrast to the gimballed ap-
proach, this approach mounts the IMU in the vehicle, without the possibility of a rotation between the
platform axis and the vehicle body axis. As a result, the vehicle angular motion must be corrected in the
navigation processor, leading to a higher electronic complexity, but also a lower mechanical complexity,
since there is no need for a rotating gimbal system.
Theoretically, both approaches are equivalent, resulting in the same navigation information. How-
ever, in practical terms, the same can’t be said. As a result of the advent and development of MEMS
technology, the cost of a higher electronic complexity has decreased in relation to the cost of a higher
mechanical complexity, to the point where gimballed systems are seldom if ever used [19].
During the remainder of this thesis, only strapdown systems will be worked upon.
3.2 Individual Sensor Overview
The two most common inertial sensors are introduced in this section, which are the accelerometer and
the gyroscope.
3.2.1 Accelerometer
As stated above, the accelerometer outputs the specific forces applied to the sensor platform. A specific
force is defined by the following expression:
f , Specific Force =Forcenon-gravitational
Mass(3.1)
As a result of this definition, f is independent of the mass of the user vehicle for a given motion. This
physical quantity can be related to the motion of a vehicle by
f + g = ri (3.2)
Where ri is the user acceleration in relation to the inertial frame and g is the gravitational field vector
felt by the user. Considering the vehicle to be static, it should be noted that f would be non-null, since the
platform normal force would not be compensated by the gravitational forces. Consequently, to compute
the motion of the user, a gravity model must be implemented, making it possible to estimate the g vector.
An example of a gravity model is the Somigliana model [20], described in section 6.4.
The most common accelerometer architecture is the pendulous accelerometer [21]. For a single-
degree-of-freedom accelerometer, this configuration is composed by a proof mass, a pickoff, a pendu-
lous arm with a hinge, supported by a pair of springs, as displayed in figure 3.1.
12
Figure 3.1: Pendulous accelerometer. Reprinted from [21].
As a force is applied along the sensitivity axis, the proof mass will move towards the force direction,
compressing and extending the two springs, resulting in spring compensation forces which will increase
until the exterior forces are compensated. At this point, the proof mass will be static and moreover, it’s
deflection in the sensitivity axis will be a linear function of the exterior force. The hinge is used to provide
damping.
The resulting deflection is measured by the pickoff, which, in the simplest cases is a variable resistor.
In modern accelerometers, the pickoff is a differential capacitor, comprised of two fixed plates and a
moving plate connected to the proof mass, forming two capacitors. As the moving plate deflects along
the axis, the capacitance of one pair of plates will increase will the other will decrease.
The differential capacitance can be measured by running the same electrical signal through each
capacitor and demodulating the phase between them, resulting in an estimate of the specific force [22].
3.2.2 Gyroscope
Gyroscopes measure the angular rate of a body in relation to the inertial frame. Assuming the body
frame is the same as the sensor frame, this sensor will output ωbib. All gyroscopes rely on the Coriolis
effect to estimate this physical quantity.
The Coriolis effect can be exemplified by considering the frame b rotating in relation to a fixed-frame
i, with the same origin. The position of an object a, can be expressed by a vector whose origin coincides
with the b-frame origin as rba or as a vector whose origin coincides with the i-frame origin as ria.
The acceleration in relation to the i-frame can be described as function of the motion in relation to
the b-frame, as,
rbia = rbba + 2Ωbibr
bba + Ωb
ibΩbibr
bba (3.3)
where Ωbib is the skew symmetric representation of the angular velocity between frame i and b and
the ( ˙ ) operator denotes a derivative with respect to time, or in other words, rbba is the velocity of a in
relation to the b-frame, while rbba and rbia is the acceleration of a in relation to the b-frame and i-frame
13
respectively. The 2Ωbib term express the Coriolis effect.
The most common configuration of a MEMS gyroscope is the vibratory gyroscope [21], which uses a
vibrating element, such as a string or beam. In the absence of any angular momentum, the string will be
describing a simple harmonic motion. Defining the center of the string as point a, the harmonic motion
is given in the body frame by,
rbba = rb0 cos [ωv(t− t0)]
vbba = rbba = −ωvrb0 sin [ωv(t− t0)]
abba = rbba = −ω2vrb0 cos [ωv(t− t0)]
(3.4)
Where rb0 is the maximum deflection vector of the string in the absence of any exterior angular
momentum, and ωv is the angular velocity of the harmonic motion alone. The axis where the oscillates
is called the drive axis as expressed in figure 3.2.
Substituting this motion into equation 3.3 gives
rbia =ΩbibΩ
bib cos [ωv(t− t0)]− 2ωvΩ
bib sin [ωv(t− t0)]− ω2
v cos [ωv(t− t0)]
rb0 (3.5)
For a single degree-of-freedom gyroscope, ωbib will be directed in the input axis. As a result, both the
first and third terms of equation 3.5 are directed in the drive axis (note that ωbib × r0 is directed in the
output axis, hence ωbib × ωbib × r0 is directed in the drive axis).
On the other hand, the second term (the Coriolis term) is directed in the output axis. By measuring
the amplitude of the oscillation along this axis, it is possible to estimate the value of ωbib.
Figure 3.2: Axis of vibratory gyro. Adapted from [21].
Alternative Orientation Sensor
There are other ways of computing the platform orientation, most notably with the use of a different
sensor, the magnetometer. The main benefit of this sensor over the gyroscope is that the magnetometer
does not require an integration to obtain an estimate for the orientation of the body frame. However, the
magnetometers present two big disadvantages:
• It’s short-term accuracy is sub-par in comparison with contemporary gyroscopes.
• Magnetometers are sensible to man-made magnetic fields, which is indistinguishable from the
geomagnetic field.
14
As a result, magnetometers are seldom used as a gyroscope substitute. Instead, they are employed
for the initial alignment phase of a vehicle [21]. To simplify the INS design, magnetometer sensors will
not be considered in this work.
3.3 IMU Error Sources
All sensors exhibit errors, and the ones that comprise the IMU are no exception. These errors can be
described in two different components, random errors and systematic errors.
Random IMU Error Sources
Random noise can be due to various sources, from the inherent white noise of electronics, such as
power supplies; the exponentially correlated noise from temperature variation. Even considering that
only white noise affects the IMU, all the sensor outputs must be integrated at least once, generating a
random walk defined in discrete time by,
νk = νk−1 + uk
ν0 = 0
⇒ νk =
k∑i=1
uk (3.6)
Where νk is the random walk and uk is gaussian white noise of variance σ2u. The variance of this
stochastic process can be calculated as,
V ar(νk) = V ar
(k∑i=1
uk
)=
k∑i=1
V ar(νk) = kσ2u (3.7)
It can be easily observed that the variance increases with time. It is for this reason that non-aided
INS presents a divergent error behavior.
Systematic IMU Error Sources
Systematic sources are mainly due to imperfect calibrations and can be modeled as the sum of three
different errors:
1. Bias error (zeroth order error)
2. Scale-factor error and Cross-coupling error (first order error)
3. Non-linearities (higher order errors)
Even in the case of a perfect calibration, these errors will still persist, this is because each of the
three components can suffer variations, either temperature-dependent variations, run-to-run variations
(variation in each start-up) or in-run variations (variations during the same run).
Temperature-dependant variations can be mitigated through the implementation of a control loop
connected to a temperature sensor, which is already standard practice in modern IMUs. The run-to-run
variations can be mitigated by calibrating and aligning the system before each new run. However, the
in-run variation can only be mitigated by estimating the sensor error as an augmented state.
15
A full estimation of the systematic errors comes with an increase of computational load, which would
lead to an increase in the cost of the system, constituting a trade-off. INS designers commonly only
model the sensor error up to a certain order.
IMU Grades
Table 3.1 describes the typical total error values for different grades of IMUs as well as their subsequent
price and bias error. It’s easy to observe that the lower grades of IMU can’t be used for navigation pur-
poses without auxiliary sensors. It should also be noted that gyro bias errors intermediate to automotive
grade IMUs exceed the Earth rotation rate, which will negatively impact any alignment process.
Table 3.1: Comparison between different grades of IMU.
Grade Marine Aviation Intermediate Tactical Automotive
Cost >1Me 50K-1Me 20K-50Ke 5K-20Ke <5Ke
Position error <1.8km/day 0.075-1.5km/h 15-40km/h 40-120km/h >2km/min
Accelerometer Bias Error <0.03mg 0.03-0.1mg 0.1-1mg 1-10mg >10mg
Gyro Bias Error <0.01o/h 0.01-0.1o/h 0.1-1o/h 1-100o/h >100o/h
Note. Adapted from [21, 23].
16
Chapter 4
Global Positioning System
The GPS is a GNSS, meaning that it relies on satellites to provide, with global coverage, the current
position of a user. Currently, only three other systems exist that completely fulfill the definition of a
GNSS, the GLONASS, the Galileu and the Beidou Navigation Satellite System (BDS). These other
systems will not be pertained further, even though the INS/GNSS integration algorithm designed in this
thesis is, in theory, applicable to the other GNSS.
This chapter will overview the basic principles of operation of this system in section 4.1. Finally, a
description will be made of the error sources that limit the GPS in section 4.2.
4.1 Basic Concepts
The GPS encompasses three segments:
• The space segment, consisting of a constellation 31 satellites (as of 2018) in orbit. The satellites
transmit a GPS signal, comprising of 2 carriers: L1 (1.57542 GHz) and L2 (1.2276 GHz). These
signals are modulated with data that include the satellite’s orbital parameters.
• The control segment, consisting of a number of control stations, whose purpose is to check a given
satellite’s health and to minimize possible errors of the space segment by updating a satellite’s
clock, ephemeris and almanac.
• The user segment, consisting of the GPS receiver which itself is compromised by the receiver
antenna and a processor which will compute the navigation information from the measurements.
In the presence of at least four functional GPS satellites, a receiver is able to compute it’s position by
measuring the pseudorange between itself and each satellite. This measurement is based on the time
delay between the transmission and reception of the signals. However, since the receiver clock presents
a bias in relation to the satellite clocks, the pseudorange measurements are different from the true-range
between a given satellite and the receiver.
The receiver clock bias must be resolved, in addition to the 3 dimensional position variables. Or in
other words, 4 variables must be solved for, requiring at least 4 linearly independent equations. Each
satellite yields an equation of the form:
17
(s(i)x − rx)2 + (s(i)y − ry)2 + (s(i)z − rz)2 = (ρ(i) − c4t)2 (4.1)
where s(i) is the position of satellite i, r is the receiver position, ρ(i) is the pseudorange measurement
from satellite i, c is the speed of light in a vacuum and 4t is the receiver clock offset. Each satellite
pseudorange equation can be linearized and, as a result, 4 satellites are needed to solve the receiver
position.
Beyond the position and receiver clock offset (and by extension the UTC of the measurement), the
GPS can estimate the velocity of a user. The combination of these three estimates is a called the PVT
(Position, Velocity and Time).
The velocity estimation is possible due to the Doppler effect, which is a frequency shift in the received
signal as a result of relative motion between a transmitter and a receiver. This estimate is particularly
important in IMU/GNSS integration, since the velocity measurement can directly correct accelerometer
errors. In contrast, for a position measurement, an integration must be made for the accelerometer
errors to propagate to an equation where this measurement can correct the system.
4.2 GPS Error Sources
All GNSS required clear line-of-sight to at least four satellites, in order to provide accurate positioning.
However, these systems are subject to a plethora of measurement error sources that will degrade the
estimate quality. Common sources of pseudorange errors include:
• Ephemeris and satellite clock errors, which should be minimized by the control segment.
• Atmospheric effects, including Ionospheric and Tropospheric delays.
• Multipath, caused by GPS signals arriving at the receiver antenna through a path different from
the line-of-sight.
• Receiver noise.
Table 4.1 lists typical values of the effects of these error sources on the GPS observables
Table 4.1: Typical errors on the GPS observables,due to various error sources.
Errors Standard deviation [m]
Clock and ephemeris 3.6Ionosphere 7.0
Troposphere 0.7Multipath 0.1-3.0
Receiver noise 0.1-0.7
Note. Adapted from [24].
These errors can be bundled together into what is know as the User Equivalent Range Error (UERE),
which is defined as,
UERE =√E2clock + E2
ephemeris + E2ionosphere + · · · (4.2)
18
where E and consequently UERE can be defined as a 1− σ precision.
Dilution of Precision
Beyond the stated range errors, the geometry of the available satellites also has an effect on the error.
Considering that each satellite possess an uncertainty in it’s pseudo-range measurement, the intersec-
tion between the range sphere of all available satellites will not be a point, but an area, designated as the
uncertainty area. This uncertainty area will increase in size as the available satellites cluster together
instead of spreading out. This phenomena is made clearer in figure 4.1, where the more spread-out
layout of subfigure 4.1a results in a smaller area of uncertainty then the layout of subfigure 4.1b.
(a) Spread-out satellite placement (b) Concentrated satellite placement
Figure 4.1: Effect of satellite layout in the area of uncertainty (dark area). Adapted from [25].
Assuming that the pseudorange measurements of each satellite present the same standard devia-
tion, the geometric effect on the GPS error is given by a dimensionless quantity, the Geometric Dilution
of Precision (GDOP), given by,
GDOP =
√σ2x + σ2
y + σ2z + σ2
t
σρ(4.3)
where σx, σy and σz are standard deviations of the each coordinate of the GPS position solution, σt
is the standard deviation of the receiver clock offset estimation, and finally, σρ is standard deviation of
the pseudorange measurement. A version of this concept can be made to specify the position solution,
called the Position Dilution of Precision (PDOP), give by,
PDOP =
√σ2x + σ2
y + σ2z
σρ(4.4)
From the definition of the PDOP, a relation can be made between the pseudorange accuracy and the
position solution accuracy:
σGPS = PDOP × σUERE (4.5)
A PDOP = 6 is considered a typical service availability threshold [26]. Considering this value and
the upper quantities of the typical pseudorange errors (table 4.1), an estimate can be made for the
19
position accuracy at the limit of availability of σ = 50[m]. Or in other words, 68% of the estimates will
have an error lower to 50 meters.1
1Disclaimer: this calculation is a very rough estimating, serving only illustrative purposes.
20
Chapter 5
Sensor Fusion Algorithms
In this chapter, the various integration strategies will be introduced. In section 5.1 the various integration
architectures will be described and one will be selected for development. Additionally, in section 5.2, an
overview of the Kalman Filter will be made, including it’s various extensions to non-linear space.
5.1 Integration Architectures
The various integration architectures distinguish themselves by the point at which the information by the
two sensor units are fused together. In the case of the uncoupled integration, both the IMU and GPS
solution have been resolved before integration, in the case of the loosely coupled integration, only the
GPS solution has a been resolved, whereas in the case of the tightly coupled integration, none of the
solution have been resolved prior to the integration.
5.1.1 Uncoupled Integration
In the Uncoupled System, the INS and GNSS operate mostly independently, outputting two redundant
navigation solutions. The only interaction between the two systems is that the GNSS solution can be
used to reset the INS solution when it’s errors grow out of bounds.
This implementation is only useful for medium-to-high grade IMUs. Moreover this implementation’s
only advantage, the fact that it returns a navigation solution in the absence of GNSS data, is also shared
by the Loosely Coupled Integration. For these reasons, this architecture is seldom used.
5.1.2 Loosely Coupled Integration
In the Loosely Coupled System, the IMU and GNSS measurements are both independently processed
by dedicated blocks which are afterwords combined in an integration block. As a result, a failure in
one of the measurement units, or measurement processing block will not compromise the system [27],
resulting in the same level of redundancy as the uncoupled approach. Additionally, this implementation
21
provides a high-level of modularity since it can be constructed from sensors already present in common
navigation systems.
This approach is illustrated in figure 5.1. It can be observed that the estimation resulting from the
GPS Filter is simultaneously a navigation output (xGPS) and an input measurement (z) for the INS/GNSS
integration filter. It should also be noted that the GPS unit can be thought of as a black box.
IMU IMU ProcessorINS/GPS Inte-gration Filter
GPS Unit
GPS Receiver GPS FilterzGPS
zIMU u
xGPS
xINS/GPS
z
Closed-loop corrections
Figure 5.1: Structure of a Loosely Coupled Integration.
The term filter refers to any recursive estimator which minimizes the effects of measurement errors.
For INS applications, the filters are usually Kalman Filters (5.2), but other filters such as Recursive Least
Squares can also be implemented.
This implementation can be used with low-grade IMUs if the system comprises a closed loop, since
the GNSS measurements can not only reset the IMU states when they grow out of bounds, but also
improve subsequent INS iterations by estimating sensor errors, such as bias errors.
This approach will be implemented in this thesis.
5.1.3 Tightly Coupled Integration
The last strategy to be described is the Tightly Coupled System, which uses the IMU and GPS raw data
to estimate a position without computing an intermediate position from the GPS alone. As a result, this
approach provides no redundancy, meaning that if the fusion block starts failing, no estimation can be
acquired from either the GNSS or the INS [28]. Additionally, Tightly Coupled Systems requires a higher
computation power, in comparison to their Loosely Coupled counterparts [23].
IMUGPS/INS Inte-gration Filter
GPS Receiver
u
zGPS
xINS/GPS
Figure 5.2: Structure of a Tightly Coupled Integration.
For these reasons, the Tightly Coupled approach will not be developed in this thesis.
22
5.2 Kalman Filter Estimation
The Kalman Filter is a recursive algorithm used to estimate parameters in the presence of noisy mea-
surements. It’s an optimal estimator, but requires accurate linearized models for the system dynamics
and the observations, both of which are not available in all applications [29].
5.2.1 Discrete Kalman Filter
A generalized linear dynamics model can be written as
xk+1 = Φkxk + wk (5.1)
where xk and wk represent the system state vector (which will include the parameters of interest)
and the dynamic noise vector respectively. The subscript indicate at which iteration the dynamic model
is applied, meaning that this model can be viewed as the propagation of the state through time. For this
reason, Φk is designated as the state transition matrix from time tk to tk+1.
Similarly, a generalized linear observation model can be written as
zk = Hkxk + vk (5.2)
where zk and vk represent the observation vector and the observation noise vector respectively. H
is designated as the observation matrix. It is assumed that w and v are uncorrelated Gaussian white
noise, implying that the noise covariance matrices can be written as
EwkwTi = Qkδki
EvkvTi = Rkδki
EwivTi = 0
(5.3)
where δki is the Kronecker delta. Matrices Qk and Rk are diagonal matrices.
The Kalman Filter is implemented in two steps, the prediction step and the filtering step as specified in
algorithm 1, where the “+” superscript indicates an a priori value, or predicted value; the “−” superscript
indicates an a posteriori value, or a filtered value; and the Pk matrix is the state covariance matrix.
5.2.2 Indirect Kalman Filter
The Kalman filtered can be applied in various different forms. One variation is in the state choice. In the
case of inertial navigation, the naive choice would be to use a total state formulation, meaning that the
state vector will include the parameters of interest, i.e. user position, velocity and attitude. This approach
is known as the Direct Kalman Filter.
This implementation presents various drawbacks when applied to INS [30]:
23
Algorithm 1 Discrete Kalman FilterPrediction Step
1: Propagate state vectorx−k = Φkx
+k−1
2: Propagate covariance matrixP−k = ΦkP
+k−1Φk + Qk
Filtering Step1: Evaluate Kalman gain
Kk = P−k HT
k
(HkP
−k HT
k + RTk
)−1
2: Update state vectorx+k = x−
k + Kk
(zk −Hkx
−k
)3: Update covariance matrix
P+k = (I−KkHk) P−
k
1. The filter would have to maintain explicit, accurate awareness of the vehicles motion at high fre-
quency.
2. Total state dynamic models are highly non-linear.
3. In case of a failure in the filter, no navigational information can be retrieved, demonstrating low
reliability.
An alternative better suited for this application is the Indirect Kalman Filter. The variable of interest is
calculated by an independent mechanism, while the Indirect Kalman Filter estimates the errors of these
variables, using as a measurement the difference between the variables of interest and an external
source data. In the case of INS, the external data can be the output of a GPS and the system can be
represented by figure 5.3, where the INS Kinematics block will be explained in chapter 6, the Kalman
Filter block can be any version of the Kalman Filter, including it’s non-linear variants. The Measurement
Prediction block can be a H−1 multiplication if the observation model is linear.
IMU INS Kinematics∑
+
+
GPS Unit∑−+
Kalman Filter
MeasurementPrediction
u x−
z
z δz
δx
Closed loop corrections
x+
Figure 5.3: Implementation of the Indirect Kalman Filter structure to the Loosely Coupled INS/GNSSIntegration problem.
Several advantages arise from this implementation, for one, the error state dynamics model are more
linear. Additionally, since the dynamics model are of lower frequency, the kalman filter clock cycle can
24
be longer, up to an order of minutes [31].
5.2.3 Extended Kalman Filter
For INS/GNSS integration applications, the models used are nonlinear, meaning that the discrete kalman
filter can not be applied. A popular alternative is to use a nonlinear version of this estimator, such as the
Extended Kalman Filter (EKF). The generalized equations for nonlinear models are as follows,
xk+1 = f(xk) + wk
zk = h(zk) + vk
(5.4)
In each time step k, the EKF requires the evaluation of the f() and h() Jacobian matrices, denoted
by Fk and Hk respectively and computed as,
Fk =
[∂f(x)
∂x
]x=x+
k−1
Hk =
[∂h(x)
∂x
]x=x−
k
(5.5)
Algorithm 2 surmises the Extended Kalman Filter.
Algorithm 2 Extended Kalman FilterPrediction Step
1: Evaluate State Transition Jacobian
Fk =
[∂f(x)
∂x
]x=x+
k−1
2: Propagate state vectorx−k = f(x+
k−1)
3: Propagate covariance matrixP−k = FkP
+k−1Fk + Qk
Filtering Step1: Evaluate Observation Jacobian
Hk =
[∂h(x)
∂x
]x=x−
k
2: Evaluate Kalman gainKk = P−
k HTk
(HkP
−k HT
k + RTk
)−1
3: Update state vectorx+k = x−
k + Kk
(zk − h(x−
k ))
4: Update covariance matrixP+k = (I−KkHk) P−
k
It should be noted that the EKF approximates the dynamic model to first-order, hence it’s not optimal
in higher order systems, leading to poor estimation accuracy in such cases. Another disadvantage of
this filter is the fact that it relies on the evaluation of Jacobian matrices, which are not guaranteed to exist
(such as in the case of a discontinuity), or might not have a finite value [32].
25
5.2.4 Unscented Kalman Filter
A different implementation of the Kalman Filter for nonlinear systems that does not rely on Jacobian
matrices is the Unscented Kalman Filter (UKF). This method relies on the Unscented Transform (UT),
which calculates the statistics of a random variable undergoing a nonlinear transformation, by applying
the same transform to a set of samples determined by the variable covariance [33].
Unscented Transform
Considering the nonlinear function y = f(x), where the statistics of x are described by a mean x and
covariance P. The UT samples, referred to as the sigma vectors X (i) can be defined as
X (0) = x
X (i) = x + (√n+ κ)rowi
(√P)
i = 1, · · · , n
X (i) = x− (√n+ κ)rowi
(√P)
i = n+ 1, · · · , 2n
(5.6)
where n is the size of x, κ is a parameter which will be described later, rowi() is a function that
returns the ith row of it’s argument. The matrix square root√
P can be any solution of√
P√
PT
= P.
Since a covariance matrix is always symmetric and positive-definitive,√
P can be computed through the
Cholesky decomposition.
Equation 5.6 can be expressed in a more compressed way as
[X (0) · · · X (2n)
]=[x x± (
√n+ κ)
(√P)]
(5.7)
Example 1. The most intuitive example of the sigma points are in the case that covariance is a diagonal
matrix,as is more common in the initialized KF covariance:
P =
c1
. . .
cn
(5.8)
It is trivial to see that√
P will have the form,
√P =
√c1
. . .√cn
(5.9)
Hence, denoting di =√
(n+ κ)ci the sigma vectors will be given by,
26
[X (0) · · · X (2n)
]=
x x +
d1...
0
· · · x +
0...
dn
· · · x−
d1...
0
· · · x−
0...
dn
(5.10)
Or in other words, in addition to the mean, each sample will be a deviation of the mean value in one
direction.
Each sigma vector can be propagated directly by applying the non-linear f(),
Y(i) = f(X (i)) i = 0, · · · , 2n (5.11)
From which the statistical information of y can be estimated by a weighted average,
y ≈2L∑i=0
WiY(i)
Pyy ≈2L∑i=0
Wi
(Y(i) − y
)(Y(i) − y
)T (5.12)
Where the weight can be calculated as follows,
W0 =κ
n+ κ
Wi =1
2(n+ κ)i = 1, · · · , 2n
(5.13)
This weight definition along with the sigma point definition of equation 5.6 fully describe the effect of
parameter κ in the unscented transform, which is twofold:
1. Control the spread of the sigma points from the mean.
2. Control the weight of these sigma points in resulting estimation of statistical properties of y.
For positive values of κ, an increase of this parameter will increase the spread of the sigma points,
however, it will also decrease the weights of these sigma vectors, giving a higher importance to transfor-
mation of the mean. For negative values, the opposite is through, meaning that higher the spread of the
sigma vectors, the more weight is given to these samples. The use of κ < 0 is discouraged, since it can
lead to the computation of a non-positive semi-definite covariance matrix [34].
This is not the only way of defining weights for the unscented transform, the only restriction is that∑2ni=0Wi = 0. Wan and Van Der Merwe, state a different implementation, extensible to Non-Gaussian
distributions [35], which will not be implemented in this thesis.
27
Implementation of the Unscented Transform in a Kalman Filter
From the UT, the UKF can be built upon, by considering at each iteration, the state as being the mean
of the distribution. As a result, the predicted covariance will not depend on a matrix (such as Jacobian
in the EKF), but on the statistical properties of the transformation of the sigma vectors.
Before the filtering step, a new set of sigma vectors must be evaluated, to take into account the a
priori covariance, an a priori sampling.
Finally in the filter step, the new sigma vectors are used to predict the measurements mean, mea-
surement covariance and the state-measurement cross-covariance. The only expression without a direct
correspondence to the UT equation is the Kalman gain. However, considering the a posteriori state co-
variance definition:
P+k = E
(xk − x+
k )(xk − x+k )T
(5.14)
where xk is the true value. It can be assumed that x+k = x−
k + Kkδzk, where δzk = zk − z−k and
z−k = Hkx−k . Therefore [29],
P+k = E
(xk − x−
k −Kkδzk)(xk − x−k −Kkδzk)T
= E
(xk − x−
k )(xk − x−k )− (xk − x−
k )δzTkKTk −Kkδzk(xk − xk)T + (Kδzk)(Kδzk)T
= P−
k −PxzKTk −KkPzx + KkPzzK
Tk
(5.15)
where Pxz = PTzx is the a priori state-measurement cross-variance and Pzz is the a priori measure-
ment covariance, not to be confused with the true measurement covariance, Rk. Defining tr(P+k
)as a
cost function (or in other words, by minimizing the state quadratic error), the Kalman gain that minimizes
this expression is the solution of
∂tr(P+k
)∂Kk
= 0⇔ −2Pxz + 2KkPzz = 0 (5.16)
Which yields
Kk = PxzP−1zz (5.17)
This solution can be verified as being a minimum, by calculating the second derivative of the cost
function,
∂2tr(P+k
)∂K2
k
= Pzz > 0 (5.18)
Which means that the solution expressed in equation 5.17 is in fact a minimum. The a posteriori
covariance then can be further simplified as
28
P+k = P−
k −Pxz
(P−1zz
)TPxz −PxzP
−1zz Pzx + PxzP
−1zz Pzz
(P−1zz
)TPzx
= P−k − 2PxzP
−1zz Pzx + PxzP
−1zz Pzx
= P−k −PxzP
−1zz Pzx
(5.19)
Additionally, PxzP−1zz Pzx = KkPzzK
Tk .
P+k = P−
k −KkPzzKTk (5.20)
Algorithm 3 surmises the Unscented Kalman Filter.
Although this algorithm is considered better behaved than the EKF, it’s implementation comes at a
cost in the processing load. Even thought both estimators have an order of complexity of n2, the UKF
generally has a computation load increase of about 40% [12]. As a result, the choice of this filter over
the Extended Variant must be supported on a performance/cost trade-off.
29
Algorithm 3 Unscented Kalman FilterA Posteriori Sampling Step
1: Factorize a posteriori state covariance matrix
P+k−1 =
√P+k−1
√P+k−1
T
2: Evaluate a posteriori sigma vectors[X (0)+k−1 · · · X (2n)+
k−1
]=[x+k−1 ±(
√n+ κ)
(√P+k−1
)]Prediction Step
1: Propagate a posteriori sigma vectors
X (i)−k = f
(X (i)+k−1
), i = 0, · · · , 2n
2: Propagate state vector
x−k =
2n∑i=0
WiX (i)−k
3: Propagate covariance matrix
P−k =
2n∑i=0
Wi
(X (i)−k − x−
k
)(X (i)−k − x−
k
)T+ Qk
A Priori Sampling Step1: Factorize a priori state covariance matrix
P−k =
√P−k
√P−k
T
2: Evaluate a priori sigma vectors[X (0)−k · · · X (2n)−
k
]=
[x−k ±(
√n+ κ)
(√P−k
)]Filtering Step
1: Predict measurementZ(i)−k = h
(X (i)−k
)z−k =
2n∑i=0
WiZ(i)−k
2: Evaluate Kalman gain
Pzz =
2n∑i=0
Wi
(Z(i)−k − zk
)(Z(i)−k − zk
)T+ Rk
Pxz =
2n∑i=0
Wi
(X (i)−k − xk
)(Z(i)−k − zk
)TKk = PxzP
−1zz
3: Update state vectorx+k = x−
k + Kk
(zk − z−k
)4: Update covariance matrix
P+k = P−
k −KkPzzKTk
30
Chapter 6
INS Kinematics
In this chapter, the main INS kinematic equations will be deduced. These equations are sometimes
called the INS Mechanization or simply the INS equations and they express the relationship between
the inertial quantities (specific forces and angular velocities) and the navigation information requested
by the user (position, velocity and attitude).
These equations do not strive to minimize and filter out erroneous measurements from the IMU, since
this is the objective of the error propagation model in chapter 7. The aim of the INS kinematics is just to
estimate a navigation solution as fast as possible, as to keep up with the output frequency of the IMU.
In section 6.1 a first iteration on the state selection is made. In section 6.2 the kinematic equations
will be deduced in the ECI frame as a starting point. In section 6.3 these equations will be transferred to
the ECEF frame to be used in the following chapters. Finally in section 6.4 the gravity field model to be
employed is described.
6.1 State Design
At this point, it should be defined what are the states of the problem. Obviously, expected output (posi-
tion, velocity and attitude) should be included in the states.
x =
r
v
q
(6.1)
Recalling that quaternions are comprised of 4 components, q =[q0 q1 q2 q3
]T, the state vector
adds up to 10 variables,
x =[rx ry rz vx vy vz q0 q1 q2 q3
]T(6.2)
As a result, the INS kinematics block will present the following form,
31
r
v
q
=
fr(r,v,q, f
b,ωbib)
fv(r,v,q, f b,ωbib)
fq(r,v,q, f b,ωbib)
(6.3)
where f b and ωbib are the accelerometer and gyroscope measurements respectively. The following
sections will deduce the expressions of fr(), fv() and fq().
6.2 INS Kinematics in ECI
The Kinematic equations will first be derived in the ECI frame. This will only be a starting point, meaning
that the desired equations will be deduced from the ECI equations.
Position kinematics in ECI
The position equation can be trivially obtained from the definition of velocity in classical mechanics,
ri = vi. (6.4)
Velocity kinematics in ECI
By definition, the inertial frame is where the Newton’s Laws of Mechanics can be applied. As a result of
Newton’s Second Law,
vαi = fα + γα(r). (6.5)
where f describes all specific forces and γ specifies all gravitational forces, which is dependent only
on the position. The superscript α refers to the frame which a given physical quantity is expressed, in
this case, it is left unspecified.
The IMU sensors will output measurements in the body axis. As a result, it is useful to express the
kinematic equations as a function of the measurements in the body frame,
vαi = Cαb f b + γα(r). (6.6)
Attitude kinematics in ECI
The attitude dynamics can be described by the following equation,
Cαb = Ωα
αbCαb = −Ωα
bαCαb = Cα
b Ωbαb (6.7)
Here the conversion to a form where the raw gyroscope measures (ωbib) can be employed in not as
trivial. The relations between the i-frame, b-frame and α is represented in figure 6.1.
As a result, it can be stated that
32
Cαb
Cαi
Cib
α
i
b
Figure 6.1: Relationship between i-frame, b-frame and α frame.
Cαb = Cα
i Cib (6.8)
Which can be differentiated as,
Cαb = Cα
i Cib + Cα
i Cib (6.9)
By the application of the DCM differentiation rules, the following expression is derived
Cαb = −Ωα
iαCαb + Cα
b Ωbib (6.10)
where it is assumed that Ωαiα is known.
For a quaternion implementation, it should be noted that the attitude kinematics described in equa-
tion 6.7 must be modified as,
qαb =1
2Ωα~αb qαb (6.11)
However, a better approach to this problem is to decompose qαb in two rotations,
qαb = qαi ~ qib (6.12)
which can be differentiated using the product rule,
qαb = qαi ~ qib + qαi ~ qib
=
(1
2Ωααiq
αi
)~ qib + qαi ~
(1
2Ωbibq
ib
)︸ ︷︷ ︸
A
(6.13)
The A term can computed using the quaternion rule b~ c = Bc = Cb, resulting in,
A =1
2Qαi Ωb
ibqib
=1
2Qαi Qi
bωbib
=1
2Qαb ω
bib
=1
2Ωbibq
αb
(6.14)
33
where, Q is the matrix representation of the Hamilton multiplication with it’s corresponding quater-
nion.
Finally, equation 6.13 can be written as,
qαb =1
2
(Ωα~αi + Ωb~
ib
)qαb (6.15)
6.3 INS Kinematics in ECEF
For the deduction of the ECEF equations, it is useful to consider the Coriolis equation, given by,
xα = xβ + Ωαβxα. (6.16)
Where x is an arbitrary vector. α and β are arbitrary frames. From this equation, it possible to deduce
the e-frame kinematic equations from their i-frame form.
Position kinematics in ECEF
The position equation can again be trivially deduced from classical mechanics. Hence, the user position
in relation to the ECEF frame and expressed in the arbitrary coordinate system α is given by,
rαe = vαe (6.17)
At this point, it is safe to specify the frame where the variable will be expressed, which will be the
ECEF frame. As result, α = e can be employed,
ree = vee (6.18)
Velocity kinematics in ECEF
Applying equation 6.16 to the position rαi yields,
rαi = rαe + Ωαier
αe ⇔ vαi = vαe + Ωα
ierα (6.19)
It should be noted that rα = rαi = rαe , since the origin of both frames coincide with the center of mass
of the Earth. The same can not be said about the velocities.
Differentiating equation 6.19 in the inertial frame, yields
vαi =
(∂vαe∂t
)i
+ Ωαier
α + Ωαier
αi (6.20)
Where(∂∂t
)i
delineates a time differentiation in the inertial frame. Note that(∂ve∂t
)i6= ri, since the
latter is differentiated twice on the inertial frame, while the former is first differentiated in ECEF frame, and
34
only then in inertial frame. The cross differentiation term,(∂vαe∂t
)i, is resolved by the Coriolis equation,
(∂vαe∂t
)i
= vαe + Ωαiev
αe . (6.21)
The angular acceleration term, Ωαier
α is null since the earth rotation is considered constant,
Ωαier
α = 0 (6.22)
The Ωαier
αi term can be resolved from equation 6.19 as,
Ωαier
αi = Ωα
ie (rαe + Ωαier)
= Ωαier
αe + Ωα
ieΩαier
= Ωαiev
αe + Ωα
ieΩαier
(6.23)
Finally, the vαi term is given by equation 6.6. Consequently, substituting equations 6.6, 6.21, 6.22
and 6.23 in equation 6.20 results in,
γα(r) + Cαb f b = vαe + 2Ωα
ievαe + Ωα
ieΩαier
⇔ vαe = γα(r)−ΩαieΩ
αier− 2Ωα
ievαe + Cα
b f b(6.24)
Due to their dependency to r, it is customary to group the γα and −ΩαieΩ
αier term into the same vari-
able, gα(r) = γα(r)−ΩαieΩ
αier
α. This new quantity is referred to as the local gravity [36]. Equation 6.24
can now be expressed in a reference frame, which is chosen to be the ECEF frame.
vαe = gα(r)− 2Ωαiev
αe + Cα
b f b (6.25)
Finally, this equation can be expressed in the e-frame as,
vee = ge(r)− 2Ωeiev
ee + Ce
bfb (6.26)
Attitude kinematics in ECEF
As for the attitude, it should stated that equation 6.15 is completely general, meaning that it can be
applied to e-frame as is. Expressing it in the e-frame yields
qeb =1
2
(Ωeei + Ωb
ib
)qeb. (6.27)
Additionally, it should be noted that Ωeei = −Ωe
ie, hence the previous equation can be rewritten once
again as,
qeb =1
2
(Ωbib −Ωe
ie
)qeb. (6.28)
35
Summary of ECEF Kinematic Equations
To surmise, the ECEF kinematic equations are
re
ve
qeb
=
ve
ge(re)− 2Ωeiev
e + C(qeb)fb
12
(Ωbib −Ωe
ie
)qeb.
(6.29)
where C(qeb) = Ceb. It is easy to observe that the velocity equation is non-linear, at least in it’s attitude
component.
6.4 Gravity Field Model
To compute the g(r) term in the kinematic model, a gravity field model must be implemented. This
section will describe the Somigliana model, employed by the WGS84 datum [20].
According to this model, the acceleration due to gravity at the ellipsoid is given as function of the
latitude φ,
g0 = 9.78032533591 + 0.001931853 sin2 φ√
1− e2 sin2 φ[ms2] (6.30)
where e = 0.0818191908425 is the Earth eccentricity. This acceleration can be approximating as
being in along the local tangent frame down axis, uD. The gravitational acceleration is calculated by
removing the effect of the centrifugal acceleration, or in other words,
γ0 = g0uD + ΩieΩiereS (6.31)
where reS is the geocentric radius at the surface, given by,
reS = R0
√cos2 φ+ (1− e2)2 sinφ
1− e2 sin2 φuD (6.32)
where R0 = 6378137[m] is the Earth Radius at the equator. To account for gravity field variation along
the Up/Down axis, a height factor can be applied,
γib ≈(
reSreS + hb
)2
γ0 (6.33)
where hb is geodetic height, i.e. the height measured from the body to the ellipsoid surface. The
gravity term can finally be calculated using,
geb = γeib −ΩeieΩ
eier
eeb = γeib + ω2
ie
1 0 0
0 1 0
0 0 0
reeb (6.34)
36
Chapter 7
Error Propagation Model
In this chapter the error propagation model will be described. In section 7.1, the two main approached
for INS error modeling will be reviewed and one will be selected. In section 7.2, an initial version of this
model will be implemented, employing only navigation states. In section 7.3, a more advance version
will be implemented, taking into account the IMU sensor errors. In section 7.4, we used the previous
section to build a discrete dynamics model. Finally, in section 7.5, the observation model is build.
7.1 Error Modeling Approach
Two different approaches are possible to model the INS error propagation:
• Phi-angle approach
• Psi-angle approach
The distinction between them is in what frame the INS equations are perturbed. In the phi-angle
approach, the true frame is perturbed [6, 37]. In the psi-angle, perturbs the platform frame into the
erroneous computer frame [5, 7]. This thesis will develop only the psi angle approach.
7.2 Basic Error Propagation Model
As stated in section 5.2.2, error variables will be used in the kalman filter. First of all it should be defined
what encompasses an error variable. Bellow are the error definitions of the position, velocity and attitude
quaternion.
δr = ree − ree
δv = vee − vee
δq = qcb = qce ~ qeb
(7.1)
For the orientation, the c-frame is employed, which replaces the b-frame on the estimated INS kine-
matic equations. The rotation described by qcb is called the psi-angle, from which the error model is
37
named after.
The state vector of the error dynamics model is,
x =
δr
δv
qcb
(7.2)
The following subsections will derivate the propagation model of each of these error variables.
Position Error Propagation
The position error model can trivially be obtained by
δr = ree − ˙ree (7.3)
where ree = vee and ˙ree = vee, resulting in,
δr = vee − vee = δv (7.4)
Velocity Error Propagation
For the velocity, the process isn’t as straightforward. The velocity estimate is given by
˙v = g − 2Ωeiev
ee + Ce
c fb. (7.5)
It should be noted that there is mismatch in the coordinate transformation, Cec fb, due to fact that the
INS processor is not aware of the true orientation of the body.
It is advantageous to also define the error forms of the local gravity and specific form term. These
versions will be described by
δg = g − g
δf = f − f .(7.6)
The velocity error can then be given by [21],
δv = v − ˙v
=(ge − 2Ωe
ievee + Ce
bfb)−(ge − 2Ωe
ievee + Ce
c fb)
= (ge − ge)− 2Ωeie (vee − vee)︸ ︷︷ ︸
A
+(Cebfb −Ce
c fb)
︸ ︷︷ ︸B
(7.7)
The A term can be trivially deduced from the error variable definition,
38
A = δg − 2Ωeieδv (7.8)
As per the B term, it should be noted from equation 7.6 that f b = f b − δf b, resulting in,
B = Cebfb −Ce
c
(f b − δf b
)= (Ce
b −Cec) f b + Ce
cδfb
(7.9)
Additionally, rotation Cec can be decomposed into Ce
c = CebC
bc. Equation 7.9 can then be rewritten
as,
B = Ceb
(I−Cb
c
)f b + Ce
cδfb (7.10)
where Cec is the DCM representation of the attitude error, qec.
Substituting equations 7.8 and 7.10 into equation 7.7, yields,
δv = δg − 2Ωeieδv + Ce
b (I−Cec) f b + Ce
cδfb (7.11)
The δf is equivalent to the uncalibrated accelerometer error. One approximation that can be made
is to consider this term as being white noise, creating a correspondence between this term and the dy-
namics process noise wk. A more accurate estimate, based on sensor modeling is given in section 7.3.
Error due to the Gravity Vector
Neglecting error due to the gravity model used, the local gravity error is given by,
δg = g − g
= (γ(r)−ΩeieΩ
eier)− (γ(r)−Ωe
ieΩeier)
= (γ(r)− γ(r))−ΩeieΩ
eieδr
(7.12)
The true gravitational field from the Taylor expansion,
γ(r) = γ(r) +∂γ
∂r
∣∣∣∣r
δr +O(δr2) (7.13)
Neglecting the higher order terms, the following can be stated,
γ(r)− γ(r) =∂γ
∂r
∣∣∣∣r
δr (7.14)
which can be substituted in equation 7.12, resulting in,
δg =
(∂γ
∂r
∣∣∣∣r
−ΩeieΩ
eie
)δr (7.15)
39
For the deduction of ∂γ∂r
∣∣∣∣r
, it is simpler to use Newton’s law of universal gravitation,
γ(r) = −GM|r|3
r = − GM
(rT r)32
r (7.16)
where G is gravitational constant and M is earth mass. This can be differentiated as [38],
∂γ
∂r
∣∣∣∣ = −GM ∂
∂r
(1
(rT r)32
r
)= −GM
[∂
∂r
(1
(rT r)32
)r +
1
(rT r)32
∂r
∂r
]= −GM
[− 3rrT
(rT r)32
+1
(rT r)32
I
] (7.17)
Denoting R = |r| and r =[rx ry rz
], the gravitational field derivative can be applied around r = r,
∂γ
∂r
∣∣∣∣r
=−GMR5
R2 − 3r2x −3rxry −3rxrz
−3rxry R2 − 3r2y −3ry rz
−3rxrz −3ry rz R2 − 3r2z
(7.18)
Attitude Error Propagation
As stated in subsection 2.1.2, the quaternion qcb satisfies the following differential equation,
qcb =1
2Ωb~cb qcb (7.19)
where Ωbcb = [ωbcb~]. By definition, ωbcb represents the rate of change of the psi-angle, hence ωbcb =
ψ =[ψx ψy ψz
]T.
Equation 7.19 can be represented in the matrix form as,
qcb =1
2
−q1 −q2 −q3q0 −q3 q2
q3 q0 −q1−q2 q1 q0
ψx
ψy
ψz
= Bψ (7.20)
where,
B =1
2
−q1 −q2 −q3q0 −q3 q2
q3 q0 −q1−q2 q1 q0
(7.21)
The psi angle model states that [11],
ψ = (I−Cbc)ω
cic − δωbib (7.22)
40
where
δωbib , gyroscope uncalibrated error
Resulting in,
qcb = B(I−Cb
c
)ωcic −Bδωbib (7.23)
7.3 Augmented States
The focus of this section is to derive a model for the instrument errors, δf b and δωbib. To this end, the
state vector must be augmented to encompass sensor error parameters,
x =
δr
δv
qcb
δxa
δxg
(7.24)
where, xa and xg are respectively the accelerometer parameters and the gyroscope parameters.
These can be, for an example the bias and scale factor errors of each individual sensor, the misalignment
of the sensor axis in relation to the IMU platform and the non-linearities of each sensor up-to a certain
order.
For this implementation, the following sensor parameters were chosen,
xa =
ba
sfa
ma
; xg =
bg
sfg
mg
(7.25)
where b is the bias of each sensor axis, sf is the scale factor of each sensor axis and m is the
misalignment of each sensor axis in relation to the remaining axes in the same sensor cluster.
Assuming that these parameters present a linear contribution in the δv and qcb equations,
δf b = Fvaδxa + νa
δωcic = Fpgδxg + νg
(7.26)
where νa and νg are respectively the accelerometer measurement noise and gyroscope noise, which
are accurately modeled as white Gaussian noise. The covariance of these noise components can be
used as tuning parameters.
Additionally, it can be assumed that these parameters present a linear dynamics model, or in other
41
words,δxa = Faaδxa + ωa
δxg = Fggδxg + ωg
(7.27)
where ωa and ωg are respectively the accelerometer instrument noise and gyroscope instrument noise,
which is generally provided by the manufacturer.
The objective of this section is to calculate the values of Fva, Fpg, Faa and Fgg. The latter two
matrices may also be provided by the manufacturer. To simplify the approach, Faa and Fgg were defined
as,
Faa = 012×12; Fgg = 012×12 (7.28)
which means that in the absence of instrument noise, the IMU sensor errors will be constant throughout
a run.
7.3.1 Accelerometer Error Modeling
Up to this point, f b = fp as been treated as the accelerometer output, however, this si actually an
abstraction. To be more precise, the output is given in the accelerometer cluster axis, denoted as a,
which itself is misaligned from the platform axis,
fp = Cpafa (7.29)
By assuming that Cpa is a small angle rotation, the following relation can be stated,
Cpa ≈ I3×3 + 4p
a = I3×3 −4ap (7.30)
where 4ap is a small angle component, given by,
4ap =
0 −axy axz
ayx 0 −ayz−azx azy 0
(7.31)
Equation 7.29 can then be rewritten as,
fp =(I3×3 −4a
p
)fa (7.32)
Through calibration, the specific force can be estimated in the following manner,
fp =(I3×3 − 4a
p
)fa (7.33)
where 4ap is the calibration of the accelerometer axis misalignment, fp is the specific force estimate
along the platform axis and fa are accelerometer measurements. Moreover, fa present it’s own errors
which can be modeled as,
42
fa = (I3×3 − δsfa) (fa − δba − νa) (7.34)
where δba is the accelerometer bias error and δsfa is the accelerometer scale factor error.
Equation 7.33 can then be expressed as a function of the true specific force fa [21],
fp =(I3×3 − 4a
p
)(I3×3 − δsfa) (fa − δba − νa)
=
I3×3 − 4ap − δsfa + 4a
pδsfa︸ ︷︷ ︸≈0
(fa − δba − νa)
=(I3×3 − 4a
p − δsfa)
fa − δba − νa +(4ap + δsfa
)(δba + νa)︸ ︷︷ ︸
≈0
(7.35)
where errors of second order or higher were neglected, resulting in,
fp =(I3×3 − 4a
p − δsfa)
fa − δba − νa (7.36)
The specific force error can finally be resolved as,
δfp = fp − fp
=(I3×3 −4a
p
)fa −
(I3×3 − 4a
p − sfa
)fa + δba − νa
=
δsfa − (4ap − 4a
p
)︸ ︷︷ ︸
δma
fa + δba − νa
(7.37)
where δma is the misalignment error. This equation can be written as,
δfp =
δsfax −δmxy δmxz
δmyx δsfay −δmyz
−δmzx δmzy δsfaz
fa +
δbx
δby
δbz
+ νa (7.38)
Finally, this can be expressed as a linear equation in respect to xa,
δfp =
1 0 0 fx 0 0 −fy fz 0 0 0 0
0 1 0 0 fy 0 0 0 fx −fz 0 0
0 0 1 0 0 fz 0 0 0 0 −fx fy
δba
δsfa
δma
+ νa (7.39)
This is equivalent to the targeted accelerometer error model,
δfp = Fvaδxa + νa (7.40)
To surmise the contributions of this subsection,
43
Fva =
1 0 0 fx 0 0 −fy fz 0 0 0 0
0 1 0 0 fy 0 0 0 fx −fz 0 0
0 0 1 0 0 fz 0 0 0 0 −fx fy
(7.41)
7.3.2 Gyroscope Error Modeling
The deduction of the gyroscope error model is identical to the accelerometer error model. Conse-
quently, only the final model will be presented. The deduction of this specific model is also presented by
Groves [21].
The gyroscope error model is expressed by the following equation,
δωpip =
1 0 0 ωx 0 0 −ωy ωz 0 0 0 0
0 1 0 0 ωy 0 0 0 ωx −ωz 0 0
0 0 1 0 0 ωz 0 0 0 0 −ωx ωy
δbg
δsfg
δmg
+ νg (7.42)
where ωgig =[ωx ωy ωz
]Tis the raw gyro measurements. The Fqg matrix can then be written as,
Fqg =
1 0 0 ωx 0 0 −ωy ωz 0 0 0 0
0 1 0 0 ωy 0 0 0 ωx −ωz 0 0
0 0 1 0 0 ωz 0 0 0 0 −ωx ωy
(7.43)
7.3.3 Higher Order Sensor Errors
In total, augmenting the KF with zeroth and first order error states (bias, scale-factor and misalignment),
leads to an increase of 12 states per sensor, hence the total increase is of 24 states.
A more accurate error estimating can be performed taking into account non-linearities of the sensor
measurements, however, this will lead to an even higher increase in the state number, more specifically
an increase of 19 states for the accelerometer and an increase of 27 states for the gyroscope, leading
to an total increase of 46 states [38], which is the reason why no further error modeling is investigated
in this thesis.
7.4 Dynamics Model
The implemented dynamics model is given by the following equation in continuous time,
44
x(t) =
δv
∂g∂r δr− 2Ωieδv + Ce
b(I−Cbc)f
b + CebC
bcFvaδxa
B(I−Cbc)ω
cic −BFqgδxg
0
0
+
03×3 03×3 03×12 03×12
CebC
bc 03×3 03×12 03×12
04×3 −B 04×12 04×12
012×3 012×3 I12×12 012×12
012×3 012×3 012×12 I12×12
νa
νg
ωa
ωg
(7.44)
Which can be represented in a more compact way as,
x(t) = fc(x, t) + Gc(t)u(t) (7.45)
This model can be converted to discrete time by considering the Taylor series,
x(tk+1) = x(tk) +4tx(tk) +O(4t2) (7.46)
Neglecting higher-order terms, results in,
x(tk+1) = x(tk) +4tfc(x, tk) +4tGc(tk)u(tk) (7.47)
The 4tGc(tk)u(tk) term can be further approximated as white noise wk of covariance,
Qk ≈ Gc(tk)
Qνa 03×3 03×12 03×12
03×3 Qνg 03×12 03×12
012×3 012×3 Qωa 012×12
012×3 012×3 012×12 Qωg
GTc (tk)4t (7.48)
where the Qνa , Qνg , Qωa and Qωg matrices are the covariances of each process noise component,
given by,
Qνa =
σ2νa,x 0 0
0 σ2νa,y 0
0 0 σ2νa,z
; Qωa =
σ2ωa,0 0 · · · 0
0 σ2ωa,1
.... . .
0 σ2ωa,11
Qνg =
σ2νg,x 0 0
0 σ2νg,y 0
0 0 σ2νg,z
; Qωg =
σ2ωg,0 0 · · · 0
0 σ2ωg,1
.... . .
0 σ2ωg,11
(7.49)
the σ2 quantities can be used as turning parameters. The discrete dynamic model can then be written
in a way that’s usable by the UKF:
45
xk+1 =
δr +4tδv
δv +4t(∂g∂r δr− 2Ωieδv + Ce
b(I−Cbc)f
b + CebC
bcFvaδxa
)qcb +4t
(B(I−Cb
c)ωcic −BFqgδxg
)δxa
δxg
+ wk (7.50)
7.5 Observation Model
For a loosely coupled integration, the navigation system must rely only on the position and velocity output
of the GPS for the filtering step. Since we are estimating the errors of the INS/GNSS solution, the KF
measurements are of the form:
z =
rGPS − rINS
vGPS − vINS
=
δrδv
(7.51)
As a result, the observation model will be a linear system,
zk =
I3×3 03×3 03×4 03×12 03×12
03×3 I3×3 03×4 03×12 03×12
xk + vk (7.52)
This model assumes that the GPS antenna is placed closely enough to the IMU. If this isn’t the case,
lever-arm compensation techniques must be applied. The vk is white noise, with covariance,
Rk =
QrGPS 03×3
03×3 QvGPS
(7.53)
where the QrGPS and QvGPS matrices are the covariances of each GPS measurement, given by,
QrGPS =
σ2rGPS,x 0 0
0 σ2rGPS,y 0
0 0 σ2rGPS,z
; QvGPS =
σ2vGPS,x 0 0
0 σ2vGPS,y 0
0 0 σ2vGPS,z
(7.54)
46
Chapter 8
Solution Validation Through
Simulation
To validate the navigation system conceived in the previous chapters, simulations were used. This also
enabled an iterative approach to the design, where each decision could be backed from the comparison
of each decision option to the typical benchmarks of the INS/GNSS solution.
In this chapter, the implemented simulation framework is described in section 8.1. In section 8.2,
the various simulated cases will be presented. Finally, in section 8.3, the navigation solution along each
trajectory is presented and discussed.
8.1 Simulation Framework
The implemented simulation framework is able to recreate a trajectory comprised of:
• Stationary periods, where the simulated body is stationary for a certain time.
• Linear motion, where the simulated body is exposed to a constant acceleration vector.
• Circular motion, where the simulated body is exposed to a constant angular velocity error.
The resulting trajectory will generate two measurement files, a GPS file containing only UBX-SOL
messages [39] and an IMU file containing messages with the following structure:
$INS,ACC X,ACC Y,ACC Z,GYRO X,GYRO Y,GYRO Z<CR><LF>
Where ACC and GYRO are raw simulated accelerometer and gyroscope measurements. The IMU
file also takes into account the effects of gravity and the earth rotation.
The errors of each sensor are also modeled. In the case of the GPS, a white noise with a variance
specified by the user is added to the measurements. In the case of the IMU, two different components
are modeled for each individual sensor: random output noise and deterministic uncalibrated sensor
errors.
47
8.2 Simulated Cases
Four different algorithms are compared in this section, all of which employ Loosely Coupled architecture
and a non-linear Indirect Kalman Filter:
• 10 state EKF, whose implementation is specified in appendix A;
• 10 state UKF, whose implementation is specified in appendix B;
• 34 state augmented EKF, denoted as AEKF. The implementation of this filter is specified in ap-
pendix C;
• 34 state augmented UKF, denoted as AUKF. The implementation of this filter was specified in
sections 7.4 and 7.5.
The 10 state versions only estimate the position, velocity and attitude quaternion, whereas the aug-
mented versions also contains the IMU sensor errors up-to the first-order model, i.e. bias, scale factor
and misalignment.
All simulations were performed with the following instrument noise:
Table 8.1: Simulated instrument noise.
Instrument Standard Deviation Unit
GPS Position 10 mGPS Velocity 0.1 m/s
Accelerometer 1× 10−2 m/s2
Gyroscope 5× 10−5 rad/s
Moreover, deterministic calibration errors are applied to each IMU sensor:
Table 8.2: Simulated IMU calibration error.
Instrument Error Value Unit
Bias 0.1 m/s2
Accelerometer Scale Factor 0.01 adimensionalMisalignment 0.01 adimensional
Bias 0.1 rad/sGyroscope Scale Factor 0.06 adimensional
Misalignment 0.02 adimensional
The IMU values of tables 8.1 and 8.2 were based on the ADXL345 accelerometers ITG-3200 gyro-
scopes [40, 41], which are employed in the experimental trials of chapter 9.
Only two trajectory cases will be studied, a relatively slow uniform circular trajectory on horizontal
plane in subsection 8.2.1 and a high-dynamic trajectory, which tries to emulate the take-off of an aircraft
in subsection 8.2.2.
8.2.1 Case 1 - 2D Circular Motion
Figure 8.1 presents the trajectory which compromise test case 1. The trajectory is comprised of different
segments:
48
• A: Vehicle accelerates East at 0.5[m/s2], until it has traveled 10[m].
• B: Vehicle continues travelling East at constant velocity, until it has traveled an additional 25[m].
• C: Vehicle starts curving left at turn rate of 5[o/s], until it has turned 270o, when it’s facing South.
Start Point A B
End Point
C
E
N
Figure 8.1: Case 1 path.
This test case serves as a sanity check, since it’s a very simple path, most algorithms should be able
to run it without issue. IMU measurements of a Monte Carlo iteration are given in figure 8.2. It can be
observed that the gyroscope uncalibrated errors are substantial, in relation to random noise. The same
can’t be said about the accelerometer, where the random noise dominated sensor error.
(a) Accelerometer raw measurements for case 1. (b) Gyroscope raw measurements for case 1.
Figure 8.2: Raw IMU measurements in case 1.
49
8.2.2 Case 2 - 3D Hybrid Motion
Figure 8.3 presents the trajectory which compromises test case 2. The trajectory is comprised of differ-
ent segments:
• A: Vehicle accelerates North at 1[m/s2], until it has traveled 5[m].
• B: Vehicle continues travelling North at constant velocity, until it has traveled an additional 50[m].
• C: Vehicle starts curving upward at a turn rate of 10[o/s], until it has turned 30o,
• D: Vehicle starts curving downward at a turn rate of 10[o/s], until it has turned 30o, when the vehicle
is levelled once more.
• E: Vehicle continues travelling North at constant velocity, until it has traveled an additional 100[m].
• F: Vehicle starts curving right at a turn rate of 5[o/s], until it has turned 270o, when it’s facing West.
• G: Vehicle travels westward at constant velocity, until it has traveled an 80[m].
• H: Keeping the same heading, the vehicle accelerates once more at 10[m/s2], until it has traveled
an 5[m].
• I: The vehicle de-accelerates once more at −10[m/s2], until it has traveled an 5[m].
• J: Vehicle starts curving left at turn rate of 10[o/s], until it has turned 90o, when it’s facing South.
• K: While traveling southward, the vehicle accelerates at 2[m/s2], until it has traveled 20[m].
• L: Vehicle travels southward at a constant velocity, until it has traveled 145[m].
Start PointA
B
C
D
E
F
GHIJ
K
L
End PointE
N
Figure 8.3: Case 2 path.
This trajectory is equivalent to the take-off of an aircraft. Furthermore, path sections H-K present a
high dynamic range, comprising of fast variations in the accelerations and angular velocity felt by the
simulated vehicle, in contrast to the initial path sections, A-G. This simulation case is designed in such
a way so the filters it can tested for both low and high dynamic motions.
The IMU measurements of a Monte Carlo iteration are given in figure 8.4.
50
(a) Accelerometer raw measurements for case 2. (b) Gyroscope raw measurements for case 2.
Figure 8.4: Raw IMU measurements in case 2.
8.3 Simulation Results and Discussion
8.3.1 Case 1 Results
The results of the algorithms in a single run are represented in figure 8.5
Figure 8.5: Algorithm comparison for case 1.
51
It can be observed that, in spite of the low-quality IMUs, the estimation has improved over the GPS
only solution. A more precise comparison of the relative performance can be observed in table 8.3,
where the Root Mean Square Error (RMSE) is presented.
Table 8.3: RMSE of each implementation for 100 Monte Carlo iterations for test case 1.
Algorithm Position RMSE [m] Improvement2 [%]
Actual GPS1 5.64842 -
Theoretical GPS1 5.47723 -
EKF 4.84217 11.6
AEKF 4.81594 12.1
UKF 4.44444 18.9
AUKF 3.77275 31.1
1 The theoretical GPS refers to extending the normally 1Hz GPS
solution to the IMU frequency (100Hz), whereas the actual GPS
compares the true path with the last available GPS solution. The
latter value would increase at higher-speed motions, making it a
inappropriate baseline, hence the need for the use of the former
parameter.2 Relative to the theoretical GPS solution.
As expected, the UKF performed better than the EKF and the augmented versions of these filters out-
performed their simpler counterparts. It should also be noted that the improvement from the augmented
EKF in relation to the basic EKF is barely noticeable (0.5%), whereas on the UKF the improvement from
the augmented states is considerable (12.2%).
The filter states are expected to present relatively high values during the initial phases of the trajec-
tory as a result of the initial alignment errors and the instantaneous increment on the acceleration that
results in the simulated motion. After this, the filter states should converge to zero, including the sensor
errors states of the augmented filters. Beyond that, a small deviation may be expected at the 15s mark,
since this is when the vehicle starts to turn.
Figure 8.6 illustrates the actual evolution of the KF error states along the trajectory. All filters present
relatively high corrections during the initial phases of the trajectory, where the vehicle starts accelerating.
After this, the position error starts converging, as expected. Before the curve, position error of the EKF
and AEKF is bounded in the [−7, 5]m interval, while the UKF and AUKF are bounded in the [−2, 2]m.
52
(a) Evolution of EKF error states for case 1. (b) Evolution of UKF error states for case 1.
(c) Evolution of AEKF error states for case 1. (d) Evolution of AUKF error states for case 1.
Figure 8.6: Evolution of the position, velocity and attitude states along simulation case 1.
53
(a) Evolution of AEKF calibration error states for case 1. (b) Evolution of AUKF calibration error states for case 1.
Figure 8.7: Evolution of the sensor calibration error states along simulation case 1.
At the start of the turn (at around the 15s mark), each filter experiences a slight increase in the
position errors as a result of the simulated vehicle starting to curve. The EKF and AEKF present nearly
identical error states all throughout the trajectory. The UKF and AUKF have notable differences, for one
the velocity error of the UKF is bounded in the [−3, 4]m/s interval, while in the UKF this error converges
to a bound of [−1, 1]m/s. It should also be noted that the AUKF was the only filter where the velocity
error seemed to converge to an absolute value lower than 1m/s.
Regarding the calibration errors, it can be observed in figure 8.7, that the AEKF states do not seem
54
to converge, which may explain why the improvement from this augmentation was unsubstantial. In the
AUKF, some state have the same behavior, most notable the accelerometer bias and scale factor, yet
the remaining errors seems to converge to zero.
Comparing the EKF/AEKF to the UKF/AUKF, the performance increase in the unscented approach
can be explained from the approximations needed for the extended filter, which is that the model be
approximately linear. As stated before the non-linearities of the INS error propagation model are all
related to the attitude quaternion terms. Knowing this, it is not surprising to see that the quaternion states
of the EKF/AEKF present a much higher variation than the UKF/AUKF, specially in the q0 component
which nearly constant on the unscented filters. What this means is that the extended filters states are
growing out off the state-region where the linear approximation is valid, leading to the additional errors.
It is also important to check the robustness of each implementation against a situation of GPS outage.
To this end, a GPS outage is implemented in the interval [30; 35]s, which is at about a third of the curve.
The results of this modification are presented on figure 8.8. It can be observed that each filter starts to
accumulate high amounts of error due to the IMU noise and calibration errors.
Of the four filters, only the AUKF can be said to still produce practical results, by deviating from the
true path by around 5m, while all the other solutions gain at least 20m of error, which is consistent with
the expected performance of low-grade IMU only solution. The higher performance of the AUKF is due
to the fact that it can estimate the IMU sensors more accurately than the AEKF, as was stated before.
After the outage, all filters converge back to their pre-outage behavior.
Figure 8.8: Behavior of algorithm in the presence of a 5s GPS outage for simulation case 1.
55
8.3.2 Case 2 Results
The results of the algorithms during one run are represented in figure 8.9, where it can be observed that
the effectiveness of the EKF is questionable.
Figure 8.9: Algorithm comparison for case 2.
The RMSE of the algorithms are given in table 8.4.
56
Table 8.4: RMSE of each implementation for 100 Monte Carlo iterations for test case 2.
Algorithm Position RMSE [m] Improvement1 [%]
Actual GPS 6.11155 -
Theoretical GPS 5.47723 -
EKF 5.43946 0.7
AEKF 5.53496 -1.1
UKF 4.78879 12.6
AUKF 4.74571 13.4
1 Relative to the theoretical GPS solution.
In this situation, it can be observed that the EKF performed poorly, providing an improvement of less
than 1% in it’s basic form and hampering the systems performance in the augmented case, although
both still show a considerable improvement relative to the actual GPS.
The UKF on the other hand shows an improvement of at least 10% in both variations, demonstrating
robustness in relation to the user motion. The augmented version presented an increase of 0.8% in the
improvement, which can be considered negligible.
As with test case 1, it is expected that the filter states will be relatively high in amplitude during
the initial phase of the trajectory and smaller deviation should occur when the there are variation in
the simulated motion. Moreover, additional deviations are to be expected during the [160 − 175]s time
interval, since the vehicle presents a higher range of motion not experienced in the previous phases of
the trajectory.
Figure 8.10 illustrates the evolution of the position, velocity and attitude quaternion error states, which
behaves similarly to case 1. However, at around the 160[s] mark, all filters experience a growth in it’s
errors states. This is due to the high-dynamic sections H-K of the trajectory. Figure 8.11 demonstrates
the same growth in the calibration errors, meaning that the augmented filters can’t precisely distinguish
the position/velocity/quaternion error from the IMU sensor errors. This explains the relative drop in
performance of the augmented filters.
57
(a) Evolution of EKF error states for case 2. (b) Evolution of UKF error states for case 2.
(c) Evolution of AEKF error states for case 2. (d) Evolution of AUKF error states for case 2.
Figure 8.10: Evolution of the position, velocity and attitude states along simulation case 2.
58
(a) Evolution of AEKF calibration error states for case 2. (b) Evolution of AUKF calibration error states for case 2.
Figure 8.11: Evolution of the sensor calibration error states along simulation case 2.
As in test case 1, a GPS outage can be emulated for this trajectory. In this situation, the outage
during the interval [160; 165]s, which is at the start of the high-dynamic section of the trajectory (sections
H-I). Because of the high speeds of the motions, the estimated paths deviate highly from the true path,
with up to 50m of error accumulated. As occurred in case 1, the AUKF presented the best results,
indicating that at least part of the sensor errors were correctly estimated.
59
Figure 8.12: Behavior of algorithm in the presence of a 5s GPS outage for simulation case 2.
8.3.3 Closing Remarks on Simulated Results
All in all, the integration algorithms demonstrated a higher accuracy in comparison to the GPS only
approach.
For low-dynamic range applications, such as pedestrian motion, the improvement of the EKFs were
of around 11 − 12%, with little distinction between the basic 10 state version and 34 state version.
The UKFs consistently demonstrated even higher accuracy, providing an improvement of 18 − 30%. In
contrast to the EKF, the augmented version of the UKF marked a substantial gain in the accuracy of the
system by about 10%.
In a high-dynamic range environment, the performance of the algorithms is partially diminished, yet
improvements in positioning accuracy are still felt in the UKF by around 12− 13%. The augmented EKF
in particular experience a deterioration in it’s accuracy, outputting navigation solution of less quality in
relation to it’s 10 state counterpart. This deterioration was less pronounced in the UKF, whose aug-
mented version still demonstrated an improvement (< 1%), although in an amount that may not justify
it’s computational cost.
60
Chapter 9
Experimental Tests
In this chapter the conceived solutions are tested in an empirical environment.
Section 9.1 describes the equipment used for the experimental tests. Section 9.2 presents the tra-
jectory performed. Finally section 9.3 discusses the results from the trials.
9.1 Equipment Specifications
The equipment used are the following:
• Sparkfun’s 6 DOF IMU Digital Combo Board [40, 41]
• u-blox’s EVU-6T [39]
• Teensy 3.5 Development Board [42]
Figure 9.1 illustrates the block diagram implemented. The Teensy microcontroller will initialize the
IMU and receive measurements from both the accelerometer and gyroscope through I2C, after which
it will pre-process the data before re-routing the measurements to the Host PC through an RS-232
serial port. In the pre-processing stage, calibration corrections are applied and the data is converted to
practical units (g to m/s2 and o/s to rad/s).
6-DOF IMU Board
ADXL345
ITG-3200
Teensy µCI2C
Host PC
GPS EVU-6T
RS-232
RS-232
Figure 9.1: Block diagram of experimental measurement system.
The implementation of this block diagram is presented in figure 9.2, where a Raspberry Pi 3B (dis-
played on the left side) is being used as a Host PC. On the right side, the u-blox GPS is installed.
61
Figure 9.2: Implementation of experimental measurement system.
The Host PC will also retrieve GPS measurements from a u-blox GPS unit through a different RS-232
serial port. The Host PC saves all measurements of a given path to a file to be processed offline.
6 DOF IMU Digital Combo Board
This breakout board from Sparkfun, contains two MEMS sensor chips: an accelerometer chip, the
ADXL345 and a gyroscope chip, the ITG-3200. The sensor specifications are given in table 9.1.
Table 9.1: Relevant ADXL345 and ITG-3200 specifications at T = 25oC.
Parameter ADXL345 Value Unit ITG-3200 UnitRange ±4 g ±2000 o/sRate 100 Hz 100 Hz
Resolution 7.8 mg 0.0695 o/sNoise Level (x-,y-Axes) 7.8 mg rms 0.38 o/s rmsNoise Level (z-Axes) 11.7 mg rms 0.38 o/s rms
Nonlinearity ±0.5 % ±0.2 %
Note. Adapted from [40, 41].
As can be observed, the IMU is of the automotive grade and it’s gyroscope noise level exceeds the
earth’s rotation velocity, meaning that this IMU cannot accurately align itself in 3-dimensional space.
EVU-6T
The u-blox Evaluation Unit EVU-6T is able to receive and process both GPS and Galileo signals and
output to a host computed through a RS232 port. Additionally this unit is able to output proprietary
messages. One message of note it the UBX NAV-SOL message, which include the ECEF position and
velocity vector, which can be directly processed by the integration algorithm.
62
9.2 Trajectory
The algorithms were tested with the pedestrian trajectory illustrated in figure 9.3. This path is character-
ized by sections with a clear view of sky and sections traveled very close to buildings, specially during
the last phase of path. As a result, it is expected that received GPS signals will degrade, resulting in an
inaccurate GPS solution.
The initial position of this trajectory has the following coordinates:
φ = 38.75973oN
λ = 9.22823oW
(9.1)
Figure 9.3: Pedestrian path used for the experimental trial.
9.3 Experimental Test Results and Discussion
The results given from the algorithms are illustrated in figure 9.4. The filtered solutions present largely
similar behaviors. Even so, the AUKF seem to give a slightly more accurate solution. During the initial
stages, the GPS measurements are noisy, but the filters are able to correctly estimate the actual motion
at this phase and through most of the trajectory.
63
Figure 9.4: Algorithm comparison for experimental trajectory.
However during the last phase, the GPS measurements greatly deviate from the true positions, to
the extent that all the filters start outputting equally incorrect estimations.
The evolution of KF states are given in figures 9.5 and 9.6. Similar to what happened in the simulated
cases, the error states present an initially high value, due to incorrect alignment, after which the error
states start converging. The position error state converge to a [−2, 2]m bound.
At around the 110s mark, every error state presents an increase in it’s absolute value as a result
of the degradation in the GPS solution. This is not the same as a GPS outage, since the GPS unit is
still able to output position solutions. The GPS/INS algorithm is unaware of the decrease in accuracy
of the GPS solution, meaning that the integration algorithm is lead to error by the GPS solution. This
is a point of improvement in the implemented integration algorithm, one possible solution is to reject
GPS solutions whose accuracy is lower than an arbitrary threshold, entering a situation of GPS outage.
However, the errors of this situation grow rapidly as demonstrated in the simulation study. An alternative
solution is specified by Caron et al., where the estimator would be based on a GPS confidence level by
which fuzzy logic rules can be established [43].
64
(a) Evolution of EKF error states for the experimental tra-jectory.
(b) Evolution of UKF error states for the experimental tra-jectory.
(c) Evolution of AEKF error states for the experimentaltrajectory.
(d) Evolution of AUKF error states for the experimentaltrajectory.
Figure 9.5: Evolution of the position, velocity and attitude states along the experimental trajectory.
65
(a) Evolution of AEKF calibration error states for the ex-perimental trajectory.
(b) Evolution of AUKF calibration error states for the ex-perimental trajectory.
Figure 9.6: Evolution of the sensor calibration error states along the experimental trajectory.
66
Chapter 10
Summary and Conclusions
This chapter is divided by two sections. Section 10.1 summarizes the main contribution of this thesis,
while section 10.2 describes recommendations for future work.
10.1 Achievements
A GPS/INS integration algorithm was achieved by using a close-looped indirect estimation approach,
based on the evolution of the error values rather than the estimate itself. This included a formulation
of an INS error propagation model, based on the established psi-angle error model. This model was
able to be augmented to provide IMU calibration errors, particularly, accelerometer and gyroscope bias,
scale-factor and misalignment.
Two sets of filters where implemented in the architecture, the EKF and the UKF. Both versions
where benchmarked through a simulation framework, wherein the IMU and GPS measurements could
be emulated in two paths: a slow and uniform 2-D circular motion, and a high-dynamics 3-D motion,
similar to that of an aircraft take-off. From these simulations, it was concluded that the UKF outperformed
the EKF by up to 15%.
The augmented versions of these algorithms demonstrated inconsistent improvement in relation to
theirs simpler version, to be more precise, the improvement from the augmented EKF was negligible
and in some cases it even jeopardized the navigation system; the augmented UKF always resulted in a
increase in accuracy, but the degree of this improvement showed to be dependent on the user motion,
ranging from 1% to 12%. As a result, unless the user motion is known before hand and the filter is tuned
accordingly to that motion, a navigation system would be better off relying on a basic 10 state UKF,
which would come at a significantly lower computational load.
The implemented algorithms were finally put to the test in an empirical environment, where the
relative performance of each filter was confirmed. It should be noted that in the experiments, situations
of large errors for the GPS were also experienced in the implemented systems, which may be due to the
low-quality of the IMU sensors.
67
10.2 Future Work
The implementation described in this thesis can be improved in many ways. The following topics of
improvement are highlighted:
• Fusion with additional sensors: Candidates include odometers, in the case of land vehicles, and
barometric altimeters in the case of aircraft. Additionally, the GPS measurements can be integrated
with measurements from other satellite constellations such as the Galileo or the GLONASS.
• Tightly Coupled Integration: Although the it incurs higher costs and is less modular, it has been
discovered that this approach present accuracy benefits over the Loosely Coupled approach, spe-
cially in terms of accuracy and robustness relative to weak GPS signals. It would be interesting to
benchmark both these implementations and identify the situations where each perform better.
• Adaptive Filter: The process and observation noise have a profound effect on the performance
of the navigation system, requiring extensive tuning. This tuning can be made optimal for a certain
level of dynamics, but to cover all types of motion is near impossible. An alternative would be to
implement a self-tuning filter (Adaptive Filter) to overcome these shortcomings.
• Particle Filters: Different estimators can be implemented in this problem. An alternative is the
particle filter, which relies on successive Monte Carlo simulations. Although this algorithm is ex-
pected to greatly outperform the EKF and UKF, it come at the cost of a high computational load. It
would be interesting to study the trade-offs related to this implementation and compare it with the
adoption of higher quality IMUs.
68
Bibliography
[1] M. Grewal, L. Weill, and A. Andrews. Global Positioning Systems, Inertial Navigation, and In-
tegration. Wiley, 2007. ISBN 9780470099711. URL https://books.google.pt/books?id=
6P7UNphJ1z8C.
[2] B. W. Parkinson, T. Stansel, R. Beard, and K. Gromov. A history of satellite navigation. Navigation,
42(1):109–164. doi: 10.1002/j.2161-4296.1995.tb02333.x.
[3] X. Meng, H. Wang, and B. Liu. A robust vehicle localization approach based on gnss/imu/dmi/lidar
sensor fusion for autonomous vehicles. Sensors, 17:2140, 09 2017.
[4] M. Niculescu. Sensor fusion algorithms for unmanned air vehicles. In Final Program and Abstracts
on Information, Decision and Control, pages 65 – 70, 03 2002. ISBN 0-7803-7270-0.
[5] B. Donald O., Jr. A comparison of two approaches to pure-inertial and doppler-inertial error analy-
sis. Aerospace and Electronic Systems, IEEE Transactions on, AES-11:447 – 455, 08 1975.
[6] T. Manh Pham. Kalman filter mechanization for ins airstart. IEEE Aerospace and Electronic Sys-
tems Magazine - IEEE AEROSP ELECT SYST MAG, 7:516 – 525, 11 1991.
[7] B. M. Scherzinger. Inertial navigator error models for large heading uncertainty. In Record - IEEE
PLANS, Position Location and Navigation Symposium, pages 477 – 484, 05 1996. ISBN 0-7803-
3085-4.
[8] H. Lan, C. Yu, Y. Zhuang, Y. Li, and N. El-Sheimy. A novel kalman filter with state constraint
approach for the integration of multiple pedestrian navigation systems. Micromachines, 2015:926–
952, 07 2015.
[9] Y. Li and J. Wang. A robust pedestrian navigation algorithm with low cost imu. In Indoor Positioning
and Indoor Navigation (IPIN), pages 1–7, 11 2012. ISBN 978-1-4673-1955-3.
[10] K.-W. Chiang, T. T. Duong, and J.-K. Liao. The performance analysis of a real-time integrated
ins/gps vehicle navigation system with abnormal gps measurement elimination. Sensors, 13(8):
10599–10622, 2013. ISSN 1424-8220. doi: 10.3390/s130810599. URL http://www.mdpi.com/
1424-8220/13/8/10599.
[11] X. Kong. Ins algorithm using quaternion model for low cost imu. Robotics and Autonomous Sys-
tems, 46:221–246, 04 2004.
69
[12] Y. Yang, J. Zhou, and O. Loffeld. Quaternion-based kalman filtering on ins/gps. In 15th International
Conference on Information Fusion, FUSION 2012, pages 511–518, 01 2012. ISBN 978-1-4673-
0417-7.
[13] G. Xia and G. Wang. Ins/gnss tightly-coupled integration using quaternion-based aupf for usv. 16:
1215, 08 2016.
[14] H. Sun, J. Fu, X. Yuan, and W. Tang. Analysis of the kalman filter with different ins error models for
gps/ins integration in aerial remote sensing applications. 37:883–890, 01 2008.
[15] F. Hamano. Derivative of rotation matrix direct matrix derivation of well known formula. In Proceed-
ings of Green Energy and Systems Conference, 11 2013.
[16] J. Vince. Quaternions for Computer Graphics. SpringerLink : Bucher. Springer London, 2011. ISBN
9780857297600. URL https://books.google.pt/books?id=CDgrYqDsSBAC.
[17] A. Hanson and S. Cunningham. Visualizing Quaternions. The Morgan Kaufmann Series in Interac-
tive 3D Technology. Elsevier Science, 2006. ISBN 9780080474779. URL https://books.google.
pt/books?id=CoUB09xzme4C.
[18] W. McClain and D. Vallado. Fundamentals of Astrodynamics and Applications. Space Technology
Library. Springer Netherlands, 2001. ISBN 9780792369035. URL https://books.google.pt/
books?id=PJLlWzMBKjkC.
[19] P. Savage. Strapdown Analytics. Number v. 1 in Strapdown analytics. Strapdown Associates, 2000.
ISBN 9780971778603. URL https://books.google.pt/books?id=pltgRAAACAAJ.
[20] National Imagery and Mapping Agency. Department of defense world geodetic system 1984 its
definition and relationships with local geodetic systems. Technical report, National Imagery and
Mapping Agency, 2000.
[21] P. Groves. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. GNSS
technology and applications series. Artech House, 2008. ISBN 9781580532556. URL https:
//books.google.pt/books?id=WssZAQAAIAAJ.
[22] A. Lawrence. Modern Inertial Technology: Navigation, Guidance, and Control. Mechanical Engi-
neering Series. Springer New York, 2001. ISBN 9780387985077. URL https://books.google.
pt/books?id=Sjl1qfyaBWcC.
[23] A. Solimeno. Low-cost ins/gps data fusion with extended kalman filter for airborne applications.
Master’s thesis, Instituto Superio Tecnico, Av. Rovisco Pais, 1049-001 Lisboa, 7 2007.
[24] J. A. Farrell. The Global Positioning System & Inertial Navigation. McGraw-Hill Education, 1999.
ISBN 9780071369442. URL https://books.google.pt/books?id=fW4foWASY6wC.
70
[25] A. El-Rabbany. Introduction to GPS: The Global Positioning System. Artech House mobile commu-
nications series. Artech House, 2006. ISBN 9781596930162. URL https://books.google.pt/
books?id=0sftAAAAMAAJ.
[26] E. Kaplan and C. Hegarty. Understanding GPS: Principles and Applications. Artech House, 2005.
ISBN 9781580538954. URL https://books.google.pt/books?id=-sPXPuOW7ggC.
[27] I. Klein, S. Filin, and T. Toledo. A modified loosely coupled approach to ins/gps integration. Journal
of Applied Geodesy, 5:87–97, 08 2011.
[28] S. Godha, G. Lachapelle, and E. Cannon. Integrated gps/ins system for pedestrian navigation in a
signal degraded environment. In ION GNSS, pages 2151–2164, 01 2006.
[29] D. Simon. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. Wiley, 2006.
ISBN 9780470045336. URL https://books.google.pt/books?id=UiMVoP_7TZkC.
[30] S. Roumeliotis, G. Sukhatme, and G. Bekey. Circumventing dynamic modeling: evaluation of the
error-state kalman filter applied to mobile robot localization. Proceedings - IEEE International Con-
ference on Robotics and Automation, 2:1656–1663, 1 1999. ISSN 1050-4729.
[31] P. Maybeck. Stochastic Models, Estimation, and Control. Mathematics in Science and Engineering.
Elsevier Science, 1982. ISBN 9780080960036. URL https://books.google.pt/books?id=L_
YVMUJKNQUC.
[32] X. Kong. Inertial Navigation System Algorithms for Low Cost IMU. PhD thesis, University of Sydney,
Camperdown NSW 2006, Australia, 8 2000.
[33] S. Julier and J. Uhlmann. A new extension of the kalman filter to nonlinear systems. In Proc. of
AeroSense: The 11th Int. Symp. on Aerospace/Defense Sensing, Simulations and Controls, 1997.
[34] S. Julier, J. Uhlmann, and H. F. Durrant-Whyte. A new method for the nonlinear transformation of
means and covariances in filters and estimators. IEEE Transactions on Automatic Control, 45(3):
477–482, Mar 2000. ISSN 0018-9286. doi: 10.1109/9.847726.
[35] E. Wan and R. Van Der Merwe. The unscented kalman filter for nonlinear estimation. In Pro-
ceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control
Symposium (Cat. No.00EX373), pages 153 – 158, 02 2000. ISBN 0-7803-5800-7.
[36] J. Pusa. Strapdown inertial navigation system aiding with nonholonomic constraints using indirect
kalman filtering. Master’s thesis, Tampere University of Technology, Korkeakoulunkatu 10, FI-33720
Tampere, Finland, 4 2009.
[37] T. Abbas, Z. Yunyan, and L. Yanjun. Sins initial alignment for small tilt and large azimuth misalign-
ment angles. pages 628–632, 05 2011.
71
[38] J. A. Farrell. Aided Navigation: GPS with High Rate Sensors. McGraw-Hill professional en-
gineering: Electronic engineering. McGraw-Hill Education, 2008. ISBN 9780071642668. URL
https://books.google.pt/books?id=yNujEvIMszYC.
[39] u-blox 6 Receiver Description. u-blox, Zuercherstrasse 68 CH-8800 Thalwil Switzerland, seventh
edition, 2013.
[40] ADXL345: 3-Axis, ±2g/±4g/±8g/±16g Digital Accelerometer Data Sheet. Analog Devices, One
Technology Way, P.O. Box 9106, Norwood, MA 02062-9106, U.S.A., fifth edition, 2009.
[41] ITG-3200 Product Specification. InvenSense, 1197 Borregas Ave, Sunnyvale, CA 94089 U.S.A.,
first edition, 2010.
[42] Teensy usb development board, 2018. URL https://www.pjrc.com/store/teensy35.html.
[43] F. Caron, E. Duflos, D. Pomorski, and P. Vanheeghe. Gps/imu data fusion using multisensor kalman
filtering: introduction of contextual aspects. Information Fusion, 7(2):221 – 230, 2006. ISSN 1566-
2535. doi: http://dx.doi.org/10.1016/j.inffus.2004.07.002. URL http://www.sciencedirect.com/
science/article/pii/S156625350400065X.
72
Appendix A
10 State EKF
For the 10 state version of the kalman filters presents the following state vector,
x =
δr
δv
qcb
=[δrx δry δrz δvx δvy δvz δq0 δq1 δq2 δq3
]T(A.1)
A.1 Dynamics Model
The dynamics model for the 10 state EKF is the following,
xk+1 =
I3×3 4tI3×3 03×4
4tG I− 24tΩie 4tF
04×3 04×3 I +4tW
xk + wk (A.2)
Sub-matrices G, F and W are Jacobian matrices, defined as,
G ,∂γ
∂r
∣∣∣∣r=r−k
−ΩieΩie
F ,∂
∂qcb
[Ceb
(I−Cb
c
)f b] ∣∣∣∣
qcb=(qcb)−k
W ,∂
∂qcb
[B(I−Cb
c
)ωcic] ∣∣∣∣
qcb=(qcb)−k
(A.3)
which resolve into,
G =−GMR5
R2 − 3r2x −3rxry −3rxrz
−3rxry R2 − 3r2y −3ryrz
−3rxrz −3ryrz R2 − 3r2z
−ΩieΩie (A.4)
A.1
F = 2Ceb
−fx − fy + fz −fx − fy − fz fx − fy + fz fx − fy − fzfx − fy − fz −fx + fy − fz −fx − fy − fz fx + fy − fz−fx + fy − fz −fx + fy + fz −fx − fy + fz −fx − fy − fz
(A.5)
W = 2B
−ωx − ωy + ωz −ωx − ωy − ωz ωx − ωy + ωz ωx − ωy − ωzωx − ωy − ωz −ωx + ωy − ωz −ωx − ωy − ωz ωx + ωy − ωz−ωx + ωy − ωz −ωx + ωy + ωz −ωx − ωy + ωz −ωx − ωy − ωz
(A.6)
where r is the nominal a priori user position, R = |r|, f =[fx fy fz
]Tand ωcic =
[ωx ωy ωz
]T.
The process noise wk of the 10 state EKF is white noise of covariance,
Qk = Gc(tk)
σ2νa
σ2νg
GTc (tk)4t (A.7)
where,
Gc(tk) =
03×3 03×3
Ceb 03×3
04×3 −B
(A.8)
A.2 Observation Model
The observation model is given by,
zk =
I3×3 03×3 03×4
03×3 I3×3 03×4
+ vk (A.9)
where measurement noise vk is white noise of covariance,
Rk =
σ2rGPS
σ2vGPS
(A.10)
A.2
Appendix B
10 State UKF
The state vector of this filter is the same as that of appendix A, or in other words,
x =
δr
δv
qcb
=[δrx δry δrz δvx δvy δvz δq0 δq1 δq2 δq3
]T(B.1)
B.1 Dynamics Model
The dynamics model of this filter is the following,
xk+1 =
δr +4tδv
δv +4t(∂g∂r δr− 2Ωieδv + Ce
b(I−Cbc)f
b)
qcb +4t(B(I−Cb
c)ωcic
)+ wk (B.2)
The process noise wk of the 10 state EKF is white noise of covariance,
Qk = Gc(tk)
σ2νa
σ2νg
GTc (tk)4t (B.3)
where,
Gc(tk) =
03×3 03×3
Ceb 03×3
04×3 −B
(B.4)
B.2 Observation Model
The observation model is given by,
zk =
I3×3 03×3 03×4
03×3 I3×3 03×4
+ vk (B.5)
B.1
where measurement noise vk is white noise of covariance,
Rk =
σ2rGPS
σ2vGPS
(B.6)
B.2
Appendix C
34 State EKF
The state vector of this filter is the same as the augmented UKF,
x =
δr
δv
qcb
δxa
δxg
(C.1)
C.1 Dynamics Model
The dynamics model of this filter is the following,
xk+1 =
I3×3 4tI3×3 03×4 03×12 03×12
4tG I3×3 − 24tΩie 4tF 4tCebC
bcFva 03×12
04×3 04×3 I4×4 +4tW 04×12 −4tBFqg
012×3 012×3 012×4 I12×12 012×12
012×3 012×3 012×4 012×12 I12×12
xk + wk (C.2)
Sub-matrices G, F and W are Jacobian matrices, defined as,
G ,∂γ
∂r
∣∣∣∣r=r−k
−ΩieΩie
F ,∂
∂qcb
[Ceb
(I−Cb
c
)f b] ∣∣∣∣
qcb=(qcb)−k
W ,∂
∂qcb
[B(I−Cb
c
)ωcic] ∣∣∣∣
qcb=(qcb)−k
(C.3)
which resolve into,
C.1
G =−GMR5
R2 − 3r2x −3rxry −3rxrz
−3rxry R2 − 3r2y −3ryrz
−3rxrz −3ryrz R2 − 3r2z
−ΩieΩie (C.4)
F = 2Ceb
−fx − fy + fz −fx − fy − fz fx − fy + fz fx − fy − fzfx − fy − fz −fx + fy − fz −fx − fy − fz fx + fy − fz−fx + fy − fz −fx + fy + fz −fx − fy + fz −fx − fy − fz
(C.5)
W = 2B
−ωx − ωy + ωz −ωx − ωy − ωz ωx − ωy + ωz ωx − ωy − ωzωx − ωy − ωz −ωx + ωy − ωz −ωx − ωy − ωz ωx + ωy − ωz−ωx + ωy − ωz −ωx + ωy + ωz −ωx − ωy + ωz −ωx − ωy − ωz
(C.6)
where r is the nominal a priori user position, R = |r|, f b =[fx fy fz
]Tand ωcic =
[ωx ωy ωz
]T.
The process noise wk of the 10 state EKF is white noise of covariance,
Qk = Gc(tk)
σ2νa
σ2νg
σ2ωa
σ2ωg
GTc (tk)4t (C.7)
where,
Gc(tk) =
03×3 03×3 03×12 03×12
Ceb 03×3 03×12 03×12
04×3 −B 04×12 04×12
012×3 012×3 I12×12 012×12
012×3 012×3 012×12 I12×12
(C.8)
C.2 Observation Model
The observation model is given by,
zk =
I3×3 03×3 03×4 03×12 03×12
03×3 I3×3 03×4 03×12 03×12
xk + vk (C.9)
where measurement noise vk is white noise of covariance,
Rk =
σ2rGPS
σ2vGPS
(C.10)
C.2