2000 Thesis Low Cost High Integrity Aided Navigation System PDF

Low Cost, High Integrity,
Aided Inertial Navigation Systems

for
Autonomous Land Vehicles
Salah Sukkarieh
SI
A thesis submitted in fulllment

of the requirements for the degree of
Doctor of Philosophy
EREM
E AD
M
E
MUTA
Australian Centre for Field Robotics

Department of Mechanical and Mechatronic Engineering
The University of Sydney
March 2000
Declaration
I hereby declare that this submission is my own work and that, to the best of my
knowledge and belief, it contains no material previously published or written by
another person nor material which to a substantial extent has been accepted for the
award of any other degree or diploma of the University or other institute of higher
learning, except where due acknowledgement has been made in the text.
Salah Sukkarieh
March 20, 2000
ii
Abstract
Salah Sukkarieh
The University of Sydney
Doctor of Philosophy
March 2000
Low Cost, High Integrity, Aided Inertial

Navigation Systems for Autonomous Land Vehicles
This thesis describes the theoretical and practical development of a low cost, high
integrity, aided inertial navigation system for use in autonomous land vehicle applications. The demand for fail safe navigation systems which can be used on large
autonomous land vehicles such as those found in container terminals, agriculture,
construction and in mines, has driven research and technology into the development
of high integrity navigation suites. Integrity, in this thesis, is dened as the ability of
a navigation system to provide reliable navigation information while also monitoring
the health of the data and either correcting any faults that may occur or rejecting
faulty data. Thus integrity encapsulates reliability while the reverse is not necessarily
true. This thesis provides, both in practical and theoretical terms, the fusion processes adopted and the implementation of fault detection techniques required for a
high integrity aided inertial navigation system. There are three main contributions:
Firstly, the development of an aided inertial navigation system using the Global
Navigation Satellite System (GNSS) as an aiding source for use in autonomous land
vehicles. This is accomplished by using a Kalman lter as the estimation algorithm
along with the addition of fault detection techniques so as to increase the integrity
of the system. The real time structure of the navigation architecture is provided
along with results of its implementation in an autonomous 65 tonne straddle carrier.
However, the algorithm development provides a generic structure thus allowing the
use of the navigation suite on any land vehicle.
Secondly is the use of vehicle modelling to bound drift errors associated with inertial
navigation. This provides a sensor-free aiding source due to the inherent constrained
motion of land vehicles. Vehicle constraints can thus be used as an extra aiding
source with other sensors which in turn improves the accuracy and integrity of the
overall navigation system. This is demonstrated with the real time implementation
of an inertial navigation system being aided by three separate aiding sources; GNSS,
vehicle modelling and speed data provided by an encoder.
Finally, the understanding of the eect of inertial sensor redundancy to navigation
accuracy and fault detection is addressed. A redundant inertial measurement unit is
developed and tested and provides the necessary physical sensor for future fault detection work. This concludes this thesis by providing the foundation for the autonomous
detection of faults in inertial units and furthermore the nal level of integrity required
for a navigation system.
iii
Acknowledgements
Sincere thanks go to both Associate Professor Eduardo Nebot and Professor Hugh
Durrant-Whyte for their direction, support, enthusiasm, funding and condence in
me. Long will I remember the days of Cobar, Plymouth, Belgium, Holland, St.Marys
and Port Botany and everything that went with them. Not to forget the years of the
Sub, the Ute, the Strad, the Tetrad and the Plane (along with the occasional economic bargaining theories, bond graphs, right wing books and stories of Argentina
and the UK and their close friendship as two countries). The laughter and vision kept
the PhD alive.
Thanks also goes to Dr.Gamini Dissanayake for the wake up calls every now and
then...the scare tactics on my thesis worked.
I'd like to thank Michael Stevens and Benjamin Rogers whose programming help and
persistence in testing on the Straddle project proved invaluable, the system wouldn't
have worked without you both. Also to Keith Willis whose super dooper electronic
boards turned the Tetrad from ction to fact, and Chris Mifsud for the constant
wiring of GPS cards.
To the guys in Rm 346: Eric (Ekka) Nettleton for the good laughs, emails, pictures
and jokes, Paul Newman for the many laughs in our corner of \Wit and Intelligence",
Stefan Williams for the very, very, very close games of squash ;-), Raj Madhavan (Latex man - to whom I owe many drinks), Xiaoying Kong (who made me feel that the
maths I knew was all pre-school stu), Tim Bailey, Monica Louda, Som Majumder
and to everyone else including the guys in C4: Steve Scheding, Graham Brooker and
Quang Ha .....the stay was great.
To the guys at British Aerospace Systems and Equipment, I thank you all for a
wonderful stay in what was always a warm 14 deg. Special thanks to Dr. Richard
Fountain and Richard Reilly for their support and knowledge while I was there.
Special thanks goes to the MUA for making my stay exciting, and to the lady at
the security gate who could never remember my name.
To my parents, you have worked hard all your life in the hope that your
son will make you proud one day...I hope I have not failed. Thank you
for the support, love and wisdom (which no PhD can obtain) that you
gave me throughout my life.
To my brothers and sisters, thank you all for the laughs and the headaches,
without you I would be lost in the material world.
And to Nada....get ready for the ride!
To Knowledge
Contents
Declaration
Abstract
ii
Acknowledgements
iii
Contents
List of Figures
List of Tables
xix
1 Introduction
1.1
1.2
1.3
1.4
1.5
1.6
Objectives . . . . . . . . . .
Navigation Systems . . . . .
Statistical Filtering . . . . .
Navigation System Integrity
Contributions . . . . . . . .
Thesis Structure . . . . . . .
2 Statistical Estimation
.
.
.
.
.
.
2.1 Introduction . . . . . . . . . .
2.2 The Kalman Filter . . . . . .
2.2.1 Algorithm . . . . . . .
2.3 The Extended Kalman Filter .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
..
..
..
..
v
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
3
4
5
7
8
10
10
10
11
13
vi
CONTENTS
2.4 The Information Filter . . . . . . . . . . . . . . . . . . . . . . . . . .

2.5 The Extended Information Filter . . . . . . . . . . . . . . . . . . . .
2.6 Aided Inertial Navigation System Structures . . . . . . . . . . . . . .
2.6.1 Advantages and Disadvantages of the Tightly and Loosely Coupled Congurations . . . . . . . . . . . . . . . . . . . . . . . .
2.6.2 Aiding in Direct Feedback Congurations . . . . . . . . . . . .
2.7 Filter Consistency and Fault Detection . . . . . . . . . . . . . . . . .
2.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3 Inertial Navigation
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . .
3.2 Inertial Navigation . . . . . . . . . . . . . . . . . . . .
3.2.1 Inertial Systems . . . . . . . . . . . . . . . . . .
3.2.2 Physical Implementation . . . . . . . . . . . . .
3.2.3 Gyroscopic Sensors . . . . . . . . . . . . . . . .
3.3 Inertial Navigation Equations . . . . . . . . . . . . . .
3.3.1 The Coriolis Theorem . . . . . . . . . . . . . .
3.3.2 The Transport Frame . . . . . . . . . . . . . . .
3.3.3 Wander Azimuth Frame . . . . . . . . . . . . .
3.3.4 The Earth Frame . . . . . . . . . . . . . . . . .
3.4 Schuler Damping . . . . . . . . . . . . . . . . . . . . .
3.5 Attitude Algorithms . . . . . . . . . . . . . . . . . . .
3.5.1 Euler Representation . . . . . . . . . . . . . . .
Discretisation . . . . . . . . . . . . . . . . . . .
3.5.2 Direction Cosine Matrix (DCM) Representation
Discretisation . . . . . . . . . . . . . . . . . . .
3.5.3 Quaternion Representation . . . . . . . . . . . .
Discretisation . . . . . . . . . . . . . . . . . . .
3.5.4 Discussion of Algorithms . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
17
18
22
23
25
26
28
28
28
31
32
33
34
35
36
37
38
39
40
41
42
43
43
44
45
46
vii
CONTENTS
3.6
3.7
3.8
3.9
3.10
Errors in Attitude Computation . . . . . . . .

Vibration . . . . . . . . . . . . . . . . . . . .
Minimising the Attitude Errors . . . . . . . .
Error Analysis . . . . . . . . . . . . . . . . . .
Sensor Errors . . . . . . . . . . . . . . . . . .
3.10.1 Evaluation of the Error Components .
3.10.2 Faults Associated with Inertial Sensors
3.11 Alignment and Calibration . . . . . . . . . . .
3.11.1 Alignment Techniques . . . . . . . . .
3.11.2 Alignment for Low Cost Inertial Units
3.11.3 Calibration . . . . . . . . . . . . . . .
3.12 Summary . . . . . . . . . . . . . . . . . . . .
4 Aided Inertial Navigation
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 GNSS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.1 Position Evaluation and Accuracy . . . . . . . . . . . . . . . .
4.2.2 Transformation of the GNSS Frame . . . . . . . . . . . . . . .
4.3 Overview of the Navigation Loops . . . . . . . . . . . . . . . . . . . .
4.4 GNSS Aiding . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.4.1 Navigation for an Autonomous Straddle Carrier . . . . . . . .
4.4.2 Linear, Direct Feedback Implementation . . . . . . . . . . . .
4.4.3 Detection of Multipath . . . . . . . . . . . . . . . . . . . . . .
4.4.4 Filter Tuning . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5 Real Time Implementation of an Inertially Aided GNSS Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5.1 Latency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5.2 Hardware and Algorithms . . . . . . . . . . . . . . . . . . . .
4.6 Aiding Using a Vehicle Model . . . . . . . . . . . . . . . . . . . . . .
47
49
51
52
56
57
59
63
63
64
65
66
68
68
69
70
72
74
75
77
79
84
86
87
88
88
92
viii
CONTENTS
4.6.1 General Three-Dimensional Motion . . . . . . . . . . .

4.6.2 Motion of a Vehicle on a Surface . . . . . . . . . . . .
4.6.3 Estimation of the Vehicle State Subject to Constraints
4.6.4 Observability of the States . . . . . . . . . . . . . . . .
4.7 Multiple Aiding of an Inertial System . . . . . . . . . . . . . .
4.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5 Experimental Results
5.1 Introduction . . . . . . . . . . . .
5.2 Experimental Setup . . . . . . . .
5.2.1 Vehicles . . . . . . . . . .
5.2.2 Sensors . . . . . . . . . . .
5.2.3 Environment . . . . . . .
5.3 Inertial/GNSS Results . . . . . .
5.3.1 Fusion . . . . . . . . . . .
5.3.2 Alignment Correction . . .
5.3.3 Fault Detection . . . . . .
5.4 Vehicle Model Constraint Results
5.5 Multiple Aiding Results . . . . .
5.6 Inertial/GNSS Plots . . . . . . .
5.7 Vehicle Model Constraint Plots .
5.8 Multiple Aiding Plots . . . . . . .
5.9 Conclusion . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
93
94
95
96
99
102
104
104
105
105
105
109
114
114
115
116
117
119
121
137
144
148
6 Redundancy, Navigation Accuracy and Autonomous Fault Detection

150
6.1 Introduction . . . . . . . . . . . . . . . . . .
6.2 Discussion . . . . . . . . . . . . . . . . . . .
6.3 Conguration of Redundant Inertial Sensors
6.3.1 Information Space . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
150
151
152
153
ix
CONTENTS
6.4
6.5
6.6
6.7
6.3.2 Directed Information . . .

6.3.3 Unequal Noise Variances .
6.3.4 Sensitivity and Resolution
Fault Detection . . . . . . . . . .
6.4.1 GNSS . . . . . . . . . . .
6.4.2 Inertial Navigation . . . .
Truncated Tetrahedron . . . . . .
Results . . . . . . . . . . . . . . .
6.6.1 Ground Tests . . . . . . .
6.6.2 Airborne Tests . . . . . .
Conclusion . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
158
159
160
161
161
163
164
168
168
169
179
7 Contributions, Conclusion and Future Work
180
Bibliography
187
List of Figures
1.1 A typical lter uses a dead reckoning sensor to predict the vehicle
states. When an observation from an absolute sensor occurs, the lter
updates (corrects) the predicted vehicle states. . . . . . . . . . . . . .
2.1 The \direct" structure implements a non-linear lter to estimate the
position, velocity and attitude of the vehicle. The inertial data is
provided by an IMU and the aiding data from a navigation system. .
2.2 The \indirect feedback" method allows a linear lter implementation
and minimises the computational overhead on the lter structure. . .
2.3 The \direct feedback" method overcomes the problem of large observation values being provided to the lter by correcting the INS directly.
2.4 The \tightly coupled" conguration treats both inputs as sensors and
not as navigation systems. Furthermore, the lter estimates are sent
to the aiding sensor and not the inertial sensor. This conguration
allows for a more robust system. . . . . . . . . . . . . . . . . . . . . .
2.5 Illustration of how the observation measurements zP and zV are obtained by the inertial and aiding information. . . . . . . . . . . . . .
5
19
20
20
22
24
3.1 A photograph showing low cost accelerometers (bottom) and gyroscopes (top) implemented in this thesis. . . . . . . . . . . . . . . . . . 29
3.2 A photograph showing inside of the Watson IMU implemented in this
thesis. The gyros implemented in this unit are the same gyros shown
in the previous photograph. The accelerometers are hidden in the back. 30
3.3 x,y and z represents the body frame of the vehicle as observed by the
inertial unit. _, _ and _ represent the rotation rates roll, pitch and
yaw about these axes. . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
x
LIST OF FIGURES
xi
3.4 The change in bias values of the accelerometers due to the internal
temperature of the inertial unit. Each accelerometer has a dierent
characteristic even though they are the same make. This is simply
because they are low cost sensors and hence have dramatically dierent
characteristics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
3.5 Several runs of integrating the x-gyro and obtaining a unique random
walk each time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
4.1 GNSS accuracies are dramatically improved when dierential corrections are applied. Since the base station is at a known position then
the errors associated with GNSS can be evaluated and transmitted to
all rovers for use. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Coordinate representations. XYZ represents the orthogonal axes of the
ECEF coordinate frame used by the GNSS receiver. represents the
longitude and the latitude of the vehicle. NED represents the local
navigation frame at which the vehicle states are evaluated to. . . . . .
4.3 The three navigation systems implemented in this thesis. . . . . . . .
4.4 A straddle carrier . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5 The container terminal . . . . . . . . . . . . . . . . . . . . . . . . . .
4.6 Real time architecture implementing four Transputers for parallel processing. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.7 Motion of a vehicle on a surface. The navigation frame is xed and the
body frame is on the local tangent plane to the surface and is aligned
with the kinematic axes of the vehicle. . . . . . . . . . . . . . . . . .
72
74
76
77
78
90
97
5.1 The utility is used as a test bed for the sensors. It houses the Transputers which log data from the GNSS receiver and inertial unit. It also
handles the real time code of the Inertial/GNSS lter. . . . . . . . . . 106
5.2 The position and orientation of a 65 tonne straddle carrier in a port
environment is determined using the Inertial/GNSS navigation system.
The objective of this navigation system is to provide the navigation
data needed for guidance and hence a high amount of integrity is required.106
5.3 The Watson IMU houses three accelerometers and three gyros in an
orthogonal arrangement. It also contains two tilt sensors to measure
the bank and elevation of the inertial unit. . . . . . . . . . . . . . . . 107
5.4 The Ashtech G12 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108
5.5 The Novatel RT2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
LIST OF FIGURES
5.6 Outside the University campus showing extensive tree foliage and buildings which will aect the performance of the GPS receiver. . . . . . .
5.7 A major road outside of the University campus where multipath occurs
due to the building structures. . . . . . . . . . . . . . . . . . . . . . .
5.8 The utility was driven in a relatively open area where sparse building
structures are found. Since the sky is relatively open to the receiver,
minimal faults will occur. . . . . . . . . . . . . . . . . . . . . . . . . .
5.9 Another view of the area shows a small amount of tree foliage, however
again there is still a large portion of the sky which is visible by the
receiver antenna. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.10 The port where the straddle carrier is located comprises mainly of
containers and quay cranes. Although the containers have no aect
on the GNSS signal the quay cranes do and hence fault detection is
particularly important. . . . . . . . . . . . . . . . . . . . . . . . . . .
5.11 The extension on the top right hand corner of the quay crane causes
slight multipath errors when the straddle carrier passes underneath
this extension. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.12 As the straddle carrier approaches the quay crane in order to travel
underneath it, multipath errors occur until the straddle carrier is under
the crane at which stage total satellite blockage occurs. . . . . . . . .
5.13 Raw data from the inertial unit and GPS. The inertial solution wanders
o due to the changing bias terms and due to the unit being mounted
directly onto the vehicle. . . . . . . . . . . . . . . . . . . . . . . . . .
5.14 Fusion of inertial and GPS data with no multipath rejection. . . . . .
5.15 Enlarged view of region 1 where GPS multipath errors have occurred.
The vehicle is heading from West to East. The light crosses show
where GPS multipath has occurred and the dashed lines (which is a
collection of points from the inertial navigation solution) closely follow
these points since their is no fault detection on the observations. . . .
5.16 Enlarged view of region 2 where GPS multipath errors have occurred.
The vehicle is heading from South to North. As in the previous plot,
the inertial navigation solution closely follows the GPS multipath. . .
5.17 Enlarged view of region 1 with multipath rejection. The validation
function has rejected the multipath. Small jumps in the fused result
remain and this is due to the accuracy of the GPS receiver. . . . . . .
5.18 Enlarged view of region 2 with multipath rejection. As in the previous plot, multipath has been rejected but the result is limited to the
accuracy of the GPS receiver. . . . . . . . . . . . . . . . . . . . . . .
xii
110
110
111
111
112
113
113
121
121
122
122
123
123
LIST OF FIGURES
5.19 Fusion result using 0:02m position and 0:02m=s velocity GPS technology. The straight located at 150m North and 240 to 270m East
corresponds to Figure 5.8. The straight located at 200m North and
200 to 100m East corresponds to Figure 5.9. . . . . . . . . . . .
5.20 Close up of an area showing the heading of the vehicle. The bottom
path shows the vehicle slightly after the test has started while the top
path shows the vehicle on the return towards the end of the run. In
this example the initial heading is given the correct value. . . . . . .
5.21 Enhanced view of the same area showing the heading of the vehicle
after an initial error of 5 deg is placed on the heading. The heading is
corrected by the time the vehicle returns. . . . . . . . . . . . . . . . .
5.22 Enhanced view of the acceleration of the vehicle at the end of the run
with attitude correction. . . . . . . . . . . . . . . . . . . . . . . . . .
5.23 Enhanced view of the velocity of the vehicle at the end of the run with
attitude correction. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.24 Enhanced view of the acceleration of the vehicle at the end of the run
without attitude correction. . . . . . . . . . . . . . . . . . . . . . . .
5.25 Enhanced view of the velocity of the vehicle at the end of the run
without attitude correction. . . . . . . . . . . . . . . . . . . . . . . .
5.26 Close up of the pitch of the vehicle with attitude correction. The dark
line represents the pitch as determined by the direction cosine matrix.
The lighter line is the pitch as determined by the tilt sensors. . . . . .
5.27 Close up of the pitch of the vehicle without attitude correction. The
dark line represents the pitch as determined by the direction cosine
matrix. The lighter line is the pitch as determined by the tilt sensors.
5.28 Position of the straddle carrier as it manoeuvres around containers
before driving under a quay crane. The vehicle starts at position
0m North; 0m East and moves in a counter clockwise direction. . .
5.29 Enhanced view of the area where the vehicle approaches the quay crane.
As the vehicle passes under the crane total satellite blockage occurs and
hence there are no GPS xes. During this period the inertial errors are
not corrected however, the on-line properties of the lter have ensured
that the inertial unit is aligned accurately before the multipath region
so that the position evaluations of the inertial data are accurate. . . .
5.30 Before the vehicle approaches the crane multipath errors occur. Theses
GPS xes however have been detected as faults and hence are not used
as observations and the inertial data is not inaccurately corrected. . .
xiii
124
125
125
126
126
127
127
128
128
129
130
130
LIST OF FIGURES
5.31 The position innovations of the lter show that the lter places more
emphasis on the inertial position evaluations. This is due to the large
uncertainty of the position xes delivered by this GPS unit. . . . . .
5.32 The velocity innovations show that the lter utilises the GPS velocity
xes to a great extent in order to correct the inertial errors. Since
the velocity evaluations from the inertial data is corrected with the
accurate GPS xes then the position determination delivered by the
inertial data will also be accurate. Hence the greater certainty in the
position evaluations as illustrated in the position innovations. . . . .
5.33 With the same tuning parameters but with no fault detection routines
the inertial solution closely follows the GPS multipath errors. During
this period the inertial errors are inaccurately corrected and hence the
position estimates overshoot the correct path taken by the vehicle. . .
5.34 Enhanced view of the multipath region. Notice that the corrections
have altered the heading such that the perceived motion of the vehicle
is not in line with its true heading. The situation would erroneously
suggest that the vehicle is slipping. . . . . . . . . . . . . . . . . . . .
5.35 The position innovations show where multipath errors occur. . . . . .
5.36 Similarly the velocity innovations show where the multipath errors occur. Even beyond the multipath region the innovations exceed the two
sigma bound. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.37 Keeping with no GPS fault detection, the path of the vehicle can be
made to resemble the true path by placing greater accuracy in the state
model and hence in the inertial data, with less weighting placed on the
GPS xes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.38 The position innovations show that the lter is behaving inconsistently
even when it is tuned to place little emphasis on the GPS xes. . . .
5.39 The velocity innovations further magnify the inconsistency of the lter
when it is tuned to seemingly reject multipath errors. . . . . . . . . .
5.40 Illustration of the velocity of the vehicle when it is properly tuned and
with GPS fault detection. . . . . . . . . . . . . . . . . . . . . . . . .
5.41 Velocity of the vehicle when the navigation loop is implemented without fault detection however with tuning so that the position of the
vehicle seemingly matches the true trajectory. During the multipath
region (approximately around iteration 8500) the velocity as determined by the inertial solution is inaccurately corrected. This is also
seen with the attitude states. . . . . . . . . . . . . . . . . . . . . . .
xiv
131
131
132
132
133
133
134
135
135
136
136
xv
LIST OF FIGURES
5.42 Errors in vehicle speed when the vehicle is moving at a constant velocity
of 10 m/s while the angular velocity !x is non-zero in the time interval
700 1300s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
of 10 m/s while the angular velocity !y is non-zero in the time interval
700 1300s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
of 10 m/s while the angular velocity !z is non-zero in the time interval
700 1300s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.45 Errors in roll, pitch and yaw when the vehicle is moving at a constant
velocity of 10 m/s while the angular velocity about Bz is non-zero in
the time interval 700-1300 sec. . . . . . . . . . . . . . . . . . . . . .
5.46 Errors in roll, pitch and yaw when the vehicle is moving at a constant
velocity of 10 m/s while the angular velocity !z is non-zero in the time
interval 700 1300s. A constant unestimated bias of 10 rad/s is
introduced to all angular velocity observations . . . . . . . . . . . . .
5.47 Errors in the vehicle speed when the vehicle is moving at a constant
acceleration of 0.05 m=s for 1000s and then decelerating at the same
rate for another 1000s. The angular velocity !z is non-zero in the time
interval 700 1300s. . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.48 The result of the three dierent cases: the position of the inertial unit
with only raw data, the fused data with the GPS and the constrained
data. The attitude of the inertial unit with the raw data slowly drifts
thus giving inaccurate position results. The dierence between the position obtained by the Inertial/GPS fusion and the proposed algorithm
is too small to be clearly seen in this gure. . . . . . . . . . . . . . .
5.49 The position error of the constrained and raw data. The reference
position is from the fused Inertial/GPS navigation system. As illustrated, the error in the raw data grow quadratically with time while
the constrained data keeps the error bounded. . . . . . . . . . . . . .
5.50 The velocity error of the constrained and raw data. The reference
velocity is from the fused Inertial/GPS navigation system. The constrained data is bounded well while the raw data grows linearly with
time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.51 The roll data from the three dierent cases. The tilt information is
presented for comparison. As can be seen the constrained data follows
the Inertial/GPS solution quite well. The raw data provides good
results as well. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
137
137
138
138
139
139
140
141
141
142
LIST OF FIGURES
xvi
5.52 The pitch data from the three cases. The tilt information is presented
for comparison. The constrained data along with the Inertial/GPS
navigation data lie well within the results provided by the tilt sensor
information. It is the error in the raw data which predominately causes
the error in the position and velocity evaluation. . . . . . . . . . . . . 142
5.53 Error in roll at the end of the trajectory for the three cases. The tilt
sensor information is used as the reference. . . . . . . . . . . . . . . . 143
5.54 Error in pitch at the end of the trajectory for the three cases. the
tilt sensor information is used as the reference. this plot enforces the
benet of using the constraint equations. . . . . . . . . . . . . . . . . 143
5.55 Position plot of the path taken by the vehicle. This path was obtained
by using the Inertial/GPS navigation system. . . . . . . . . . . . . . 144
5.56 These two plots show the error growth in position of the inertial unit
free of any external observations and when it is fused with the vehicle
model constraints. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
5.57 Plots of error growth in velocity of the inertial unit when it is performing free of any external observations and when it is fused with the
vehicle model constraints. . . . . . . . . . . . . . . . . . . . . . . . . 145
5.58 Plots of attitude error. These errors cause the velocity and hence
position error growth shown in the previous plots. The constrained
inertial data has been bounded extremely well when compared to the
free inertial data. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
5.59 Plots of position error of the inertial unit for two cases. The rst case is
with velocity observation, the second with velocity and GPS observations every 15 seconds. Since position is unobservable when only using
the vehicle model constraints, the GPS dramatically improves the result.146
5.60 Plots of velocity error show only slight improvement. This is because
velocity is observable when using the vehicle model and speed data, and
so the extra information from the GPS benets the velocity estimate
only slightly. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
5.61 The attitude estimation only benets slightly from the GPS information since the velocity information provided by the vehicle model constraints delivers a signicant amount of information to the estimation. 147
6.1 One of the optimal congurations for ve sensors. The cone's half angle
is 54:74 and the sensors are separated equally by 72. . . . . . . . . . 154
6.2 Another optimal conguration is to place four sensors on the cone
equally separated by 90 while the fth sensor is placed on the cone
axis. The cone's half angle is now 65:91. . . . . . . . . . . . . . . . . 154
LIST OF FIGURES
xvii
6.3 The cones in this plot show the area dened as the feasibility region. If
a sensor's sensitive axis lies outside this region then the sensor cannot
detect small changes in the inertial properties of the vehicle. The objective is to get this region as large as possible so that the conguration
of the inertial suite does not need to be altered. . . . . . . . . . . . . 161
6.4 The truncated tetrahedron showing the faces and how it is hollowed
out to conserve weight. Each large face, holding a gyro, is parallel to a
smaller face, holding an accelerometer, thus allowing an accelerometer
gyro pair to be coplanar. . . . . . . . . . . . . . . . . . . . . . . . . . 166
6.5 The inertial unit from the top. The electronics consist of the ADC
and the DAC along with serial communications. The processor is also
housed with the inertial unit and shown on the left. The black boxes
are the gyros and the silver cylinders are the accelerometers. . . . . . 168
6.6 A comparison of the acceleration along the x-axis as indicated by both
the Tetrad and Watson inertial unit. . . . . . . . . . . . . . . . . . . 171
6.7 A comparison of the rotation rate along the x-axis as indicated by both
the Tetrad and Watson inertial unit. . . . . . . . . . . . . . . . . . . 171
6.8 Position of the vehicle as indicated by the Tetrad and Watson. The
circles represent the position as indicated by the GPS receiver. . . . . 172
6.9 Raw position as provided by the Tetrad (dark line) and GPS (light line)
for a ight test. The test lasted for approximately 10min however only
the rst 120s are considered here. The GPS data is used as a reference. 173
6.10 The same position run however illustrating all three axes. . . . . . . . 173
6.11 The RPV implemented in this ight test. . . . . . . . . . . . . . . . . 174
6.12 The raw velocity data as presented by the Tetrad (dark line) and GPS
(light line). Note the drift in velocity along all three axes, which once
integrated, causes the drift in position as indicated in the previous
gures. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174
6.13 The position information from the Tetrad once fused with GPS. A
linear error model is implemented in an information lter format. . . 175
6.14 The roll angle of the vehicle as provided by the aided Tetrad data. . . 175
6.15 The pitch angle of the vehicle as provided by the aided Tetrad data. . 176
LIST OF FIGURES
6.16 The heading angle of the vehicle as provided by the aided Tetrad data.
Note the marked dierence between the Tetrad and GPS data. This
is due to interpretation; the GPS heading data is provided by GPS
velocity while the Tetrad heading data is provided by the integration
of the gyros. Thus during sideslip the two results will be dierent. The
lter is not aected by this since it fuses the velocity vectors provided
by the two systems and not the heading data. . . . . . . . . . . . . .
6.17 Illustrates an example of sideslip occurring during a portion of the run.
It is this sideslip which causes the dierence between the heading data
provided by the Tetrad and GPS. . . . . . . . . . . . . . . . . . . . .
6.18 Position of the vehicle as indicated by the Tetrad unit when it has only
been aided by the GPS for 60s. Note that drift still occurs however the
unit has been aligned during the fused portion of its ight and hence
the drift is less then that provided by the raw Tetrad data. . . . . . .
6.19 Position as provided by the Tetrad data along the three axis when it
has been aided by GPS for only 60s. . . . . . . . . . . . . . . . . . .
6.20 Velocity as indicated by the Tetrad data when aided by GPS for only
60s. Note that the drift is less compared to when there was no aiding
at all. This is due to the alignment which occurred during the initial
60s of the ight. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
xviii
176
177
177
178
178
List of Tables
3.1 Algorithm Drift Rates in o=hr caused by sampling a continuous rotation rate of 20deg=sec . . . . . . . . . . . . . . . . . . . . . . . . . . 48
3.2 Comparison of some of the major errors with various gyro implementations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.1 The specication for the Watson IMU implemented in this work. . . . 108
6.1 Specications of the gyros implemented. The top row represents the
model numbers of the individual sensors. . . . . . . . . . . . . . . . .
6.2 Specications of the accelerometers implemented. The top row represents the model numbers of the individual sensors. . . . . . . . . . . .
6.3 Physical characteristics of the redundant inertial unit. . . . . . . . . .
6.4 The theoretical and actual face angles of the redundant inertial unit.
xix
165
165
167
167
Chapter 1
Introduction
1.1 Objectives
This thesis addresses the issue of providing a low cost, high integrity, aided inertial
navigation system for autonomous land vehicle applications. The denition of the
terms \low cost", \integrity" and \ aided inertial navigation" are as follows:
Although no monetary value is equated with the term \low cost", it symbol-
ises the ability to provide a navigation system which can be implemented cost
eectively by the civilian sector.
Integrity, in this thesis, is dened as the \..robustness to system failure" [48]. In
the context of the navigation systems developed in this thesis, integrity represents the ability of a navigation system to provide reliable navigation information while also monitoring the health of navigation data and either correcting
any faults that may occur or rejecting faulty data.
Inertial Navigation is the implementation of inertial sensors to determine the
pose (position and orientation) of a vehicle. Inertial sensors are classied as

dead reckoning sensors since the current evaluation of the state of the vehicle
is formed by the relative increment from the previous known state. As such,
1.1 Objectives
inertial navigation has unbounded error growth since the error accumulates at
each step. Thus in order to contain these errors some form of external aiding is
required. In this thesis, the aided information will derive predominantly from
Global Navigation Satellite Systems (GNSS) such as the Global Positioning
System (GPS) and the Russian equivalent Global Navigation Satellite System
(GLONASS). However, alternative aiding sources such as wheel encoder data
and the use of vehicle modeling are also discussed.
In summary, the goal of this thesis is to provided an aided inertial navigation system
which can be used cost eectively by the civilian sector for autonomous land vehicles
and in turn provide a sucient level of integrity so as to not compromise the safety
of the overall system.
The objectives of this thesis in order to reach this goal are:
To understand the implications of implementing low cost inertial units for navi-
gation. High grade inertial sensors can be an expensive approach to navigation.

However, by implementing low cost inertial sensors one correspondingly introduces greater errors to the navigation solution. The sources of these errors need
to be understood in order to minimise their impact on the performance of the
system.
To understand the eect of GNSS accuracy to moving land vehicles. High accuracy satellite navigation receivers are generally produced for surveying purposes
and not for dynamic movement. Furthermore, associated errors such as multipath and satellite signal blockage common with terrain based navigation need
to be comprehended.
To develop navigation algorithms which assist in bounding the errors of the inertial navigation system while also detecting multipath errors and providing data
during satellite blockages, and hence increasing the integrity of the navigation
loop.
To develop this algorithm in real time so as to provide navigation data to an
autonomous land vehicle control system. Furthermore, address the issue of
1.2 Navigation Systems
data latency commonly associated with satellite based navigation systems and
its aect on real time applications.
To investigate the addition of vehicle modelling to the navigation system in
order to increase the performance and integrity of the navigation data.
Furthermore to address the issue of multiple sensor aiding to a single inertial
unit for further improvement in performance.
To investigate and develop a redundant inertial unit in order to provide the
foundations for future work and to address the issues behind increase in navigation performance and autonomous fault detection techniques. Redundancy
in satellite numbers and its aect on navigation and fault detection is well documented for satellite based positioning systems. This theory is in turn re ected
in the development of this redundant inertial unit.
1.2 Navigation Systems

For the greater part of history, navigation has meant the ability to be able to determine ones position while somewhere on sea, land or in air. Initially, this would have
taken the form of watching the coastline or following hearsay but as voyages became
more adventurous, navigation dealt with the observance of the sun, moon and stars,
or through the use of a compass and a clock.
Nowadays, due to the complexity of vehicle systems and in particular those of the
autonomous nature, navigation is encapsulated by the science and technology of being
able to determine the position, velocity and orientation of a vehicle in real time with
a greater demand on accuracy. This has been spurred on by the demand for higher
productivity and lower costs, both of which are improved through automation.
A navigation system provides the required information by either sensing the relative movement of the vehicle, or by determining where the vehicle is with respect
to external features, or both. This is accomplished through the implementation of
1.3 Statistical Filtering
either dead reckoning or absolute sensors. Dead reckoning sensors measure the relative movement of the vehicle with respect to a previously known state. Examples
include inertial units, wheel encoders and air data systems. Absolute sensors observe
the external environment and relate the vehicle's state to those observations. Examples include vision, radar and the Global Positioning System (GPS). Dead reckoning
sensors usually output their data at high frequencies, however, due to their relative
accumulation of data, errors also accumulate with time. The errors associated with
absolute sensors on the other hand are xed. However, the update rates are generally
low.
To enjoy the benets of both, navigation systems generally include both types of
sensors and either select which is the most appropriate/correct piece of information,
or employ a system which fuses the data from both in some optimal fashion. A
common methodology for fusion is through the implementation of a statistical lter.
1.3 Statistical Filtering

In the navigation sense, a statistical lter is simply a set of equations which combines
information from separate sensors to optimally evaluate the pose of a vehicle. The
criterion for optimality is varied, but all work to minimise the error in the estimated
pose.
Generally, a lter implements a model which transforms the state of interest from
one time step to a future point in time. In navigation systems, the model is generally
a kinematic representation of the vehicle. The information from a dead reckoning
sensor is used to drive this model. However, error accumulation associated with dead
reckoning sensors also causes unbounded error growth on the output of the lter.
Thus an absolute sensor is required to bound the errors. Figure 1.1 illustrates a
simple layout of a navigation system.
When there is no absolute x from the aiding sensor, the lter is said to be \predicting" the state of interest. During this prediction stage the lter also evaluates the
uncertainty in the predicted values. Once a x occurs, the lter estimates the state
1.4 Navigation System Integrity
Dead Reckoning
Sensor
Predict
Observation
Update
Absolute
Sensor
Figure 1.1: A typical lter uses a dead reckoning sensor to predict the vehicle states.
When an observation from an absolute sensor occurs, the lter updates (corrects) the
predicted vehicle states.
of the vehicle by applying a weighting between the observation and the prediction.
This stage is called the \update". The Kalman lter for example, which is one of the
most widely implemented fusion algorithms, evaluates this weighting by Minimising
the Mean Squared Error (MMSE) of the estimate.

A growing number of research groups around the world are developing autonomous
land vehicle systems for various applications [6,11,20,28,58]. However, few of these
make explicit the essential need for system integrity that will be essential in any future
commercial development of this technology.
To achieve integrity, there not only has to be an associated level of reliability
with the sensors implemented, but there also has to be a level of condence in the
navigation software and hardware which fuses the information between the various
sensors.
Reliability is concerned with minimising failures. However, no matter how reliable a
system is, failure will occur. Integrity addresses the issue of ensuring safe and correct
operations in the face of these inevitable failures. Integrity is essential in any kind
of autonomous vehicle when there are no human operators available to detect and in
turn rectify any failures. Integrity thus encapsulates reliability. Integrity enables a
navigation system to detect if a fault has occurred and to either rectify that fault, or
to reject its contribution to the navigation solution, or to degrade the performance
of the navigation system gracefully. To achieve a level of integrity in a navigation
system, three issues need to be addressed [48]:
Filter Consistency - deals with the assurance that the models implemented
within the lter are a proper indication of the real system.
Navigation System Design - deals with the design of the lter structure and
algorithms along with the proper sensor choice to encapsulate the information
required to observe the states of interest.
Fault Detection - handles the broad area of ensuring that if faults do occur in
either the models, sensors or lter code, then techniques are available to detect
these faults.
All three areas need to be examined by the system engineer when designing a navigation system.
To reduce the risk of impact that a component has on a navigation system, redundancy can be employed. Not only does redundancy increase the reliability of a system
it can also increase the performance of the navigation solution. A good example of
this is with GPS. Only four satellites are required to determine the position of a GPS
receiver anywhere on Earth. However, if more than four satellites are observed, then
based on a simple least squares algorithm, there is a corresponding increase in accuracy of the solution. Furthermore, since only four satellites are required, a simple
combinatorial procedure can determine if any satellite is delivering faulty data.
1.5 Contributions
1.5 Contributions
This thesis is part of a wider project aimed at developing a general purpose, high
integrity navigation system for autonomous land vehicles. The objective of this wider
project is to investigate a sensor suite that provides accuracy, robustness and integrity.
A system based on the implementation of two decentralised navigation loops has been
proposed [40]. The rst loop fuses the data from an inertial unit and a satellite based
navigation receiver (which is part of this thesis), while the second loop fuses positional
data from a Millimetre Wave Radar (MMWR) with velocity and steering encoders
[12]. An information lter is then used to combine the data from the two loops and
provide estimates of the states of the vehicle.
No single reference has provided the analysis of low cost inertial systems for land
vehicle navigation. This thesis does so, with the knowledge gained from real applications, and work conducted alongside satellite navigation manufacturers and the
defense industry.
The main contributions of this thesis are:
The qualication and quantication of errors associated with low cost inertial

navigation systems when implemented in land vehicle applications.

The development of an inertial navigation system aided by GNSS and the understanding of the limitations in accuracy and information available to aid the
inertial system.
The determination of low frequency faults associated with inertial sensors and
high frequency faults associated with GNSS, the techniques needed to detect
them, and the algorithms implemented to remove them.
The development of a real time inertial navigation system aided by GNSS. The
navigation system provides the navigation data needed to control a 65 tonne
container handling vehicle.
The development of an inertial navigation system being aided by vehicle modelling. This implementation requires no external sensors to aid the inertial unit.
1.6 Thesis Structure
An understanding of the limitations of vehicle modelling to aid inertial naviga-
tion systems.
The development of an inertial navigation system aided by GNSS, vehicle mod-
elling, and speed provided by a wheel encoder. The lter architecture is in an

Information lter format. The multiple aiding allows for better fault detection
and better accuracy of the inertial navigation solution.
An information approach for the optimal conguration of redundant inertial
sensors.
The development of a redundant inertial unit based on four gyros and four
accelerometers. Issues associated with temperature compensation and sampling,

often omitted in most low cost inertial units, is addressed in this inertial system.

Chapter 2 provides the necessary background into statistical ltering required for
this thesis. In particular, the linear and non-linear versions of the Kalman and information (Inverse Covariance) lters are looked at.
Various implementations of aided inertial navigation systems is also provided, and
the advantages and disadvantages of such implementations are discussed.
Chapter 3 describes and explains the implication of using low cost inertial units
for land vehicle applications. Minimising the cost of inertial units in turn means that
low quality inertial sensors must be used. This chapter discusses the errors associated with low cost inertial sensors. Techniques of quantifying these errors are also
discussed.
This chapter also denes inertial equations which are ideally suited for land based
navigation. These equations are linearised (or perturbed) to provide inertial error
equations allowing the analysis and understanding of the behaviour of an inertial
navigation system given inertial sensor accuracies.
Finally the chapter discusses the alignment and calibration techniques implemented
in this thesis.
Chapter 4 considers the issue of aiding an inertial navigation system using satellite
navigation systems, vehicle model constraints and speed data provided by encoders,
and does so by combining Chapters 2 and 3 together.
Three navigation systems are developed: an inertial navigation system aided with
GNSS, an inertial navigation system aided with vehicle modelling, and an inertial
navigation system aided with GNSS, vehicle modelling and speed data provided by a
wheel encoder.
Chapter 5 provides results for the navigation systems developed in Chapter 4. Two
types of vehicles are implemented: a standard utility and a 65 tonne straddle carrier.
The three navigation systems developed are implemented on the utility. A real time,
high integrity inertial/GNSS navigation system is developed for the straddle carrier.
Results are provided on the fault capabilities of this navigation loop.
Chapter 6 looks at how redundancy can be used to improve accuracy and fault
detection capabilities in inertial navigation systems. Redundancy of inertial sensors

has commonly been used for control and stabilisation systems. Furthermore, redundancy of low cost inertial sensors for navigation purposes has not previously been
employed.
The objective of this chapter is to provide a framework for the use of redundant,
low cost inertial sensors, and to analyse the conguration so as to increase navigation
accuracy.
Furthermore, redundancy permits autonomous fault detection thus increasing the
overall integrity of the navigation system.
Finally, Chapter 7 provides a conclusion of this thesis along with suggestions for
future developments.
Chapter 2
Statistical Estimation
2.1 Introduction
This chapter provides the necessary background on statistical estimation that is required for this thesis. In particular this chapter provides the algorithms which describe
both the linear discrete and non-linear discrete Kalman and information lters, which
are used for estimating states of interest. Only the equations are provided, the reader
is referred to [3,22,34] for the derivation of these algorithms.
This chapter also provides a detailed discussion on the various lter structures that
can be implemented for aided inertial navigation systems, and the advantages and
disadvantages of such implementations.
Fault detection and lter consistency are also brie y discussed since an understanding of these terms is required for developing high integrity navigation systems.
2.2 The Kalman Filter

A linear discrete time system can be described by
x(k) = F(k)x(k 1) + B(k)u(k) + w(k);
(2.1)
11
where x(k) is the state vector of interest at time k, F(k) is a state transition matrix
which relates the state vector from time k 1 to time k, u(k) is the input control
vector while B(k) relates the control vector to the states, and w(k) is the process
noise injected into the system due to uncertainties in the transition matrix and control
input. This noise is assumed to be a zero mean, E[w(k)] = 0 8k, uncorrelated random
sequence with covariance
E[w(i)wT (j )] =
8
<
Q(k) i = j = k
: 0
i=
6 j:
When observations of the states are taken the observation vector z(k) at time k is
given by
z(k) = H(k)x(k) + v(k)
(2.2)
where H(k) is the observation model relating the state vector at time k to the observation vector, and v(k) is the observation noise vector which is related to the
uncertainty in the observation. The observation noise is also assumed to be a zero
mean, E[v(k)] = 0 8k, uncorrelated random sequence with covariance
E[v(i)vT (j )] =
8
<
R(k) i = j = k
: 0
i=
6 j:
It is assumed that the process and observation noises are uncorrelated,
E[w(i)v(j )] = 0 8i; j:
2.2.1 Algorithm
The Kalman lter is a statistical recursive algorithm which provides an estimate of the
states at time k given all observations up to time k, x^ (kjk). The estimate of the state
at time k given only information up to time k 1 is called the prediction, x^ (kjk 1).
12
Given that the process and observation noises are zero mean and uncorrelated, the
Kalman lter provides an optimal Minimal Mean Squared Error (MMSE) estimate
of the states.
The predicted state is evaluated by taking expectations of Equation 2.1 conditioned
upon the previous k 1 observations,
x^(kjk 1) = E[x(k)jZk ]
= F(k)^x(k 1jk 1) + B(k)u(k):
1
(2.3)
The uncertainty in the predicted states is described as the expected value of the
variance of the error in the states at time k given all information up to time k 1
and is represented by the covariance matrix P(kjk 1),
P(kjk 1) = E[(x(k) x^(kjk 1))(x(k) x^(kjk 1))T jZk ]

= F(k)P(k 1jk 1)FT (k) + Q(k):
1
(2.4)
Equations 2.3 and 2.4 are evaluated during each time step. In a navigation system,
this occurs each time a sample from the dead reckoning sensor is obtained.
When an observation from an external sensor is obtained (Equation 2.2), an estimate of the state can be obtained and is given by
x^(kjk) = x^(kjk 1) + W(k) (k);
(2.5)
where W(k) is a gain matrix produced by the Kalman lter and (k) is the innovation
vector. The innovation vector is the dierence between the actual observation and a
predicted observation. That is,
(k)
= z(k) H(k)^x(kjk 1):
(2.6)
The predicted observation is determined by taking the expected value of Equation

2.2 conditioned on previous observations. Equation 2.5 denes the update as simply
the latest prediction plus a weighting on the innovation (which can be viewed as an
2.3 The Extended Kalman Filter
13
error). This weighting, the Kalman gain, is what sets the Kalman lter aside from
all other lters. The gain is chosen so as to minimise the mean squared error of the
estimate,
W(k) = P(kjk 1)HT (k)S (k);

1
(2.7)
where S(k) is known as the innovation covariance and is obtained by
S(k) = H(k)P(kjk 1)HT (k) + R(k):
(2.8)
The covariance matrix is updated due to this observation and is obtained by taking
the expectation of the variance of the error at time k given all observations up to time
k,
P(kjk) = E[(x(k) x^(kjk))(x(k) x^(kjk))T jZk ]

= (I W(k)H(k))P(kjk 1)(I W(k)H(k))T +
W(k)R(k)WT (k)
(2.9)

In most real cases the process and/or observation models do not behave linearly and
hence the linear Kalman lter described above cannot be implemented. To overcome
this, the extended Kalman lter (EKF) is implemented. It provides the best MMSE
of the estimate and in principle it is a linear estimator which linearises the process
and observation models about the current estimated state.
The non-linear discrete time system is described as
x(k) = F(x(k 1); u(k); k) + w(k);
(2.10)
where F(; ; k) is a non-linear state transition function at time k which forms the
current state from the previous state and the current control input.
14
The non-linear observation model is represented as
z(k) = H(x(k)) + v(k):
(2.11)
Following the same denitions outlined in the Kalman lter, the state prediction
equation is
x^(kjk 1) = F(^x(k 1jk 1); u(k));
(2.12)
and the predicted covariance matrix is
P(kjk 1) = rFx(k)P(k 1jk 1)rFTx (k) + Q(k):
(2.13)
The term rFx(k) is the Jacobian of the current non-linear state transition matrix
F(k) with respect to the predicted state x(kjk 1).
When an observation occurs, the state vector is updated according to
x^(kjk) = x^(kjk 1) + W(k) (k)
(2.14)
where (k) is the nonlinear innovation,

(k )
= z(k) H(^x(kjk 1)):
(2.15)
The gain and innovation covariance matrices are
W(k) = P(kjk 1)rHTx (k)S (k);

S(k) = rHx(k)P(kjk 1)rHTx (k) + R(k);
1
(2.16)
(2.17)
where the term rHx(k) is the Jacobian of the current observation model with respect
to the estimated state x(kjk).
2.4 The Information Filter
15
The updated covariance matrix is
P(kjk) = (I W(k)rHx(k))P(kjk 1)(I W(k)rHx(k))T +

W(k)R(k)W(k):
(2.18)
In the linear Kalman Filter, if the models are time invariant, then the Kalman gains
can be computed o line and used within the lter in real time. However, in the nonlinear implementation, since the process and observation models are dependent on
the current state, this benet is not available. Furthermore, the non-linear lter must
be properly initialised to ensure that the linearised models are accurate, since this
can lead to lter instabilities. Finally, the presence of the Jacobians adds a higher
level of complexity and furthermore increases the computational processing required.

The information lter is mathematically equivalent to the Kalman lter. That is, if
both lters use the same data then identical results would be obtained (the reader is
referred to [39] for a derivation of the information lter).
The key components in the information lter are the information matrix Y(kjk),
and the information vector y^ (kjk). Assuming that the noise is Gaussian then Y(kjk)
is the inverse of the covariance matrix,
Y(kjk) = P (kjk);
1
(2.19)
and hence represents the amount of information present amongst the states of interest
and their correlations. The information vector represents the information content in
the states themselves and can be evaluated by transforming the state values over to
information space,
y^(kjk) = Y(kjk)^x(kjk):
(2.20)
16
The predicted information vector is
y^(kjk 1) = L(kjk 1)^y(k 1jk 1) + B(k)u(k);

where L(kjk 1) = Y(k 1jk 1)F(k)Y (k 1jk 1):
(2.21)
The transformation matrix B(k) transforms the control input u(k) to information
space. The corresponding predicted information matrix is
Y(kjk 1) = [F(k)Y (k 1jk 1)FT (k) + Q(k)]

1
(2.22)
Again, these two equations are evaluated at each sample of the dead reckoning sensor.
The information contribution to the states due to an observation is in the form of
the information observation vector,
i(k) = HT (k)R (k)z(k):

1
(2.23)
The amount of certainty associated with this observation is in the form of the information observation matrix,
I(k) = HT (k)R (k)H(k)

1
(2.24)
The benet of implementing the information lter lies in the update stage. Since the
observation has been transformed over to information space, the update procedure is
simply the information contribution of this observation to the prediction,
y^(kjk) = y^(kjk 1) + i(k)

Y(kjk) = Y(kjk 1) + I(k):
(2.25)
(2.26)
If there is more than one sensor providing observations, the information observation
vector and matrix is simply the sum of the individual observation information vectors
2.5 The Extended Information Filter
17
and matrices and hence,

X
y^(kjk) = y^(kjk 1) + ij (k)

X
Y(kjk) = Y(kjk 1) + Ij (k):
(2.27)
(2.28)
where j is the number of sensors providing information at time k.

There are a number of advantages associated with the Information lter:
No innovation covariance matrix S(k) has to be computed;
Furthermore, there is no gain matrix W(k) which needs to be computed;
The initial information matrix can be easily initialised to zero information;
It is computationally easier to implement an information lter in a multisensor
environment since it is simply the summation of the individual information contributions. Furthermore, a Kalman lter cannot accommodate for the situation
where there are multiple observations to be handled by a lter at the same time,
due to the correlation of the innovation amongst the observations.
2.5 The Extended Information Filter

The derivation of the extended information lter (EIF) can be found in [39]. As with
the EKF, the EIF estimates the states of interest given non-linear process and/or
observation models, however in information space.
The predicted information vector and matrix is
y^(kjk 1) = Y(k 1jk 1)F(^x(k 1jk 1); u(k));

Y(kjk 1) = [rFx(k)Y (k 1jk 1)rFTx (k) + Q(k)]
1
(2.29)
(2.30)
2.6 Aided Inertial Navigation System Structures
18
When an observation occurs, the information contribution and its associated information matrix is
i(k) = rHTx (k)R (k)[ (k) + rHx(k)^x(kjk 1)];

I(k) = rHTx (k)R (k)rHx(k);
1
(2.31)
(2.32)
where the innovation is

(k )
= z(k) H(^x(kjk 1)):
(2.33)
Although the non-trivial derivation of the Jacobians still occurs in this form, the
EIF has the benet of being easily initialised, that is, one can set the initial conditions
to zero information. Furthermore, there is the added benet of decentralising the lter
across multiple nodes and the incorporation of multiple observations to a single lter
[39].

How the lter is structured within the navigation system depends on the types of
sensors and models employed. For aided inertial navigation systems, the inertial
component can either be an Inertial Measurement Unit (IMU), which only provides
the raw acceleration and rotation rate data, or an Inertial Navigation System (INS)
providing position, velocity and attitude information. The aiding source can either
be considered as a sensor providing raw sensor information, or as a navigation system
providing the position, velocity and/or attitude information. The principle states of
interest which are estimated by the lter, and hence which governs the type of model
implemented, are the position, velocity and attitude of the vehicle, or the position,
velocity and attitude errors.
The most natural implementation of an aided inertial navigation system is to drive
a non-linear lter with the raw acceleration and rotation rate data provided by the
IMU, as shown in Figure 2.1. The implementation is known as a \direct" lter
19
structure. The process model usually represents the kinematic relationship of the
vehicle and the states of interest. The state vector is propagated by the model and
inertial data. The aiding information can be obtained from a navigation system where
observations such as position and velocity are supplied to the system. The estimate
would be in the form of the vehicle states.
IMU
Acceleration and
Rotation Rates
Non-Linear
Kalman/Information
Filter
Position, Velocity and

Attitude Estimates
Observations
AIDING NAVIGATION
SYSTEM
Figure 2.1: The \direct" structure implements a non-linear lter to estimate the
position, velocity and attitude of the vehicle. The inertial data is provided by an
IMU and the aiding data from a navigation system.
The disadvantage of such an implementation is that the prediction equations have
to be evaluated at each sample of the inertial data. This requires substantial processing due to the high sample rates of IMUs. To overcome this, an INS should be
employed so that a constant stream of vehicle state information is available external
to the lter. To correct any errors, the lter estimates the errors in these states.
Figure 2.2 illustrates this implementation and is known as the \indirect feedback"
method. The observation which is delivered to the lter is the \observed error" of
the inertial navigation solution, that is, the dierence between the inertial navigation
solution and the navigation solution provided by the aiding source. Since the observation is the observed error of the inertial navigation solution, and since the lter is
estimating the errors in the inertial navigation solution, then the process model has to
be in the form of an error model of the standard inertial navigation equations. Thus

Position, Velocity
and Attitude
INERTIAL NAVIGATION
SYSTEM
20
+
Estimated Errors of Position,
Velocity and Attitude
+
Observed Error
AIDING NAVIGATION
SYSTEM
Linear
Kalman/Information
Filter
Observations
Figure 2.2: The \indirect feedback" method allows a linear lter implementation and
minimises the computational overhead on the lter structure.
Position, Velocity
and Attitude
INERTIAL NAVIGATION
SYSTEM
+
Observed Error
AIDING NAVIGATION
SYSTEM
Linear
Kalman/Information
Filter
Observations
Figure 2.3: The \direct feedback" method overcomes the problem of large observation
values being provided to the lter by correcting the INS directly.
21
the inertial navigation equations are linearised to form error equations (Chapter 3).
Since the equations are linearised the lter implementation takes on a linear form.
Although the prediction stage is still implemented, it can run at the same rate as the
sampling rate of the INS or at lesser intervals.
The disadvantage of the indirect feedback method is the unbounded error growth
of the INS which causes an unbounded growth in the error observation delivered to
the lter. This poses a problem to the linear lter since only small errors are allowed
due to the linearisation process. That is, large drift in the state values from the
INS cause large observation errors being fed into the lter and thus invalidating the
assumptions held by the lter. The optimal implementation is illustrated in Figure
2.3 and is known as the \direct feedback" method. In this structure the estimated
errors are fed back to the INS, thus minimising the growth of the observed error that
is delivered as an observation to the lter.
The three methods discussed so far are also known as \loosely coupled" implementations since there is no feedback to the aiding sensor/navigation system. If feedback
is provided to the aiding source a tighter conguration is obtained which in turn improves system integrity. Figure 2.4 illustrates what is known as a \tightly coupled"
conguration. It oers the advantages of being robust and increases performance
since it allows the systems designer to delve into the operation and algorithms of
both sensors. The inertial sensor provides raw data to the lter which usually incorporates a kinematic model of the vehicle. The aiding sensor also provides raw
information. The lter estimates the states of the vehicle, and uses these estimates
to assist the aiding sensor in its attainment of observations. For example, the aiding
information can help GNSS with tracking satellites or assist a scanning radar with
pointing and stabilisation.
22
INERTIAL SENSOR
Acceleration and
Rotation Rates
Non-Linear
Kalman/Information
Filter

Attitude Estimates
Raw Observation
AIDING SENSOR

Attitude Estimates
Figure 2.4: The \tightly coupled" conguration treats both inputs as sensors and not
as navigation systems. Furthermore, the lter estimates are sent to the aiding sensor
and not the inertial sensor. This conguration allows for a more robust system.
2.6.1 Advantages and Disadvantages of the Tightly and Loosely
Coupled Congurations
The loosely coupled congurations oer the distinct advantage of being highly modular in accuracy and cost. The system's designer can implement the model of choice
along with the desired INS in whatever navigation structure is preferred. Any aiding
sensor can then be added to the navigation system.
Although the tightly coupled conguration is more robust than the loosely coupled
conguration, it is more expensive to implement and more dicult to develop. Furthermore, if a dierent aiding sensor is employed, the models and algorithms must
change substantially.
The use of GNSS as an aiding system for inertial navigation systems has been the
subject of study for a number of years. The majority of implementations have been
loosely coupled [44]. This is due to companies specialising in inertial systems developing INS units with built in ltering techniques in a loosely coupled conguration
23
and in turn, GNSS companies focusing on delivering state of the art GNSS navigation
systems. Thus an appropriate fusion of the two systems is formed. To implement
a tightly coupled conguration requires close collaboration with GNSS companies,
since the aiding information from the navigation lter which is fed back to the GNSS
sensor assists with the satellite tracking algorithms. These algorithms are kept secret
since the speed of satellite reacquisition, the ability to locate and track satellites after they have been lost, is what separates the quality of receivers between dierent
manufactures, and hence is a strong marketing tool.
Work has been carried out in [13] to implement a tightly coupled navigation loop.
It was observed that the benets of implementing a loosely coupled form outweigh
those of its counterpart since the increase in optimality of the tightly coupled conguration is slight. This was mainly due to the diculty of developing GNSS algorithms
and hardware that can be aided by navigation lters.
Thus the majority of implementations of aiding inertial navigation systems have
been in a direct feedback conguration, and is the approach taken in this thesis for
two of the three navigation systems implemented (Chapter 4).
2.6.2 Aiding in Direct Feedback Congurations
In a direct feedback structure, the model implemented in the lter is a linear error
model representing the errors in the vehicle states, generally position, velocity and
attitude. When an observation becomes available, the lter estimates the errors
in these states. Since the model is an error model of the inertial equations, the
observation z(k) is the observed error of the inertial navigation solution and not the
observation delivered by the aiding sensor. Thus if an aiding system provides position
and velocity data then the observation vector becomes,
2
z(k) =
zP (k) 5 = 4 P (k) P (k) 5

zV (k)
Vinertial (k) Vaiding (k)
inertial
aiding
(2.34)
24
Figure 2.5 illustrates the observation structure [35]. The true acceleration, velocity
and position of the vehicle have noise added to them to represent measurements taken
by the sensors. The acceleration, as measured by the inertial navigation system, is
integrated twice to obtain the indicated velocity and position of the vehicle. The
acceleration information is obtained by the accelerometers and it is assumed that
acceleration due to gravity has been compensated for.
v
A
+
A
inertial
inertial
inertial
v
P
aiding
Z
V
aiding
+
-
Figure 2.5: Illustration of how the observation measurements zP and zV are obtained
by the inertial and aiding information.
By dening the terms P(k) and V(k) as the position and velocity errors in the
2.7 Filter Consistency and Fault Detection
25
inertial data after the integration process, the observation model becomes
2
inertial
P
(
k) Paiding (k) 5
4
z(k) =
Vinertial (k) Vaiding (k)
2
3
2
3 2
3
= 4 (PT (k) + P(k)) (PT (k) vP (k)) 5 = 4 P(k) 5 + 4 vP (k) 5
(VT (k) + V(k)) (VT (k) vV (k))
V(k)
vV (k)
(2.35)
The observation is thus the error between the inertial indicated position and velocity
and that of the aiding sensor, and the uncertainty in this observation is re ected by
the noise of the aiding observation. This oers another benet in the direct feedback
implementation and involves the tuning implementation; the noise on the observation
is the noise on the aiding sensor. Thus once an inertial unit and process model is
xed then the process noise matrix Q(k) is also xed, and tuning the lter is solely
based on the observation noise matrix R(k).
2.7 Filter Consistency and Fault Detection

The lter implementations discussed provide various methods of estimating the states
of interest. However, unless the true values of those states is known, there is no way of
determining whether the lter is computing correct estimates. The only information
available from the real world is the observation, and hence the only form of measure
for determining the behaviour of the lter is the dierence between the observation
and the predicted observation, that is, the innovation (Equations 2.6 and 2.17 for the
Kalman lter and EKF).
The innovation has the property that it must be both unbiased and white, and
have covariance S(k) if the lter is operating correctly. To determine whether this is
2.8 Summary
26
the case, the innovation is normalised,

S (k) (k):
T (k)
(2.36)
If the lter assumptions are correct then the samples of are distributed as a
distribution in m degrees of freedom (the number of observations being estimated).
Instead of using Equation 2.36 as a method of determining lter consistency, it
can be used as a gating function. When an observation is obtained, Equation 2.36 is
formed, and if the value is less than a predened threshold, then the observation
is accepted. This allows for a means of detecting any faults within the observation.
The threshold value is obtained from standard tables and is chosen based on the
condence level required. Thus for example, a 95% condence level, and for a state
vector which includes three states of position and three of velocity, then = 12:6.
The information gate, which is implemented for both the information lter and the
EIF, is the information equivalent of the normalised innovation,
2
(k )
= VT (k)B (k)V(k);
1
(2.37)
where
I (k) = HT (k)[H(k)I(k)HT (k)] H(k);

V(k) = I(k)[I (k)i(k) Y (kjk 1)y(kjk 1)];
and B(k) = I(k)[I (k) + Y (kjk 1)]I(k):
+
It can be shown that for any lter = .
2.8 Summary
This chapter has developed the equations for the discrete linear and non-linear versions of the Kalman and information lters. This chapter has also detailed the various
lter structures that can be used to aid inertial navigation systems. The \direct" im-
2.8 Summary
27
plementation is the most natural implementation although it requires a non-linear

lter and the lter runs at the same rate as the sampling of the inertial system. To
overcome this, either a \direct feedback" or \indirect feedback" structure can be implemented. These implementations provide a reduction in computational processing
since the lter is implemented in a linear fashion and does not need to run at the
sampling rate of the inertial system.
These lters were classied as \loosely coupled" implementations since there is no
aiding to the external sensor. A \tightly coupled" conguration was presented and
it was discussed that even though it provided a more robust solution to the ltering
problem, it required substantially increased eort and cost to implement.
It was also shown that the greatest modularity can be achieved through the implementation of the feedback structures. Only once does the system designer have to set
up the inertial navigation algorithms along with the lter code. Any external aiding
can then be used with the only modication being the observation noise matrix.
Finally the chapter discussed how the validation and information gates commonly
associated with evaluating lter consistency can be used to determine if an observation is at fault.
Chapter 3
Inertial Navigation
3.1 Introduction
This chapter will provide the necessary background knowledge on inertial sensors
and their associated errors. Furthermore, the chapter will provide, as a contribution,
the inertial navigation equations needed for land vehicle navigation and compare
these equations to other forms used in missile and aircraft applications. The inertial
navigation equations are then linearised to develop error equations. These equations
provide the mathematical foundation to analyse the propagation of errors in inertial
navigation.
Finally the chapter will detail the methods adopted in this thesis to determine the
alignment of low cost inertial units.
3.2 Inertial Navigation

Inertial navigation is the determination of the pose of a vehicle through the implementation of inertial sensors. It is based on the principle that an object will remain in
uniform motion unless disturbed by an external force. This force in turn generates an
acceleration on the object. If this acceleration can be measured and then mathematically integrated, then the change in velocity and position of the object with respect
29
to an initial condition can be determined.

The inertial sensor which measures acceleration is known as an accelerometer. However, the total acceleration encountered by the object is what is measured, that is,
both the acceleration due to gravity and that due to all other external forces. To
remove the component of acceleration due to gravity, the tilt (or attitude) of the
accelerometer with respect to the local vertical needs to be known.
To measure the attitude, an inertial sensor known as a gyroscope is required.
This sensor measures angular velocity, and if mathematically integrated provides the
change in angle with respect to an initially known angle. Figure 3.1 shows a photograph of low cost accelerometers and gyros used in the nal chapter of this thesis.
The combination of accelerometers and gyros allows for the determination of the
pose of the vehicle. Figure 3.2 is a photograph showing inside the Watson Inertial
Measurement Unit (IMU). This is classied as a low cost device and is the IMU
employed in most of this thesis.
Figure 3.1: A photograph showing low cost accelerometers (bottom) and gyroscopes
(top) implemented in this thesis.
A tri-axial inertial unit comprises of three accelerometers in an orthogonal arrangement along with three gyros also in an orthogonal arrangement. The accelerometers
provide the acceleration of the vehicle in the body axes in which they are aligned,
30
Figure 3.2: A photograph showing inside of the Watson IMU implemented in this
thesis. The gyros implemented in this unit are the same gyros shown in the previous
photograph. The accelerometers are hidden in the back.
generally denoted as the forward x, lateral y, and vertical z axes, while the gyros
provide the rotation rates about these axes respectively and are denoted as the roll _,
pitch _ and yaw _ rates, Figure 3.3. The principal advantage of using inertial units
Figure 3.3: x,y and z represents the body frame of the vehicle as observed by the
inertial unit. _, _ and _ represent the rotation rates roll, pitch and yaw about these
axes.
31
is that given the acceleration and angular rotation rate data in three dimensions, the
velocity and position of the vehicle can be evaluated in any navigation frame. For
land vehicles, a further advantage is that unlike wheel encoders, an inertial unit is
not aected by wheel slip.
However, the errors caused by bias, scale factors and non-linearities in the sensor readings cause an accumulation in navigation errors with time and furthermore
inaccurate readings are caused by the misalignment of the unit's axes with respect
to the local navigation frame. This misalignment blurs the distinction between the
acceleration measured by the vehicles motion and that due to gravity, thus causing
inaccurate velocity and position evaluation. Since an inertial unit is a dead reckoning
sensor, any error in a previous evaluation will be carried onto the next evaluation,
thus as time progresses the navigation solution drifts.
3.2.1 Inertial Systems
A package of inertial sensors is classied into one of three groups:

An Inertial Sensor Assembly (ISA) : In which the raw data from the inertial
sensors is the only data output from the unit;
An Inertial Measurement Unit (IMU) : Is an ISA however the raw data output
is compensated for errors such as scale factors and bias; and
An Inertial Navigation System (INS) : Is an IMU however the output from the
unit is fed to navigation algorithms so that the position, velocity, attitude and
heading of the vehicle can be evaluated. The unit also provides the compensated
raw data which can be used for control or stabilisation purposes.
INS systems are generally found in almost all forms of long distance aircraft and
sea vessels, submarine and missile applications, and this is due to their initial wide
spread use in military roles. In such applications the inertial sensors implemented
have to be of supreme quality, providing stable readings, extremely high resolution
32
and high-bandwidth. The algorithms and electronics implemented are also of high
quality in order to minimise the introduction of any errors. With the current trend
to better navigation performance for civilian applications, INS systems can provide a
useful sensor, however, current high accuracy INS systems are too expensive for land
vehicle or robotic applications.
The predominant cost derives from the type of inertial sensors being implemented
and in particular that of the gyroscopes. Reducing the cost of these sensors through
the type of materials being used, manufacturing process and through the actual physical implementation, in turn decreases the accuracy of the inertial system.
3.2.2 Physical Implementation
The physical implementation of the inertial sensors can take on two forms:
Gimballed arrangement: The accelerometers are mounted on a gimballed mech-
anised platform such that the platform always remains aligned with the navigation frame. This is done by constantly actuating the gimbals based on the
transition of the navigation frame. Thus the accelerometers are directly integrated to provide velocity and position in the navigation frame.
Strapdown arrangement: The accelerometers and gyros are mounted directly
onto the body of the vehicle. The rotation rates measured by the gyros are
used to constantly update the transformation between the body and navigation
frames. The accelerometer data is then passed through this transformation to
obtain the acceleration in the navigation frame.
In a strapdown arrangement the sensors experience the full eects of vehicle motion, and thus higher bandwidth and dynamic range are required. The higher dynamic range in turn aects the stability of scale factor terms, and may also introduce
larger non-linearity errors. Higher bandwidth implies noisier data provided by the
sensors. Furthermore, a gimballed arrangement requires minimal computational processing since the platform is maintained to correspond with the navigation frame. In
33
contrast, a strapdown arrangement needs to compute and resolve terms relating the
body, navigation and inertial frames.
These factors alone would suggest that the better approach is to implement gimballed arrangements, and for highly accurate systems such as those used on submarines and ICBMs, this is how it is done. However, the advantage of strapdown
inertial systems is the weight, size, power and more importantly the cost reduction
associated with such systems.
3.2.3 Gyroscopic Sensors
The focus of this section is to provide an introduction to the various gyroscopes (gyros) that can be implemented in low cost applications. The references [8,10,46,57],
provide excellent information on the development and implementation of these inertial
sensors along with their various characteristics. Accelerometers are not considered
here since high grade versions are available for relatively low cost as compared to
similar performing gyros.
The current word in the market, if one can use such a cliche, is the advent of silicon gyros. Silicon inertial sensors have been around for approximately ten years and
have made enormous strides particular in the development of silicon accelerometers.
Silicon gyros have only recently gained in popularity as performance has improved.
Today it is possible to place a tri-axial arrangement of silicon gyros and accelerometers, along with all electronics, into a package weighing no more than half a kilogram,
and consuming no more than two watts of power. The use of such systems is already
established in areas such as control and stabilisation for missile [14] and automobile
applications [17] . The problem of course, is the relatively high level of noise and
consequently the poor navigation performance.
Ceramic gyros have been available for approximately thirty years. Again they have
only been used for control and stabilisation purposes. They are heavier and consume
more power than their silicon counterparts, however their navigation performance is
currently superior to silicon gyros.
Both the silicon and ceramic gyros are known as Vibratory Structure Gyroscopes
3.3 Inertial Navigation Equations
34
(VSGs). The main component in these sensors is an element which is linearly vibrating at a known frequency. If an angular rotation is encountered perpendicular to this
vibration then Coriolis acceleration is generated which in turn modies the resonant
frequency. This modication is measured and interpreted as the angular rotation
encountered. Other gyros are mechanical systems (Dry Tuned Gyros and Rate Integrating), Ring Laser Gyros (RLGs), Fibre Optic Gyros (FOGs), Cryogenic versions,
Electrostatic, and the list goes on [57]. Except for FOGs, all are too expensive for
use in most civilian applications. If the current trend in decreasing cost continues,
FOGs may become a cost eective sensor in the coming years, although predictions
on the accuracy of silicon sensors seem to suggest that the future of low cost inertial
sensing lies with them.
A ceramic VSG developed by British Aerospace is the gyro implemented in this thesis. The immediate start-up time, low power consumption, weight and cost of this
sensor meet the specications and requirements needed for the low cost applications
encountered with civilian land vehicles. The predominant error associated with these
sensors is the bias encountered due to temperature eects on the resonator. Temperature compensation and/or xing the temperature of the inertial unit is the most
practical way of handling such errors.

The inertial navigation equations derived here are not new. These equations can be
found in any inertial navigation text book and the derivation here follows that of [57].
However, the purpose of this section is to show why a particular framework, known
as the \Earth Frame", may be used for land vehicle applications conned to small
areas. This requires analysis of the other frameworks, so as to oer a comparison.
35
3.3.1 The Coriolis Theorem
Navigation with respect to a rotating frame such as the earth requires the Coriolis
theorem. The theorem states that the velocity of a vehicle with respect to a xed
inertial frame vi, is equal to the ground speed of a vehicle ve (the velocity of the
vehicle with respect to the earth), plus the velocity of the vehicle due to the rotation
rate of the earth with respect to the inertial frame !ie, at the point on earth where
the vehicle is located r, that is,
vi = ve + !ie r;
(3.1)
where !ie = [0 0
] and
is the rotation rate of the Earth . Dierentiating this
equation with respect to the inertial frame gives
v_ iji = v_ eji + !_ ieji r + !ie vji:
(3.2)
Now assuming that the angular acceleration of the Earth is zero, that is !_ ie = 0,
and substituting Equation 3.1 into 3.2,
ve + !ie r]
= !ie ve + !ie [!ie r];
(3.3)
v_ iji = v_ eji + !ie ve + !ie [!ie r]:
(3.4)
!ie
vji =
!ie [
hence Equation 3.2 becomes

Now v_ i ji = f + g where f is the specic force encountered due to the vehicle motion
and g is the force due to gravity. Hence Equation 3.4 becomes
f + g = v_ eji + !ie ve + !ie [!ie r];
(3.5)
36
that is,
v_ eji = f [!ie ve] + [g
!ie [! ie
r]]:
(3.6)
Equation 3.6 simply states that the acceleration over the Earth's surface is equal to the
acceleration measured by the accelerometers compensated for the Coriolis acceleration
encountered due to the velocity of the vehicle over a rotating Earth, !ie ve, and
for the local gravity acceleration, which comprises of the Earth's gravity, g, and that
due to the rotation of the Earth, also known centripetal acceleration !ie [!ie r].
3.3.2 The Transport Frame
The Transport frame is used when a vehicle travels over large distances around the
earth such as in missile or air transport applications. The objective of this framework
is to obtain the ground speed of a vehicle with respect to a navigation frame, generally
expressed in North, East and Down coordinates, at a given location, expressed in
latitude, longitude and height. In such situations the rotation of the earth needs to be
taken into consideration along with the fact that the navigation frame is also rotating.
As an example, constantly keeping the North axis aligned on a ight from Sydney to
London will require the navigation frame to constantly rotate. This, coupled with the
fact that the earth is rotating during this transition, causes a Coriolis acceleration
between the navigation frame, the earth rotation and the ground speed of the vehicle.
Hence, the Transport framework is dened as: the acceleration of the vehicle with
respect to the navigation frame v_ ejn, is equal to the acceleration of the vehicle with
respect to the inertial frame v_ eji, minus the Coriolis acceleration due to the navigation
frame rotating !en, on a rotating earth !ie. That is,
v_ ejn = v_ eji (!ie + !en) ve:
(3.7)
37
Substituting in Equation 3.6,
v_ ejn = fn [!ie ve] + [g !ie [!ie r]] (!ie + !en) ve

= fn (2!ie + !en) ve + [g !ie [!ie r]];
(3.8)
where fn represents the acceleration in the navigation frame. Since the inertial unit
measures the acceleration in the body frame fb, this acceleration needs to be transformed into the navigation frame, that is,
fn = fb:
(3.9)
The transformation , can take on three forms as will be discussed in Section 3.5.
Regardless of how is obtained, the rotation data relating the body to navigation
frame !bn is required. However, the gyros measure the total inertial rotation !ib
which comprises of !bn plus the rotation of the navigation frame with respect to the
inertial, which is the rotation rate of the navigation frame with respect to the earth
plus the rotation rate of the earth with respect to the inertial frame,
!bn
!ib
[!ie + !en];
(3.10)
where !en is known as the transport rate and is dened as

ve
ven
v tan L
! en = [
;
; n
]:
(3.11)
Ro + h Ro + h Ro + h
L is the latitude of the vehicle, Ro is the radius of the Earth and h is the height of the
vehicle above the earth's surface. The transport rate is what rotates the navigation
frame as the vehicle travels across the surface of the earth.
3.3.3 Wander Azimuth Frame
The tan function in the third term of Equation 3.11 adds a complication to the
Transport frame. As the vehicle travels to higher latitudes the third term becomes
38
larger and its rate of change also increases dramatically, until a singularity is reached
at the poles. To overcome this problem for vehicles traveling around the poles, and
hence obtaining a truly global framework, a Wander Azimuth frame is used. This is
a locally level frame, in which the axis is dened along the local vertical and planar
to the earth's surface at the position of the vehicle. The frame however moves with
the vehicle, and the azimuth angle between the x axis of the frame and that of the
North axis varies depending on position. This variation is determined so as to remove
any singularities. The derivation of this framework is the same as for the Transport
frame.
3.3.4 The Earth Frame
The Earth frame, as in the Wander Azimuth frame, is also a locally level frame. In
the Earth frame the acceleration of the vehicle with respect to the earth v_ eje, is equal
to the acceleration of the vehicle with respect to the inertial frame v_ eji, minus the
Coriolis acceleration due to the velocity of the vehicle ve, over a rotating earth !ie,
v_ eje = v_ eji
!ie
ve :
(3.12)
The Earth frame will be used throughout this work since its denition is well suited
for land vehicle applications. Substituting in Equation 3.6 gives
v_ eje = fe 2[!ie ve] + [g
!ie [!ie
re]]:
(3.13)
Again, since the inertial unit measures the acceleration in the body frame, the acceleration measurements need to be transformed into the earth frame,
fe = f b;
(3.14)
where now comprises of the rotation rates !be which relates the body to earth
frame. However, the gyros measure the total inertial rotation !ib which comprises of
!be plus the rotation of the earth with respect to the inertial frame transformed over
3.4 Schuler Damping
39
to the body frame, so

!be
!ib
be!ie:
(3.15)
3.4 Schuler Damping

The major dierence between the Transport and Earth frame equations is the transport rate dened by Equation 3.11. The transport rate is subtracted from the body
rates and hence denes the attitude of the system with respect to the Transport
frame. However, incorporating the transport rate also adds a bound to the errors
associated with inertial systems. This is because the acceleration in the navigation
frame is twice integrated to provide velocity and then position. The velocity is used
to determine the transport rate which is then subtracted from the body rates.
For example, assume that the pitch is in error by a small angle . This error
in turn causes an incorrect calculation of the acceleration due to the misalignment
and the measurement of gravity given by g . Repeating this process shows that
misalignment feeds back onto itself. Looking at the characteristic equation of such a
system, it may be observed that it exhibits simple harmonic motion with period
!s
g
;
Ro
or 84.4 minutes. This is known as the Schuler oscillation. In essence, by implementing

the Transport frame, the errors are bounded within the Schuler oscillations. If a
system is tuned to the Schuler frequency then some general rules of thumb can be
obtained for inertial systems:
An initial velocity error vo , causes a position error of vo
!s t)
!s
sin(
An initial attitude error , causes a position error of R(1 cos(!st));

An acceleration bias f , causes a position error of f (
A gyro bias !b, causes a position error of !bR(t
!s2
!s t)
!s
sin(
!s t)
cos(
).
); and
3.5 Attitude Algorithms
40
Thus for all errors, except for gyro bias, the position error is contained within the
Schuler oscillation. The gyro bias causes Schuler oscillations which however grow
linearly with time. It is for this reason that, with accurate gyros, passenger airlines
can travel vast distances relying solely on inertial navigation.
A reasonable question is \Why not use the Transport frame for land vehicle applications and hence benet from this bounding property?" The Transport frame
can be used for any inertial navigation implementation and thus allow for bounded
error growth irrespective of how large this error may be. However, as an example,
a low cost inertial system with a typical gyro bias of 0:01deg=sec will give rise to
a bounded position error of 160000km while a gyro bias of less than 0:001deg=hr,
such as encountered with ring laser gyros found in passenger airlines, gives rise to a
bounded position error of 4km. Thus Schuler bounding has no purpose when using
low grade inertial units. If the transport term from Equation 3.8 is removed, the
resulting equation has a similar structure to that of the Earth frame, Equation 3.13.
However, the Earth frame provides a better conceptual understanding when dealing
with navigation within conned areas.

The transformation matrix is required to evaluate the acceleration vector in the
desired frame given the acceleration vector in the body frame. This transformation
matrix has to be accurate since misalignment (errors in estimated attitude) causes
the components of the gravity vector measured by the accelerometers to be confused
with true vehicle acceleration. Integrated over time, even small misalignment errors
will cause large estimated errors.
The transformation matrix consists of the roll, pitch and yaw angles required to
rotate the body frame to the navigation frame and hence is updated continuously
since the body frame is always rotating with respect to the navigation frame. The
updating process propagates this matrix based on data obtained from the gyros, thus
any errors in is caused by both the physical errors associated with the gyros and the
41
errors in the algorithms used to propagate the transformation matrix. The physical
errors is discussed in Section 3.10.
There are a number of algorithms available for attitude propagation. However,
regardless of the type of attitude algorithm implemented, in their analytical forms
they provide identical results. The choice of algorithm depends on the processing
power available, on the errors introduced due to discretisation of algorithms, and on
the errors introduced due to the simplication of the algorithms. The simplication
of algorithms is to ease the computational load on the processor.
There are in principle three approaches to propagating the transformation matrix:
Euler, Direction Cosine and Quaternion methods. The Euler approach is conceptually
easy to understand although it has the greatest computational expense. Although
the Quaternion approach is computational inexpensive compared to the other two,
it has no direct physical interpretation to the motion of the vehicle. The Direction
Cosine approach ts in between, both in terms of physical understanding and in
computational expense.
3.5.1 Euler Representation
The Euler representation is the easiest form to understand. It is the mathematical

analog form of a gimbal system. The roll , pitch and yaw angles used to represent
the rotations from the body frame to the navigation frame are placed in a transformation matrix Enb, by multiplying the consecutive rotation matrices representing a
roll, followed by a pitch and then a yaw,
2
E nb
32
32
0 7 6 c 0 s 7 6 1 0 0 7
6
= 64
0 75 64 0 1 0 75 64 0 c s 75
0 0 1
s 0 c
0 s c
2
3
c c c s + s s c s s + c s c
6
7
= 64 c s c c + s s s s c + cs s 75
c
s
s
c
s c
c c
(3.16)
42
where subscripts s and c represent sine and cosine of the angle. As the body frame
rotates, the new angles are obtained from
_
= (!y sin + !z cos ) tan + !z

_ = !y cos !z sin
_ = (!y sin + !x cos ) sec :
(3.17)
(3.18)
(3.19)
where ! is the rotation rate measured by the gyros about the x; y and z axes. As
is apparent from these equations, both the roll and yaw updates have singularities
when the pitch of the vehicle is . This is equivalent to the physical phenomenon of
\gimbal lock" in gimbal systems. In a strapdown system, as the vehicle approaches

then theoretically innite word length and iteration rates are required in order to
accurately obtain the result. It is for this reason that Euler updates are not generally
implemented, although this is not a problem for land vehicle applications.
Furthermore, this approach is computationally expensive due to the trigonometry required in the updates and in forming E nb. The transformation matrix can be
simplied if one assumes small angular rotations (< 0:05deg) thus,
2
E nb
6
6
4

1
1
3
7
7
5
(3.20)
Discretisation
The discretisation procedure is as follows:
The new roll angle is obtained through integration by
(i + 1)
_ + (i)
dt
(3.21)
and similarly for pitch and yaw. The angles are then placed into the matrix to
form Enb(i + 1).
43
Obtain the acceleration data in the body frame fb = [fx; fy ; fz ] and evaluate
the acceleration in the navigation frame
fn = Enbfb
(3.22)
Integrate the acceleration to obtain velocity and then position.

3.5.2 Direction Cosine Matrix (DCM) Representation
The direction cosine matrix Cnb, is a 3 3 matrix containing the cosine of the angles
between the body frame and the navigation frame. Cnb is propagated by
_ n = Cnb
bn;
(3.23)
Cb
where
bn is a skew-symmetric matrix representing rotation rates as measured by the
gyros,
2
bn = 664
!z
!z
!y
!x
!y
!x
3
7
7:
5
(3.24)
The initial Cnb is simply the initial Enb.
Discretisation
Obtain the gyro outputs !x; !y ; !z and integrate to determine the change in
angle x; y ; z .
Determine the angular vector magnitude

x + y + z
2
(3.25)
44
Determine the series coecients

= sin
= 1 cos
(3.26)
(3.27)
Form the angular skew matrix

0
B
B
@
z
y
z
y
C
x C
A
x
(3.28)
Update the direction cosine matrix
Cnb(i + 1) = Cnb(i)[I + + 2]

3
(3.29)
Obtain the acceleration data in the body frame and evaluate the acceleration
in the navigation frame
fn = Cnb(i + 1)fb
(3.30)

3.5.3 Quaternion Representation
In the Quaternion approach the rotation from one frame to another can be accomplished by a single rotation about a vector q through an angle q. A quaternion
consists of four parameters which are a function of this vector and angle. The initial quaternion is obtained from the roll, pitch and yaw angles dened in the Euler
45
representation or alternatively through the Direction Cosine parameters,

0
p
0
:5 1 + Cbn + Cbn + Cbn
B
B
n
Cbn )
q (Cb
q(i) = BBB
n
Cbn )
q (Cb
@
n
Cbn )
q (Cb
11
22
33
(1)
32
23
(1)
13
31
21
12
1
C
C
C
C
C
A
(3.31)
(1)
Discretisation
Obtain the gyro outputs !x; !y ; !z and integrate to determine change in angle
x; y ; z .
Determine the quaternion operator coecients

sin
= 2
= cos 2
(3.32)
(3.33)
Determine the quaternion operator

0
h(i) =
B
C
B C
xC
B
B
C
B C
yA
@
z
(3.34)
Generate the quaternion matrix and update

0
q (i)
B
Bq (i)
B
B
Bq (i)
@
q (i)
q(i + 1) =
q ( i)
q (i)
q (i)
q ( i)
q (i)
q (i)
q (i)
q (i)
q (i)
q (i)
q (i)
q (i)
C
C
3 C
C
C
2A
h(i)
(3.35)
46
Obtain the acceleration data in the body frame and evaluate the acceleration
in the navigation frame.
fn = q(i + 1)fTb q(i + 1);
(3.36)
where q is the complex conjugate of q. A less computationally expensive

approach is taken by converting the quaternion back into Cnb by
0
(q + q q q ) 2(q q q q )
2(q q + q q ) C
B
n
Cb = B@ 2(q q + q q ) (q q + q q ) 2(q q q q ) C
A;
2(q q q q )
2(q q + q q ) (q q q + q )
2
and then
fn = Cnbfb
(3.37)

3.5.4 Discussion of Algorithms
All three approaches produce identical results in their complete form.

The Euler approach is not commonly used due to the presence of the roll/yaw
singularity. This does not pose a problem for land vehicle applications. However, the
Euler approach requires high computational eort and thus powerful processing if fast
updates are required. The algorithm can be simplied when small angle assumptions
are made. In order to satisfy this small angle assumption, the update rates generally
have to be high. For example, land vehicles can undertake rotation rates of 50deg=sec,
and so a sampling rate of at least 1kHz is required (to keep the angles below 0:05deg).
Such a sampling rate is high for low cost inertial systems although in more expensive
systems used in military and space applications 20kHz is not uncommon.
The Quaternion has the benet of only requiring the update of four variables. It is
most often used in military and space applications. In order to save on computational
3.6 Errors in Attitude Computation
47
cost, the quaternion is converted to a DCM representation for transformation of the

acceleration from the body frame to the navigation frame. This can be a signicant
computational burden if fast sampling rates are required. To overcome this most
military users implement a dual sampling system.
As an example, a Laser INS (LINS) system typically samples Ring Laser Gyros
(RLGs) at 1200Hz and the accelerometers at 400Hz. The lower sampling rate is
sucient for typical LINS navigation applications. Three samples of the RLGs are
used to update the quaternion, thus forming a \super" quaternion representing a
single rotation. This quaternion is then converted to the DCM representation and
used to transform the acceleration measurements.
The DCM approach is commonly used in all forms of inertial navigation. It also
oers the best representation for land navigation. Although nine variables are updated
at each sample, it is less computationally expensive than the Euler representation, and
unlike the Quaternion approach, requires no conversion over to another representation
in order to transform the acceleration data from the body frame to the navigation
frame.

Equations 3.26 and 3.27 presented the series coecients required for the DCM updating process. If these coecients can be determined exactly then the DCM generated
would be exact. However, in practice a Taylor series expansion is required,

= 1 3! + 5!
= 2!1 4! + 6!
2
:::
:::
(3.38)
(3.39)
The number of terms which are included in the expansion, and hence the accuracy of
the series, describes the order of the algorithm. In [57] it is shown that the error in
48
the DCM algorithm can be represented as

1
Ddc = (a cos sin + a sin );
t
(3.40)
where a = 1 and a = 0 for a rst order algorithm, a = 1 and a = 0:5 for a

second order algorithm, a = 1 and a = 0:5 for a third order algorithm and
so on.
For example, consider a land vehicle whose axis experiences rotations in the order
of 20deg=sec. Table 3.1 shows the drift in the algorithm when the sampling rates are
set to 10, 125, 200 and 500Hz and the order of the algorithm is increased from a rst
to a third.
1
Order of Algorithm
1
2
3
3!
10Hz 125Hz
200 Hz
30
0.23
0.04
14.7 0.11
0.021
0.001 5 10 4:85 10
7
500 Hz
0.012
0.006
1:4 10
Table 3.1: Algorithm Drift Rates in o=hr caused by sampling a continuous rotation
rate of 20deg=sec
What is apparent from the table is that although the performance of the algorithm
increases at each increase in sampling rate, there is a more marked improvement by
increasing the order of the algorithm.
Furthermore, although the sampling rates suggested in this table are not comparable to the kHz sampling rates implemented by high grade inertial systems, it is
the relative magnitude between the sampling rate and the continuous rotation rate
that is of concern. Hence, as is apparent from the table, the drift errors caused by
the simplication of the algorithm are kept small as long as the sampling is of higher
magnitude than the continuous rotation rate.
3.7 Vibration
49
3.7 Vibration
Vehicle vibration has a detrimental aect on the performance of inertial navigation
systems. This is due to the low bandwidth of low cost inertial sensors, causing
attenuation or total rejection of motion vibration at high frequencies. Vibration is
generally a combination of both angular and translational motion. If the bandwidth
of both the gyros and accelerometers are equal, and the vibration of the vehicle at
frequencies above this bandwidth has smaller magnitude than the resolution of the
sensors (which is likely with low cost units), then vibratory motion can safely be
ignored. However, the bandwidth of low cost gyros is almost always signicantly
less than for low cost accelerometers. Vibrations cause small changes in attitude
which is measured by the accelerometers as a component of gravity. This acceleration
component will be inaccurately evaluated as apparent vehicle motion since the gyros
can not measure this change in attitude.
It is possible to introduce a low pass lter in the sampling circuitry to remove high
frequency vibration. The problem then becomes the consequent lag in the signal
response of the inertial unit. The best approach is to incorporate vibration absorbers
into the physical system.
As an example, ceramic VSGs have a bandwidth of approximately 70Hz, those
of a particular brand of piezo-accelerometers 300Hz. Thus the approach would be
to obtain vibration absorbers that attenuate as much of the signal as possible above
70Hz. However, while low cost inertial units have the benet of being light weight, it
is dicult to nd absorbers that can attenuate high frequencies given the light load
on the absorbers themselves. Furthermore, it is important to ensure that the natural
frequency associated with the absorbers does not lie in a region where any excitement
of the vibration causes inaccurate readings of the sensors.
The eect of vibration on the attitude algorithms is termed \coning" or \noncommutativity". The denition of coning motion is simply stated as the cyclic motion
of one axis due to the rotational motion of the other two axes. If one axis has an
angular vibration rotation of A sin(!t) and the other axis A sin(!t + %), then the third
axis will have a motion describing a cone. A perfect cone motion would occur if the
3.7 Vibration
50
phase dierence % between the two axes is ninety degrees, otherwise the motion will
exhibit some other form (and in reality would be random). In [18] three forms of
coning errors are discussed:
True Coning Errors Type 1: Where the true coning motion due to vibration
stimulates the presence of coning motion in the algorithms used to determine

the attitude of the inertial unit.
True Coning Errors Type 2: Where the true coning motion due to vibration
causes a real net angular rotation rate !c, which if not properly taken into
account will be assumed to be rotation of the vehicle when it is not.
Pseudo Coning Errors Type 3: When there is no real coning motion due to
vibration but coning motion is produced in the algorithms due to induced errors
in the inertial unit or in the algorithms themselves.
Type 1 errors arise because of sensor errors, namely gyro scale factor and gyro orthogonality errors, both of which are large with low cost inertial units.
An example of Type 2 errors may occur when an inertial unit uses RLGs. The
operation of this gyro works on the principle of two opposing light beams operating
in a xed optical path having the same optical frequencies. When the gyro undergoes
rotation the frequencies change in order to maintain the resonant frequency required
for the laser beams. At very low rotations this does not happen and the two beams
assume the same frequency, thus it appears as though there is no rotation; a phenomenon known as \lock-in". To remove this the gyros are \dithered". That is, a
low amplitude angular vibration is applied to the gyro. This vibration occurs at high
frequency and hence can cause coning motion. One way to minimise the aect is to
dither the gyros at dierent frequencies.
Type 3 errors are of most concern. They arise from errors associated with truncation in the attitude algorithm and the limited bandwidth of the algorithm, both of
which are alleviated as the order of the algorithm is increased along with the sampling
rate.
3.8 Minimising the Attitude Errors
51
The problem with coning motion is determining whether the right order algorithm
or sampling rate has been chosen, and whether aects such as quantisation errors
or dithering is causing any errors. The approach taken in industry is to place the
system on a vibration table which can subject the unit to coning motion. The drift
in position and attitude is an indication of coning error magnitude. This however, is
only benecial in cases where the application is known and the inertial unit is built to
suite. However, for generic low cost inertial applications one purchases the unit \o
the shelf" and hence such techniques are not available. In these situations vibration
can cause errors in the attitude evaluation and hence drive navigation errors, thus
requiring aiding from an external sensor in order to bound these errors.
3.8 Minimising the Attitude Errors

In light of the arguments presented, the approach taken for minimising the errors associated with attitude algorithms for low cost inertial units will consist of the following
steps:
The bandwidth of the gyros employed will be the limiting factor in performance.
Generally the accelerometers employed and the sampling rate achievable will
be higher than the bandwidth of the gyro. If vehicle vibration exceeds that of
the bandwidth of the gyro, then the only reasonable choice is to use vibration
absorbers to attenuate incoming signals above the bandwidth of the gyro, taking
into consideration the natural frequency of the absorber.
The sampling rate of the inertial sensors should be above the Nyquist frequency
and furthermore should be as high as possible given the discussion in Section

3.6.
The higher the order of the algorithm the better (Section 3.6), and this will
come down to the sampling rate and the processing power available.
3.9 Error Analysis
52
3.9 Error Analysis

The accuracy of inertial navigation will depend on the accuracy of the sensors employed and on the computer implementation of the inertial algorithms. To analyse
how these two eects contribute to the inertial navigation system development, inertial error equations are derived. These equations provide the position, velocity and
attitude error growth, given the sensor and algorithm errors. The development of the
error equations is accomplished by perturbing (or linearising) the nominal equations.
The perturbation can occur in two forms
The Computer Approach ( angle) where the analysis occurs about the com-
puted geographical position determined by the inertial computer. Since the

computer frame is known the perturbation of the angular position and rate are
zero.
The True Frame Approach ( angle) where the perturbation occurs about the
true position of the vehicle. The true position is not known and hence all the
elements are perturbed.
The benet of implementing the perturbation in the computer frame is that the misalignment between the computer frame and the true frame is independent of the
position of the computer frame. When perturbing in the true frame the misalignment
is coupled with the position. However, perturbation in the true frame is simpler. Both
perturbation techniques produce identical results as shown in [1,2,4].
Perturbation analysis has always centered on the Transport or Wander Azimuth
frames as these are due to their wide spread use.
The perturbation of the Earth frame is the objective of this section and is done
so using the true frame approach. It will be shown that in this frame, misalignment
propagation is independent of position, thus delivering the same benet as the computer frame approach.
3.9 Error Analysis
53
Perturbation in the true frame is accomplished by stating that the error in a particular state is the dierence between the estimated state and the true state. Thus
given that the Earth frame velocity equation (Equation 3.13) is
v_ eje = Cebfb 2[!ie ve] + [g
!ie [!ie
re]];
(3.41)
and dening the total gravity acceleration as
gt = [g
!ie [!ie
re]];
(3.42)
then by denition of the perturbation
_ = v~_ee v_ ee
= [C~ ebf~b Cebfb ] [2!~ eie v~ee 2!eie vee] + [g~t
g t ]:
(3.43)
where v~_ee is the evaluated velocity vector and v_ ee is the true velocity vector. The
evaluated transformation matrix C~ eb, is the true transformation matrix C eb, misaligned
by the misalignment angles . The misalignment is represented in skew-symmetric
form [ ]. Hence
~ e = [I [ ]]C eb;
Cb
(3.44)
thus
_ =
C ebf b
C eb f b
[ ]C ebf~b
[2!eie v ee ] + g t :
(3.45)
If the errors in Coriolis and gravity terms are insignicant then
_ =
C eb f b
C eb f b
[ ]C ebf~b
[ ]f~e:
(3.46)
3.9 Error Analysis
54
Now
[ ]f~e = f~eT [ ];
(3.47)
_ = f~eT [ ] + C ebf b
= [f~e] + C ebf b:
(3.48)
hence
v
Thus the propagation of velocity errors in the Earth frame v_ is equal to the acceleration error in the Earth frame due to the misalignment of the frame [f~e] , together
with the errors associated with the measurements of the acceleration f b transformed
over to the Earth frame via C eb. The errors in the measurements of the acceleration
are associated with the accelerometers and are discussed in the next section.
Rearranging Equation 3.44
~e
(3.49)
~ e _ eT C~_ ebC ebT :
(3.50)
[ ] = I
3
C b C eb T ;
and dierentiating gives

[ _ ] =
CbCb
The updating process of the transformation matrix both for the true and evaluated
frames are
_ e = C eb
ebe and C~_ eb = C~ eb
ebe:
Cb
(3.51)
Substituting into Equation 3.50 gives

[ _ ] =
=
~ e
bbe]T [C~ eb
bbe]C ebT
~ eb[
~ bbe
bbe]C ebT :
C
C b [C eb
(3.52)
3.9 Error Analysis
55
Perturbation of the rotation update matrix gives
bbe =
~ bbe
bbe;
(3.53)
therefore
[ _ ] =
~ e
bbeC ebT :
Cb
(3.54)
Substituting in Equation 3.44

[ _ ] = [[I ]C eb]
ebeC ebT
= C eb
bbeC ebT + [ ]C eb
bbeC ebT :
3
(3.55)
The product of small terms and

, is assumed zero. Therefore
[ _ ] = C eb
bbeC ebT
= C eb
bbeC be:
= [C eb!bbe]:
(3.56)
The error form of Equation 3.15 is

!bbe
!bib
C be !eie :
(3.57)
Given that the rotation rate of the earth is known, thus !eie = 0, then Equation
3.56 can be rewritten as
[ _ ] =
_ =
C eb [!bib ]
C eb !bib :
or
(3.58)
That is, the propagation of attitude errors _ is simply the errors associated with
the rotation rate measurements !bib , transformed over to the Earth frame via C eb.
The errors associated with the measurements are purely dened with the gyros and
3.10 Sensor Errors
56
are discussed in the next section. Note that the propagation of the attitude errors
is independent of position. Thus although the derivation was approached through
perturbation of the true frame the result delivers the same benet as found in the
computer frame approach, and this is due to the structure of the inertial equations
in the Earth frame.
Thus given the velocity and attitude error propagation equations and an input trajectory, the error growth of the inertial system can be determined.
The only terms which need to be determined are the errors in the acceleration
measurement f b = [fx; fy ; fz ]T , and the errors in the rotation rate !bib =
[!x; !y ; !z ]T . These terms can be experimentally evaluated.
3.10 Sensor Errors

The measurement errors associated with inertial sensors are dependent on the physical operational principle of the sensor itself. The general error equation used for
gyroscopic performance along, say, the x direction, is written as
0
!x
= b + bg B
@
ax
ay
az
1
C
C
A
+ sf !x + my !y + mz !z + ;
(3.59)
and that of the accelerometers as

fx
= b + sf ax + my ay + mz az + ;
where
b is the residual biases
bg is a 1 3 vector representing the g-dependent bias coecients
sf is the scale factor term
(3.60)
3.10 Sensor Errors
57
m represents mounting and internal misalignments and

is random noise on the sensor signal
Other terms such as anisoelastic and anisoinertia errors mainly aect mechanical gyros while vibro-pendulous errors aect mechanical accelerometers, and hence will not
be considered here.
Apart from the random noise term , all other error terms are, in principle, predictable thus allowing for some form of compensation.
3.10.1 Evaluation of the Error Components
This section discusses tests which can be performed to gyros in order to systematically remove the errors. Similar tests can be conducted on the accelerometer. It
should be noted that temperature and memory eect play a signicant role in the
stability of the output of low cost inertial sensors. It is for this reason that when one
purchases low cost inertial units, not all the values for the error terms are available,
and so testing is required based on the application at hand.
If the gyro is left stationary then the only error term encountered is that of the
g -independent bias. One of strongest correlations that can be found in inertial sensors
is that between the g-independent bias and temperature, also known as in-run bias.
Thus by cycling through the temperature that would be encountered in the target
application the bias on the gyro can be determined. The better the gyro, the smaller
the bias variation over the temperature range. Furthermore, the better the sensor
the greater the linearity of this bias variation.
There is also a hysterisis eect encountered with most inertial sensors. Thus in an
ensemble of tests, cycling through the temperature in the same manner each time,
the variation in the bias between ensembles can be determined. This is known as the
switch-on to switch-on bias. Again the better the gyro, the better the switch-on to
switch-on bias.
When testing for the remaining error components, the g-independent bias is assumed known and is removed.
3.10 Sensor Errors
58
Start with a rate table and place the gyro such that its supposed sensitive axis is
orthogonal to input rotation vector, then any output signal will be due to internal
misalignment of the sensor's input axis. The purpose of mounting the sensor orthogonal to the input rotation is to deal with a small error about null as opposed to a
small error about a large value (as would be encountered if the sensitive axis was
parallel to the rotation input).
With the misalignment and g-independent bias available, the gyro is placed on the
rate table with its sensitive axis parallel to rotation input. The rate table is cycled
from stationary to the maximum rotation measurable by the gyro in steps, holding
the rotation rate at particular points. The scale factor and the scale factor linearity
can then be determined by comparing the output of the gyro to the rotation rate
input. With low cost gyros the temperature also has a part to play with scale factor
stability, thus the tests should also be conducted with varying temperature.
Finally the gyro can be placed in a centrifuge machine which applies a rotation
rate and acceleration to the gyro. The output reading from the gyro minus the previous terms calibrated, results in the determination of bias due to acceleration or
g-dependent bias.
By mathematically integrating Equations 3.59 and 3.60, the eect of each error
term on the performance of the gyro can be determined. For each error term that is
accounted for there is a corresponding increase in performance.
Table 3.61 compares the error values available for the RLG, FOG, ceramic and
silicon VSGs [57].
Characteristic
RLG
FOG Ceramic VSG Si VSG
g-independent bias o =hr 0.001-10 0.5-50
360-1800 > 2000
o
g-dependent bias =hr=g
0
<1
36-180
36-180
scale factor non-linearity % 0.2 - 0.3 0.05 - 0.5
5 - 100
5 - 100
Table 3.2: Comparison of some of the major errors with various gyro implementations
3.10 Sensor Errors
59
3.10.2 Faults Associated with Inertial Sensors
High frequency faults occur if for example an inertial sensor fails. However, since an
inertial sensor is a high frequency sensor, one cannot determine if it has failed or not
without looking at the data history of the sensor. Furthermore, low frequency faults
are errors which vary slowly with time and arise due the errors discussed previously.
These faults also go undetected unless there is an external form of aiding which deals
with the low frequency dynamics of the vehicle.
In [5,30], non-linearity is discussed with regards to implementing low cost accelerometers and gyros. However, with low accuracy inertial sensors, the predominant
source of error is the g-independent bias and the noise on the sensor signal.
Thus, the acceleration and rotation rates measured by the accelerometers and gyros
respectively can be simplied by considering only the most dominant terms;
=
=
=
=
fi
!i
+ fi
fiT + bfi +
!iT + !i
!iT + b!i + ;
fiT
(3.61)
(3.62)
where fi is the measured acceleration of the ith accelerometer and fiT is the true
acceleration that should be measured by the accelerometer. Similar notation is used
for the gyros.
The incremental velocity, position and rotation is then obtained by integrating
Equations 3.61 and 3.62.
Vi
Pi
and
i
=
=
ViT
+ bfi t +
bfi t
+ 2 +
Z
= iT + b!i t +
PiT
(3.63)
dt
Z Z
dt
dt
(3.64)
(3.65)
3.10 Sensor Errors
60
In these equations, the bias in the sensors play a major role in causing drift in the
velocity, position and attitude information provided by the unit; the bias terms cause
error in velocity and attitude which grows linearly with time, while the position error
grows quadratically.
As discussed in Section 3.5.2, gyro data is used to update the direction cosine
matrix C nb. As a result, any drift in angle data caused by the integration of the
gyro outputs will perturb the direction cosine matrix, causing erroneous acceleration
calculations.
For example, assume that on a standard three axis IMU there is no bias or noise
in the accelerometers, no noise in the gyros and that there is no angular rotation
measured. Assume also that the z-gyro has a constant bias, then the acceleration
error on the accelerometer along the x-axis due to this bias will be
fx
fxT sin (b!z t) :
For small angle increments

fx
=
=
=
fxT b!z t
1f b t
2 xT !z
1f b t
Px
(3.66)
6 xT !z
Hence a bias in the gyro will cause an error in the position determination which will
grow with the cube of time. Thus biases on gyros play an important role in causing
drift in position and velocity. Again it should be emphasised that the quality of
the gyros in an inertial unit are a determining factor in the overall accuracy of the
navigation performance.
Figure 3.4 presents the bias measured on the accelerometers, used in this work,
over a period of six hours whilst the unit was stationary. Evidently the biases do
not remain constant and in fact it can be clearly seen that any one accelerometer is
not indicative of the general performance for other accelerometers. The dierent bias
values are simply due to the fact that they are physically dierent low cost inertial
thus
and
Vx
3.10 Sensor Errors
61
3
x 10
X accelerometer
Y accelerometer
0.03
Acceleration (g)
Acceleration (g)
0
5
10
15
20
0.025
0.02
0.015
0.01
Z accelerometer
Temperature
30
1.01
Temp (C)
Acceleration (g)
1.02
25
20
1.03
1.04
4
Time (hours)
15
4
Time (hours)
Figure 3.4: The change in bias values of the accelerometers due to the internal temperature of the inertial unit. Each accelerometer has a dierent characteristic even
though they are the same make. This is simply because they are low cost sensors and
hence have dramatically dierent characteristics.
sensors. The changes in the bias values occur due to an increase in the temperature
of the unit from the internal circuitry and due to ambient temperature variations.
Consequently biases must be estimated each time the vehicle is stationary in order
to minimise its eect. Alternatively the bias can be estimated and applied on-line,
although in many land vehicle robotic applications the vehicles undergo frequent
stopping, and if the unit is calibrated in this time then good results can be obtained.
A second major error is due to the integration of white noise (R dt) which causes a
growing error term known as Random Walk as presented in Equations 3.63 and 3.65.
Figure 3.5 presents the eect of integrating the x-gyro on several occasions after the
bias was removed and the unit stationary. Evidently, the ensemble average error is
zero, however in any particular run, the error growth occurs in a random direction.
Assuming unity strength white Gaussian noise and letting x(t) = R t (u)u, then the
ensemble mean value is
0
E [x(t)]
Z t
= E[
=
Z t
0
(u)u]
E [ (u)]u = 0
(3.67)
3.10 Sensor Errors
62
Random Walk X gyro
0.3
0.2
0.1
Angle (degrees)
0.1
0.2
0.3
0.4
0.5
0.5
1.5
Time (min)
2.5
Figure 3.5: Several runs of integrating the x-gyro and obtaining a unique random
walk each time.
and the variance is
E [x (t)]
2
= E[
Z t
0
(u)u
Z t
0
(v )v ] =
Z tZ t
0
E [ (u) (v )]uv
(3.68)
E [ (u) (v )] is the auto-correlation function, which in this case (assuming uncorrelated

errors) is simply a Delta Dirac function (u v). Hence Equation 3.68 becomes
E [x (t)]
2
Z tZ t
0
(u v )uv
and so the standard deviation of the noise is

p
= t:
t;
(3.69)
(3.70)
Therefore without any external resetting properties, white noise will cause an unbounded error growth in the inertial sensors whose value at any particular point in
time will be within 2pt 95% of the time.
If the white noise is of strength K , then the standard deviation is = K pt. The
larger the magnitude of the noise, the greater the standard deviation of the error.
3.11 Alignment and Calibration
63

The objective of calibration is to determine the biases in the accelerometers and
gyros. This is obtained by rst determining the initial alignment of the inertial unit
and hence in turn evaluating the initial direction cosine matrix.
3.11.1 Alignment Techniques
If the accelerometer readings are known perfectly then the attitude of the inertial
unit can be determined by resolving the gravity component. In order to determine
the gravity component measured by the accelerometers, Equation 3.30 is rearranged
to give
fb = (Cnb) fn
(3.71)
Since Cnb is orthogonal, its inverse is simply its transpose. The inertial unit is stationary and hence the only acceleration measured is that due to gravity along the
vertical axis. Thus
h
fn = 0 0
iT
(3.72)
Equation 3.71 becomes

2
6
6
4
fxT
fyT
fzT
3
7
7
5
6
6
4
c c
c s + s s c
s s + c s c
c s
s
c c + s s s s c
s c + c s s c c
32
76
76
54
0
0
3
7
7
5
(3.73)
64
where the subscript T represents the true acceleration component due to gravity.
Hence
fxT
fyT
fzT
= g sin
= g sin cos
= g cos cos
(3.74)
(3.75)
(3.76)
Although no sensor is perfect, the higher the accuracy the tighter the error tolerances
and thus the accuracy of alignment which can be obtained. As the accuracy of sensors decrease, due to the errors mentioned previously, the alignment accuracy also
decreases. Rearranging Equation 3.74 to determine the pitch , and substituting this
into either Equations 3.75 or 3.76 will solve for roll . This procedure for determining
alignment is called \coarse alignment".
If the accuracy provided by coarse alignment is not sucient for navigation performance then external alignment information will be required. This information can
come from tilt sensors or GNSS attitude information for example. Coarse alignment
is generally used for rapid alignment, such as in missile applications, where the time
required for averaging data from external sensors is not available.
The nal term which needs to be evaluated is the heading of the vehicle . Gyrocompassing is the self determination of the heading. Once the attitude of the unit
is found, the reading on the gyros can be used to determine the components of the
Earth's rotation, and by knowing the initial position of the vehicle, heading can be resolved. However, with low cost gyros, gyrocompassing is generally not possible due to
the high noise and low resolution of these sensors. In such cases external information
is required to determine initial heading.
3.11.2 Alignment for Low Cost Inertial Units
Due to the low accuracy of the inertial units implemented in this thesis, none of
the common self aligning or calibration methods provide results accurate enough for
navigation. Instead, the inertial unit uses two tilt sensors which provide measurements
65
of the bank and elevation of the inertial platform, thus providing the initial alignment
information required.
A positive bank will cause the y-accelerometer to measure a component of gravity
equal to
fyT
g sin(bank)
(3.77)
Similarly a positive elevation will cause the x-accelerometer to measure

fxT
= g sin(elevation)
(3.78)
Equating Equation 3.74 with 3.78, and Equation 3.75 with 3.77, the pitch and roll
angles are

= elevation
))
= sin ( sin(cosbank

1
(3.79)
(3.80)
To resolve the heading a compass is used.

Equations 3.79 and 3.80 are used along with the initial heading angle to determine
the initial Cnb.
3.11.3 Calibration
The simplest method of obtaining the biases of the inertial sensors is to measure the
readings from each sensor whilst the vehicle is stationary. These bias values are used
to calibrate the IMU. For gyros, the bias is simply the reading from these sensors when
the vehicle is stationary. However, the alignment of the inertial unit is required in
order to determine the biases on the accelerometers. This is accomplished during the
alignment stage since the \expected" acceleration due to gravity can be determined
and hence any anomalies in these values are attributed to bias. Thus the bias on the
3.12 Summary
66
x-accelerometer is obtained by rearranging Equation 3.61;

bfx
fx
fxT
(3.81)
where fx is the measured acceleration and fxT is the expected acceleration obtained
during the alignment stage. The bias is obtained similarly for the remaining accelerometers.
3.12 Summary
The focus of this chapter has been on providing the inertial navigation equations
needed for land navigation and furthermore to detail the errors associated with low
cost inertial navigation sensors and systems. In doing so this chapter has:
Developed the inertial navigation equations in the Earth frame, which provides
the appropriate structure for land navigation. This navigation frame was compared to both the Transport and Wander Azimuth frames, both of which are
predominately used for long distance travel such as air transport;
Provided the details on the various attitude propagation implementations. It
was discussed that the appropriate attitude algorithm was the Direction Cosine
implementation, since it minimised the overall computation required.
Discussed the issues relating attitude propagation, and it was shown that there
is a compromise between the order of the algorithm and the sampling rate of the
inertial unit. Although low cost inertial units have low sampling rates, they are
generally higher than the maximum rotation rates encountered by most land
vehicles.
Taken the inertial navigation equations and linearised (or perturbed) them to
form inertial error equations. These equations can then be used to determine
how error growth would occur within an inertial navigation system. The equations were perturbed in the \True" frame giving two sets of linear equations,
3.12 Summary
67
the error propagation of the velocity and attitude errors, both of which are
independent of one another.
Listed the errors commonly associated with low cost inertial sensors. In partic-
ular it was shown that bias and random walk are the predominant errors. Furthermore, it is these errors on the gyros which cause the greatest error growth
since it aects the alignment of the inertial system and hence false acceleration
readings are evaluated.
Provided the methods of calibration and alignment implemented in this the-
sis. Due to the errors associated with low cost inertial sensors, self calibration
and alignment does not provide the accuracy required for navigation purposes.
Hence external information is required.
This then provides the necessary background and methods to develop appropriate
low-cost aided inertial navigation systems for the autonomous vehicles considered in
this thesis.
Chapter 4
Aided Inertial Navigation
4.1 Introduction
This chapter provides the theoretical and practical aspects of aided inertial navigation
systems.
It does so by combining Chapters 2 and 3, that is, by implementing statistical lters
which optimally combine inertial information with external aiding sources. This in
turn bounds the errors associated with inertial navigation systems.
The main contributions of this chapter are:
The development of an inertial navigation system aided by GNSS and the un-
derstanding of the limitations in accuracy and information available to aid the

inertial system [41,42,52{55]. This implementation allows for a linear, direct
feedback structure.
The determination of high frequency faults associated with GNSS, the tech-
niques needed to detect them, and the algorithms implemented to remove them.
The development of a real time inertial navigation system aided by GNSS. The
navigation system provides the navigation data needed to control a 65 tonne

container handling vehicle.
4.2 GNSS
69
The development of an inertial navigation system being aided by vehicle mod-
elling. This implementation requires no external sensors to aid the inertial unit
[15,56], and is implemented in a non-linear, direct form.
An understanding of the limitations of vehicle modelling to aid inertial naviga-
tion systems.
The development of an inertial navigation system aided by GNSS, vehicle mod-
elling, and speed provided by a wheel encoder [56]. The lter architecture is
in an information lter format. The multiple aiding allows for better fault
detection and better accuracy of the inertial navigation solution.
4.2 GNSS
There are two forms of GNSS navigation systems: the Global Positioning System
(GPS) and the Russian equivalent named the Global Navigation Satellite System
(GLONASS). Literature is abundant on the principles of these systems [32,44]. Some
key points are highlighted here as they are of signicant relevance in this thesis.
GPS comprises of 24 satellites in six orbits equally spaced such that a user anywhere
on the globe can see at least four satellites (without consideration of surrounding
structures). Four satellites are required to determine the three unknowns of position
and the unknown user clock bias. Each satellite transmits navigation and range data
simultaneously on two frequencies, L1(1575.42 MHz) and L2(1227.60 MHz). Civilian
access is limited to decoding data on the L1 frequency.
The GLONASS system also transmits navigation and range data on two frequencies
L1 and L2. Unlike GPS where the satellites are distinguished by the spread-spectrum
code, the satellites in GLONASS are distinguished by the frequency of the signal, L1
between 1597-1617 MHz and L2 between 1240-1260 MHz. The system is also envisioned to handle 24 satellites in almost the same conguration as the GPS equivalent.
Both systems employ a military only code package which is modulated onto both
the L1 and L2 frequencies. The corresponding civilian access code lies only on the
4.2 GNSS
70
L1 frequency. The initial purpose of implementing two frequencies is to remove the

ionospheric errors associated with the transmission of the signal through the atmosphere.
If both systems are combined then a theoretical doubling of the number of satellites in view is achievable. This increase in the number of satellites increases both the
navigation performance and fault detection capabilities.
4.2.1 Position Evaluation and Accuracy
The basic principle behind satellite based positioning navigation techniques is similar
to any range based navigation systems, that is, the time of ight of the signal from the
transmitter/receiver to the beacon represents the range to the beacon. The dierence
lies in the fact that the beacon is a satellite which transmits a message at a transmit
time tt , while the user is only a receiver who picks up this transmission at a receiving
time tr . The dierence in time multiplied by the speed of light represents the range
to the satellite. Three satellites are required to solve the three unknowns of position.
Since timing between the user and the satellites is paramount, accurate clocks are
required. Since this is costly to implement on the user side, an extra unknown of
\clock bias" needs to be solved for, and hence four satellites are needed to determine
the position. This method is known as point-positioning.
The accuracy of the point-positioning method can be improved if ltering techniques are employed to model the dynamics of the receiver. This is the most common
method implemented by GNSS manufacturers. The lter incorporates a model which
can either be a:
Constant Position Model: Where the estimated states are position and clock
bias. This model is used when the receiver is stationary. The process noise terms
re ect the accuracy of this assumption. Thus placing a receiver on vibrating
vehicle, for example, will require larger noise terms than if the vehicle was not
vibrating.
4.2 GNSS
71
Position-Velocity Model: When the receiver has constant velocity. The velocity
is modelled as random walk, and position the integral of velocity. The clock
bias terms are modelled as normal and acceleration is modelled as white noise.
Position-Velocity-Acceleration Model: Acceleration is modelled as a Markov
process. The velocity and position being the integrals of their derivative terms.
Once a model is chosen, the estimation procedure begins where the observation vectors
used to estimate these states come from the range information obtained from the
observed satellites.
To lock onto satellite signals, a receiver generates an internal code which matches
either the GPS civilian code or the GLONASS code or both. A phase lock loop then
tries to correlate the internally generated signal to that of an incoming satellite signal.
Maximum correlation signies a locked satellite. The information from that satellite
can then be read.
With civilian access, GPS alone provides a \one sigma" positioning accuracy of
100m, that of GLONASS is 50m. When combined the value drops to 30m. To achieve
sub-metre accuracy required for autonomous land vehicle applications, dierential
positioning is required, Figure 4.1. In this approach, a base station is set up with a
known position. The calculated position from the satellites is then dierenced from
the known position, leaving an error value, which would be common amongst all
receivers within the local vicinity (< 20km). The base station transmits this error
to all receivers on board the vehicles (rovers) thus removing the errors from their
calculated position. When combined with the base station this is known as Standard
Dierential and can deliver accuracies around one metre depending on the number of
satellites in view. The accuracy can be further improved if the change in phase of the
incoming signal is observed, to which the standard code generation can be smoothed,
and is known as carrier-aided smoothing.
High accuracy can be achieved in what is known as Real Time Kinematic (RTK)
solutions. Here the observable is the phase of the incoming signal and not the range
code in the signal. Changes in phase can directly relate to changes in position and
the high accuracy is achieved since the L1 wavelength is 19cm and that of L2 21cm.
4.2 GNSS
72
Satellites
Differential Corrections
Base Station
Rover
Figure 4.1: GNSS accuracies are dramatically improved when dierential corrections
are applied. Since the base station is at a known position then the errors associated
with GNSS can be evaluated and transmitted to all rovers for use.
What needs to be known is the number of wavelengths between the receiver and that
of the satellite, and is known as the integer ambiguity. Accuracies down to 20cm
can be achieved with ve satellites in view and using a technique known as double
dierencing. Accuracies down to the centimetre level are obtained when six satellites
are in view and employing a technique known as triple dierencing.
Velocity data can be derived by time dierencing two phase measurements. If the
relative acceleration of the base and the rover is small, then doppler measurements
can provide very accurate velocity data.
Attitude data can also be derived by using more than one antenna, and calculating
what is known as the baseline length between the antennae. This length is simply
the dierence in carrier phase measurements between a pair of antennae.
4.2.2 Transformation of the GNSS Frame
The underlying theory behind the following equations are found in [38].
The position Pg = fPX ; PY ; PZ g and velocity Vg = fVX ; VY ; VZ g obtained from
4.2 GNSS
73
the GNSS receiver is delivered in the WGS-84 datum, represented by the ECEF coordinate frame, Figure 4.2.
The latitude and longitude is obtained by transforming the position observation received by the GNSS receiver unit from ECEF to geodetic coordinates. The equations
are
Semimajor Earth axis (metres) a = 6378137:0
Undulation f = 0:003352810664747
Semiminor Earth axis (metres) b = a(1 f )
Coecients :
q
p = Px + Py
P a
t = tan ( z
)
pb
e1 = 2f f
a b
e2 =
b
sin t )
latitude (degrees) = tan ( PpZ +ee12abcos
t
longitude (degrees) = tan ( PPY )
2
Given the latitude and longitude, a transformation matrix Cng can be formed which
transforms the position and velocity data given by the GNSS receiver from the ECEF
frame over to the navigation frame.
2
Cng =
6
6
4
sin cos
sin
cos cos
sin sin cos 7

cos
0 75
cos sin sin
(4.1)
Thus the position and velocity of the vehicle in the local navigation frame is
Pn = CngPg
and Vn = CngVg
(4.2)
(4.3)
4.3 Overview of the Navigation Loops
74
Z
N
E
Figure 4.2: Coordinate representations. XYZ represents the orthogonal axes of the
ECEF coordinate frame used by the GNSS receiver. represents the longitude and
the latitude of the vehicle. NED represents the local navigation frame at which the
vehicle states are evaluated to.
If the vehicle's work area has negligible change in latitude and longitude, then Cng is
eectively xed.
Equations 4.2 and 4.3 form the observations needed for the aided inertial navigation
system.
4.3 Overview of the Navigation Loops

Figure 4.3 illustrates the three navigation systems implemented in this thesis.
Figure 4.3 a) is a linear, direct feedback implementation. In this approach the inertial unit and the GNSS receiver behave as separate navigation systems. The Kalman
lter estimates the errors in position, velocity and attitude of the inertial navigation
system. The errors are fed back to correct the inertial navigation system. The observation vector is the observed error of the inertial system, that is, the dierence
between the inertial navigation solution and that of GNSS.
Figure 4.3 b) is a direct, extended Kalman lter implementation. The inertial unit
is classed as a sensor and the input into the lter is the acceleration and rotation
4.4 GNSS Aiding
75
rates measured. This data is fed into the lter as the control input and propagates
through the system model. The system model implemented uses standard Newtownian motion equations. Observations are derived from a set of constraint equations
which re ect the behaviour of a land vehicle which does not slip and always remains
in contact with the ground.
Figure 4.3 c) is a linear Information lter, in a direct feedback implementation.
The Information lter is implemented so as to facilitate the estimation stage where
the GNSS, vehicle modelling and speed data are provided as observations. Again, the
lter estimates the error in the inertial navigation system and hence the observations
are the observed errors of the position, velocity and attitude evaluated by the inertial
navigation system.
4.4 GNSS Aiding

Integrated Inertial/GNSS navigation systems have been the subject of considerable
research in recent years due to both an increase in sensor accuracy and a decrease in
cost. The advantages of fusing inertial units with GNSS are well known [21,33].
The application of Inertial/GNSS navigation loops has centred on ight vehicles
for either military, space or civilian needs [7,9,23,37]. In such applications the sensors employed are generally of high accuracy. Furthermore, the environments in which
the applications are intended for are generally benign (except with reference to GNSS
jamming within military circles). Land vehicles however, tend to operate in a more
active environment, where the surroundings can have a detrimental eect on the accuracy of GNSS units. This is the case with multipath errors caused by the re ection
of the GNSS signals from surrounding surfaces, and GNSS signal outages caused by
the surrounding structures. Furthermore, the accuracy of the sensors implemented
in civilian land vehicle applications is generally low due to cost constraints. There
has been some work on the application of Inertial/GNSS navigation loops for land
vehicles [47,49]. However, this has generally been aimed at manned vehicles and consequently does not address the need for integrity inherent in an autonomous vehicle
4.4 GNSS Aiding
76
Position, Velocity
and Attitude
INS
Kalman
Filter
Observed Error
GNSS
Position and
Velocity
Observations
(a) Inertial/GNSS navigation system.
Vehicle
Constraints
IMU
Velocity
Observations
Acceleration and
Rotation Rates
Non-Linear
Kalman Filter
Attitude Estimates
(b) Inertial/Vehicle constraint navigation system

Position, Velocity
and Attitude
INS
+
Observed Error
+
-
GNSS
Observed Error
Position and
Velocity
Observations
Velocity
Observations
Kalman
Filter
Vehicle
constraints +
wheel encoder
(c) Inertial/GNSS/Vehicle Constraint/Speed

navigation system
Figure 4.3: The three navigation systems implemented in this thesis.
4.4 GNSS Aiding
77
system.
A key question addressed in this thesis is \What considerations need to be taken
into account when using Inertial/GNSS navigation systems for autonomous land vehicle applications?" The best way to approach such a question is with the use of a
real example which motivated the work in this thesis.
4.4.1 Navigation for an Autonomous Straddle Carrier
A port operator wants to autonomise a container handler, also known as straddle

carrier (Figure 4.4) in an area of approximately 10km (Figure 4.5). Thus dierential
corrections are valid within the area of consideration. Containers are picked up from
the docking ship by quay cranes and placed below the crane. The objective of the
straddle carrier is to pick up the container and move it to a desired place in the port.
Alternatively, the straddle carrier can pick up the container from a designated place
and move it to below the crane at which stage it is loaded onto the ship. Since the
2
Figure 4.4: A straddle carrier
4.4 GNSS Aiding
78
Figure 4.5: The container terminal

receiver antenna needs to view the satellites without obstruction, it is placed on top of
the vehicle. When the vehicle travels around the stacks of containers the antenna has
the full view of the sky. However, as the straddle carrier approaches the quay crane,
re ection of satellite signals from the cranes will result in inaccurate evaluation of
the range of the receiver to the satellites due to increase in the time of ight causing
multipath, and is physically represented by jumps in the GNSS solution.
The design of the receiver correlator is important in the removal of multipath since
if the correlator can distinguish between the re ected signal and that of the same signal coming directly from the satellite then multipath can be removed. Furthermore,
multipath aects the L1 and L2 frequencies dierently, and this eect can be used in
determining if multipath has occurred. Finally, redundancy in the number of satellites also assists in removing the eect of multipath. Thus the ultimate receiver would
be a dual frequency GPS, dual frequency GLONASS unit. Such units are available
and oer a number of features along with 24 channel access allowing the receiver to
see 12 GPS and 12 GLONASS satellites. If only a single navigation system is to be
used then dierential GPS, either standard or RTK, may be used.
4.4 GNSS Aiding
79
Since six satellites are required to provide an RTK solution of centimetre accuracy, this limits the number of potential redundant satellites. Furthermore, due to
obstructions by quay cranes and buildings there is always the real situation of seeing less than six satellites. The problem with RTK is that there always has to be a
lock of satellites once the integer ambiguity is resolved. If a satellite is lost and then
reappears then the process of re-attaining this integer is repeated. This can take a
number of seconds, and even a few minutes during start up when the position of the
receiver is unknown.
As presented in Chapter 3, the calibration and alignment of inertial units is of
immense importance. Due to the error growth in inertial navigation, the system
will never be as accurate as its initial alignment and calibration, unless the satellite
navigation system can provide better accuracy. Velocity information can constrain
the error growth of the attitude evaluation since errors in attitude are re ected into
errors in velocity. However, direct attitude observations can quickly align and hence
calibrate an inertial unit. The problem is that an attitude based satellite navigation
system consists of a receiver for each antenna and hence the cost of such a package
is extremely expensive.
In the examples presented in this thesis, the vehicle undergoes frequent stopping,
hence calibration and alignment may also occur frequently. Furthermore, since greater
than six satellites can be seen at all times around most of the port, a standard RTK
system is employed implementing only a single receiver.
The following subsections will discuss the implementation of the Inertial/GNSS
navigation systems as shown in Figure 4.3 a).
4.4.2 Linear, Direct Feedback Implementation
Equations 3.48 and 3.58 described the inertial error equations in the Earth frame for
velocity and attitude. The rate of change of the position error can be equated as the
4.4 GNSS Aiding
80
velocity error. Thus the error equations are
p = v
v_ = [f~e ] + C eb f b
[ _ ] = C eb!bib:
_
(4.4)
(4.5)
(4.6)
Ceb transforms vectors from the body frame to the Earth frame. As in the GNSS
implementation, the Earth frame is represented by North (N), East (E) and Down
(D) vectors and hence for clarication the transformation matrix is represented as
Cnb.
The navigation loop implements a linear Kalman lter. The state vector consists
of the error states,
x =
pN pE pD vN vE vD
iT
D
(4.7)
The process model is

2
0
6
6 0
6
6
6 0
6
6
6 0
6
F = 666 0
6
6 0
6
6 0
6
6
6 0
4
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
fD
fE
0
0
0
0
0
0
fD
fN
0
0
0
0
0
0
7
7
7
7
7
7
7
fE 7
7
7
fN 7
7
7
7
7
7
7
7
7
5
0
0
0
0
(4.8)
Note that in this implementation, the acceleration inputs are fed directly into the
process model and hence there is no control vector, u = 0. The process model F
comprises of time-varying terms. Thus in order to determine the state transition
matrix numerical methods are required. If however it is constant over the sampling
interval then the state transition matrix is simply F(k) = exp(Ft), where t is the
4.4 GNSS Aiding
81
sampling time of the inertial unit. In the case of land vehicles, the dynamics are of a
much lower frequency than the sampling frequency. Hence over the sampling period
F remains constant, hence
F(k) = exp(Ft)
= I + Ft + (F2!t) + : : :
2
The discretisation is only taken to the second term since any extra terms which are
added to the nal state transition matrix are of negligible value.
Thus Equation 2.3 is simplied to
x^(kjk 1) = F(k)^x(k 1jk 1):
(4.9)
The advantage of using this model is that the implementation is linear and the model
is independent of the vehicle dynamics.
Initially the inertial sensors are calibrated and all the errors are removed, thus
x(1j0) is set to zero. Thus, from Equation 4.9, the state prediction in the next
cycle is also zero, and so forth. Therefore the state prediction is always zero and
no correction of the inertial errors occurs during the prediction cycle. That is, the
position, velocity and attitude information obtained from the navigation system is
simply the data from the INS since there are no predict errors to correct it.
However, due to the drift in the INS solution, there is a corresponding growth
in uncertainty in the states and this is evaluated through the predicted covariance
matrix P(kjk 1), Equation 2.4. This is a 9 9 matrix representing the uncertainty
in the inertial predicted errors.
Q(k) is the discrete process noise matrix also of dimension 9 9 and is evaluated
using [35]
Q(k) = 21 F(k)G(k)Qc(k)G(k)T F(k)T + G(k)Qc(k)G(k)T t: (4.10)
4.4 GNSS Aiding
82
Equations 4.4 - 4.6 show how the noise terms (in acceleration and angular velocity)
are converted from the body frame to the navigation frame and hence Qc(k), which
is the uncertainty in the process model over a period of one second, is dened as
2
Qc(k) =
~ 0
fb
0
0
b
!ib
6
6
6
6
4
0
0
3
7
7
7:
7
5
(4.11)
~ is the noise injected into the position error evaluation and its value is purely
dependent on the uncertainty in the evaluation of the position from the integration
of velocity. The remaining error terms in this matrix are the noise values on the
sensor readings, that is, the errors in the body frame, which can be obtained from
manufacturer specications or through experimentation as discussed in Chapter 3.
G(k) is
2
I
6
3
G(k) =
6
6
6
4
0
0 7
7
0 Cnb(k) 0 77 :
5
n
0
0
Cb (k)
3
(4.12)
The observations from the GNSS receiver are position and velocity only and hence
when a GNSS x becomes available, an error observation is obtained and the lter
then updates the estimate of the error states in the inertial unit.
2
inertial
P
(
k) PGNSS (k) 5
4
z(k) =
Vinertial(k) VGNSS (k)
(4.13)
The estimate of the error states at time k given all observations up to time k is
evaluated using Equations 2.5 - 2.9. However, since the prediction of the error states
4.4 GNSS Aiding
83
x^(kjk 1) is always zero then the state estimate becomes

x^(kjk) = W(k)z(k)
(4.14)
That is, the update is simply a weighted sum of the observations. The observation
model H(k) is
2
H(k) =
I
4
0 05
0 I 0
(4.15)
The updated position and velocity errors can now be used to correct the position
and velocity of the INS,
Pinertial (kjk) = Pinertial(kjk 1) p(kjk)

Vinertial (kjk) = Vinertial(kjk 1) v(kjk)
(4.16)
(4.17)
Equation 3.44 related the true direction cosine matrix to the estimated direction
cosine matrix. In the context of the terminology used in this chapter this is written
as
Cnb(kjk 1) = [I [ ]]Cnb(kjk):
3
(4.18)
Rearranging this equation as
Cnb(kjk) = [I [ ]] Cnb(kjk 1);

1
(4.19)
provides the update equation for the direction cosine matrix once an estimate of the
attitude errors occurs. Since the term in the square brackets is orthogonal then its
inverse is simply its transpose (A = AT ), and furthermore it is a skew-symmetric
1
4.4 GNSS Aiding
84
matrix (thus AT = A), hence Equation 4.19 is written as
Cnb(kjk) = [I + [ ]]Cnb(kjk 1)
3
(4.20)
where
2
[ ] =
6
6
4
D
E
E
N
3
7
7
5
(4.21)
Note that [ ] is in the navigation frame yet it is used to correct the Cnb matrix
whose elements are dened in the body frame. It is assumed that the misalignment
errors are small and hence the errors associated about the navigation frame are equal
to those about the body frame. When large misalignments are encountered, the linear
assumptions held are not valid. [26] deals with such situations.
4.4.3 Detection of Multipath
High frequency faults arise when the GNSS signals undergo multipath errors. This
results in a longer time delay of the signals aecting the x of the Standard Dierential receiver, and also altering the phase of the signal thus aecting the Carrier
Phase Dierential x. Another high frequency fault, although it occurs less often
and with less eect, is when the receiver utilises a dierent set of satellites in order
to determine the position x. The accuracy of the x is dependent on the geometry
of the observed satellites. Changes in satellite conguration due to blockages of the
satellite view will in turn alter the resulting x. Both forms of high frequency faults
cause abrupt jumps in the position and velocity xes obtained by the GNSS receiver.
High frequency faults are therefore environment dependent. An open area such as
a quarry will be less likely to produce multipath errors than will a container terminal
for example. Consequently, the tuning of the lter which fuses the inertial unit and
GNSS data is dependent on the environment.
The most common method of rejecting multipath errors is obtained when the re-
4.4 GNSS Aiding
85
ceiver can distinguish between the true signal and the re ected signal. How well the
receiver can distinguish between these two signals is dependent on the accuracy of
the receiver's internal timing procedures.
However, these systems cannot reject multipath errors completely, and even with
the constant improvement of GNSS technology high frequency faults such as multipath always need to be factored into the development of the navigation system.
Chapter 2 highlighted a method of sustaining lter consistency by evaluating a
gating function which has the property of being a distribution (Equation 2.36).
This gating function can be used to determine if the process or observation models
are valid or if any observations are spurious. Thus it can be used to determine if the
multipath errors have occurred.
Due to satellite geometry, the GNSS x in the vertical plane is signicantly less
accurate than that in the horizontal plane. Consequently the x in North and East
may lie well within the validation region, whilst that of Down may exceed it and force
the result of the gating function beyond the threshold if the same noise values where
used for all the terms in the observation noise matrix R(k). However, if these noise
terms are accounted for by taking the values from the GNSS receiver directly, than
the validation gate will detect multipath errors correctly.
Similarly, changes in satellite geometry cause the Dilution of Precision (DOP) to
vary. The DOP is a measure of the accuracy of the GNSS x and is used in the GNSS
position solution to obtain the optimum conguration of satellites which the receiver
should track, if it cannot track all of them due to hardware limitations. Changes in
satellite geometry occur when part of the sky is invisible to the receiver antenna due
to obstacles blocking the GNSS signals. The receiver then has to obtain a new set of
satellites. Consequently, a change in DOP will aect the GNSS solution causing high
frequency faults. These faults can be detected using the same technique as discussed
for multipath errors. However, these changes are not as large as those encountered
for multipath errors and generally go undetected, unless the accuracy of the inertial
unit is comparable to that of the GNSS receiver.
2
4.4 GNSS Aiding
86
4.4.4 Filter Tuning
There are two stages in the lter ow; the prediction stage where the predicted inertial errors are always zero and the uncertainty grows with time, and the estimation
stage where the estimates of the inertial errors are obtained by a weighted sum of
the observations and the uncertainty is bounded. If no observation is obtained for
an extended period of time, or equivalently if GNSS xes are rejected due to errors,
the lter will continuously cycle in prediction mode and no corrections to the inertial
navigation solution will be made. The longer the duration without correction, the
greater the uncertainty in the navigation solution. When a x does occur, the observation may pass the gating function test even though the x may be in error since
the uncertainty is the inertial navigation solution is large.
As with any Kalman lter implementation, tuning lies in the choice of values for the
process Q(k) and observation R(k) covariance matrices. For example, a large Q(k)
will imply an inaccurate inertial system. During the prediction stage the uncertainty
in the inertial data will grow according to the magnitude of Q(k). When a GNSS
x does occur there is a greater possibility the inertial unit will be corrected using
the rst available GNSS x, irrespective of the accuracy of this x. Likewise, small
values in R(k) will imply accurate GNSS xes which may pose a problem when the
x is in error and is being fused with low accuracy inertial sensors.
Thus tuning becomes a delicate adjustment of both the Q(k) and R(k) matrices
along with the employment of the gating function, Equation 2.36, in order to reject
the high frequency faults of the GNSS.
The variances along the diagonal of R(k) are determined simply by obtaining the
GDOP values from the receiver and assuming there is no correlation between the xes
of the navigation axes (which is how the GDOP is provided).
Determining the values for Q(k) depends on the noise level of the sensors implemented which can be obtained from the manufacturer or through experimentation.
The principle tuning parameters which need to be addressed are the variances of velocity and attitude. A large variance in the velocity terms will imply greater reliance
on the GNSS velocity xes. Furthermore, the greater the accuracy of the velocity
4.5 Real Time Implementation of an Inertially Aided GNSS Navigation System 87

estimates, the greater the dampening on the attitude errors. If there are no attitude
xes then only the velocity information can contain the drift in attitude and how
quickly and accurately this can happen depends on the variance on the attitude.
4.5 Real Time Implementation of an Inertially Aided

GNSS Navigation System
There are two points of concern in a real time implementation of an inertially aided
GNSS navigation system: processing power; and latency. It was shown in Section
3.6 that extremely high sampling of inertial measurement sensors is not required
in land eld robotics since the maximum rotation and acceleration frequencies is
not high (with obvious consideration to vibration). In turn the power of modern
processors is sucient to handle the data throughput. Data latency on the other
hand is extremely important, especially with the use of GNSS sensors, where it is
common to nd position latencies in the order of tens of milliseconds and that of
velocity reaching over a second. The processing power internally in a GNSS sensor
is used to control the correlators to lock onto satellites, determine the ephemeris of
the satellites, and then compute the position and velocity of the receiver with respect
to some coordinate frame. The complexity of these calculations increases with the
number of satellites used to determine the solution.
Standard pseudorange solutions require the least computational processing. RTK
technology requires the most processing power. In addition if attitude information
is required this also requires additional processing. The processing time is usually
not of concern to most GNSS users; surveyors for example or more generally, those
users that do not require real time information. To overcome this latency problem,
careful consideration needs to be given to the real time implementation, and when
the latency is large a complete redesign of the lter structure may be required.

4.5.1 Latency
When the latency of the solution is low the position information can be propagated
using velocity data and a constant velocity model. This is quite sucient for low speed
vehicles and is the approach taken in this thesis. Problems arise when the latency
of the GNSS solution is high, and the vehicle dynamics are fast. A GNSS position
solution with a latency of 50ms will have an error of 0:8m for a vehicle traveling at
60km=hr. If the vehicle is moving with constant velocity and moving in a straight line
then the position can be simply propagated forward. However, any deviation from
a straight line will lead to incorrect estimates when the GNSS x is fused with the
current inertial state. What is required is to store the inertial data and process the
estimates at the time that the GNSS x should have occurred, and then propagate
the inertial solution through the backlog of data. All is well if both the position
and velocity GNSS data have equal latencies. However, if there is a requirement for
the GNSS receiver to obtain velocity and position with independent measurements
then this requires alternative methods. For example, in RTK systems a doppler
approach is used to obtain velocity while the determination of the phase ambiguity
is used for position measurement. In such systems the latency of the velocity data
can sometimes be over 1s, and hence the position and velocity determination occurs
at dierent points in time. To overcome this GNSS manufacturers propagate the
velocity through the position data and hence both the position and velocity output
occur at the same time with the same latencies. However such an approach produces
correlation between the position and velocity data, which is not ideal for navigation
systems. The GNSS hardware implemented in this thesis delivers position and velocity
measurements independently and at the same time step.
4.5.2 Hardware and Algorithms
In the straddle carrier implementation described in this thesis, Transputers were used
as the processor for the implementation of the lter. The computational power of the
Transputer was sucient for the implementation of the lter and also provided for

an operating system framework that allowed parallel processing. That is, within each
Transputer the code was structured to allow for parallel programming and multiple
Transputers were congured for parallel operation. It can be envisioned however
that with current DSP technology only one processor may be required, and given the
simplicity of the navigation loop, it may be possible to implement the lter without
the requirement for parallel processing.
The objective of the navigation loop for the straddle carrier experiment is to provide
the position and heading of the straddle carrier so that it can be positioned anywhere
in the port within 0:1m, to either load or unload a container. In this work the
implementation consisted of four Transputers as illustrated in Figure 4.6.
The observation Transputer reads in the raw data from the inertial unit and GPS.
The GPS data is then converted into the NED frame. This data is then passed
onto the central Transputer. The central Transputer also converts the inertial data
into the NED frame and obtains the vehicle's velocity and position. When a GPS
x is obtained the inertial and GPS data at that time are sent to the lter stage
Transputers. The lter is divided amongst these two Transputers due to the heavy
computational load. The lter validates the observation and then estimates the errors
in the inertial navigation solution. The errors are then sent back to the central
Transputer to correct the inertial readings navigation solution.
The navigation algorithm ow is implemented as follows:
1. Since the vehicle is autonomous, all guidance commands are known in advance
and hence the navigation lter knows when the vehicle is stationary. Whilst
the vehicle is stationary the objective is to read in all acceleration, rotation
rate and tilt sensor data from the inertial unit and provide the average of these
sensor readings. The straddle carrier itself does not travel for large amounts
of time without frequent stopping. It is during the stationary period that the
alignment and calibration may be implemented. The code is structured so that
the previous position before alignment and calibration is used to initialise the
lter and so the continuous ow of the navigation output is available. Initially
the GPS sensor does not provide the position of the vehicle until it has reached
FILTER
TRANSPUTER
1
IMU
INS data
Prediction
Acceleration and Rotation Rates
Prediction
OBSERVATION
TRANSPUTER
Conversion of
GPS from
WGS84 to NED
Raw IMU
GPS in NED
CENTRAL
TRANSPUTER
INS algorithm
Estimates
Position and Velocity in WGS84
Observation and
Prediction values
GPS
FILTER
TRANSPUTER
2
Estimation
and
Validation
Figure 4.6: Real time architecture implementing four Transputers for parallel processing.

its most accurate solution, which in the straddle case is centimeter accuracy.
When this occurs the guidance computer is activated and the averaging of the
data begins. This lasts for approximately ten seconds before the calibration begins. Ten seconds is sucient to attain the required calibration data. Since the
vehicle undergoes frequent stopping, and so calibration is common, temperature
compensation is not required;
2. Once the averaging process has been completed, calibration of the inertial unit
is accomplished as described in Section 3.11. This is used to determine biases
and to obtain the initial Cnb matrix. At this stage the GPS position x is used
to determine the initial Cng matrix, Section 4.2.2.
3. The navigation system then proceeds onto the inertial navigation system with
the initial position as determined by the GPS receiver and the initial attitude as
obtained from the alignment stage. The lter is initialised. The guidance computer is informed that the navigation system is ready to provide the navigation
solution;
4. As the vehicle moves the acceleration and gyro values are read in and the biases
removed, Equation 3.81. Cnb is updated, Equation 3.29, and the acceleration
in the navigation frame is computed, Equation 3.30. These values are then
integrated to provide the position and heading of the vehicle;
5. If no GPS x is available then return to Step 3, otherwise determine the GPS x
in the navigation frame, Equations 4.2 and 4.3. Since the latency is small and
the vehicle dynamics are low, use the velocity data to propagate the position
data using a constant velocity model;
6. Fuse the inertial and GPS data as described in Section 4.4.2;
7. Use the gating function, Equation 2.36, to determine if there are any high
frequency faults. If high frequency faults exist then only send out the current
state values as determined by the inertial system and return to Step 3. If the
validation check has passed then correct the inertial unit as shown in Section
4.6 Aiding Using a Vehicle Model
92
4.4.2 and then return to Step 3. The estimation procedure occurs within the
sampling time of the inertial unit, however, if this was not the case and again
the latency is low, then the corrected velocity is used to propagate the corrected
position value.
This structure and algorithms have been successful in providing the sole means of
navigation for the straddle carrier.
Experimental results of the Inertial/GNSS implementation are presented in Chapter 5.

Aiding information does not have to come only from external sensors. An alternative
method is to apply constraints on the inertial equations if the motion of the vehicle
is itself constrained in some way. In [25] the author develops the theory required
for aiding low cost inertial units by using a model of the aircraft's dynamics. In a
parallel eort, the work presented in this thesis also uses the land vehicle's motion
to constrain the error growth of inertial units, and the lter structure illustrated in
Figure 4.3 b) is implemented here.
The general three dimensional inertial equations are derived from Newton's laws
and are thus applicable to all forms of motion. The inertial equations on board a
train apply equally as well to an aerobatic aircraft. However, the motion of the train
is clearly dierent to that of the aircraft, in particular, the train is constrained to
move along the rails. This fact can be used as a \virtual observation", constraining
the error growth along the lateral and vertical directions. The motion of a general
land vehicle is not much dierent from that of the train, and hence can also use
constrained motion equations. If the vehicle undergoes excessive slip or movement in
the vertical direction o the ground, then extra modelling is required, however, the
use of constraints as virtual observations is still valid.
93
4.6.1 General Three-Dimensional Motion
Let the motion of the vehicle be described by the state equation,
x_ = f(x,u)
(4.22)
where the vehicle state vector x = VTn ; PTn ; ; ; T , and the measurements u= fTb ; !Tb T .
Using the kinematic relationship between !b and the rates of changes of the Euler
angles, and assuming that the rate of rotation of the earth is negligible, the state
equations for vehicle motion can be written as
_ = Cnbfb
P_ n = Vn
Vn
!y sin + !z cos
cos
_ = !y cos !z sin
_ =
_ = !x + (!y sin + !z cos ) tan
(4.23)
(4.24)
(4.25)
(4.26)
(4.27)
Equations 4.23 - 4.27 are the fundamental equations that enable the computation of
the state x of the vehicle from an initial state x(0) and a series of measurements fb and
!b . The Euler method is implemented here for a better conceptual understanding.
It is important to note the following with respect to these equations.
1. These equations are valid for the general motion of a body in three-dimensional
space.
2. Equations 4.23-4.27 represent a set of non-linear dierential equations that can
easily be solved using a variety of dierent techniques.
3. It is possible to linearise these equations, for suciently small sampling intervals, by incorporating all the elements of the direction cosine matrix Cnb into
the state equation.
94
Clearly, the rate of error growth can be reduced if the velocity of the vehicle and the
Euler angles and can be estimated directly. It will be shown in the next section
that this is indeed possible for a vehicle moving on a surface.
4.6.2 Motion of a Vehicle on a Surface
Motion of a wheeled vehicle on a surface is governed by two non-holonomic constraints.

When the vehicle doesn't jump o the ground and does not slide on the ground, the
velocity of the vehicle in the plane perpendicular to the forward direction (x-axis) is
zero. Under ideal conditions, there is no side slip along the direction of the rear axle
(y-axis) and no motion normal to the road surface (z-axis), so the constraints are
Vy = 0
Vz = 0
(4.28)
(4.29)
In any practical situation, these constraints are to some degree violated due to the
presence of side slip during cornering and vibrations caused by the engine and suspension system. In particular the side slip is a function of the vehicle state as well
as the interaction between the vehicle tyres and the terrain. A number of models are
available for determining side slip, but these models require the knowledge of the vehicle, tyre and ground characteristics that are not generally available. Alternatively,
information from external sensors can be used to estimate slip on-line. As a rst
approximation however, it is possible to model the constraint violations as follows:
Vy
Vz
y = 0
z = 0
(4.30)
(4.31)
where y and z are Gaussian, white noise sources with zero mean and variance y
and z respectively. The strength of the noise can be chosen to re ect the extent of
the expected constraint violations.
Using the following equation that relates the velocities in the body frame Vb =
2
95
[Vx; Vy ; Vz ]T to Vn;
Vb = [Cnb]T Vn
it is possible to write constraint Equations 4.30 and 4.31 as a function of the vehicle
state x and a noise vector w= [y ; z ]T ;
2
4
Vy
Vz
3
5
= M + 4 y 5 ;
z
(4.32)
where M is
2
4
VN cos cos + VE cos sin VD sin

VN (sin sin cos cos sin ) + VE (cos cos + sin sin sin ) + VD sin cos
(4.33)
The navigation frame implemented here is North (N), East (E) and Down (D). It
is now required to obtain the best estimate for the state vector x modeled by the
state Equations 4.23-4.27 from a series of measurements fb and !b , subjected to the
constraint Equation 4.32.
4.6.3 Estimation of the Vehicle State Subject to Constraints
The state equation, obtained by the discretisation of Equations 4.23-4.27, is
x^(kjk 1) = f(^x(k 1jk 1); u(k));
(4.34)
and the discrete time version of the constraint equation obtained from Equation 4.32
z(k) = H (x(k)) + v(k):
(4.35)
Estimation of the state vector x subjected to stochastic constraints can be done in
3
5:
96
the framework of an extended Kalman lter. It is proposed to treat Equation 4.35

as an observation equation where the \observation" at each time instant k is in fact
identical to zero.
The Kalman lter algorithm proceeds recursively in three stages:
Prediction: The algorithm rst generates a prediction for the state estimate and
the state estimate covariance at time k according to the Equations 2.12 and 2.13.
Observation: Following the prediction, it is assumed that an observation z(k) that
is identical to zero is made. An innovation is then calculated as follows
(k) = z(k)
^z(kjk 1)
(4.36)
where z(k) is in fact set to zero. An associated innovation covariance is computed

using Equation 2.17.
Update: The state estimate and corresponding state estimate covariance are then
updated according to Equations 2.14 and 2.18
4.6.4 Observability of the States
The extended Kalman lter algorithm obtains estimates of the state x. However, not
all the state variables in this estimate are observable. For example, inspection of the
state and observation equations suggest that the estimation of the vehicle position,
Pn, requires direct integrations and therefore is not observable.
Furthermore, if the vehicle moves in a trajectory that does not excite the relevant
degrees-of-freedom, the number of observable states may be further reduced. Intuitively, forward velocity is the direct integral of the measured forward acceleration
during motion along straight lines, therefore is not observable. Clearly an analysis is
required to establish the conditions for observability.
As the state and observation equations are non-linear, this is not straightforward.
In this section an alternative formulation of the state equations, that directly incorporates the non-holonomic constrains, are developed in order to examine this issue.
Consider the motion of a vehicle on a surface as shown in Figure 4.7. Assume that
97
x
Body Frame
b
y
Vehicle
Vn , P n
N
E
Navigation Frame
n
Figure 4.7: Motion of a vehicle on a surface. The navigation frame is xed and
the body frame is on the local tangent plane to the surface and is aligned with the
kinematic axes of the vehicle.
the non-holonomic constraints are strictly enforced and therefore the velocity vector
V of the vehicle in the navigation frame is aligned with bx. Let s; s_ and s be the
distance measured from some reference location to the current vehicle location along
its path, together with its rst and second derivatives with respect to time.
Therefore,
V=s_bx:
Acceleration of the vehicle is given by,
f =V_ = sbx+s_b_ x:
As the angular velocity of the coordinate frame b is given by !b ,
f = sbx+s_!b bx;
f = sbx+s_!z by-s_!y bz:
98
Components of the acceleration of the vehicle in the body frame b become,
f.bx = s;
f.by = s_!z ;
f.bz = s_!y :
(4.37)
(4.38)
(4.39)
Given that fn = Cnbfb and using this in the above equations,

2
6
6
4
fx
fy
fz
7
7
5
= 64
V_ f
Vf !z
Vf !y
7
7
5
+ 64
c c
c s + s s c
s s + c s c
c s
s
c c + s s s s c
s c + c s s c c
32
76
76
54
0
0
3
7
7;
5
where g is the gravitational constant and Vf = s_ is the speed of the vehicle.

Rearranging this, the following three equations relating the vehicle motion to the
measurements from the inertial unit can now be obtained,
V_ f
Vf !z
fy
fx + g sin = 0;
g sin cos = 0;
Vf !y + fz + g cos cos = 0:
(4.40)
(4.41)
(4.42)
It is clear from the above equations that,

when the forward acceleration is zero the roll () and pitch ( ) can be directly
computed from the inertial measurements
if one of the angular velocities !y or !z is not zero, the forward velocity can
also be computed directly.
even when the forward acceleration is non-zero, it is possible to write a dieren-
tial equation containing only the forward velocity and the inertial measurements
by substituting Equations 4.41 and 4.42 into 4.40. Therefore, Vf can be obtained
by one integration step involving the inertial measurements.
If the constraints are not used, two integration steps are required to obtain
4.7 Multiple Aiding of an Inertial System
99
velocities. This result is of signicant importance. The fact that the forward
acceleration is observable makes the forward velocity error growth only a function of the random walk due to the noise present in the observed acceleration.
It is possible to use the Equations 4.41 and 4.42 directly to obtain the complete
vehicle state without going through the Kalman lter proposed in the previous
section. This, however, makes it dicult to incorporate models for constraint
violations in the solution. Also, when the constraint violation is signicant,
such as in o road situations or cornering at high speeds, the white noise model
is inadequate. For example, if there is signicant side slip, explicit slip modeling
may be required.
Experimental results of this implementation are provided in Chapter 5.

This section will discuss the aiding of an inertial unit with three forms of external observations; position and velocity derived from GPS, speed from a drive shaft encoder
and the vehicle model constraints. By incorporating all three pieces of observations,
more information is provided for correction of errors in the inertial data. Furthermore,
the constraint algorithms contain the growth of the attitude error when there are no
GPS xes, thus sustaining the accuracy of the inertial unit. The speed information
provided by the encoder data makes the forward velocity observable.
The approach taken is to use a linear information lter with the inertial error model
developed as a process model. Thus a direct feedback structure is implemented as
shown in Figure 4.3 c). This makes the system practically achievable, and also allows
easy information addition from the aiding sensors, especially since they are delivered asynchronously and at dierent rates. It is shown that the additional sensors
signicantly improve the quality of the location estimate. This is of fundamental
importance since it makes the inertial system less dependent on external information.
100
The inertial error model as outlined in Section 4.4.2 is implemented here.

Prediction: The prediction stage is implemented using Equations 2.21 and 2.22 and
mimics that of the Kalman lter implementation as presented in Section 4.4.2.
Observation: When an observation from the aiding sensor or constraint is made,
the observation vector generated is the observed error of the inertial system,
z(k) = zinertial(k) zaiding (k):

At each sampling of the inertial unit, a constraint observation is made, that is, Vy = 0
and Vz = 0. At this stage the velocity vector is only partly completed, requiring the
speed of the vehicle along the body x-axis which is obtained from the speed encoder Vx.
The sampling rate of the speed encoder is 25Hz. However, it can be safely assumed
that constant velocity occurs between these samples, due to the slow dynamics of
the vehicle, and hence the observation formed can occur at the sampling rate of the
inertial unit. This observation, which is now in the body frame, is converted to the
North (N), East (E), Down (D) frame using Cnb. That is,
0
Vx(k)
Vy (k)
Vz (k)
B
nB
zvelocity
(
k
)
=
C
b @
V
0
B
B
@
1
C
C
A
(4.43)
Vx (k) cos cos

Vx (k) cos sin
Vx (k) sin
1
C
C:
A
(4.44)
Thus the observation vector is
z(k) = zinertial
(k) zvelocity
(k):
V
V
The observation model is given by

Hvelocity
= 0 I 0
V
3

3
(4.45)
101
and the observation covariance matrix is

0
Rvelocity
V
0 0C
B
C
= B
@ 0 y 0 A :
0 0 z
x
2
(4.46)
Since the velocity vector is transformed from the body frame to the navigation frame,
the observation noise covariance needs to be transformed as well and is done so by
velocity = CnbRvelocity CnbT
(4.47)
When the position and velocity are obtained from the GNSS receiver the observation
vector is
2
inertial
z
(
k) zGNSS
(k) 5
P
P
4
z(k) = inertial
zV (k) zGNSS
(k)
V
(4.48)
The observation model is

0
HGN SS (k) =
I 0 0 A;
0 I 0
3
(4.49)
and the observation noise matrix is

0
P N
2
RGN SS (k) =
B
B
B
B
B
B
B
B
B
B
B
@
0
0
0
0
0
P E
2
0
0
0
0
0
0
P D
2
0
0
0
0
0
0
V N
2
0
0
0
0
0
0
V E
2
0
0
0
0
0
V D
1
C
C
C
C
C
C
C
C
C
C
C
A
(4.50)
where the individual variances are obtained from the GDOP values.
Update: Once the observations are formed, the information state vector is gener-
4.8 Conclusion
102
ated along with the corresponding information matrix, Equations 2.23 and 2.24. The
estimate then proceeds according to Equations 2.25 and 2.26.
Experimental results of this implementation are provided in Chapter 5.
4.8 Conclusion
This chapter has provided the details of the three aided inertial navigation systems
that are implemented in this thesis. The main contributions in this chapter are these
navigation systems along with the real time implementation of an inertial/GNSS navigation system, and discussion on the fault characteristics and detection of the GNSS
system.
The rst implementation is that of the Inertial/GNSS navigation system. It comprises of a Kalman lter which estimates the errors in position, velocity and attitude
of the inertial navigation system given external observations from a GNSS aiding
system. The implementation is in a direct feedback structure and the lter algorithm
is implemented so that any aiding sensor can be used. The functionality of the two
navigation systems within GNSS; GPS and GLONASS, were brie y discussed along
with the transformations to take the observation from these aiding systems and converting them to a common navigation frame with the inertial navigation system, in
this case being a frame represented by North, East and Down. A simple case study
was provided of how an Inertial/GNSS navigation system needs to be implemented
on a straddle carrier. The structure of the navigation lter was also provided. Furthermore, the detection of multipath errors and the tuning implementation required
for the navigation lter were also detailed.
The real time implementation of this navigation system was discussed. Two primary concerns associated with such an implementation are processing power and
latency. The real issue of latency occurs with the delay in processing the GNSS information within the GNSS receiver. Velocity latencies of up to a second can occur.
Both the hardware and software implementation to address these eects was detailed.
4.8 Conclusion
103
The second navigation system implemented consisted of an inertial navigation system being aided by vehicle model constraints. The core of this algorithm lies in the
exploitation of constraints on the motion of a land vehicle. In particular, under ideal
conditions the velocity of the vehicle perpendicular to the forward direction is zero.
To account for slip and vibrations present in any practical situation, a white noise
model is used for velocities in this plane. This is sucient when the amount of slip
and vibrations is relatively small, more accurate representation of the motion of the
vehicle is required when this is not the case.
Using the vehicle model constraints a sensor free aiding system is developed. Constraint equations were developed along with the non-linear Kalman lter implementation required to fuse this data together. A direct lter structure is implemented. An
observability analysis was also provided and it was shown that during constant velocity motion, the roll and pitch of the vehicle is observable when using the constraint
algorithm. Since the attitude is observable, and hence can be estimated by the lter,
this bounds the error growth of the velocity as indicated by the inertial navigation
system. Furthermore, if the pitch or yaw rotation rates are not zero then the forward
velocity can be estimated, however this requires the constant excitation of the pitch
and yaw axes. Furthermore, since the observations contain velocity information only,
position is unobservable.
To overcome the conditions of observability with the forward velocity an extra
source of information is required; speed along the x-axis. Thus the third navigation system implementation involves the aiding of an inertial navigation system using
the vehicle model constraints and speed provided by a drive shaft encoder. Furthermore, since position is not observable, GNSS information is also added. The lter is
implemented in a direct feedback structure, and the information lter algorithm is
employed to fuse all the data. This allows for a simple update stage for the information from the external sensors. This navigation system thus allows for multiple
aiding which in turn provides greater accuracy and better fault detection.
Chapter 5
Experimental Results
5.1 Introduction
This chapter details the experimental results on the implementation of the three
navigation structures discussed in Chapter 4. Section 5.2 describes the vehicles and
sensors used to conduct the tests.
Section 5.3 presents the results of the Inertial/GNSS navigation structure. The
focus here is on the correcting properties of the aiding sensor and the fault detection
capabilities of the lter. Results are provided for a utility vehicle travelling around
the University campus where signicant multipath occurs and at an industrial site
where there is little multipath. The results from a straddle carrier experiment at the
port is also provided. It illustrates the fault detection capabilities of the navigation
system.
Section 5.4 provides results of a utility vehicle being driven around an industrial area
and using vehicle model constraints to aid the inertial navigation system. These
results are compared to the inertial data being unaided and being aided by GNSS.
Simulation results are also provided since they provide a better understanding of the
observability analysis as described in Chapter 4.
Finally, Section 5.5 provides results of the information lter approach to aiding an
inertial navigation system using GNSS, vehicle model constraints and speed data
5.2 Experimental Setup
105
provided by a drive shaft encoder. The test comprises of the utility driven around in
a relatively open area so that no multipath occurs. The objective is to see how long
the inertial navigation system can go without GNSS observations when a particular
amount of accuracy is required in position, and when vehicle model constraints are
used to aid the inertial system.

This section provides the necessary details on the vehicles and sensors implemented
in this thesis. The environments in which most of the testing was conducted in is
also presented. A Transputer based system is implemented to log data and to run
the algorithms in real time.
5.2.1 Vehicles
The utility shown in Figure 5.1 is the primary vehicle that was used for all testing.
The tray in the rear houses the computers needed to read in the data from the sensors
and log all necessary data. Although the vehicle can maneuver under high speeds,
the work conducted in this thesis is to provide an aided inertial navigation system
for large vehicles such as a straddle carrier, Figure 5.2, and hence the utility is driven
around under relatively low speeds (30kmph).
The straddle carrier is a vehicle which maneuvers around a port picking up containers at a known location and placing them at a desired location. The straddle
carrier weighs 63500kg and has a safe working load of 40000kg. It has a maximum
speed of 26km=hr. Its width is 4:94m, length is 10:34m and height is 12:89m.
5.2.2 Sensors
A Watson's Inertial Measurement Unit is employed in this thesis, Figure 5.3. The
sampling rate of the unit is 125Hz and it provides the accelerations and rotation rates
106
Figure 5.1: The utility is used as a test bed for the sensors. It houses the Transputers
which log data from the GNSS receiver and inertial unit. It also handles the real time
code of the Inertial/GNSS lter.
Figure 5.2: The position and orientation of a 65 tonne straddle carrier in a port
environment is determined using the Inertial/GNSS navigation system. The objective
of this navigation system is to provide the navigation data needed for guidance and
hence a high amount of integrity is required.
107
measured by three accelerometers and three gyros mounted orthogonally. The unit
also houses two tilt sensors to measure the bank and elevation of the unit. Although it
is an IMU, it can also function as an ISA, that is, by providing uncompensated data,
and this is how it is implemented in this thesis. Table 5.1 lists the characteristics of
the unit.
Figure 5.3: The Watson IMU houses three accelerometers and three gyros in an
orthogonal arrangement. It also contains two tilt sensors to measure the bank and
elevation of the inertial unit.
The GNSS equipment implemented includes:
An Ashtech G12 GPS in standard dierential mode, Figure 5.4. The unit
delivers xes at 10Hz as long as there are at least four satellites available. The
position accuracy is 1m and that of velocity is 0:05m=s. The cost of such a unit
in todays market is less than $1k AUD;
A Novatel RT2 GPS unit in RTK mode providing 0:02m position accuracy once
the integer ambiguity is solved and xed. Velocity accuracy is 0:02m=s, Figure
108
Specication
Rate Range
Rate Bias
Rate Resolution
Acceleration Range
Acceleration Accuracy
Acceleration Bias
Acceleration Resolution
Sensor Alignment
Accelerometer Frequency
Gyro Frequency
Value
o
50 =s
2% F S
< 0:05o =s
2g
< 0:5% F S
< 0:5% F S
< 2 mG0 s
< 0:25o
300 Hz
70 Hz
Table 5.1: The specication for the Watson IMU implemented in this work.
5.5. The unit provides these states at 4Hz. The cost of such a unit in todays
market is approximately $25k AUD;
Figure 5.4: The Ashtech G12

The inertial navigation code and lter structure remain the same, irrespective of
the type of receiver implemented. Only the observation covariance matrix, R(k), in
109
Figure 5.5: The Novatel RT2

the lter is modied when the receiver is changed.
5.2.3 Environment
The utility was driven in two areas: around the University grounds which is heavily
populated with buildings (Figures 5.6 and 5.7), and at an industrial area with sparse
building structures and minimal tree foliage. Figures 5.8 and 5.9 illustrate sections
in the industrial site where the utility was driven.
The straddle carrier is located in a container terminal, Figure 5.10. The containers,
which can be seen in the forefront of the photo, are lower than the straddle carrier.
Thus by mounting the GNSS aerial on top of the straddle carrier no multipath errors
will occur from these containers. However, as the straddle carrier passes alongside
a quay crane, Figure 5.11, then multipath errors will occur. Furthermore, if the
straddle carrier passes under the crane, Figure 5.12, then total satellite blockage will
most likely occur.
110
Figure 5.6: Outside the University campus showing extensive tree foliage and buildings which will aect the performance of the GPS receiver.
Figure 5.7: A major road outside of the University campus where multipath occurs
due to the building structures.
111
Figure 5.8: The utility was driven in a relatively open area where sparse building
structures are found. Since the sky is relatively open to the receiver, minimal faults
will occur.
Figure 5.9: Another view of the area shows a small amount of tree foliage, however
again there is still a large portion of the sky which is visible by the receiver antenna.
112
Figure 5.10: The port where the straddle carrier is located comprises mainly of containers and quay cranes. Although the containers have no aect on the GNSS signal
the quay cranes do and hence fault detection is particularly important.
113
Figure 5.11: The extension on the top right hand corner of the quay crane causes
slight multipath errors when the straddle carrier passes underneath this extension.
Figure 5.12: As the straddle carrier approaches the quay crane in order to travel
underneath it, multipath errors occur until the straddle carrier is under the crane at
which stage total satellite blockage occurs.
5.3 Inertial/GNSS Results
114

This section provides the results of the Inertial/GNSS navigation system algorithm
as presented in Chapter 4, Section 4.4.2. Both the Ashtech G12 and the Novatel RT2
are implemented as the GNSS receivers. The lter structure and inertial navigation
code remains the same in both cases. The utility vehicle and the straddle carrier
are used as the test vehicles. In the gures describing the results, the light coloured
crosses and circles are the GPS xes while the darker lines (which comprises or the
sample points of the INS) is the fused data.
5.3.1 Fusion
Figure 5.13 presents the raw data taken from the inertial unit and the Ashtech G12
in a run around the University of Sydney. The area is heavily populated with tall
buildings and trees as shown in Figures 5.6 and 5.7 thus causing substantial multipath
errors and poor performance in particular locations. Similarly, the inertial data is
extremely poor due to:
The unit was powered for only a short period of time not allowing for thermal
stability of the unit, and consequently changes in the bias occur during the run;
and
The unit was mounted directly onto the vehicle. As a result, the vibrations of
the vehicle were transmitted directly to the unit.
Figure 5.14 presents the fused data without GPS fault detection. Figures 5.15 and
5.16 present enlarged views of two areas (regions 1 and 2) where multipath has occurred. Since no fault detection was implemented, and consequently high frequency
faults were not rejected, the fused data was drawn into the multipath region. The
innovation sequences of the states show large spikes due to the multipath errors being
accepted. This occurs more than 95% of the time. As a result, the lter cannot be
considered consistent.
115
Figures 5.17 and 5.18 show the same multipath regions with the fault detection
technique implemented. The threshold was set to ignore innovations exceeding the
95% probability gate and hence the multipath signals were rejected when this occurred. As can be seen however, the fused path does not provide a smooth trajectory.
This is due to the accuracy of the G12 receiver. Thus although multipath is rejected,
the accuracy is only bounded by that of the GPS receiver.
5.3.2 Alignment Correction
Figure 5.19 presents the fused data from the Novatel GPS unit in the environment
shown in Figures 5.8 and 5.9. Figure 5.20 is an enhanced view of the vehicle on two
occasions when the vehicle was given the correct initial heading. The bottom path
portrays the vehicle just after it has begun its journey while the top path presents the
vehicle on the return. Figure 5.21 illustrates exactly the same two occasions with the
vehicle's initial heading incorrect by ve degrees. The on-line alignment of the inertial
unit has corrected this error by the time the vehicle returns to the nal position.
Figures 5.22 and 5.23 are enhanced views of the acceleration and velocity at the
end of the test with attitude correction, while Figures 5.24 and 5.25 illustrate the
acceleration and velocity of the vehicle without any attitude correction. The velocity
in Figure 5.25 has a noticeable saw-tooth characteristic as compared to Figure 5.23.
This is due to the uncompensated bias in the acceleration as shown in Figure 5.24.
Between GPS xes this oset in acceleration causes the velocity of the vehicle to drift
before being corrected by the next GPS x. This oset in the acceleration is due to
the inaccurate computed pitch angle of the vehicle. Figures 5.26 and 5.27 present
the computed pitch of the vehicle with and without on-line alignment respectively.
Thus Figures 5.21 and 5.23 demonstrate the importance of on-line alignment. In
particular Figure 5.25 illustrates that if no GPS xes are present at the end of the
run the inertial solutions will drift substantially quicker than when on-line alignment
is available.
116
5.3.3 Fault Detection
Figure 5.28 shows the fused result of the navigation loop onboard a straddle carrier
at a port. This data employs the Ashtech G12 receiver. The vehicle starts in position
0m North; 0m East and moves in a counter clockwise direction. The vehicle rst
passes around some containers before approaching a crane indicated in the lower right
portion of the gure. As the vehicle approaches the crane multipath errors occur until
the vehicle reaches a stage where the GPS receiver cannot locate any more satellites.
At this stage no GPS xes occur. As the vehicle departs from underneath the crane,
slight multipath errors still occur. The multipath errors stop once the vehicle has
reached the position 68m North; 18m East. Figures 5.29 and 5.30 are enhanced
views of this fault region. The lter rejects incorrect GPS xes until the end of the
fault region where there is a slight adjustment since the uncertainty in the inertial
solution is, at this stage, greater then that of the GPS x. During the faulty portion
of the trajectory the lter remains in the prediction stage and the inertial unit runs
unaided. The on-line alignment algorithm has also aligned the inertial unit such that
the unit is suciently accurate to complete the trajectory whilst there are no GPS
xes.
Figures 5.31 and 5.32 show the position and velocity innovations for this run.
Figures 5.33 and 5.34 show the fused result with the same tuning parameters but
with no multipath rejection. The corresponding position and velocity innovations are
presented in Figures 5.35 and 5.36. Without fault rejection the innovations not only
exceed the two sigma bound but the fused result incorrectly follows the GPS xes.
However, the lter can be tuned so as to place less weighting on the observations.
This is accomplished by increasing the accuracy of the model and by placing smaller
covariances in the process covariance matrix Q(k). Figure 5.37 shows the fused result
in this case and Figures 5.38 and 5.39 show the corresponding velocity and position
innovations. Although Figure 5.37 closely resembles the true fused result as shown in
Figure 5.29, the innovations judge it to be unacceptable since it exceeds the two sigma
bound. By not implementing any GPS fault detection techniques not only is the lter
inconsistent since the innovations lie beyond the two sigma bound, but incorrect error
117
estimates will be obtained. Thus if for some reason the GPS was shut o during the
multipath region the inertial solutions would have been inaccurately corrected. An
example of this is shown in Figures 5.40 and 5.41 which present the velocity of the
vehicle with fault detection and proper tuning, and without fault detection and false
tuning respectively. At (approximately) iteration number 8500 in Figure 5.41, it can
be seen that multipath aects the velocity corrections even though the position of
the vehicle is judged acceptable.

This section describes the results of the inertial/vehicle constraint navigation system
presented in Chapter 4, Section 4.6. The Novatel RT2 GPS unit is implemented here
for path determination. The utility is used as the test vehicle and the test is conducted at the industrial site. The vehicle was driven at speeds up to 10m=s.
In order to see the theoretical limitations of the proposed algorithm, computer simulations were also performed so that trial conditions can be accurately controlled. A
program was written to simulate a motion of a wheeled vehicle on a predened trajectory and to generate the accelerations and angular velocities. These accelerations and
velocities, corrupted with noise, were then used to generate estimates of the vehicle
position and velocity. To examine the eect of the angular velocities in !x, !y and
!z on the estimation algorithm, simulated data corresponding to a vehicle moving at
constant velocity was generated. This test set was generated using simulation because
it is very dicult to obtain the experimental data required to demonstrate the lack
of observability of forward velocity in particular circumstances.
All angular velocities of the vehicle were set to zero, with the exception that one of
the angular velocities is set to a random walk in the time interval between 700 to 1300
sec. Figures 5.42, 5.43 and 5.44 show the error in the predicted speed of the vehicle
Vx . The light coloured line represents the raw inertial navigation solution while the
darker line is the constrained solution. It may be seen from these gures that, as
118
expected, any excitation due to !y and !z results in a zero error in predicted vehicle
speed whereas motion in !x has no eect on this error. It may also be seen from
Figure 5.45 that the errors in roll and pitch do not grow during prediction using the
proposed algorithm. This is an important result because, as can be seen from Figure
5.42, although the error in velocity is not reduced to zero its noise induced growth
is a rst order Gauss Markov (Random Walk). Again, as expected, the error in yaw
grows with time. This eect is clearer when there are unestimated biases present in
the gyroscope readings (see Figure 5.46). Figure 5.47 shows that the error in the
predicted speed of the vehicle reduces to zero even when the velocity of the vehicle is
not constant.
The experimental results corresponding to the implementation of the full non-linear
Kalman Filter are now discussed. The vehicle was driven at low speeds so that slip
was minimised.
Figure 5.48 shows the position of the inertial solution in the navigation frame using
the raw inertial data, the fused Inertial/GPS navigation system, and the inertial data
with constraints applied. This plot shows a large error in position estimate caused by
the drift in attitude when only the raw inertial data is used. Furthermore, the close
correspondence between the constrained inertial data and the fused Inertial/GPS data
can also be seen. Figures 5.49 and 5.50 show the errors in position and velocity of the
raw inertial data and the constrained inertial data using the fused Inertial/GPS data
as the \truth". As shown, the position error increases quadratically and the velocity
error increases linearly when using only the raw inertial data. The constrained data
however, shows excellent ability in keeping the error bounded.
Figures 5.51 and 5.52 show the results of the roll and pitch angle as determined by
the three cases at the end of the trajectory when the vehicle was stationary. The tilt
sensor information is also provided. As can be seen, the constrained inertial solution
closely resembles that of the Inertial/GPS navigation system. This can be clearly
seen with the pitch data, both of which lie well within the results obtained by the
tilt sensor. Averaging the tilt sensor data, the error in the roll and pitch angle of the
three cases is given in Figures 5.53 and 5.54.
As can be seen it is the pitch angle which predominantly causes the drift in raw
5.5 Multiple Aiding Results
119
inertial data. The constraints have kept the roll and pitch angles to within acceptable limits and in turn allow the constrained system to be comparable with the
Inertial/GNSS navigation system.

This section describes the results of the Inertial/vehicle constraint/speed navigation
system as presented in Chapter 4, Section 4.7. The Novatel RT2 GPS unit is implemented here along with a drive shaft encoder providing forward speed with an
accuracy of 1m=s at 25Hz. The utility is used as the test vehicle and the test is
conducted at the industrial site in an area where the ground texture is rough and so
larger vibrations are encountered by the vehicle.
Since the drive shaft encoder provides data at 25Hz, constant velocity can be assumed between samples and hence the constraint vector, Equation 4.43, is applied at
the same rate as the sampling rate of the inertial unit.
Figure 5.55 shows the path taken by the vehicle. The path was obtained from the
Inertial/GNSS navigation system. Along with position, the velocity, attitude and
heading of the inertial system was obtained. These states are used as the \truth" in
all further analysis presented here.
Figure 5.56 shows the North and East position error growth. The plots show the
error growth when the inertial unit is used as a stand alone sensor, and when it is
bounded by the vehicle constraints. As can be seen, the error growth of the position
is bounded as a result of the incorporation of vehicle modelling. Likewise Figure 5.57
shows the containment of the velocity.
Figure 5.58 shows the roll and pitch of the vehicle as deduced from the Cnb matrix.
Any drift in these states causes the velocity, and hence position evaluation, to drift
as well. Thus by analysing these plots it can be seen that the cause of the drift in
velocity and position of the unaided inertial navigation system is primarily due to the
drift in the roll and pitch of the computed attitude. The constrained attitude shows
equally good results.
120
The addition of the vehicle model constraints corrects the attitude and velocity of
the inertial unit, thus minimising the impact of drift on these states. Since the attitude is corrected, the velocity error of the unit is contained, and hence the position
error is bounded as well.
The addition of GPS observations will in turn provide more information for the
alignment of the combined navigation system, and also provides position observability.
The greater the frequency of observations from the GPS unit, the more information
is added to the navigation system. Figures 5.59, 5.60 and 5.61 show the same plots as
in the previous example, however they compare the constrained inertial unit, to the
constrained inertial unit with GPS xes provided every 15 seconds. When comparing
the plots, the greatest improvement can be seen with the position error, Figure 5.59,
since this is unobservable with the vehicle model constraints only. Improvements are
also seen in the velocity, Figure 5.60, and attitude, Figure 5.61, although these are
more modest. This is because these states are estimated directly using the vehicle
model constraints. More importantly however, this shows that the inertial data can
be contained between GPS xes. This dramatically improves the navigation suite as
a whole, since the inertial system can navigate for a substantially greater period of
time without the need for GPS xes.
5.6 Inertial/GNSS Plots
121
POSITION
NORTH (m)
500
500
2000
1500
1000
EAST (m)
500
Figure 5.13: Raw data from the inertial unit and GPS. The inertial solution wanders
o due to the changing bias terms and due to the unit being mounted directly onto
the vehicle.
POSITION
100
NORTH (m)
100
200
300
500
400
300
200
100
EAST (m)
100
200
Figure 5.14: Fusion of inertial and GPS data with no multipath rejection.
122
POSITION
290
300
NORTH (m)
310
320
330
340
350
360
200
190
180
170
160
150
140
EAST (m)
130
120
110
100
Figure 5.15: Enlarged view of region 1 where GPS multipath errors have occurred.
The vehicle is heading from West to East. The light crosses show where GPS multipath has occurred and the dashed lines (which is a collection of points from the inertial
navigation solution) closely follow these points since their is no fault detection on the
observations.
POSITION
150
100
NORTH (m)
50
50
100
150
100
50
0
EAST (m)
50
100
Figure 5.16: Enlarged view of region 2 where GPS multipath errors have occurred.
The vehicle is heading from South to North. As in the previous plot, the inertial
navigation solution closely follows the GPS multipath.
123
POSITION
290
300
NORTH (m)
310
320
330
340
350
360
200
190
180
170
160
150
140
EAST (m)
130
120
110
100
Figure 5.17: Enlarged view of region 1 with multipath rejection. The validation
function has rejected the multipath. Small jumps in the fused result remain and this
is due to the accuracy of the GPS receiver.
POSITION
150
100
NORTH (m)
50
50
100
150
100
50
0
EAST (m)
50
100
Figure 5.18: Enlarged view of region 2 with multipath rejection. As in the previous
plot, multipath has been rejected but the result is limited to the accuracy of the GPS
receiver.
124
POSITION
250
200
NORTH (m)
150
100
50
50
300
250
200
150
100
EAST (m)
50
50
Figure 5.19: Fusion result using 0:02m position and 0:02m=s velocity GPS technology.
The straight located at 150m North and 240 to 270m East corresponds to Figure
5.8. The straight located at 200m North and 200 to 100m East corresponds to
Figure 5.9.
125
POSITION
61
60
NORTH (m)
59
58
57
56
55
69
68
67
66
65
64
EAST (m)
63
62
61
60
59
Figure 5.20: Close up of an area showing the heading of the vehicle. The bottom
path shows the vehicle slightly after the test has started while the top path shows the
vehicle on the return towards the end of the run. In this example the initial heading
is given the correct value.
POSITION
61
60
NORTH (m)
59
58
57
56
55
69
68
67
66
65
64
EAST (m)
63
62
61
60
59
Figure 5.21: Enhanced view of the same area showing the heading of the vehicle after
an initial error of 5 deg is placed on the heading. The heading is corrected by the
time the vehicle returns.
126
ACCELERATION
North (m/s )
0.5
0.5
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
East (m/s )
0.5
0.5
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
Down (m/s )
0.5
0.5
1.33
1.335
1.34
1.345
1.35
1.355
iteration
1.36
1.365
1.37
1.375
1.38
4
x 10
Figure 5.22: Enhanced view of the acceleration of the vehicle at the end of the run
with attitude correction.
VELOCITY
North (m/s)
0.2
0.1
0
0.1
0.2
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
East (m/s)
0.2
0.1
0
0.1
0.2
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
Down (m/s)
0.2
0.1
0
0.1
0.2
1.33
1.335
1.34
1.345
1.35
1.355
iteration
1.36
1.365
1.37
1.375
1.38
4
x 10
Figure 5.23: Enhanced view of the velocity of the vehicle at the end of the run with
attitude correction.
127
ACCELERATION
North (m/s )
0.5
0.5
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
East (m/s )
0.5
0.5
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
Down (m/s )
0.5
0.5
1.33
1.335
1.34
1.345
1.35
1.355
iteration
1.36
1.365
1.37
1.375
1.38
4
x 10
Figure 5.24: Enhanced view of the acceleration of the vehicle at the end of the run
without attitude correction.
VELOCITY
North (m/s)
0.2
0.1
0
0.1
0.2
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
East (m/s)
0.2
0.1
0
0.1
0.2
1.33
1.335
1.34
1.345
1.35
1.355
1.36
1.365
1.37
1.375
1.38
4
x 10
Down (m/s)
0.2
0.1
0
0.1
0.2
1.33
1.335
1.34
1.345
1.35
1.355
iteration
1.36
1.365
1.37
1.375
1.38
4
x 10
Figure 5.25: Enhanced view of the velocity of the vehicle at the end of the run
without attitude correction.
128
PITCH CBN (dark) and Pendulum Gyro (light)
2.5
1.5
Angle (deg)
0.5
0.5
1.5
1.33
1.335
1.34
1.345
1.35
1.355
iteration
1.36
1.365
1.37
1.375
1.38
4
x 10
Figure 5.26: Close up of the pitch of the vehicle with attitude correction. The dark
line represents the pitch as determined by the direction cosine matrix. The lighter
line is the pitch as determined by the tilt sensors.
PITCH CBN (dark) and Pendulum Gyro (light)
2.5
1.5
Angle (deg)
0.5
0.5
1.5
1.33
1.335
1.34
1.345
1.35
1.355
iteration
1.36
1.365
1.37
1.375
1.38
4
x 10
Figure 5.27: Close up of the pitch of the vehicle without attitude correction. The
dark line represents the pitch as determined by the direction cosine matrix. The
lighter line is the pitch as determined by the tilt sensors.
129
POSITION
20
NORTH (m)
20
40
60
80
100
120
140
120
100
80
60
40
EAST (m)
20
20
40
Figure 5.28: Position of the straddle carrier as it manoeuvres around containers before
driving under a quay crane. The vehicle starts at position 0m North; 0m East and
moves in a counter clockwise direction.
130
POSITION
50
60
NORTH (m)
70
80
90
100
110
120
60
50
40
30
20
EAST (m)
10
10
20
Figure 5.29: Enhanced view of the area where the vehicle approaches the quay crane.
As the vehicle passes under the crane total satellite blockage occurs and hence there
are no GPS xes. During this period the inertial errors are not corrected however, the
on-line properties of the lter have ensured that the inertial unit is aligned accurately
before the multipath region so that the position evaluations of the inertial data are
accurate.
POSITION
80
85
NORTH (m)
90
95
100
105
110
40
35
30
25
20
EAST (m)
15
10
Figure 5.30: Before the vehicle approaches the crane multipath errors occur. Theses
GPS xes however have been detected as faults and hence are not used as observations
and the inertial data is not inaccurately corrected.
131
POSITION INNOVATIONS
North (m)
2
1
0
1
2
100
200
300
400
500
600
700
100
200
300
400
500
600
700
100
200
300
400
500
600
700
East (m)
2
1
0
1
2
Down (m)
2
1
0
1
2
iteration
Figure 5.31: The position innovations of the lter show that the lter places more
emphasis on the inertial position evaluations. This is due to the large uncertainty of
the position xes delivered by this GPS unit.
VELOCITY INNOVATIONS
North (m/s)
0.2
0.1
0
0.1
0.2
0
100
200
300
400
500
600
700
100
200
300
400
500
600
700
100
200
300
400
500
600
700
East (m/s)
0.2
0.1
0
0.1
0.2
Down (m/s)
0.2
0.1
0
0.1
0.2
iteration
Figure 5.32: The velocity innovations show that the lter utilises the GPS velocity
xes to a great extent in order to correct the inertial errors. Since the velocity
evaluations from the inertial data is corrected with the accurate GPS xes then the
position determination delivered by the inertial data will also be accurate. Hence the
greater certainty in the position evaluations as illustrated in the position innovations.
132
POSITION
20
30
40
NORTH (m)
50
60
70
80
90
100
70
60
50
40
30
20
EAST (m)
10
10
20
30
Figure 5.33: With the same tuning parameters but with no fault detection routines
the inertial solution closely follows the GPS multipath errors. During this period the
inertial errors are inaccurately corrected and hence the position estimates overshoot
the correct path taken by the vehicle.
POSITION
80
85
NORTH (m)
90
95
100
105
110
40
35
30
25
20
EAST (m)
15
10
Figure 5.34: Enhanced view of the multipath region. Notice that the corrections have
altered the heading such that the perceived motion of the vehicle is not in line with
its true heading. The situation would erroneously suggest that the vehicle is slipping.
133
North (m)
5
0
5
10
15
1000
1050
1100
1150
1200
1250
1300
1050
1100
1150
1200
1250
1300
1050
1100
1150
iteration
1200
1250
1300
East (m)
10
0
10
20
1000
Down (m)
15
10
5
0
5
1000
Figure 5.35: The position innovations show where multipath errors occur.
North (m/s)
2
0
2
1000
1050
1100
1150
1200
1250
1300
1050
1100
1150
1200
1250
1300
1050
1100
1150
iteration
1200
1250
1300
East (m/s)
2
1
0
1
2
1000
Down (m/s)
1
0
1
1000
Figure 5.36: Similarly the velocity innovations show where the multipath errors occur.
Even beyond the multipath region the innovations exceed the two sigma bound.
134
POSITION
50
60
NORTH (m)
70
80
90
100
110
120
60
50
40
30
20
EAST (m)
10
10
20
Figure 5.37: Keeping with no GPS fault detection, the path of the vehicle can be
made to resemble the true path by placing greater accuracy in the state model and
hence in the inertial data, with less weighting placed on the GPS xes.
135
North (m)
10
0
10
20
200
400
600
800
1000
1200
1400
200
400
600
800
1000
1200
1400
200
400
600
800
1000
1200
1400
20
East (m)
10
0
10
20
Down (m)
20
10
0
10
iteration
Figure 5.38: The position innovations show that the lter is behaving inconsistently
even when it is tuned to place little emphasis on the GPS xes.
North (m/s)
1
0.5
0
0.5
1
200
300
400
500
600
700
800
900
1000
1100
1200
300
400
500
600
700
800
900
1000
1100
1200
300
400
500
600
700
iteration
800
900
1000
1100
1200
East (m/s)
1
0.5
0
0.5
1
200
Down (m/s)
1
0.5
0
0.5
1
200
Figure 5.39: The velocity innovations further magnify the inconsistency of the lter
when it is tuned to seemingly reject multipath errors.
136
VELOCITY
10
North (m/s)
5
0
5
10
2000
4000
6000
8000
10000
12000
2000
4000
6000
8000
10000
12000
2000
4000
6000
iteration
8000
10000
12000
East (m/s)
10
5
0
5
Down (m/s)
0.4
0.2
0
0.2
0.4
Figure 5.40: Illustration of the velocity of the vehicle when it is properly tuned and
with GPS fault detection.
VELOCITY
10
North (m/s)
5
0
5
10
2000
4000
6000
8000
10000
12000
2000
4000
6000
8000
10000
12000
2000
4000
6000
iteration
8000
10000
12000
East (m/s)
10
5
0
5
Down (m/s)
2
1
0
1
Figure 5.41: Velocity of the vehicle when the navigation loop is implemented without
fault detection however with tuning so that the position of the vehicle seemingly
matches the true trajectory. During the multipath region (approximately around
iteration 8500) the velocity as determined by the inertial solution is inaccurately
corrected. This is also seen with the attitude states.
5.7 Vehicle Model Constraint Plots
137

50
Constrained INS
Direct Integration
40
Error in Velocity (m/sec)
30
20
10
10
20
30
200
400
600
800
1000
Time (sec)
1200
1400
1600
1800
Figure 5.42: Errors in vehicle speed when the vehicle is moving at a constant velocity
of 10 m/s while the angular velocity !x is non-zero in the time interval 700 1300s.
Constrained INS
Direct Integration
30
20
10
10
20
30
200
400
600
800
1000
Time (sec)
1200
1400
1600
1800
of 10 m/s while the angular velocity !y is non-zero in the time interval 700 1300s.
138
30
Constrained INS
Direct Integration
20
10
10
20
200
400
600
800
1000
Time (sec)
1200
1400
1600
1800
of 10 m/s while the angular velocity !z is non-zero in the time interval 700 1300s.
Constrained INS
Direct Integration
Error in yaw (rad)
0.3
0.2
0.1
0
0.1
200
400
600
800
1000
1200
Time (sec)
1400
1600
1800
2000
200
400
600
800
1000
1200
Time (sec)
1400
1600
1800
2000
200
400
600
800
1000
1200
Time (sec)
1400
1600
1800
2000
Error in pitch(rad)
0.15
0.1
0.05
0
0.05
Error in roll(rad)
0.1
0.05
0
0.05
0.1
Figure 5.45: Errors in roll, pitch and yaw when the vehicle is moving at a constant
velocity of 10 m/s while the angular velocity about Bz is non-zero in the time interval
700-1300 sec.
139
Error in yaw (rad)
0.1
Constrained INS
Direct Integration
0
0.1
0.2
0.3
200
400
600
800
1000
1200
Time (sec)
1400
1600
1800
2000
200
400
600
800
1000
1200
Time (sec)
1400
1600
1800
2000
200
400
600
800
1000
1200
Time (sec)
1400
1600
1800
2000
Error in pitch(rad)
0.05
0
0.05
0.1
0.15
Error in roll(rad)
0.4
0.2
0
0.2
0.4
Figure 5.46: Errors in roll, pitch and yaw when the vehicle is moving at a constant
velocity of 10 m/s while the angular velocity !z is non-zero in the time interval
700 1300s. A constant unestimated bias of 10 rad/s is introduced to all angular
velocity observations
4
Constrained INS
Direct Integration
60
40
20
20
40
60
80
100
200
400
600
800
1000
Time (sec)
1200
1400
1600
1800
Figure 5.47: Errors in the vehicle speed when the vehicle is moving at a constant
acceleration of 0.05 m=s for 1000s and then decelerating at the same rate for another
1000s. The angular velocity !z is non-zero in the time interval 700 1300s.
2
140
POSITION
200
NORTH (m)
150
<Constrained data
<IMU/GPS
100
50
<
Raw data
50
300
250
200
150
100
EAST (m)
50
50
Figure 5.48: The result of the three dierent cases: the position of the inertial unit
with only raw data, the fused data with the GPS and the constrained data. The
attitude of the inertial unit with the raw data slowly drifts thus giving inaccurate
position results. The dierence between the position obtained by the Inertial/GPS
fusion and the proposed algorithm is too small to be clearly seen in this gure.
141
POSITION ERROR
North (m/s)
500
|
v
Constrained data
>
Raw data
500
1000
5000
10000
15000
East (m/s)
400
200
>
Raw data
0
200
|
0
5000
Constrained data
10000
15000
Down (m/s)
40
20
>
Raw data
Constrained data
|
|
v
0
20
5000
10000
15000
iteration
Figure 5.49: The position error of the constrained and raw data. The reference
position is from the fused Inertial/GPS navigation system. As illustrated, the error
in the raw data grow quadratically with time while the constrained data keeps the
error bounded.
VELOCITY ERROR
North (m/s)
10
| Constrained data
v
0
Raw data
>
10
20
5000
10000
15000
East (m/s)
10
5
Raw data
>
|
Constrained data
5000
10000
15000
Down (m/s)
2
1
|
v
Raw data
0
Constrained data
1
5000
10000
15000
iteration
Figure 5.50: The velocity error of the constrained and raw data. The reference
velocity is from the fused Inertial/GPS navigation system. The constrained data is
bounded well while the raw data grows linearly with time.
142
ROLL
<
Roll tilt sensor
Angle (deg)
0.5
IMU/GPS
|
1.5
Constrained data
|
Raw data
1.36
1.38
1.4
1.42
iteration
1.44
1.46
4
x 10
Figure 5.51: The roll data from the three dierent cases. The tilt information is presented for comparison. As can be seen the constrained data follows the Inertial/GPS
solution quite well. The raw data provides good results as well.
PITCH
2.5
2
< Pitch tilt sensor
1.5
Raw data
|
|
Angle (deg)
Constrained data
||
0.5
|
IMU/GPS
0.5
1.5
1.34
1.36
1.38
1.4
iteration
1.42
1.44
4
x 10
Figure 5.52: The pitch data from the three cases. The tilt information is presented
for comparison. The constrained data along with the Inertial/GPS navigation data
lie well within the results provided by the tilt sensor information. It is the error
in the raw data which predominately causes the error in the position and velocity
evaluation.
143
ROLL ERROR
0.2
0.25
0.3
Angle (deg)
0.35
0.4
IMU/GPS
v|
0.45
|
Constrained data
0.5
|
0.55
Raw data
0.6
0.65
1.39
1.395
1.4
1.405
1.41
1.415
iteration
1.42
1.425
1.43
1.435
1.44
4
x 10
Figure 5.53: Error in roll at the end of the trajectory for the three cases. The tilt
sensor information is used as the reference.
PITCH ERROR
1.2
1
< Raw data
Angle (deg)
0.8
0.6
Constrained data
|v
0.4
< IMU/GPS
0.2
0
1.36
1.37
1.38
1.39
1.4
iteration
1.41
1.42
1.43
1.44
4
x 10
Figure 5.54: Error in pitch at the end of the trajectory for the three cases. the tilt
sensor information is used as the reference. this plot enforces the benet of using the
constraint equations.
5.8 Multiple Aiding Plots
144

POSITION
300
250
NORTH (m)
200
150
100
50
50
100
100
200
300
EAST (m)
400
500
600
700
Figure 5.55: Position plot of the path taken by the vehicle. This path was obtained
by using the Inertial/GPS navigation system.
145
Position Error
North Error (m)
200
150
Free Inertial
100
50
| Constrained Inertial
0
20
40
60
80
100
Time (s)
120
140
160
180
200
180
200
500
East Error (m)
400
Free Inertial
300
200
100
0
20
40
60
80
100
Time (s)
120
140
160
Figure 5.56: These two plots show the error growth in position of the inertial unit free
of any external observations and when it is fused with the vehicle model constraints.
Velocity Error
3
North Error (m/s)
2.5
2
Free Inertial
1.5
1
0.5
0
20
40
60
80
100
Time (s)
120
140
160
180
200
East Error (m/s)
6
Free Inertial
20
40
60
80
100
Time (s)
120
140
160
180
200
Figure 5.57: Plots of error growth in velocity of the inertial unit when it is performing free of any external observations and when it is fused with the vehicle model
constraints.
146
Attitude Error
0.7
Roll Error (deg)
0.6
0.5
Free Inertial
0.4
0.3
0.2
0.1
0
20
40
60
80
100
Time (s)
120
140
160
180
200
0.7
Pitch Error (deg)
0.6
0.5
Free Inertial
0.4
0.3
0.2
0.1
0
20
40
60
80
100
Time (s)
120
140
160
180
200
Figure 5.58: Plots of attitude error. These errors cause the velocity and hence position
error growth shown in the previous plots. The constrained inertial data has been
bounded extremely well when compared to the free inertial data.
Position Error
6
North Error (m)
4
3
2
| Constrained + GPS
1
0
20
40
60
80
100
Time (s)
120
140
160
180
200
35
East Error (m)
30
25
20
15
10
| Constrained + GPS
5
0
20
40
60
80
100
Time (s)
120
140
160
180
200
Figure 5.59: Plots of position error of the inertial unit for two cases. The rst case
is with velocity observation, the second with velocity and GPS observations every 15
seconds. Since position is unobservable when only using the vehicle model constraints,
the GPS dramatically improves the result.
147
Velocity Error
1.4
North Error (m/s)
1.2
1
0.8
0.6
0.4
0.2
0
| Constrained + GPS
0
20
40
60
80
100
Time (s)
120
140
160
180
200
East Error (m/s)
1.5
0.5
0
20
40
60
80
100
Time (s)
120
140
160
| Constrained + GPS
180
200
Figure 5.60: Plots of velocity error show only slight improvement. This is because
velocity is observable when using the vehicle model and speed data, and so the extra
information from the GPS benets the velocity estimate only slightly.
Attitude Error
0.5
Roll Error (deg)
0.4
0.3
0.2
0.1
0
| Constrained + GPS
20
40
60
80
100
Time (s)
120
140
160
180
200
0.7
Pitch Error (deg)
0.6
0.5
0.4
0.3
0.2
0.1
0
| Constrained + GPS
0
20
40
60
80
100
Time (s)
120
140
160
180
200
Figure 5.61: The attitude estimation only benets slightly from the GPS information
since the velocity information provided by the vehicle model constraints delivers a
signicant amount of information to the estimation.
5.9 Conclusion
148
5.9 Conclusion
This chapter has presented experimental results on the implementation of the three
navigation systems developed in Chapter 4.
The Inertial/GPS navigation system developed provides results which are accurate,
reliable and robust. The navigation system is not only simple to install into any land
vehicle, it is also modular and hence allows for a wide range of GPS sensors to be
implemented without any change to the lter structure.
The results illustrate two key points:
Firstly, the navigation system corrects the errors in position, velocity and atti-
tude of the inertial navigation solution, thus providing accurate aided inertial
navigation data.
Secondly, the fault detection techniques implemented provided a navigation sys-
tem which has a level of robustness to failure. A number of examples were provided of multipath rejection. It was also shown that the lter could be tuned to
provide seemingly accurate results in position when there was no multipath rejection although, the innovations indicated that the lter behaves inconsistently.
Furthermore, if the inertial navigation system is being aided by incorrect GPS
observations, and these observation were to cease, then the inertial unit would
quickly drift since the attitude of the unit is incorrectly aligned.
The navigation system was implemented on two dierent vehicles and in dierent environments without any modication to the code or lter structure, since the process
model is not a vehicle model, thus proving its versatility.
The inertial unit has proven to be a very versatile dead reckoning sensor for land
vehicles. However, information from external sensors at frequent intervals is required
in order to prevent larger errors in position estimates due to the noise present in the
gyros and accelerometers, and this was clearly shown with the Inertial/GPS navigation system. An alternative algorithm was presented using vehicle model constraints
5.9 Conclusion
149
and it was shown that this dramatically extends the duration of time which an inertial unit can be used as a sole navigation sensor. Results comparing the navigation
system with an inertial navigation system aided with GPS observations, and with an
unaided inertial navigation system clearly indicated the eciency of aiding an inertial navigation system with vehicle model constraints. In the tests conducted it was
shown that the results of constraining the pitch of the inertial unit is comparable to
the results obtained from the inertial/GPS navigation system. The pitch evaluation
from unaided inertial navigation system drifts and is the predominate cause of error
in velocity and position. A disadvantage of this implementation is the unobservability
of the position and forward velocity of the vehicle under certain manoeuvres.
Finally, results were presented for an inertial navigation system being aided by
GPS, vehicle model constraints and speed information. By employing GPS with the
vehicle model constraints, the position estimate of the navigation system becomes observable. Furthermore, the aiding by vehicle model constraints allows periodic loss of
GPS signal without a signicant degradation in accuracy of the navigation solution.
The speed information in turn provides a means of ensuring that the forward velocity
of the vehicle is observable. This provides a navigation system which has both an
increase in reliability and integrity from the conventional aided inertial navigation
systems presented.
Chapter 6
Redundancy, Navigation Accuracy

and Autonomous Fault Detection
6.1 Introduction
This chapter will discuss the issue of redundancy for inertial measurement units. Although redundancy in satellite numbers is implemented widely in GNSS, redundancy
for low cost inertial navigation systems has never previously been developed. The
main focus of this chapter is not to provide the means of being able to detect faults
or to even judge the improvement in accuracy of a redundant inertial unit, since this
is a thesis topic in itself, but to provide the necessary theoretical foundation for the
development of such a system.
This chapter will look at how redundancy in inertial sensors aects navigation
accuracy and autonomous fault detection. The approach taken here draws on the
properties of the information lter, providing a means of comparing and fusing similar redundant data, and a better conceptual understanding of how accuracy and fault
detection are aected by dierent sensor congurations. This forms one of the main
contributions of this chapter.
A second contribution comes in the development of a redundant inertial measurement unit using four low cost accelerometers and four gyros [51]. Such a low cost unit
6.2 Discussion
151
has never previously been developed and it can be implemented for future fault detection work in sensor redundancy. Experimental results are provided when the inertial
unit is implemented on both a land and air vehicle. The design of the unit is to cater
for a Remotely Piloted Vehicle (RPV), however the unit can also be implemented on
land vehicles.
6.2 Discussion
It was stated in Chapter 1 that autonomous vehicles require a navigation system
which is not only reliable but also provides a high level of integrity. A navigation
system was proposed which comprised of two navigation loops [40]: an Inertial/GPS
navigation system presented in Chapters 4 and 5, and an Encoder/Radar navigation
system [12]. The two navigation systems are then fused in an information lter format
to provide both optimal and high integrity estimates of the state of the vehicle.
The primary purpose of implementing a dual navigation system is that redundancy
is employed; on a simplistic level, if the two navigation systems provide dierent
results then a fault has occurred. How the data from the two navigation systems
should be fused, and how to determine which is at fault is still the subject of current
research.
However, it is known that to further increase the integrity of the system as a
whole, each navigation loop should have its own independent and local means of fault
detection. This in turn minimises the probability of a fault being undetected in a
single navigation system and thus passed onto an arbitrator which decides which of
the navigation systems is at fault. The Inertial/GNSS navigation system developed
in this thesis demonstrated its integrity though rejection of high frequency faults in
GNSS caused by multipath, and through the minimisation of the low frequency faults
in the inertial unit caused by bias.
However, a third level of integrity is also required; at the individual sensor level.
Whatever faults can be detected on this level will minimise the probability of faults
moving on into the navigation system, and which may possibly go undetected at this
152
stage and move onto the next level of the dual navigation system. To increase integrity
at the sensor level, redundancy needs to be employed. Not only does redundancy allow
for the sensor to detect whether it is at fault (autonomous fault detection), it also
improves navigation accuracy since there is more information being provided by more
sensors.

Redundant inertial units have primarily been used for safety critical operations such
as in the control of military and space aircraft [29,59]. The aircraft is designed to
be dynamically unstable in order to increase maneouverability, and it is the inertial
acceleration and rotation rate data that is used to observe the vehicle parameters
that stabilise the vehicle. In such implementations it is important that redundancy is
employed. Furthermore, these aircraft generally require two inertial units, one for the
control of the vehicle and the other for navigation data. It is only recently that ghter
aircraft such as the Euroghter have implemented a single inertial unit for the dual
purpose of control and navigation [16]. In all cases however, the sensors employed,
and in particular the gyros, are of high performance. The accuracy of the gyroscopes
not only decides the accuracy of the unit as a navigation sensor but also the ability
to accurately and reliably detect faults.
One of the earliest references to redundancy in inertial units uses two sets of orthogonal triads skewed against one other [60]. One set is the main inertial suite while
the second is used to monitor the health of the rst. Development of subsequent redundant inertial suites over the years has revolved around the idea of voting schemes
for fault detection. Essentially, if two sensors are placed colinear to each other, then
by simply monitoring the output of both sensors it is possible to detect if a fault has
occurred in any one sensor. To isolate the fault, three sensors are required and thus
for a full three dimensional system, nine sensors are necessary (three on each axis). In
a worst case scenario, this allows only one isolation and a corresponding fault (known
as fail-operational, fail-safe), since once a sensor has failed and been isolated, then
153
only two functioning sensors remain on this axis, and hence the worst situation that
can occur is that a fault occurs on this axis in which case it can only be detected and
not isolated.
Only four sensors are required for fault detection, and ve for a fail-operational,
fail-safe system. Historically, the reason why nine sensors were employed is rstly for
the ease of fault detection (a simple voting scheme) and secondly because it was not
known how to optimally congure any number of sensors if the number was not a
multiple of three.
It was not until [45], that a theory of obtaining an optimal conguration for any
number of sensors was developed. The work considers the situation where any number of sensors are equally spaced on a cone of half-angle . The criterion employed is
to obtain the half-angle which in turn minimises the average statistical uncertainty
of the conguration. This average uncertainty is a function of the half-angle and the
uncertainty of each sensor. The result obtained is that a cone with half angle
1
= cos p
(6.1)
3
provides the optimal result for any number of sensors as long as the sensors each have
equal statistical uncertainty and are spaced equally around the cone. If however a
sensor is placed along the central cone axis, while the remaining sensors are placed
equally around the cone then the half angle becomes
1
3
(6.2)
3
where n is the number of sensors. Figures 6.1 and 6.2 illustrate the situation where
ve sensors are congured in these two dierent cases.

cos
n
3n
6.3.1 Information Space
This section will describe how the information matrix, Y, can be used to evaluate
the increase in information and the direction of maximum information with dier-
154
72 deg.
Figure 6.1: One of the optimal congurations for ve sensors. The cone's half angle
is 54:74 and the sensors are separated equally by 72.
90 deg.
Figure 6.2: Another optimal conguration is to place four sensors on the cone equally
separated by 90 while the fth sensor is placed on the cone axis. The cone's half
angle is now 65:91.
155
ent congurations of redundant inertial sensors, thus providing a better conceptual
understanding of how information is distributed. Although some of the following
equations have been provided in Chapter 2, they are repeated here for clarication.
In state space, the observation at time k is given by
z(k) = H(k)x(k) + v(k);
(6.3)
where x(k) is the current state, H(k) is the observation model and v(k) is the observation noise with covariance E[v(k)v(k)T ] = R(k). Although the number of states
remains xed for a given application, the size of the observation matrix varies depending on the number of sensors employed. For example, if acceleration measurements
are desired then the number of states is generally 3, one for each orthogonal axis.
However if seven accelerometers are used to measure these states then the observation model is a 3 7 matrix, thus providing seven observations, one for each sensor.
In information space, the information contribution of these sensors to states is given
by the information state vector
i(k) = HT (k)R (k)z (k)

1
(6.4)
where the variances on the measurements is provided in R(k). The information

matrix is given by
I(k) = HT (k)R (k)H(k):

1
(6.5)
and provides a measure of certainty in the observations. Note how this matrix is
independent of the observations, since there is no z(k) in the equation, but it provides
a measure of the certainty in these observations based purely on the geometry of the
sensors, H(k).
Since all the sensors employed are assumed to be exactly the same, that is, all the
accelerometers are of the same manufacture and likewise with the gyros, then without
loss in applicability, the covariance matrix R(k) is diagonal with equal variances, that
156
is,
0
0 0 0C
B
B
C
R(k) = BBB 0 .0. 0 CCC
0 . 0A
@0
0 0 0 n

(6.6)
where n is the number of sensors employed. Thus both the information vector and
information matrix become
i(k) = 1 HT (k)z(k)
(6.7)
(6.8)
I(k) = 1 HT (k)H(k)
2
Since the information matrix is positive semi-denite, its corresponding eigenvectors are orthogonal to one another. These eigenvectors represent the axes of an ellipsoid in information space. If the eigenvalues of these eigenvectors are dierent, then
the major axis of the ellipsoid will represent the direction of maximum information.
If they are all equal, then there is equal information in all directions. Furthermore,
equal eigenvalues will form a sphere, and a sphere is an ellipsoid with maximum
volume. Thus by maximising the volume of the ellipsoid not only will the greatest information be obtained but also equal information in all directions. The volume of the
ellipsoid is directly related to the determinant of the information matrix. Hence by
maximising the determinant for a given number of sensors the optimal conguration
for equal information in all directions and maximum information will be obtained.
Thus a sucient measure of information is
max[jI(k)j] = max[jH(k)T H(k)j]:
(6.9)
Note that is not required since it is simply a scaling factor.

In the two dimensional case, the optimal conguration for n identical sensors is
when they are equally spaced on a unit circle. That is, the conguration takes on the
1
157
shape of regular polygons.

In the three dimensional case there is no single solution to the problem since,
although there may only be one optimal conguration, the conguration can take
any orientation. However, for 4, 6, 8, 12 and 20 sensors, the corresponding optimal
sensor congurations are regular polyhedrons; the tetrahedron, cube, octahedron,
dodecahedron and the isocohedron respectively, thus verifying [45]. These solids are
perfectly symmetrical in that they look the same regardless of orientation. It is well
known that there are only ve such regular polyhedron shapes - the Platonic Solids.
This approach to analysing optimal sensor congurations is new and provides an
immediate visual understanding of the distribution of information. For the remaining
non-regular sensor congurations the results obtained were taken from [45]. From
these results it was found that regardless of the number of sensors, once an optimal
conguration was found, the eigenvalues equal the number of sensors divided by three
(the number of dimensions). That is,
for all
i
i
= n3
= 1:::n
(6.10)
This suggests that, in information space, each sensor contributes a third of its information to each eigenvector. Thus four sensors corresponds to a one third increase in
information and with six sensors the amount of information doubles, which is easily
seen when analysing the cubic conguration and comparing it to the standard orthogonal conguration. Furthermore, when the conguration is optimal, the determinant
is equal to the product of the eigenvalues, that is,
(6.11)
= ( n3 ) :
thus the gain in information by adding in an extra sensor can be easily evaluated.
D
158
6.3.2 Directed Information
An even more powerful result can be obtained when the two techniques are combined.
That is, using the information form to study the cone conguration. For example,
consider the situation where there are four sensors of equal variance placed equally
around a cone of half angle . The Information matrix becomes
I(k) = H
(
k)T H(k)
0
2 cos
B
= B
0
@
0
0
2 cos
0
0
0
4 sin
1
C
C
A
(6.12)
The determinant, representing the volume of information, is

J
= det(I(k))
= 16 cos () sin ():
4
(6.13)
The maximum determinant is obtained by dierentiating and setting to zero,

p
2
= tan
2
o
= 54:74 ;
(6.14)
1
which is the result for maximum total information. The advantage of this method
is that the optimum cone angle can be determined when directed information is
required. For example, if three times as much information is required along the z axis
as compared to the x and y axes, then by equating and solving
3 I ; (k) = I ; (k);
2 2
3 3

a solution for the cone angle of

159
= tan 26
= 39:23o:
1
(6.15)
is obtained. That is, the cone angle has lessened to allow for the greater amount of
information required along the z axis. Another typical example may be the requirement for greater information along the yaw axis of the land vehicle, or along the spin
axis of a missile.
This provides a new method for quantifying such situations. It thus allows the
design engineer to congure the inertial sensors such that it provides the required
information along the desired axes. This may be needed when other external sensors
provide unequal information along particular axes. For example, it is well known
that the vertical accuracy of GPS is less than in the horizontal. Thus, the vertical
velocity and position xes are less accurate than the horizontal, and if attitude GPS
is employed, roll and pitch is less accurate than heading. Thus to improve accuracy,
the accelerometers can be congured to provide better accuracy along the vertical
while the gyros can be used to obtain better roll and pitch data.
6.3.3 Unequal Noise Variances
If the variances of the sensors are not equal then a symmetric conguration cannot
be obtained, even though the amount of information may be greater due to some
sensors having smaller variances than others. Thus Equation 6.9 needs to include the
observation noise matrix R(k).
As an example, consider four sensors in a tetrahedron conguration. As the noise
variance is increased on one of the sensors, then the remaining three sensors recongure themselves so that they approach an orthogonal conguration (the optimal for
three sensors).
Situations requiring non-equal variances will arise when the acceleration or rotation rate along the three axes need to cover dramatically dierent ranges. Examples
160
include an air vehicle where accelerations along the z-axis and rotation rates about
the x-axis are typically an order of magnitude greater than those in the remaining
axes. Similarly, for land vehicle applications the acceleration along the x-axis and
the rotation rates about the z-axis are far greater than the remaining axes. Hence, a
requirement of the inertial system may be to have accelerometers and gyros of various
ranges or of various sensitivity.
6.3.4 Sensitivity and Resolution
As the sensitive axis of the sensor moves away from the principle platform axes, the
resolution and threshold properties projected onto the platform axes also change.
As an example, if an accelerometer is placed along a body axis then the minimum
acceleration that can be measured on that axis is the threshold of the accelerometer. If
the accelerometer is at an angle from this body axis then the minimum acceleration
which will be measured by the accelerometer is the threshold of the accelerometer
divided by the cosine of the angle subtended from this axis. Resolution has exactly the
same characteristics. This can be a problem when dealing with slow moving vehicles
(such as many large outdoor land vehicles) which encounter smaller accelerations and
rotation rates, and hence require sensors of greater sensitivity.
The region where acceleration can be measured may be represented as a cone where
the central axis of the cone lies on the body axis, and the apex of the cone is at the
origin. The cone angle will then depend on the threshold or resolution of the sensor
coupled with the dynamics of the vehicle. With the same cone placed on all body
axes the feasibility region will be formed. This is shown in Figure 6.3. The objective
is to make the cones as large as possible since this will maximise the area from which
the sensors can be placed in. Increasing the cone angle will obviously require sensors
of greater sensitivity and hence of greater cost.
6.4 Fault Detection
161
Z
Figure 6.3: The cones in this plot show the area dened as the feasibility region. If
a sensor's sensitive axis lies outside this region then the sensor cannot detect small
changes in the inertial properties of the vehicle. The objective is to get this region
as large as possible so that the conguration of the inertial suite does not need to be
altered.
6.4 Fault Detection

This section will provide background for the fault detection methods implemented in
GNSS and in redundant inertial units.
6.4.1 GNSS
GNSS systems are used in a number of applications, the most crucial civilian application being that for take o and landing of civilian aircraft. With the growing demand
on reliable GNSS systems, manufactures and government alike have proposed and
developed methods for increasing the integrity of such systems. One current method
is the Wide Area Augmentation Segment (WAAS) service developed by the DOD for
GPS. This system has a satellite in orbit (the monitoring satellite) that transmits to
the GPS receivers the health of all the GPS satellites. The ground monitoring station
keeps an eye on all the satellites and transmits the health status to the monitoring
satellite which then passes the information to the user. There are current plans by
the Japanese and Europeans to also provide such services. The Europeans also have
6.4 Fault Detection
162
a model of a similar navigation system to the GPS, named Galileo, prompted by the
desire of removing the control of the GPS system by the Americans. The Americans
on the other hand propose introducing a third signal (L5), which will provide a second civilian signal. All methods will improve the integrity of the system considerably.
However, for the time being the method adopted by manufacturers is to provide self
checking routines.
The self checking routines are known as \Receiver Autonomous Integrity Methods"
(RAIM). The main objective of RAIM is to detect if there is a fault in the navigation
solution, and if so which satellite caused the fault. The basic principle behind RAIM
is to compare redundant measurements at a single point in time. This comparison
takes three forms:
1. [31] proposes a method called the \Range Comparison" method and is the
simplest integration method of the three. Consider ve satellites in view. Only
four are required for determining the navigation solution. If the navigation
solution is solved using the rst four satellites then the range to the fth satellite
can be predicted. If the result ts within a certain threshold then the navigation
solution is deemed good. If a sixth satellite is available then the method can
isolate which satellite is causing the fault. The problem with this method is
determining an appropriate threshold.
2. The \Least-Squares" method rst proposed by [43]. If there are more than four
satellites in view then the navigation solution is developed using least-squares.
The resulting navigation solution is then passed back through the geometry
of the satellites to determine the range, known as the predicted range. The
dierence between the range as obtained by the satellites and the predicted
range forms the residual. The sum of the residuals forms a distribution,
assuming the noise on the range is zero mean Gaussian. Thus standard tests
can be performed to determine if there is a fault.
2
3. Finally the \parity space" method as proposed in [50] forms its detection routines in parity space which is orthogonal to the observation space and hence has
6.4 Fault Detection
163
no reliance on the observation itself. This is the most common method used
amongst GNSS manufactures, and is also widely used in inertial navigation
literature.
6.4.2 Inertial Navigation
There has been considerable work on the detection and isolation of faults in inertial units. These have ranged from standard voting schemes to maximum likelihood
strategies [61] to articial neural networks [27]. In skewed arrangements, the most
commonly used technique is the parity space method [19,24,36,50]. The method is
simple and popular. However, parity space methods have problems with singularities
caused by the relationship between the conguration of the sensors and the motion
of the vehicle [16]. Faults cannot be detected within these singularities.
The complexity with the implementation of fault detection techniques is further
increased when using low cost units since false alarms are more common due to the
poor performance of the sensors. This coupled with the fact that if a sensor has
failed, then the overall information distribution will be drastically altered, and will
be re ected through a reduction in total information as presented in Equation 6.9, as
well as the information distribution taking on a direction of maximum information.
As an example, consider the situation of having ve identical sensors equally spaced
around a cone of half angle 54:74, and four sensors placed equally around a cone of
half angle 65:91 with the fth sensor passing through the cone axis. The angles were
obtained using Equations 6.1 and 6.2. Both these congurations are optimal and are
shown in Figures 6.1 and 6.2, and have an information content value of 4:6296 as evaluated by Equation 6.9. If a sensor fails in either conguration, then the information
content will be less than that for ve sensors, 1:8519 (which is signicantly less than
the optimal conguration for four sensors, 2:3704). However, although the information content may be equal in these two congurations, the information distribution
is not. This is easily conrmed by removing the central cone sensor as the failed one
from Figure 6.2, and removing a sensor from the conguration represented by Figure
6.1, and visualising the nal conguration in both with respect to their information
6.5 Truncated Tetrahedron
164
distribution.
Hence fault detection and isolation, with the consideration of the resulting conguration's aect on navigation performance is of paramount importance.

The remaining part of this chapter will discuss the development of a low cost redundant inertial unit.
The primary objective is to develop a low cost inertial unit which provides an increase in navigation performance with the use of redundant sensors. It is required
to be fail-safe, hence only four of each of the sensors is required. The four sensors
can be congured in two dierent optimal congurations, namely the tetrahedron or
a half-octahedron. These two congurations are equivalent to placing three sensors
equally around a cone (120) with the fourth sensor through the cone axis with a
half angle of 70:5, or by placing all four sensors around a cone and equally separated
(90), with a cone angle of 54:74. Constraints are also placed on the weight and size
of the unit so that it can be available for weight critical projects such as in unmanned
air vehicles.
The gyros implemented were the British Aerospace Ceramic VSGs. The choice was
based on their low cost, small size, low power consumption and their light weight.
The accelerometers used are the QLC 400. These sensors, although the signal-to-noise
ratio is slightly lower than the piezo accelerometers used in the Watson IMU, have
extremely stable bias characteristics over a wide temperature range, thus providing
stability from which the attitude can be determined. This stability is due to the design and manufacture of these sensors. Tables 6.1 and 6.2 provide the specications
of the gyros and accelerometers respectively.
The sensors were congured on a truncated tetrahedron base, which could be easily
manufactured and made light weight. This design was chosen since it provides the
smallest volume given the size of the individual inertial sensors. The shape is the
same as that of a standard tetrahedron however the apexes are modied so that the
Range
Scale Factor
Temperature
Linearity
Temperature
Bias
Temperature
=s
mV= =s
165
No.63372 No.63363 No.50093 No.63352

500
200
200
200
+20
-30
+60
10.065
9.656
9.585
24.829
23.862
23.574
24.721
23.719
23.240
24.882
23.956
23.647
+20
-30
+60
0.050
-0.300
0.050
0.020
0.040
0.020
0.04
-0.82
-0.08
0.020
-0.060
0.040
+20
-30
+60
0.041
-1.226
0.147
- 0.070
-0.912
-0.893
-0.013
0.04
-0.08
0.028
1.241
0.441
0.003
0.018
0.008
0.005
=s
=s
Bias Repeatability =s
Temperature
+20
Table 6.1: Specications of the gyros implemented. The top row represents the model
numbers of the individual sensors.
BCAC064 BCAC083 BCAC07M BCAC076N

Range
up to 20g up to 20g up to 20g up to 20g
Scale Factor
mA=g
1.333
1.344
1.330
1.327
Bias
mg
+0.3
-0.8
+3.2
+3.3
Sensor Axis Misalignment mrad
1.39
0.68
1.65
1.65
Table 6.2: Specications of the accelerometers implemented. The top row represents
the model numbers of the individual sensors.
166
faces lie parallel to each other. In this way a gyro accelerometer pair are coplanar.
The shape was milled from a solid block of alluminium and hollowed out to conserve
weight, Figure 6.4. The hollowing out also allowed for a gyro to t inside the block
thus conserving space.
Figure 6.4: The truncated tetrahedron showing the faces and how it is hollowed out
to conserve weight. Each large face, holding a gyro, is parallel to a smaller face,
holding an accelerometer, thus allowing an accelerometer gyro pair to be coplanar.
Bias on these gyros is strongly correlated to the temperature. Although temperature compensation can be used, the switch-on to switch-on variability is too great
to be of any benet. The more appropriate method is to heat the unit to a xed
temperature. Attached to the block are three heating devices which raise the temperature of the block. Each inertial sensor has a temperature sensor attached to it
so that an average temperature reading can be performed of the total block. A local
microprocessor then uses all the temperature sensors to control the temperature of
the block via a PI controller. Thus by xing the temperature it places the biases
within tight limits and minimises the reliance on temperature compensation. An 8
bit ADC reads in the temperature sensors at the same rate as the sampling of the
inertial sensors.
The inertial unit incorporates a 16 bit ADC which samples the gyroscopes and
accelerometers at 200Hz-400Hz. Given that the bandwidth of the gyros is at ap-
167
proximately 70Hz the sampling rate is sucient to encapsulate the total information
without aliasing. The only point not addressed here is the vibration mounts which
need to suppress any vibrations in excess of 70Hz which can in turn cause coning
motion or poor performance of the attitude algorithms. This however, is dependent
on the characteristics of the vehicle. One would like to obtain as high a resolution on
the ADC, especially for land vehicle applications where the dynamics are quiet low.
However, it is the cost and complexity of increasing the resolution of the ADC which
limits the resolution for low cost inertial units.
The physical characteristic of the inertial unit is given in Table 6.3.
Inertial Unit
Specication
Weight
2 kg
Dimensions
20 20 15 cm
Power Consumption
12 W
Table 6.3: Physical characteristics of the redundant inertial unit.
Figure 6.5 shows a top view of the inertial unit. The sensor block was placed on
a coordinate measurement machine to determine the misalignment of the faces. The
theoretical and actual angles of the faces are provided in Table 6.4, all angles are in
degrees and represent the horizontal and vertical angles required to place a face in
3D space.
face Theoretical - Horiz. Actual - Horiz. Theoretical - Vert. Actual - Vert.
1
0
0
0
0
2
60
59.94
289.47
289.498
3
120
120.02
109.47
109.539
4
180
179.93
180
179.986
5
240
239.898
109.47
109.951
6
300
299.98
289.47
289.438
7
0
0
70.53
70.555
8
0
0
250.53
250.46
Table 6.4: The theoretical and actual face angles of the redundant inertial unit.
The misalignment of the sensors' axes along with the scale factor values need to be
6.6 Results
168
obtained. Given the misalignment of the faces, the inertial unit can then be placed
on a rate table to determine the misalignment of the sensors' axes. The scale factors
are dependent on the temperature and so the temperature should be xed at the
temperature required for the particular application.
Figure 6.5: The inertial unit from the top. The electronics consist of the ADC and
the DAC along with serial communications. The processor is also housed with the
inertial unit and shown on the left. The black boxes are the gyros and the silver
cylinders are the accelerometers.
6.6 Results
This section provides limited results to show that Tetrad functions according to specications. As stated in the introduction, the issue of autonomous fault detection and
navigation accuracy with relation to redundant inertial units is a thesis topic in itself,
and is left for future work.
6.6.1 Ground Tests
The Tetrad (as the unit is now known as) was placed on the back of a utility vehicle
along with the Watson IMU, and driven around in two loops.
6.6 Results
169
Figure 6.6 illustrates the acceleration encountered along the body x-axis of the
vehicle as indicated by both the Tetrad and the Watson. It should be noted that the
acceleration as indicated by the Tetrad was resolved from the four accelerometers.
Similarly, Figure 6.7 represents the rotation rate about the x-axis as indicated by the
two inertial units.
There are two points that are noticed from these plots:
Firstly, the Tetrad closely resembles the Watson data, thus indicating that it
functions appropriately; and
Secondly, there is a lag associated with the Watson IMU. This is due to an
internal 20Hz analog lter which is implemented by the Watson IMU. This is
also apparent from the fact that the Watson data is smoother than the Tetrad.
Figure 6.8 shows the position plot of the vehicle as indicated by the Novatel RT2
(2cm) GPS receiver and both inertial units. The Tetrad drifts slightly more than its
counterpart.
6.6.2 Airborne Tests
Figures 6.9 and 6.10 show 2D and 3D plots of the Tetrad position data (dark line)
when own in a Remotely Piloted Vehicle (RPV), Figure 6.11. The test lasted for
approximately 10min however only the rst 120s are considered here. The axes are
represented by North, East and Down. The light coloured line is the GPS data and
is used as a reference.
Figure 6.12 illustrates the velocity as presented by the Tetrad data. Note the drift
in velocity as time progresses, which when mathematically integrated causes the drift
in position.
Figure 6.13 shows the position data when the Tetrad is aided with GPS. The
fusion algorithm implemented is a linear information lter which incorporates an
inertial error model to obtain estimates of the errors in position, velocity and attitude.
These estimates are then fed back to the inertial unit to correct it. The navigation
6.6 Results
170
architecture is exactly the same as that developed and tested in Chapters 4 and 5,
thus again proving its versatility.
Figures 6.14 to 6.16 show the roll, pitch and heading of the vehicle when the Tetrad
is aided by GPS. As illustrated in Figure 6.16, there is a dierence between the Tetrad
and GPS data. This is due to a matter of interpretation; the heading data as provided
by the GPS system is determined by the velocity information, while that of the Tetrad
is provided by the integration of the gyro data. During the course of this test there
was excessive sideslip occurring and thus the GPS heading data will be dierent from
that as provided by the Tetrad data. An example of sideslip during this test is shown
in Figure 6.17. This does not aect the lter since it is not the GPS heading data that
is fused with the Tetrad heading data, but the velocity vectors from both systems,
from which the heading of the Tetrad data is corrected via the state model in the
lter.
Figures 6.18 and 6.19 illustrate the position data of the vehicle for the same test
however the Tetrad data is aided by GPS for only 60s. Once the aiding stops the
Tetrad data slowly begins to drift, however due to the alignment of the Tetrad by
the GPS during the initial 60s of the ight, the drift is signicantly less. This is also
illustrated in the velocity data, Figure 6.20.
Based on these results, the Tetrad IMU can be implemented as a low cost unit
providing inertial navigation data, and furthermore, since it is a redundant inertial
system, it also has the option of autonomous fault detection.
6.6 Results
171
Figure 6.6: A comparison of the acceleration along the x-axis as indicated by both
the Tetrad and Watson inertial unit.
Figure 6.7: A comparison of the rotation rate along the x-axis as indicated by both
the Tetrad and Watson inertial unit.
6.6 Results
172
Figure 6.8: Position of the vehicle as indicated by the Tetrad and Watson. The circles
represent the position as indicated by the GPS receiver.
6.6 Results
173
Figure 6.9: Raw position as provided by the Tetrad (dark line) and GPS (light line)
for a ight test. The test lasted for approximately 10min however only the rst 120s
are considered here. The GPS data is used as a reference.
Figure 6.10: The same position run however illustrating all three axes.
6.6 Results
174
Figure 6.11: The RPV implemented in this ight test.
Figure 6.12: The raw velocity data as presented by the Tetrad (dark line) and GPS
(light line). Note the drift in velocity along all three axes, which once integrated,
causes the drift in position as indicated in the previous gures.
6.6 Results
175
Figure 6.13: The position information from the Tetrad once fused with GPS. A linear
error model is implemented in an information lter format.
Figure 6.14: The roll angle of the vehicle as provided by the aided Tetrad data.
6.6 Results
176
Figure 6.15: The pitch angle of the vehicle as provided by the aided Tetrad data.
Figure 6.16: The heading angle of the vehicle as provided by the aided Tetrad data.
Note the marked dierence between the Tetrad and GPS data. This is due to interpretation; the GPS heading data is provided by GPS velocity while the Tetrad
heading data is provided by the integration of the gyros. Thus during sideslip the
two results will be dierent. The lter is not aected by this since it fuses the velocity
vectors provided by the two systems and not the heading data.
6.6 Results
177
Figure 6.17: Illustrates an example of sideslip occurring during a portion of the run.
It is this sideslip which causes the dierence between the heading data provided by
the Tetrad and GPS.
Figure 6.18: Position of the vehicle as indicated by the Tetrad unit when it has only
been aided by the GPS for 60s. Note that drift still occurs however the unit has
been aligned during the fused portion of its ight and hence the drift is less then that
provided by the raw Tetrad data.
6.6 Results
178
Figure 6.19: Position as provided by the Tetrad data along the three axis when it has
been aided by GPS for only 60s.
Figure 6.20: Velocity as indicated by the Tetrad data when aided by GPS for only
60s. Note that the drift is less compared to when there was no aiding at all. This is
due to the alignment which occurred during the initial 60s of the ight.
6.7 Conclusion
179
6.7 Conclusion
The Information approach to determining the optimal conguration of any number of
sensors, not only veries previous work, but also adds to it by allowing the visual interpretation of how the information is spread. This is benecial for the determination
of the correct conguration to implement once factors such as resolution, sensitivity
and the resulting conguration after a fault has been isolated, are accounted for.
Furthermore, unlike previous work, the information approach does not look at the
average uncertainty (or certainty) in the statistical sense, but instead determines the
total information incorporated in the conguration, thus providing a complete denition.
Combining the information structure with the cone conguration, the theory also
provides for a method of obtaining a conguration with directed information. It was
also shown that by adding an extra sensor and nding the optimal conguration results in a 33% increase in information. Furthermore, given the optimum conguration
then the amount of information represented by the determinant is simply the product
of the eigenvalues of the information matrix, which is the number of sensors over the
dimension of the platform axes, cubed.
The electronic implementation of the inertial unit allows for temperature stabilisation and sampling of the inertial sensors at a rate which encapsulates the required
information without aliasing.
Finally, results were provided to illustrate that the Tetrad can be used as an inertial
measurement unit, and become the sensor for future fault detection work.
Chapter 7
Contributions, Conclusion and

Future Work
Although there are many avenues that can be progressed from this thesis, those that
are of most concern relate to the increase in integrity when using low cost inertial
units. The increase in integrity can result from the use of redundancy or through
the increase in accuracy of the inertial sensors. The increase in accuracy is of most
concern since it not only relates to navigation performance but also to improve fault
detection for external sensors such as GNSS. An increase in accuracy can be obtained
through the development of better sensors, or the correction of the errors in those
sensors through the use of external information, or through the understanding of the
physical source of the inertial errors. Each chapter in this thesis is an element of these
future developments.
Chapter 3 was concerned with the development of inertial equations required for
the navigation of a land vehicle. To understand the eect of error propagation, the inertial equations were perturbed (linearised). The perturbation of
the \Earth" frame is not explicitly found in literature, since the application of
inertial navigation systems has always been predominately concerned with ight
vehicles and missiles which deal in the Transport or Wander Azimuth frames.
It was shown that algorithmic errors are of little concern when developing low
Contributions, Conclusion and Future Work
181
cost inertial units on land vehicles, since the sampling rate is signicantly higher
than the dynamics of the vehicle.
This chapter also provided error equations for both the accelerometer and gyro
sensors. A comparison of these errors amongst various gyros was also given and
it was demonstrated why low cost inertial sensors are generally not used for
navigation purposes. Temperature is a major contributor towards the uctuation of these errors, and hence temperature compensation is the ideal solution.
This requires extensive laboratory work, however dramatic improvements are
likely. Fixing the temperature of the inertial unit is another solution. Although
the noise on low cost inertial sensors is larger than higher grade sensors, error
contributions also arise from low frequency faults associated with bias, misalignment and scale factor errors; rectication of these errors will improve the
navigation results.
The main contributions of this chapter are
Providing a detailed understanding of the errors eecting land navigation
when implementing low cost inertial sensors, and

Developing the inertial error equations needed for land vehicle applications.
Chapters 4 and 5 detail the results of three aided inertial navigation systems de-
veloped in this thesis.

First, the Inertial/GNSS navigation system was described. This forms the core
component of the thesis. Of prime concern was the detection and compensation
of low and high frequency faults. Low frequency faults are related to the bias
in inertial sensors, and were removed through the alignment and calibration of
the inertial unit. Tilt sensors were required for the alignment since the uncertainty in information from the sensors themselves was too great to provide any
meaningful alignment information. High frequency faults were associated with
multipath, that is, the increase in travel time of the GNSS signal due to the
re ection from surrounding structures. Of equal importance is the blockage of
the signal, resulting in loss of position and velocity information. The combination of these two sensors along with the fault detection techniques proved to be
182
a successful partnership.
The structure of the real time implementation of the navigation loop was also
described.
This navigation loop was implemented in a loosely-coupled structure, that is,
where the GNSS receiver is used as a navigation aiding system providing position and velocity. The tightly-coupled structure, although superior in terms of
manageability, can only be of relevance if the internal structure of the GNSS
receiver can be controlled. At the time of writing this seemed an impossible situation for land vehicle applications, since the cost of doing this was far too great.
However, the growing need of autonomous land vehicles has led many GNSS
companies to address automation issues more closely. The tightly-coupled approach would be ideal for land vehicle applications since a single satellite could
provide valuable aiding information. Such a structure would be benecial in
situations such as mining where signicant portions of the sky are blocked due
to highwalls, or in standard city driving where buildings do the same.
Secondly, an innovative means of bounding the error growth of inertial errors
was provided using vehicle modelling. The predominant use of inertial units
has been in \black box" form. However, it was shown that the use of a simple
vehicle model can provide valuable observation constraint information. This is
a very powerful concept since the accuracy of the navigation solution of a low
cost inertial unit can be improved to become comparable to inertial units of
higher grades. Not only is navigation accuracy improved, but fault detection
improves as well. Through simulation, it was shown that relevant degrees of
freedom must be excited in order for certain states to become observable.
The approach taken in this chapter was to assume a no-slip situation, which
is practically satised by a number of land vehicle applications. Thus further
work needs to look at appropriate vehicle modelling which can be applied to
constrain the errors of the inertial unit in cases where these constraints are not
satised. The application of vehicle modelling to the GNSS solution will in turn
increase the navigation performance and fault detection capability.
Finally, it was discussed that if the inertial unit could be aided by vehicle mod-
183
elling or GNSS then both pieces of information would increase both the overall
navigation performance and the accuracy of the inertial unit, thus increasing
integrity in high frequency fault detection and also providing accurate inertial
navigation during signal blockage. It was shown that through the use of the
vehicle modelling, that the inertial unit could sustain accurate navigation performance for extended periods of time without GNSS. Furthermore, by using
the information lter, multiple observations can be applied with less computational expense then when using a Kalman lter. This was shown with the
combined use of GPS position and velocity data, vehicle modelling and velocity
from a speed encoder. The velocity data provided by the speed encoder ensured that the forward velocity of the vehicle was observable, unlike the case
when only vehicle modelling was used. Position is unobservable when modelling
and speed data are used alone, hence GNSS provides the necessary positional
information. By comparison it was shown that with all pieces of information
navigation accuracy was far better.
The use of additional sensors means more information for estimation of the inertial errors. Thus an analysis should be undertaken to determine the distribution
of information in the estimation procedure with both the standard error model
and with the extension of this model to include scale factor and misalignments
terms.
The main contributions of these two chapters are:
The development of an inertial navigation system aided by GNSS and the
understanding of the limitations in accuracy and information available to

aid the inertial system.
The determination of low frequency faults associated with inertial sensors
and high frequency faults associated with GNSS, the techniques needed to
detect them, and the algorithms implemented to remove them.
The development of a real time inertial navigation system aided by GNSS.
The navigation system provides the navigation data needed to control a
65 tonne container handling vehicle.
184
Experimental results on two types of vehicles showing the versatility of
this navigation system. The results more importantly illustrated the fault
detection capabilities of the navigation system.
The development of an inertial navigation system being aided by vehicle
modelling. This implementation requires no external sensors to aid the
inertial unit.
An understanding of the limitations of vehicle modelling to aid inertial
navigation systems.
Simulation and experimental results of this navigation system showing the
potential of this algorithm for land navigation.
The development of an inertial navigation system aided by GNSS, vehicle
modelling, and speed provided by a wheel encoder. The lter architecture
is in an information lter format. The multiple aiding allows for better
fault detection and better accuracy of the inertial navigation solution.
Experimental results of this navigation system showing the improvement in
accuracy of using the various observation sources, and how the navigation
system can operate for longer periods of time without GNSS.
Chapter 6 describes a novel approach to the issue of integrity. Although the use
of redundancy in inertial units is not a new idea, its application towards low
cost inertial sensors for navigation purposes is. Furthermore this chapter also
provided a means of determining the optimal conguration of redundant inertial components and also evaluating the appropriate conguration for directed
information, through the use of the information lter. The development of
this theory provides a means of comparing and fusing similar redundant data,
and a better conceptual understanding of how accuracy and fault detection are
aected by dierent sensor congurations. The culmination of the previous
chapters coupled with the theory developed in this chapter led to the development of a low cost inertial unit. The unit was tested on both a land and
air vehicle and shows promising results as a low cost inertial measurement unit
185
and in turn provides the foundation for work on fault detection and navigation
using such units. This includes:
The characterisation of the unit and its navigation performance.
The autonomous detection of faults.
The incorporation of the unit with GNSS in a tightly-coupled structure.
This will result in greater integrity and will also provide the necessary
information to isolate any faults in this inertial unit.
A complete high integrity system may implement multiple inertial units on
a single platform. By placing multiple inertial units around the platform
such that their information ensemble is identical to a single redundant inertial unit, then one not only encapsulates the total movement of the vehicle,
but also ensures that no single node controls the complete navigation architecture, thus increasing the integrity of the navigation suite. For example,
12 sensors congured on a dodecahedron form the optimal conguration.
The sensors can be grouped in sets of three and placed around the vehicle.
Thus, each set can provide its own navigation solution, and as long as the
orientation remains the same as it was in the original conguration, then
their total information will be optimal.
Finally, a more ambitious scheme would be to develop an inertial unit
that is recongurable in real time. For example, by placing the inertial
sensors equally spaced around a cone where the half angle of the cone is
adjustable, then the inertial unit can go from being optimally congured
to a conguration with directed information. Deciding on the cone angle
will require information from the vehicle model, external sensors and the
vehicle motion.
The main contributions associated with this chapter are:
Unifying the theory developed in [45] with the information lter to form an al-
gorithm that will provide a means of directed information for redundant inertial
sensors.
186
The development of a low cost, redundant inertial measurement unit imple-
menting four accelerometers and four gyros.
Experimental testing of this inertial unit showing that the unit functions ac-
cordingly and hence can be implemented for future fault detection work.
Inertial sensing has provided a means of navigation for a number of years within
the military, space and civilian aircraft industries. As the cost of these inertial units
has decreased, their popularity amongst the entire civilian sector has grown. Along
with the cost reduction of inertial units however, there has been a decline in sensor
accuracy. However, through the use of ltering techniques, external aiding and/or
redundancy, inertial navigation can provide a low cost, high integrity, accurate means
of navigation.
Bibliography
[1] I.Y. Bar-Itzhack. Identity Between INS Position and Velocity Error Models. In
, pages 568{570, September 1981.
[2] I.Y. Bar-Itzhack and D. Goshen-Meskin. Identity Between INS Position and
Velocity Error Equations in the True Frame. In
,
pages 590{592, November 1988.
[3] Y. Bar-Shalom and X. Li.
. Artech House, Inc, 1993.
[4] D.O. Benson. A Comparison of Two Approaches to Pure-Inertial and DopplerInertial Error Analysis. In
, pages 447{455, July 1975.
[5] J. Borenstein. Experimental Evaluation of Fibre Optic Gyroscopes for Improving
Dead Reckoning Accuracy in Mobile Robots. In
, pages 3456{3461, May 1998.
[6] J. Borenstein, H.R. Everett, L. Feng, and D. Wehe. Mobile Robot Positioning
Sensors and Techniques. In
, volume 14, No.4, pages
231{249, 1997.
[7] K. Braden, C. Browning, H. Gelderloos, F. Smith, C. Marttila, and L. Vallot.
Integrated Inertial Navigation System/Global Position System (INS/GPS) for
Manned Return Vehicle Autoland Application. In
, pages 74{82, 1990.
[8] K.R. Britting.
. John Wiley and Sons, Inc,
1971.
[9] L. Camberlein and B. Capit. ULISS G, A Fully Integrated "All-In-One" and
"All-In-View" Inertia-GPS Unit. In
, pages 399{406, 1990.
[10] A.B. Chateld.
. American
Institute of Aeronautics and Astronautics, Inc, 1997.
Journal of Guidance and Control
Journal of Guidance and Control
Estimation and Tracking - Principles, Techniques and
Software
IEEE Transactions on Aerospace and Electronic Sys-
tems
IEEE International Conference
on Robotics and Automation
Journal of Robotic Systems
IEEE Position, Location and
Navigation Symposium
Inertial Navigation System Analysis
IEEE Position, Location and Navigation
Symposium
Fundamentals of High Accuracy Inertial Guidance
BIBLIOGRAPHY
188
[11] R. Chatila, S. Lacroix, S. Betge-Brezetz, M. Devy, and T. Simeon. Autonomous

Mobile Robot Navigation for Planet Exploration - The EDEN Project. In
, 1996.
[12] S. Clark.
.
PhD thesis, Department of Mechanical and Mechatronic Engineering, The University of Sydney, 1999.
[13] S. Cooper.
. PhD thesis, Department of Engineering Science, University of Oxford, 1996.
[14] B.S. Davis. Using Low-Cost MEMS Accelerometers and Gyroscopes as Strapdown IMUs on Rolling Projectiles. In
, pages 594{601, 1998.
[15] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durrant-Whyte. A New Algorithm for the Alignment of Inertial Measurement Units without External Observation for Land Vehicle Applications. In
, volume 3, pages 2274{2279, May 1999.
[16] R.E. Ebner and J.G. Mark. Redundant Integrated Flight-Control/Navigation
Inertial Sensor Complex. In
, volume 1, pages
143{149, 1978.
[17] G. Fetzer, W. Golderer, and J. Gerstenmeier. Yaw Rate Sensor in Silicon Micromachining Technology for Automotive Applications. In
, pages 8.0{8.8, 1998.
[18] D.J. Flynn. A Discussion of Coning Errors Exhibited by Inertial Navigation
Systems. In
,
1984.
[19] J.P. Gilmore and R.A. McKern. A Redundant Strapdown Inertial Reference Unit
(SIRU). In
, volume 9, pages 39{47, January
1972.
[20] G. Giralt, L. Boissier, and L. Marechal. The Iares Project: Rovers for the Human
Conquest of the Moon and Mars. In
, 1996.
[21] R.L. Greenspan. Gps and inertial navigation. In
, volume 2, chapter 7. American Institute of Aeronautics
and Astronautics, Inc, 1996.
IEEE
International Conference on Robotics and Automation - Planetary Rover Technology and Systems Workshop
Autonomous Land Vehicle Navigation using Millimetre Wave Radar
A Frequency Response Method for Sensor Suite Selection with an
Application to High-Speed Vehicle Navigation
Symposium
IEEE International Conference on
Robotics and Automation
Journal Guidance and Control
Symposium of Gyro
Technology
Royal Aircraft Establishment. Technical Memorandum Rad-Nav 243
Journal of Spacecraft and Rockets
IEEE International Conference on Robotics
and Automation - Planetary Rover Technology and Systems Workshop
Global Positioning System:
Theory and Applications
BIBLIOGRAPHY
189
[22] M.S. Grewal and A.P. Andrews.

.
Prentice-Hall, Inc, 1993.
[23] G. Hyslop, D. Gerth, and J. Kraemer. GPS/INS Integration on the Stando Land
Attack Missile (SLAM). In
,
pages 407{412, 1990.
[24] M. Jeerage and K. Boettcher. Fault Tolerant Highly Reliable Inertial Navigation
System. In
, pages 456{460,
1986.
[25] M. Koifman and I.Y. Bar-Itzhack. Inertial Navigation System Aided by Aircraft
Dynamics. In
, pages 487{493,
July 1999.
[26] X. Kong. Development of a Non-Linear Psi-Angle Model for Large Misalignment Errors and its Application in INS Alignment and Calibration. In
, pages 1430{1435, May
1999.
[27] U. Krogmann. Articial Neural Networks for Inertial Sensor Fault Diagnosis. In
, pages 50{59, 1995.
[28] E. Krotkov, R. Simmons, F. Cozman, and S. Koenig. Safeguarded Teleoperation
for Lunar Rovers: From Human Factors to Field Trials. In
, 1996.
[29] W.J. Kubat. Application of Strapdown Inertial Navigation to High Performance
Aircraft. In
, pages
7.1{7.16, 1978.
[30] A. Lawrence.
. Springer Verlag, 1993.
[31] Y.C. Lee. Analysis of Range and Position Comparison Methods as a Means
to Provide GPS Integrity in the User Receiver. In
, pages 1{4, June 1986.
[32] A. Leick.
. John Wiley and Sons, Inc, 1995.
[33] A.H. Lewantowicz. Architectures and GPS/INS Integration: Impact on Mission
Accomplishment. In
, pages
284{289, 1992.
[34] P. Maybeck.
, volume 1,2. Academic
Press, 1982.
Kalman Filtering - Theory and Practice
IEEE Position, Location and Navigation Symposium
IEEE Transactions on Control Systems Technology
IEEE
International Conference on Robotics and Automation
Symposium of Gyro Technology
IEEE International
Conference on Robotics and Automation - Planetary Rover Technology and Systems Workshop
AGARD Lecture Series No.95, Strap-Down Inertial Systems
Modern Inertial Technology
Proceedings of the Annual
Meeting of the Institute if Navigation

GPS Satellite Surveying
Stochastic Models, Estimation and Control
BIBLIOGRAPHY
190
[35] P. Maybeck.
, volume 1. Academic
Press, 1982.
[36] A. Medvedev. Fault Detection and Isolation by a Continuous Space Method. In
, volume 31, pages 1039{1044, 1995.
[37] J. Meyer-Hilberg and T. Jacob. High Accuracy Navigation and Landing System
Using GPS/IMU System Integration. In
, pages 298{305, 1994.
[38] G. Minkler and J. Minkler.
.
Magellan Book Company, 1990.
[39] A.G.O Mutambara.
. PhD thesis, Department of Engineering Science, University of
Oxford, 1994.
[40] E. Nebot, M. Bozorg, and H. Durrant-Whyte. Decentralised Architecture for
Asynchronous Sensors. In
, volume 6, pages 147{
164, May 1999.
[41] E.M. Nebot, S. Sukkarieh, and H. Durrant-Whyte. Inertial Navigation Aided
with GPS Information. In
, pages 169{174, September 1997.
[42] E.M. Nebot, S. Sukkarieh, and H. Durrant-Whyte. Inertial Navigation Applied
to a High Speed Vehicle. In
, pages 61{66, October 1997.
[43] B.W. Parkinson and P. Axelrad. Autonomous GPS Integrity Monitoring Using
Pseudorange Residual. In
, volume 35, pages 255{274, 1988.
[44] B.W. Parkinson and Jr. J.J. Spiker.
. American Institute of Aeronautics and Astronautics, Inc, 1996.
[45] A.J. Pejsa. Optimum Skewed Redundant Inertial Navigators. In
,
volume 12, pages 899{902, July 1974.
[46] G.R. Jnr. Pittman.
. John Wiley and Sons, Inc, 1962.
[47] R.M. Rogers. Integrated INU/DGPS for Autonomous Vehicle Navigation. In
, pages 471{476, 1996.
[48] S. Scheding.
. PhD thesis, Department of Mechanical
and Mechatronic Engineering, The University of Sydney, 1997.
[49] W. Sohne, O. Heinze, and E. Groten. Integrated INS/GPS System for High
Precision Navigation Applications. In
, pages 310{313, 1994.
Stochastic Models, Estimation and Control
Automatica
Symposium
Aerospace Coordinate Systems and Transformations
Decentralised Estimation and Control with Applications to
a Modular Robot
Journal of Autonomous Robots
M2VIP-97 Mechatronic and Machine Vision in Prac-
tice
CONTROL-97
Navigation
Global Positioning System:
Theory and
Applications
AIAA Journal
Inertial Guidance

High Integrity Navigation
Symposium
BIBLIOGRAPHY
191
[50] M.A. Sturza. Navigation System Integrity Monitoring Using Redundant Measurements. In
, volume 35, pages 483{501, 1988.
[51] S. Sukkarieh, P. Gibbens, and H. Durrant-Whyte. The Development of a Low
Cost, Strapdown Inertial Unit with Redundant Sensors. In
, pages 121{126, August 1999.
[52] S. Sukkarieh, E.M. Nebot, and H. Durrant-Whyte. The GPS Aiding of INS
for Land Vehicle Navigation. In
, pages 278{285,
December 1997.
[53] S. Sukkarieh, E.M. Nebot, and H. Durrant-Whyte. Achieving Integrity in an
INS/GPS Navigation Loop for Autonomous Land Vehicle Applications. In
, pages 3437{3442, May
1998.
[54] S. Sukkarieh, E.M. Nebot, and H. Durrant-Whyte. A High Integrity IMU/GPS
Navigation Loop for Autonomous Land Vehicle Applications. In
, volume 15, pages 572{578, June 1999.
[55] S. Sukkarieh, E.M. Nebot, and H. Durrant-Whyte. A High Integrity Navigation
Loop for Large Autonomous Land Vehicles. In
, July 1999.
[56] S. Sukkarieh, G. Nebot, E.M. Dissanayake, and H. Durrant-Whyte. The Aiding of
a Low Cost, Strapdown Inertial Unit for Land Vehicle Applications using GPS,
Speed Encoder Data and Vehicle Modeling Constraints all in an Information
Filter Framework. In
, pages 395{400, August
1999.
[57] D.H. Titterton and J.L. Weston.
.
Peter Peregrinus Ltd, 1997.
[58] R. Volpe, J. Balaram, T. Ohm, and R. Ivlev. The Rocky 7 Mars Rover Prototype. In
, 1996.
[59] T. Weber. A Unique Solution to Provide A High Integrity Inertial Measurement
Unit (IMU) for the EF 2000 Flight Control System. In
, 1996.
[60] R. Weiss and I. Nathan. An Ultrareliable Sensing System for Vehicles with
Limited Sparing Capability. In
, volume 4,
pages 1151{1158, September 1967.
Navigation
FSR99 Field Service
Robotics
FSR97 Field Service Robotics
IEEE
International Conference on Robotics and Automation
IEEE Transac-
tions
The 4th International Symposium
on Satellite Navigation Technology and Applications - Session 7
FSR99 Field Service Robotics
Strapdown Inertial Navigation Technology
IEEE International Conference on Robotics and Automation - Planetary
Rover Technology and Systems Workshop
Avionics Conference and
Exhibition
Journal of Spacecraft and Rockets
BIBLIOGRAPHY
192
[61] J.C Wilcox. Maximum Likelihood Failure Detection for Redundant Inertial Systems. In
, number 72-864,
August 1972.
AIAA Guidance and Control Conference - AIAA Paper

2000 Thesis Low Cost High Integrity Aided Navigation System PDF

Încărcat de

Informații document

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

2000 Thesis Low Cost High Integrity Aided Navigation System PDF

Încărcat de

Drepturi de autor:

Formate disponibile

Low Cost, High Integrity,

Aided Inertial Navigation Systems

A thesis submitted in ful llment

Australian Centre for Field Robotics

Low Cost, High Integrity, Aided Inertial

And to Nada....get ready for the ride!

2.4 The Information Filter . . . . . . . . . . . . . . . . . . . . . . . . . .

Errors in Attitude Computation . . . . . . . .

4 Aided Inertial Navigation

4.6.1 General Three-Dimensional Motion . . . . . . . . . . .

6 Redundancy, Navigation Accuracy and Autonomous Fault Detection

6.3.2 Directed Information . . .

7 Contributions, Conclusion and Future Work

 Integrity, in this thesis, is de ned as the \..robustness to system failure" [48]. In

 Inertial Navigation is the implementation of inertial sensors to determine the

pose (position and orientation) of a vehicle. Inertial sensors are classi ed as

gation. High grade inertial sensors can be an expensive approach to navigation.

1.2 Navigation Systems

order to increase the performance and integrity of the navigation data.

 Furthermore to address the issue of multiple sensor aiding to a single inertial

unit for further improvement in performance.

 To investigate and develop a redundant inertial unit in order to provide the

1.2 Navigation Systems

1.3 Statistical Filtering

1.3 Statistical Filtering

1.4 Navigation System Integrity

1.4 Navigation System Integrity

1.4 Navigation System Integrity

within the lter are a proper indication of the real system.

navigation systems when implemented in land vehicle applications.

1.6 Thesis Structure

 An understanding of the limitations of vehicle modelling to aid inertial naviga-

 The development of an inertial navigation system aided by GNSS, vehicle mod-

elling, and speed provided by a wheel encoder. The lter architecture is in an

 An information approach for the optimal con guration of redundant inertial

accelerometers. Issues associated with temperature compensation and sampling,

1.6 Thesis Structure

1.6 Thesis Structure

detection capabilities in inertial navigation systems. Redundancy of inertial sensors

2.2 The Kalman Filter

x(k) = F(k)x(k 1) + B(k)u(k) + w(k);

2.2 The Kalman Filter

z(k) = H(k)x(k) + v(k)

It is assumed that the process and observation noises are uncorrelated,

2.2 The Kalman Filter

P(kjk 1) = E[(x(k) x^(kjk 1))(x(k) x^(kjk 1))T jZk ]

x^(kjk) = x^(kjk 1) + W(k) (k);

= z(k) H(k)^x(kjk 1):

The predicted observation is determined by taking the expected value of Equation

2.3 The Extended Kalman Filter

W(k) = P(kjk 1)HT (k)S (k);

where S(k) is known as the innovation covariance and is obtained by

S(k) = H(k)P(kjk 1)HT (k) + R(k):

P(kjk) = E[(x(k) x^(kjk))(x(k) x^(kjk))T jZk ]

2.3 The Extended Kalman Filter

x(k) = F(x(k 1); u(k); k) + w(k);

2.3 The Extended Kalman Filter

The non-linear observation model is represented as

z(k) = H(x(k)) + v(k):

x^(kjk 1) = F(^x(k 1jk 1); u(k));

and the predicted covariance matrix is

P(kjk 1) = rFx(k)P(k 1jk 1)rFTx (k) + Q(k):

x^(kjk) = x^(kjk 1) + W(k) (k)

A thesis submitted in fulllment

Integrity, in this thesis, is dened as the \..robustness to system failure" [48]. In

Inertial Navigation is the implementation of inertial sensors to determine the

pose (position and orientation) of a vehicle. Inertial sensors are classied as

Furthermore to address the issue of multiple sensor aiding to a single inertial

To investigate and develop a redundant inertial unit in order to provide the

An understanding of the limitations of vehicle modelling to aid inertial naviga-

The development of an inertial navigation system aided by GNSS, vehicle mod-

An information approach for the optimal conguration of redundant inertial

x^(kjk) = x^(kjk 1) + W(k) (k);

x^(kjk) = x^(kjk 1) + W(k) (k)

where (k) is the nonlinear innovation,

i(k) = rHTx (k)R (k)[ (k) + rHx(k)^x(kjk 1)];

It can be shown that for any lter = .

A package of inertial sensors is classied into one of three groups:

Strapdown arrangement: The accelerometers and gyros are mounted directly

where !en is known as the transport rate and is dened as