O DER E ·MM M UT A S·EA D · PDF fileno material previously published or written b y another p erson nor material whic h to a ... e. Thanks also go es to Dr.Gamini Dissana y ak e

Low Cost, High Integrity,

Aided Inertial Navigation Systems

for

Autonomous Land Vehicles

Salah Sukkarieh

A thesis submitted in ful�llment

of the requirements for the degree of

Doctor of Philosophy

SID

ERE·MEN

S· EADEM

·MUTAT

O

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

i

ii

Abstract

Salah Sukkarieh Doctor of Philosophy

The University of Sydney 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 appli-cations. 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 de�ned as the ability of

a navigation system to provide reliable navigation information while also monitoringthe health of the data and either correcting any faults that may occur or rejectingfaulty data. Thus integrity encapsulates reliability while the reverse is not necessarily

true. This thesis provides, both in practical and theoretical terms, the fusion pro-cesses 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 algorithmalong with the addition of fault detection techniques so as to increase the integrityof 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 inertialnavigation. 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 implementationof 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 e�ect of inertial sensor redundancy to navigationaccuracy and fault detection is addressed. A redundant inertial measurement unit is

developed and tested and provides the necessary physical sensor for future fault detec-

tion 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 con�dence 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 eco-

nomic 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 andthen...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 electronicboards turned the Tetrad from �ction to fact, and Chris Mifsud for the constantwiring 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 (La-

tex 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 Majumderand to everyone else including the guys in C4: Steve Scheding, Graham Brooker andQuang 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 i

Abstract ii

Acknowledgements iii

Contents v

List of Figures x

List of Tables xix

1 Introduction 1

1.1 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Navigation Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3 Statistical Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4 Navigation System Integrity . . . . . . . . . . . . . . . . . . . . . . . 5

1.5 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.6 Thesis Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2 Statistical Estimation 10

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 The Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.3 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . 13

v

CONTENTS vi

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

2.5 The Extended Information Filter . . . . . . . . . . . . . . . . . . . . 17

2.6 Aided Inertial Navigation System Structures . . . . . . . . . . . . . . 18

2.6.1 Advantages and Disadvantages of the Tightly and Loosely Cou-pled Con�gurations . . . . . . . . . . . . . . . . . . . . . . . . 22

2.6.2 Aiding in Direct Feedback Con�gurations . . . . . . . . . . . . 23

2.7 Filter Consistency and Fault Detection . . . . . . . . . . . . . . . . . 25

2.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3 Inertial Navigation 28

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.2 Inertial Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.2.1 Inertial Systems . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.2.2 Physical Implementation . . . . . . . . . . . . . . . . . . . . . 32

3.2.3 Gyroscopic Sensors . . . . . . . . . . . . . . . . . . . . . . . . 33

3.3 Inertial Navigation Equations . . . . . . . . . . . . . . . . . . . . . . 34

3.3.1 The Coriolis Theorem . . . . . . . . . . . . . . . . . . . . . . 35

3.3.2 The Transport Frame . . . . . . . . . . . . . . . . . . . . . . . 36

3.3.3 Wander Azimuth Frame . . . . . . . . . . . . . . . . . . . . . 37

3.3.4 The Earth Frame . . . . . . . . . . . . . . . . . . . . . . . . . 38

3.4 Schuler Damping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

3.5 Attitude Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

3.5.1 Euler Representation . . . . . . . . . . . . . . . . . . . . . . . 41

Discretisation . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

3.5.2 Direction Cosine Matrix (DCM) Representation . . . . . . . . 43

Discretisation . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

3.5.3 Quaternion Representation . . . . . . . . . . . . . . . . . . . . 44

Discretisation . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

3.5.4 Discussion of Algorithms . . . . . . . . . . . . . . . . . . . . . 46

CONTENTS vii

3.6 Errors in Attitude Computation . . . . . . . . . . . . . . . . . . . . . 47

3.7 Vibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

3.8 Minimising the Attitude Errors . . . . . . . . . . . . . . . . . . . . . 51

3.9 Error Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

3.10 Sensor Errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

3.10.1 Evaluation of the Error Components . . . . . . . . . . . . . . 57

3.10.2 Faults Associated with Inertial Sensors . . . . . . . . . . . . . 59

3.11 Alignment and Calibration . . . . . . . . . . . . . . . . . . . . . . . . 63

3.11.1 Alignment Techniques . . . . . . . . . . . . . . . . . . . . . . 63

3.11.2 Alignment for Low Cost Inertial Units . . . . . . . . . . . . . 64

3.11.3 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

3.12 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

4 Aided Inertial Navigation 68

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

4.2 GNSS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

4.2.1 Position Evaluation and Accuracy . . . . . . . . . . . . . . . . 70

4.2.2 Transformation of the GNSS Frame . . . . . . . . . . . . . . . 72

4.3 Overview of the Navigation Loops . . . . . . . . . . . . . . . . . . . . 74

4.4 GNSS Aiding . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

4.4.1 Navigation for an Autonomous Straddle Carrier . . . . . . . . 77

4.4.2 Linear, Direct Feedback Implementation . . . . . . . . . . . . 79

4.4.3 Detection of Multipath . . . . . . . . . . . . . . . . . . . . . . 84

4.4.4 Filter Tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

4.5 Real Time Implementation of an Inertially Aided GNSS Navigation

System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87

4.5.1 Latency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

4.5.2 Hardware and Algorithms . . . . . . . . . . . . . . . . . . . . 88

4.6 Aiding Using a Vehicle Model . . . . . . . . . . . . . . . . . . . . . . 92

CONTENTS viii

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

4.6.2 Motion of a Vehicle on a Surface . . . . . . . . . . . . . . . . 94

4.6.3 Estimation of the Vehicle State Subject to Constraints . . . . 95

4.6.4 Observability of the States . . . . . . . . . . . . . . . . . . . . 96

4.7 Multiple Aiding of an Inertial System . . . . . . . . . . . . . . . . . . 99

4.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

5 Experimental Results 104

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

5.2 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

5.2.1 Vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

5.2.2 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

5.2.3 Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . 109

5.3 Inertial/GNSS Results . . . . . . . . . . . . . . . . . . . . . . . . . . 114

5.3.1 Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114

5.3.2 Alignment Correction . . . . . . . . . . . . . . . . . . . . . . . 115

5.3.3 Fault Detection . . . . . . . . . . . . . . . . . . . . . . . . . . 116

5.4 Vehicle Model Constraint Results . . . . . . . . . . . . . . . . . . . . 117

5.5 Multiple Aiding Results . . . . . . . . . . . . . . . . . . . . . . . . . 119

5.6 Inertial/GNSS Plots . . . . . . . . . . . . . . . . . . . . . . . . . . . 121

5.7 Vehicle Model Constraint Plots . . . . . . . . . . . . . . . . . . . . . 137

5.8 Multiple Aiding Plots . . . . . . . . . . . . . . . . . . . . . . . . . . . 144

5.9 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148

6 Redundancy, Navigation Accuracy and Autonomous Fault Detec-

tion 150

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150

6.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151

6.3 Con�guration of Redundant Inertial Sensors . . . . . . . . . . . . . . 152

6.3.1 Information Space . . . . . . . . . . . . . . . . . . . . . . . . 153

CONTENTS ix

6.3.2 Directed Information . . . . . . . . . . . . . . . . . . . . . . . 158

6.3.3 Unequal Noise Variances . . . . . . . . . . . . . . . . . . . . . 159

6.3.4 Sensitivity and Resolution . . . . . . . . . . . . . . . . . . . . 160

6.4 Fault Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161

6.4.1 GNSS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161

6.4.2 Inertial Navigation . . . . . . . . . . . . . . . . . . . . . . . . 163

6.5 Truncated Tetrahedron . . . . . . . . . . . . . . . . . . . . . . . . . . 164

6.6 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168

6.6.1 Ground Tests . . . . . . . . . . . . . . . . . . . . . . . . . . . 168

6.6.2 Airborne Tests . . . . . . . . . . . . . . . . . . . . . . . . . . 169

6.7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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. . . . . . . . . . . . . . 5

2.1 The \direct" structure implements a non-linear �lter to estimate the

position, velocity and attitude of the vehicle. The inertial data isprovided by an IMU and the aiding data from a navigation system. . 19

2.2 The \indirect feedback" method allows a linear �lter implementation

and minimises the computational overhead on the �lter structure. . . 20

2.3 The \direct feedback" method overcomes the problem of large obser-vation values being provided to the �lter by correcting the INS directly. 20

2.4 The \tightly coupled" con�guration 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 con�gurationallows for a more robust system. . . . . . . . . . . . . . . . . . . . . . 22

2.5 Illustration of how the observation measurements zP and zV are ob-tained by the inertial and aiding information. . . . . . . . . . . . . . 24

3.1 A photograph showing low cost accelerometers (bottom) and gyro-

scopes (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 di�erent

characteristic even though they are the same make. This is simply

because they are low cost sensors and hence have dramatically di�erent

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 di�erential correc-tions 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. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

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. . . . . . 74

4.3 The three navigation systems implemented in this thesis. . . . . . . . 76

4.4 A straddle carrier . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

4.5 The container terminal . . . . . . . . . . . . . . . . . . . . . . . . . . 78

4.6 Real time architecture implementing four Transputers for parallel pro-

cessing. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

4.7 Motion of a vehicle on a surface. The navigation frame is �xed and thebody frame is on the local tangent plane to the surface and is aligned

with the kinematic axes of the vehicle. . . . . . . . . . . . . . . . . . 97

5.1 The utility is used as a test bed for the sensors. It houses the Trans-

puters which log data from the GNSS receiver and inertial unit. It alsohandles 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 navigationdata 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 anorthogonal 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 xii

5.6 Outside the University campus showing extensive tree foliage and build-

ings which will a�ect the performance of the GPS receiver. . . . . . . 110

5.7 A major road outside of the University campus where multipath occurs

due to the building structures. . . . . . . . . . . . . . . . . . . . . . . 110

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. . . . . . . . . . . . . . . . . . . . . . . . . . 111

5.9 Another view of the area shows a small amount of tree foliage, howeveragain there is still a large portion of the sky which is visible by the

receiver antenna. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

5.10 The port where the straddle carrier is located comprises mainly of

containers and quay cranes. Although the containers have no a�ect

on the GNSS signal the quay cranes do and hence fault detection is

particularly important. . . . . . . . . . . . . . . . . . . . . . . . . . . 112

5.11 The extension on the top right hand corner of the quay crane causes

slight multipath errors when the straddle carrier passes underneaththis extension. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113

5.12 As the straddle carrier approaches the quay crane in order to travelunderneath it, multipath errors occur until the straddle carrier is under

the crane at which stage total satellite blockage occurs. . . . . . . . . 113

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. . . . . . . . . . . . . . . . . . . . . . . . . . 121

5.14 Fusion of inertial and GPS data with no multipath rejection. . . . . . 121

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 acollection of points from the inertial navigation solution) closely followthese points since their is no fault detection on the observations. . . . 122

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. . . 122

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. . . . . . . 123

5.18 Enlarged view of region 2 with multipath rejection. As in the previ-ous plot, multipath has been rejected but the result is limited to the

accuracy of the GPS receiver. . . . . . . . . . . . . . . . . . . . . . . 123

LIST OF FIGURES xiii

5.19 Fusion result using 0:02m position and 0:02m=s velocity GPS technol-

ogy. 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. . . . . . . . . . . . 124

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. . . . . . . 125

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 iscorrected by the time the vehicle returns. . . . . . . . . . . . . . . . . 125

5.22 Enhanced view of the acceleration of the vehicle at the end of the run

with attitude correction. . . . . . . . . . . . . . . . . . . . . . . . . . 126

5.23 Enhanced view of the velocity of the vehicle at the end of the run with

attitude correction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126

5.24 Enhanced view of the acceleration of the vehicle at the end of the runwithout attitude correction. . . . . . . . . . . . . . . . . . . . . . . . 127

5.25 Enhanced view of the velocity of the vehicle at the end of the run

without attitude correction. . . . . . . . . . . . . . . . . . . . . . . . 127

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. . . . . . 128

5.27 Close up of the pitch of the vehicle without attitude correction. Thedark line represents the pitch as determined by the direction cosine

matrix. The lighter line is the pitch as determined by the tilt sensors. 128

5.28 Position of the straddle carrier as it manoeuvres around containers

before driving under a quay crane. The vehicle starts at position0m North; 0m East and moves in a counter clockwise direction. . . 129

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 arenot 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. . . . 130

5.30 Before the vehicle approaches the crane multipath errors occur. Theses

GPS �xes however have been detected as faults and hence are not usedas observations and the inertial data is not inaccurately corrected. . . 130

LIST OF FIGURES xiv

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. . . . . . 131

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. . . . . 131

5.33 With the same tuning parameters but with no fault detection routinesthe 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. . . 132

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. . . . . . . . . . . . . . . . . . . . 132

5.35 The position innovations show where multipath errors occur. . . . . . 133

5.36 Similarly the velocity innovations show where the multipath errors oc-

cur. Even beyond the multipath region the innovations exceed the twosigma bound. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133

5.37 Keeping with no GPS fault detection, the path of the vehicle can bemade to resemble the true path by placing greater accuracy in the statemodel and hence in the inertial data, with less weighting placed on the

GPS �xes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134

5.38 The position innovations show that the �lter is behaving inconsistentlyeven when it is tuned to place little emphasis on the GPS �xes. . . . 135

5.39 The velocity innovations further magnify the inconsistency of the �lter

when it is tuned to seemingly reject multipath errors. . . . . . . . . . 135

5.40 Illustration of the velocity of the vehicle when it is properly tuned and

with GPS fault detection. . . . . . . . . . . . . . . . . . . . . . . . . 136

5.41 Velocity of the vehicle when the navigation loop is implemented with-out fault detection however with tuning so that the position of the

vehicle seemingly matches the true trajectory. During the multipathregion (approximately around iteration 8500) the velocity as deter-

mined by the inertial solution is inaccurately corrected. This is also

seen with the attitude states. . . . . . . . . . . . . . . . . . . . . . . 136

LIST OF FIGURES xv

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. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137

5.43 Errors in vehicle speed when the vehicle is moving at a constant velocity

of 10 m/s while the angular velocity !y is non-zero in the time interval

700� 1300s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137

5.44 Errors in vehicle speed when the vehicle is moving at a constant velocityof 10 m/s while the angular velocity !z is non-zero in the time interval

700� 1300s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138

5.45 Errors in roll, pitch and yaw when the vehicle is moving at a constantvelocity of 10 m/s while the angular velocity about Bz is non-zero in

the time interval 700-1300 sec. . . . . . . . . . . . . . . . . . . . . . 138

5.46 Errors in roll, pitch and yaw when the vehicle is moving at a constantvelocity of 10 m/s while the angular velocity !z is non-zero in the time

interval 700 � 1300s. A constant unestimated bias of 10�4 rad/s is

introduced to all angular velocity observations . . . . . . . . . . . . . 139

5.47 Errors in the vehicle speed when the vehicle is moving at a constant

acceleration of 0.05 m=s2 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. . . . . . . . . . . . . . . . . . . . . . . . . . . . 139

5.48 The result of the three di�erent cases: the position of the inertial unit

with only raw data, the fused data with the GPS and the constraineddata. The attitude of the inertial unit with the raw data slowly driftsthus giving inaccurate position results. The di�erence between the po-

sition obtained by the Inertial/GPS fusion and the proposed algorithmis too small to be clearly seen in this �gure. . . . . . . . . . . . . . . 140

5.49 The position error of the constrained and raw data. The referenceposition is from the fused Inertial/GPS navigation system. As illus-

trated, the error in the raw data grow quadratically with time while

the constrained data keeps the error bounded. . . . . . . . . . . . . . 141

5.50 The velocity error of the constrained and raw data. The reference

velocity is from the fused Inertial/GPS navigation system. The con-

strained data is bounded well while the raw data grows linearly with

time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141

5.51 The roll data from the three di�erent 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. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 tiltsensor 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 thebene�t of using the constraint equations. . . . . . . . . . . . . . . . . 143

5.55 Position plot of the path taken by the vehicle. This path was obtainedby 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 per-forming free of any external observations and when it is fused with thevehicle 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 constrainedinertial 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 iswith velocity observation, the second with velocity and GPS observa-

tions 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 becausevelocity is observable when using the vehicle model and speed data, and

so the extra information from the GPS bene�ts the velocity estimate

only slightly. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147

5.61 The attitude estimation only bene�ts slightly from the GPS informa-tion since the velocity information provided by the vehicle model con-

straints delivers a signi�cant amount of information to the estimation. 147

6.1 One of the optimal con�gurations for �ve sensors. The cone's half angleis 54:74Æ and the sensors are separated equally by 72Æ. . . . . . . . . . 154

6.2 Another optimal con�guration 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 de�ned 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 ob-

jective is to get this region as large as possible so that the con�guration

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 ADCand 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 boththe 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 xviii

6.16 The heading angle of the vehicle as provided by the aided Tetrad data.

Note the marked di�erence 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 di�erent. The

�lter is not a�ected by this since it fuses the velocity vectors providedby the two systems and not the heading data. . . . . . . . . . . . . . 176

6.17 Illustrates an example of sideslip occurring during a portion of the run.

It is this sideslip which causes the di�erence between the heading dataprovided by the Tetrad and GPS. . . . . . . . . . . . . . . . . . . . . 177

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. . . . . . . 177

6.19 Position as provided by the Tetrad data along the three axis when it

has been aided by GPS for only 60s. . . . . . . . . . . . . . . . . . . 178

6.20 Velocity as indicated by the Tetrad data when aided by GPS for only60s. 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 initial60s of the ight. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178

List of Tables

3.1 Algorithm Drift Rates in o=hr caused by sampling a continuous rota-

tion rate of 20deg=sec . . . . . . . . . . . . . . . . . . . . . . . . . . 48

3.2 Comparison of some of the major errors with various gyro implemen-tations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

5.1 The speci�cation for the Watson IMU implemented in this work. . . . 108

6.1 Speci�cations of the gyros implemented. The top row represents themodel numbers of the individual sensors. . . . . . . . . . . . . . . . . 165

6.2 Speci�cations of the accelerometers implemented. The top row repre-sents the model numbers of the individual sensors. . . . . . . . . . . . 165

6.3 Physical characteristics of the redundant inertial unit. . . . . . . . . . 167

6.4 The theoretical and actual face angles of the redundant inertial unit. 167

xix

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 de�nition 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

e�ectively by the civilian sector.

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

the context of the navigation systems developed in this thesis, integrity repre-

sents the ability of a navigation system to provide reliable navigation informa-

tion 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 classi�ed 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 2

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 e�ectively by the civilian sector for autonomous land vehicles

and in turn provide a suÆcient 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 intro-

duces 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 e�ect of GNSS accuracy to moving land vehicles. High accu-

racy satellite navigation receivers are generally produced for surveying purposes

and not for dynamic movement. Furthermore, associated errors such as multi-

path 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 iner-

tial 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 3

data latency commonly associated with satellite based navigation systems and

its a�ect 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 nav-

igation performance and autonomous fault detection techniques. Redundancy

in satellite numbers and its a�ect on navigation and fault detection is well doc-

umented 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 deter-

mine 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 rel-

ative 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 4

either dead reckoning or absolute sensors. Dead reckoning sensors measure the rel-

ative 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. Exam-

ples 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 bene�ts 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 \pre-

dicting" 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 5

Dead ReckoningSensor

AbsoluteSensor

Predict Observation Update

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.

1.4 Navigation System Integrity

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 con�dence 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

1.4 Navigation System Integrity 6

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 navi-

gation system.

To reduce the risk of impact that a component has on a navigation system, redun-

dancy 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 ac-

curacy 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 7

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 ap-

plications, and work conducted alongside satellite navigation manufacturers and the

defense industry.

The main contributions of this thesis are:

� The quali�cation and quanti�cation 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 un-

derstanding 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 mod-

elling. This implementation requires no external sensors to aid the inertial unit.

1.6 Thesis Structure 8

� 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 con�guration 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.

1.6 Thesis Structure

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 infor-

mation (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 associ-

ated with low cost inertial sensors. Techniques of quantifying these errors are also

discussed.

This chapter also de�nes 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.

1.6 Thesis Structure 9

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, redun-

dancy 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 con�guration 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 re-

quired 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 understand-

ing 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)

2.2 The Kalman Filter 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 ob-

servation 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 algorithmwhich 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).

2.2 The Kalman Filter 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�1]

= F(k)x(k � 1jk � 1) +B(k)u(k): (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�1]

= F(k)P(k � 1jk � 1)FT (k) +Q(k): (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 esti-

mate of the state can be obtained and is given by

x(kjk) = x(kjk � 1) +W(k)�(k); (2.5)

whereW(k) is a gain matrix produced by the Kalman �lter and �(k) is the innovation

vector. The innovation vector is the di�erence 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 de�nes 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�1(k); (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)

2.3 The Extended Kalman Filter

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.

2.3 The Extended Kalman Filter 14

The non-linear observation model is represented as

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

Following the same de�nitions 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

�1(k); (2.16)

S(k) = rHx(k)P(kjk � 1)rHTx (k) +R(k); (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 non-

linear implementation, since the process and observation models are dependent on

the current state, this bene�t 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.

2.4 The Information Filter

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�1(kjk); (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)

2.4 The Information Filter 16

The predicted information vector is

y(kjk � 1) = L(kjk � 1)y(k � 1jk � 1) +B(k)u(k); (2.21)

where L(kjk � 1) = Y(k � 1jk � 1)F(k)Y�1(k � 1jk � 1):

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�1(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�1(k)z(k): (2.23)

The amount of certainty associated with this observation is in the form of the infor-

mation observation matrix,

I(k) = HT (k)R�1(k)H(k) (2.24)

The bene�t 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) (2.25)

Y(kjk) = Y(kjk � 1) + I(k): (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,

y(kjk) = y(kjk � 1) +X

ij(k) (2.27)

Y(kjk) = Y(kjk � 1) +X

Ij(k): (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 con-

tributions. 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)); (2.29)

Y(kjk � 1) = [rFx(k)Y�1(k � 1jk � 1)rFT

x (k) +Q(k)]�1: (2.30)

2.6 Aided Inertial Navigation System Structures 18

When an observation occurs, the information contribution and its associated infor-

mation matrix is

i(k) = rHTx (k)R

�1(k)[�(k) +rHx(k)x(kjk � 1)]; (2.31)

I(k) = rHTx (k)R

�1(k)rHx(k); (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 bene�t of being easily initialised, that is, one can set the initial conditions

to zero information. Furthermore, there is the added bene�t of decentralising the �lter

across multiple nodes and the incorporation of multiple observations to a single �lter

[39].

2.6 Aided Inertial Navigation System Structures

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


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

Non-LinearKalman/Information

Filter

AIDING NAVIGATIONSYSTEM

Acceleration andRotation Rates

Observations

Position, Velocity andAttitude Estimates

Figure 2.1: The \direct" structure implements a non-linear �lter to estimate theposition, 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 pro-

cessing 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 di�erence between the inertial navigation

solution and the navigation solution provided by the aiding source. Since the obser-

vation 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


INERTIAL NAVIGATIONSYSTEM

LinearKalman/Information

Filter


Position, Velocityand Attitude

+-

Observed Error+

-

Observations

Estimated Errors of Position,Velocity and Attitude

Figure 2.2: The \indirect feedback" method allows a linear �lter implementation andminimises the computational overhead on the �lter structure.

INERTIAL NAVIGATIONSYSTEM

LinearKalman/Information

Filter



Observed Error+

-

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.


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" implemen-

tations since there is no feedback to the aiding sensor/navigation system. If feedback

is provided to the aiding source a tighter con�guration is obtained which in turn im-

proves system integrity. Figure 2.4 illustrates what is known as a \tightly coupled"

con�guration. It o�ers 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 in-

corporates 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.


INERTIAL SENSOR

Non-LinearKalman/Information

Filter

AIDING SENSOR


Raw Observation



Figure 2.4: The \tightly coupled" con�guration treats both inputs as sensors and not

as navigation systems. Furthermore, the �lter estimates are sent to the aiding sensorand not the inertial sensor. This con�guration allows for a more robust system.

2.6.1 Advantages and Disadvantages of the Tightly and Loosely

Coupled Con�gurations

The loosely coupled con�gurations o�er the distinct advantage of being highly mod-

ular 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 con�guration is more robust than the loosely coupled

con�guration, it is more expensive to implement and more diÆcult to develop. Fur-

thermore, if a di�erent 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 devel-

oping INS units with built in �ltering techniques in a loosely coupled con�guration


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 con�guration 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 af-

ter they have been lost, is what separates the quality of receivers between di�erent

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 bene�ts of implementing a loosely coupled form outweigh

those of its counterpart since the increase in optimality of the tightly coupled con�gu-

ration is slight. This was mainly due to the diÆculty 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 con�guration, 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 Con�gurations

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,

z(k) =

24 zP (k)

zV (k)

35 =

24 Pinertial(k)�Paiding(k)

Vinertial(k)�Vaiding(k)

35 (2.34)


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.

A

+

+A

T

P

+

-

V

+

-V

T

inertialA inertialV inertialP

+

-Paiding

Vaiding +

-

PT

ZP

ZV

v

v

v

Figure 2.5: Illustration of how the observation measurements zP and zV are obtainedby the inertial and aiding information.

By de�ning 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

z(k) =

24 Pinertial(k)�Paiding(k)

Vinertial(k)�Vaiding(k)

35

=

24 (PT (k) + ÆP(k))� (PT (k)� vP (k))

(VT (k) + ÆV(k))� (VT (k)� vV (k))

35 =

24 ÆP(k)

ÆV(k)

35 +

24 vP(k)

vV(k)

35

(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 o�ers another bene�t 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 di�erence 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,

= �T (k)S�1(k)�(k): (2.36)

If the �lter assumptions are correct then the samples of are distributed as a �2

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 prede�ned 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 �2 tables and is chosen based on the

con�dence level required. Thus for example, a 95% con�dence 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,

�(k) = VT (k)B�1(k)V(k); (2.37)

where

I+(k) = HT (k)[H(k)I(k)HT (k)]�1H(k);

V(k) = I(k)[I+(k)i(k)�Y�1(kjk � 1)y(kjk � 1)];

and B(k) = I(k)[I+(k) +Y�1(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 ver-

sions 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 im-

plemented. 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 classi�ed as \loosely coupled" implementations since there is no

aiding to the external sensor. A \tightly coupled" con�guration was presented and

it was discussed that even though it provided a more robust solution to the �ltering

problem, it required substantially increased e�ort and cost to implement.

It was also shown that the greatest modularity can be achieved through the imple-

mentation 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 modi�cation 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 observa-

tion 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 imple-

mentation 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 mathemat-

ically integrated, then the change in velocity and position of the object with respect

3.2 Inertial Navigation 29

to an initial condition can be determined.

The inertial sensor which measures acceleration is known as an accelerometer. How-

ever, 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 photo-

graph 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 classi�ed 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 arrange-

ment 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,


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 previousphotograph. 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.


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 a�ected by wheel slip.

However, the errors caused by bias, scale factors and non-linearities in the sen-

sor 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 classi�ed 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


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 phys-

ical 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 navi-

gation frame. This is done by constantly actuating the gimbals based on the

transition of the navigation frame. Thus the accelerometers are directly inte-

grated 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 e�ects of vehicle mo-

tion, and thus higher bandwidth and dynamic range are required. The higher dy-

namic range in turn a�ects 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 pro-

cessing since the platform is maintained to correspond with the navigation frame. In


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 gim-

balled arrangements, and for highly accurate systems such as those used on sub-

marines 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 (gy-

ros) 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 sili-

con 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 accelerome-

ters, 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 vibrat-

ing at a known frequency. If an angular rotation is encountered perpendicular to this

vibration then Coriolis acceleration is generated which in turn modi�es the resonant

frequency. This modi�cation is measured and interpreted as the angular rotation

encountered. Other gyros are mechanical systems (Dry Tuned Gyros and Rate Inte-

grating), 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 e�ective 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 the-

sis. The immediate start-up time, low power consumption, weight and cost of this

sensor meet the speci�cations 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 e�ects on the resonator. Tem-

perature compensation and/or �xing the temperature of the inertial unit is the most

practical way of handling such errors.

3.3 Inertial Navigation Equations

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 con�ned to small

areas. This requires analysis of the other frameworks, so as to o�er a comparison.


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 . Di�erentiating this

equation with respect to the inertial frame gives

_viji = _veji + _!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,

!ie � vji = !ie � [ve + !ie � r]

= !ie � ve + !ie � [!ie � r]; (3.3)

hence Equation 3.2 becomes

_viji = _veji + !ie � ve + !ie � [!ie � r]: (3.4)

Now _viji = f + g where f is the speci�c force encountered due to the vehicle motion

and g is the force due to gravity. Hence Equation 3.4 becomes

f + g = _veji + !ie � ve + !ie � [!ie � r]; (3.5)


that is,

_veji = 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 de�ned as: the acceleration of the vehicle with

respect to the navigation frame _vejn, is equal to the acceleration of the vehicle with

respect to the inertial frame _veji, minus the Coriolis acceleration due to the navigation

frame rotating !en, on a rotating earth !ie. That is,

_vejn = _veji � (!ie + !en)� ve: (3.7)


Substituting in Equation 3.6,

_vejn = 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 trans-

formed 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 de�ned as

!en = [ve

Ro + h;�venRo + h

;�vn tanLRo + h

]: (3.11)

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


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 de�ned 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 _veje, is equal

to the acceleration of the vehicle with respect to the inertial frame _veji, minus the

Coriolis acceleration due to the velocity of the vehicle ve, over a rotating earth !ie,

_veje = _veji � !ie � ve: (3.12)

The Earth frame will be used throughout this work since its de�nition is well suited

for land vehicle applications. Substituting in Equation 3.6 gives

_veje = fe � 2[!ie � ve] + [g� !ie � [!ie � re]]: (3.13)

Again, since the inertial unit measures the acceleration in the body frame, the accel-

eration 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 di�erence between the Transport and Earth frame equations is the trans-

port rate de�ned by Equation 3.11. The transport rate is subtracted from the body

rates and hence de�nes 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 =

rg

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 Ævosin(!st)

!s;

� An initial attitude error Æ�, causes a position error of Æ�R(1� cos(!st));

� An acceleration bias Æf , causes a position error of Æf(1�cos(!st)

!2s

); and

� A gyro bias Æ!b, causes a position error of Æ!bR(t�sin(!st)

!s).

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 ap-

plications and hence bene�t 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 con�ned areas.

3.5 Attitude Algorithms

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


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 simpli�cation of the algorithms. The simpli�cation

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 transfor-

mation matrix Enb , by multiplying the consecutive rotation matrices representing a

roll, followed by a pitch and then a yaw,

Enb =

2664 c � s 0

s c 0

0 0 1

3775

2664

�c 0 �s

0 1 0

��s 0 �c

3775

26641 0 0

0 �c ��s

0 �s �c

3775

=

2664�c c ��c s + �s�s c �s s + �c�s c

�c s �c c + �s�s s ��s c + �c�s s

��s �s�c �c�c

3775 (3.16)


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 (3.17)

_� = !y cos � � !z sin � (3.18)

_ = (!y sin � + !x cos �) sec �: (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 �2. This is equivalent to the physical phenomenon of

\gimbal lock" in gimbal systems. In a strapdown system, as the vehicle approaches

�

2then theoretically in�nite 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 trigonome-

try required in the updates and in forming Enb . The transformation matrix can be

simpli�ed if one assumes small angular rotations (< 0:05deg) thus,

Enb =

2664

1 � �

1 ��

�� 1

3775 (3.20)

Discretisation

The discretisation procedure is as follows:

� The new roll angle is obtained through integration by

�(i+ 1) =

Z_�dt+ �(i) (3.21)

and similarly for pitch and yaw. The angles are then placed into the matrix to

form Enb (i+ 1).


� Obtain the acceleration data in the body frame fb = [fx; fy; fz] and evaluate

the acceleration in the navigation frame

fn = Enb fb (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

_Cn

b = Cnbbn; (3.23)

where bn is a skew-symmetric matrix representing rotation rates as measured by the

gyros,

bn =

2664

0 �!z !y

!z 0 �!x

�!y !x 0

3775 : (3.24)

The initial Cnb is simply the initial En

b .

Discretisation


� Obtain the gyro outputs !x; !y; !z and integrate to determine the change in

angle �x; �y; �z.

� Determine the angular vector magnitude

� =q�2x + �2

y + �2z (3.25)


� Determine the series coeÆcients

� =sin�

�(3.26)

� =1� cos�

�2(3.27)

� Form the angular skew matrix

� =

0BB@

0 ��z �y

�z 0 ��x

��y �x 0

1CCA (3.28)

� Update the direction cosine matrix

Cnb (i + 1) = Cn

b (i)[I3�3 + ��+ ��2

] (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 accom-

plished 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 ini-

tial quaternion is obtained from the roll, pitch and yaw angles de�ned in the Euler


representation or alternatively through the Direction Cosine parameters,

q(i) =

0BBBBB@

0:5 �p1 + Cn

b11+ Cn

b22+ Cn

b33

14�q(1)

(Cnb32

� Cnb23)

14�q(1)

(Cnb13

� Cnb31)

14�q(1)

(Cnb21

� Cnb12)

1CCCCCA

(3.31)

Discretisation


� Obtain the gyro outputs !x; !y; !z and integrate to determine change in angle

�x; �y; �z.

� Determine the quaternion operator coeÆcients

=sin �

2

2(3.32)

Æ = cos�

2(3.33)

� Determine the quaternion operator

h(i) =

0BBBBB@

Æ

�x

�y

�z

1CCCCCA

(3.34)

� Generate the quaternion matrix and update

q(i+ 1) =

0BBBBB@

q(i)1 �q(i)2 �q(i)3 �q(i)4

q(i)2 q(i)1 �q(i)4 q(i)3

q(i)3 q(i)4 q(i)1 �q(i)2q(i)4 �q(i)3 q(i)2 q(i)1

1CCCCCAh(i) (3.35)


� 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

Cnb =

0BB@(q21 + q22 � q23 � q24) 2(q2q3 � q1q4) 2(q2q4 + q1q3)

2(q2q3 + q1q4) (q21 � q22 + q23 � q24) 2(q3q4 � q1q2)

2(q2q4 � q1q3) 2(q3q4 + q1q2) (q21 � q22 � q23 + q24)

1CCA ;

and then

fn = Cnb fb (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 e�ort and thus powerful processing if fast

updates are required. The algorithm can be simpli�ed 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 bene�t 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 signi�cant

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

suÆcient 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

o�ers 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.

3.6 Errors in Attitude Computation

Equations 3.26 and 3.27 presented the series coeÆcients required for the DCM updat-

ing process. If these coeÆcients can be determined exactly then the DCM generated

would be exact. However, in practice a Taylor series expansion is required,

� = 1��2

3!+�4

5!� ::: (3.38)

� =1

2!��2

4!+�4

6!� ::: (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

3.6 Errors in Attitude Computation 48

the DCM algorithm can be represented as

Ddc =1

Æt(�a1 cos�� sin�+ �2a2 sin�); (3.40)

where a1 = 1 and a2 = 0 for a �rst order algorithm, a1 = 1 and a2 = 0:5 for a

second order algorithm, a1 = 1� �2

3!and a2 = 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.

Order of Algorithm 10Hz 125Hz 200 Hz 500 Hz

1 30 0.23 0.04 0.012

2 14.7 0.11 0.021 0.006

3 0.001 5� 10�7 4:85� 10�7 1:4� 10�9

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 compa-

rable 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 simpli�cation 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 a�ect 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 signi�cantly

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 bene�t of being light weight, it

is diÆcult 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 e�ect of vibration on the attitude algorithms is termed \coning" or \non-

commutativity". The de�nition 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 di�erence % 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 or-

thogonality 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 phe-

nomenon 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 a�ect is to

dither the gyros at di�erent frequencies.

Type 3 errors are of most concern. They arise from errors associated with trunca-

tion 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 a�ects 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 bene�cial 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 asso-

ciated 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 em-

ployed and on the computer implementation of the inertial algorithms. To analyse

how these two e�ects contribute to the inertial navigation system development, iner-

tial 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 bene�t of implementing the perturbation in the computer frame is that the mis-

alignment 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 bene�t as the com-

puter frame approach.


Perturbation in the true frame is accomplished by stating that the error in a par-

ticular state is the di�erence between the estimated state and the true state. Thus

given that the Earth frame velocity equation (Equation 3.13) is

_veje = Cebfb � 2[!ie � ve] + [g� !ie � [!ie � re]]; (3.41)

and de�ning the total gravity acceleration as

gt = [g� !ie � [!ie � re]]; (3.42)

then by de�nition of the perturbation

Æ _v = ~_ve

e � _vee

= [ ~Ce

b~f b �Ce

bfb]� [2~!eie�~v

ee �2!

eie � vee] + [~gt � gt]: (3.43)

where ~_ve

e is the evaluated velocity vector and _vee is the true velocity vector. The

evaluated transformation matrix ~Ce

b, is the true transformation matrixCeb, misaligned

by the misalignment angles Æ . The misalignment is represented in skew-symmetric

form [Æ �]. Hence

~Ce

b = [I3�3 � [Æ �]]Ceb; (3.44)

thus

Æ _v = Ceb~f b �C

ebf b � [Æ �]Ce

b~f b � Æ[2!

eie � v

ee] + Ægt: (3.45)

If the errors in Coriolis and gravity terms are insigni�cant then

Æ _v = CebÆf b � [Æ �]Ce

b~f b

= CebÆf b � [Æ �] ~f e: (3.46)


Now

�[ �] ~f e = ~feT[Æ �]; (3.47)

hence

Æ _v = ~feT[Æ �] +Ce

bÆf b

= [ ~f e�]Æ +CebÆf b: (3.48)

Thus the propagation of velocity errors in the Earth frame Æ _v is equal to the accelera-

tion 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 Ceb. The errors in the measurements of the acceleration

are associated with the accelerometers and are discussed in the next section.

Rearranging Equation 3.44

[Æ �] = I3�3 � ~Ce

bCebT ; (3.49)

and di�erentiating gives

_[Æ �] = � ~Ce

b_Ce

b

T� _~C

e

bCebT : (3.50)

The updating process of the transformation matrix both for the true and evaluated

frames are

_Ce

b = Ceb

ebe and

_~Ce

b =~Ce

bebe: (3.51)

Substituting into Equation 3.50 gives

_[Æ �] = � ~Ce

b[Ceb

bbe]

T � [ ~Ce

bbbe]C

ebT

= � ~Ce

b[~b

be �bbe]C

ebT : (3.52)


Perturbation of the rotation update matrix gives

Æbbe = ~

b

be �bbe; (3.53)

therefore

_[Æ �] = � ~Ce

bÆbbeC

ebT : (3.54)

Substituting in Equation 3.44

_[Æ �] = �[[I3�3 � Æ �]Ceb]Æ

ebeC

ebT

= �CebÆ

bbeC

ebT + [Æ �]C

e

bÆbbeC

ebT : (3.55)

The product of small terms Æ � and Æ, is assumed zero. Therefore

_[Æ �] = �CebÆ

bbeC

ebT

= �CebÆ

bbeC

be:

= �[CebÆ!

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

_[Æ �] = �Ceb[Æ!

bib�] or

Æ _ = �CebÆ!

bib: (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 Ce

b.

The errors associated with the measurements are purely de�ned 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 bene�t 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 tra-

jectory, 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 Æ!b

ib =

[Æ!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 phys-

ical operational principle of the sensor itself. The general error equation used for

gyroscopic performance along, say, the x direction, is written as

Æ!x = b+ bg

0BB@

ax

ay

az

1CCA+ sf!x +my!y +mz!z + �; (3.59)

and that of the accelerometers as

Æfx = b + sfax +myay +mzaz + �; (3.60)

where

� b is the residual biases

� bg is a 1� 3 vector representing the g-dependent bias coeÆcients

� sf is the scale factor term


� m represents mounting and internal misalignments and

� � is random noise on the sensor signal

Other terms such as anisoelastic and anisoinertia errors mainly a�ect mechanical gy-

ros while vibro-pendulous errors a�ect mechanical accelerometers, and hence will not

be considered here.

Apart from the random noise term �, all other error terms are, in principle, pre-

dictable 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 systemat-

ically remove the errors. Similar tests can be conducted on the accelerometer. It

should be noted that temperature and memory e�ect play a signi�cant 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 e�ect 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 as-

sumed known and is removed.


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 orthog-

onal 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 pre-

vious 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 e�ect 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

g-dependent bias o=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.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 ac-

celerometers 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 simpli�ed by considering only the most dominant terms;

fi = fiT + Æfi

= fiT + bfi + � (3.61)

!i = !iT + Æ!i

= !iT + b!i + �; (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 = ViT + bfit +

Z� dt (3.63)

Pi = PiT +bfit

2

2+

Z Z� dt (3.64)

and �i = �iT + b!it+

Z� dt (3.65)


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 Cnb . 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!zt) :

For small angle increments

fx = fxT b!zt

thus Vx =1

2fxT b!zt

2

and Px =1

6fxT b!zt

3 (3.66)

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 di�erent bias

values are simply due to the fact that they are physically di�erent low cost inertial


0 2 4 6−20

−15

−10

−5

0

5x 10

−3

Acc

eler

atio

n (g

)

X accelerometer

0 2 4 60.01

0.015

0.02

0.025

0.03

Acc

eler

atio

n (g

)

Y accelerometer

0 2 4 6−1.04

−1.03

−1.02

−1.01

−1

Time (hours)

Acc

eler

atio

n (g

)

Z accelerometer

0 2 4 615

20

25

30

Time (hours)

Tem

p (C

)

Temperature

Figure 3.4: The change in bias values of the accelerometers due to the internal tem-

perature of the inertial unit. Each accelerometer has a di�erent characteristic even

though they are the same make. This is simply because they are low cost sensors and

hence have dramatically di�erent 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 e�ect. 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 e�ect 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 t0�(u)Æu, then the

ensemble mean value is

E[x(t)] = E[

Z t

0

�(u)Æu]

=

Z t

0

E[�(u)]Æu = 0 (3.67)


0 0.5 1 1.5 2 2.5 3−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3Random Walk − X gyro

Time (min)

Ang

le (

degr

ees)

Figure 3.5: Several runs of integrating the x-gyro and obtaining a unique random

walk each time.

and the variance is

E[x2(t)] = E[

Z t

0

�(u)Æu

Z t

0

�(v)Æv] =

Z t

0

Z t

0

E[�(u)�(v)]ÆuÆv (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[x2(t)] =

Z t

0

Z t

0

Æ(u� v)ÆuÆv = t; (3.69)

and so the standard deviation of the noise is

�� =pt: (3.70)

Therefore without any external resetting properties, white noise will cause an un-

bounded 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 �� = Kpt. The

larger the magnitude of the noise, the greater the standard deviation of the error.

3.11 Alignment and Calibration 63

3.11 Alignment and Calibration

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 )

�1fn (3.71)

Since Cnb is orthogonal, its inverse is simply its transpose. The inertial unit is sta-

tionary and hence the only acceleration measured is that due to gravity along the

vertical axis. Thus

fn =h0 0 �g

iT: (3.72)

Equation 3.71 becomes

2664fxT

fyT

fzT

3775 =

2664

�c c �c s ��s

�c s + �s�s c �c c + �s�s s �s�c

�s s + �c�s c ��s c + �c�s s �c�c

3775

2664

0

0

�g

3775 (3.73)


where the subscript T represents the true acceleration component due to gravity.

Hence

fxT = g sin� (3.74)

fyT = �g sin � cos � (3.75)

fzT = �g cos � cos � (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 sen-

sors 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 suÆcient for navigation per-

formance 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 . Gy-

rocompassing 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 re-

solved. 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


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 (3.79)

� = sin�1(sin(bank)

cos �) (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 ac-

celerometers.

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 com-

pared 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 equa-

tions 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. Fur-

thermore, it is these errors on the gyros which cause the greatest error growth

since it a�ects 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 signi�cant 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 envi-

sioned to handle 24 satellites in almost the same con�guration 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 atmo-

sphere.

If both systems are combined then a theoretical doubling of the number of satel-

lites 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 di�erence

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 di�erence 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 tech-

niques 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 signi�es 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, di�erential

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 di�erenced 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

Di�erential 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

Base Station Rover

Differential Corrections

Figure 4.1: GNSS accuracies are dramatically improved when di�erential corrections

are applied. Since the base station is at a known position then the errors associatedwith 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

di�erencing. Accuracies down to the centimetre level are obtained when six satellites

are in view and employing a technique known as triple di�erencing.

Velocity data can be derived by time di�erencing 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 di�erence 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 ; PZg and velocity Vg = fVX ; VY ; VZg obtained from

4.2 GNSS 73

the GNSS receiver is delivered in the WGS-84 datum, represented by the ECEF co-

ordinate frame, Figure 4.2.

The latitude and longitude is obtained by transforming the position observation re-

ceived 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)

CoeÆcients :

p =

qPx

2 + Py2

t = tan�1(Pz � a

p� b)

e1 = 2f � f 2

e2 =a2 � b2

b2

latitude (degrees) � = tan�1(PZ + e2� b sin3 t

p� e1� a cos3 t)

longitude (degrees) = tan�1(PY

PX)

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.

Cng =

2664� sin� cos� � sin� sin� cos�

� sin� cos� 0

� cos � cos� � cos � sin� � sin�

3775 (4.1)

Thus the position and velocity of the vehicle in the local navigation frame is

Pn = CngPg (4.2)

and Vn = CngVg (4.3)

4.3 Overview of the Navigation Loops 74

X

Z

D

N

E

λ

φ Y

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

e�ectively �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 iner-

tial 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 ob-

servation vector is the observed error of the inertial system, that is, the di�erence

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 Newtow-

nian 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 sen-

sors 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 e�ect on the ac-

curacy 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 con-

sequently does not address the need for integrity inherent in an autonomous vehicle

4.4 GNSS Aiding 76

INS

KalmanFilter

GNSS


Observed Error+

-

Position andVelocity

Observations


(a) Inertial/GNSS navigation system.

IMU

Non-LinearKalman Filter

VehicleConstraints


VelocityObservations


(b) Inertial/Vehicle constraint navigation sys-tem

INS

KalmanFilter

GNSS


Observed Error

Position andVelocity

Observations


Vehicleconstraints +

wheel encoder

+

-+

-

VelocityObservations

Observed Error

(c) Inertial/GNSS/Vehicle Constraint/Speednavigation 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 ve-

hicle 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 10km2 (Figure 4.5). Thus di�erential

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

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 sig-

nal coming directly from the satellite then multipath can be removed. Furthermore,

multipath a�ects the L1 and L2 frequencies di�erently, and this e�ect can be used in

determining if multipath has occurred. Finally, redundancy in the number of satel-

lites also assists in removing the e�ect of multipath. Thus the ultimate receiver would

be a dual frequency GPS, dual frequency GLONASS unit. Such units are available

and o�er 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 di�erential 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 accu-

racy, this limits the number of potential redundant satellites. Furthermore, due to

obstructions by quay cranes and buildings there is always the real situation of see-

ing 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 (4.4)

Æ _v = [ ~f e�]Æ +CebÆf b (4.5)

_[Æ �] = �CebÆ!

bib: (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 clari�cation the transformation matrix is represented as

Cnb .

The navigation loop implements a linear Kalman �lter. The state vector consists

of the error states,

x =hÆpN ÆpE ÆpD ÆvN ÆvE ÆvD Æ N Æ E Æ D

iT: (4.7)

The process model is

F =

266666666666666666664

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 0 �fN

0 0 0 0 0 0 �fE fN 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

377777777777777777775

(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(F�t), 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(F�t)

= I+ F�t+(F�t)2

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 simpli�ed 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) =1

2

�F(k)G(k)Qc(k)G(k)TF(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 de�ned as

Qc(k) =

266664

Æ~p 0 0

0 Æfb 0

0 0 Æ!b

ib

377775 : (4.11)

Æ~p 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 speci�cations or through experimentation as discussed in Chapter 3.

G(k) is

G(k) =

266664

I3�3 0 0

0 Cnb (k) 0

0 0 �Cnb (k)

377775 : (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.

z(k) =

24 Pinertial(k)�PGNSS(k)

Vinertial(k)�VGNSS(k)

35 (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

H(k) =

24 I3�3 0 0

0 I3�3 0

35 (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) (4.16)

Vinertial(kjk) = Vinertial(kjk � 1)� Æv(kjk) (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) = [I3�3 � [Æ �]]Cn

b (kjk): (4.18)

Rearranging this equation as

Cnb (kjk) = [I3�3 � [Æ �]]�1Cn

b (kjk � 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�1 = AT ), and furthermore it is a skew-symmetric

4.4 GNSS Aiding 84

matrix (thus AT = �A), hence Equation 4.19 is written as

Cnb (kjk) = [I3�3 + [Æ �]]Cn

b (kjk � 1) (4.20)

where

[Æ �] =

2664

0 �Æ D Æ E

Æ D 0 �Æ N�Æ E Æ N 0

3775 (4.21)

Note that [Æ �] is in the navigation frame yet it is used to correct the Cnb matrix

whose elements are de�ned 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 a�ecting the �x of the Standard Di�er-

ential receiver, and also altering the phase of the signal thus a�ecting the Carrier

Phase Di�erential �x. Another high frequency fault, although it occurs less often

and with less e�ect, is when the receiver utilises a di�erent 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 con�guration 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 multi-

path 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 �2 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 signi�cantly 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 con�guration 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 a�ect 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.

4.4 GNSS Aiding 86

4.4.4 Filter Tuning

There are two stages in the �lter ow; the prediction stage where the predicted iner-

tial 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 obser-

vation 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 imple-

mented which can be obtained from the manufacturer or through experimentation.

The principle tuning parameters which need to be addressed are the variances of ve-

locity 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 suÆcient 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 suÆcient 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 di�erent 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 suÆcient 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 con�gured 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


CENTRALTRANSPUTER

INS algorithm

FILTERTRANSPUTER

1

Prediction

FILTERTRANSPUTER

2

Estimationand

Validation

OBSERVATIONTRANSPUTER

Conversion ofGPS from

WGS84 to NED

IMU

GPS

Acceleration and Rotation Rates

Position and Velocity in WGS84

Raw IMUGPS in NED

INS data

Observation andPrediction values

Prediction

Estimates

Figure 4.6: Real time architecture implementing four Transputers for parallel pro-

cessing.


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 be-

gins. Ten seconds is suÆcient 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 com-

puter 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 Chap-

ter 5.

4.6 Aiding Using a Vehicle Model

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 e�ort, 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 di�erent 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 di�erent 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.


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 =�VT

n ;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

_V n = Cnb fb (4.23)

_P n = Vn (4.24)

_ =!y sin � + !z cos �

cos �(4.25)

_� = !y cos � � !z sin � (4.26)

_� = !x + (!y sin � + !z cos �) tan� (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 di�erential equations that can

easily be solved using a variety of di�erent techniques.

3. It is possible to linearise these equations, for suÆciently small sampling inter-

vals, by incorporating all the elements of the direction cosine matrix Cnb into

the state equation.


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 (4.28)

Vz = 0 (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 sus-

pension 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 ve-

hicle, 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 � �y = 0 (4.30)

Vz � �z = 0 (4.31)

where �y and �z are Gaussian, white noise sources with zero mean and variance �2y

and �2z 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 =


[Vx; Vy; Vz]Tto Vn;

Vb = [Cnb ]TVn

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;

24 Vy

Vz

35 =M+

24 �y

�z

35 ; (4.32)

where M is

24 VN cos � cos + VE cos � sin � VD sin�

VN(sin � sin� cos � cos � sin ) + VE(cos � cos + sin � sin� sin ) + VD sin � cos �

35 :

(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


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. Intu-

itively, 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 incor-

porates 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


Vn , P n

Navigation Framen

Vehicle

Body Frameb

N

E

D

xy

z

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= _sbx:

Acceleration of the vehicle is given by,

f = _V = �sbx+ _s _bx:

As the angular velocity of the coordinate frame b is given by !b,

f = �sbx+ _s!b � bx;

f = �sbx+ _s!zby- _s!ybz:


Components of the acceleration of the vehicle in the body frame b become,

f.bx = �s; (4.37)

f.by = _s!z; (4.38)

f.bz = � _s!y: (4.39)

Given that fn = Cnb fb and using this in the above equations,

2664fx

fy

fz

3775 =

2664

_Vf

Vf!z

�Vf!y

3775 +

2664

�c c �c s ��s

��c s + �s�s c �c c + �s�s s �s�c

�s s + �c�s c ��s c + �c�s s �c�c

3775

2664

0

0

�g

3775 ;

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,

_Vf � fx + g sin� = 0; (4.40)

Vf!z � fy � g sin � cos � = 0; (4.41)

Vf!y + fz + g cos � cos � = 0: (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 di�eren-

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 signi�cant importance. The fact that the forward

acceleration is observable makes the forward velocity error growth only a func-

tion 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 diÆcult to incorporate models for constraint

violations in the solution. Also, when the constraint violation is signi�cant,

such as in o� road situations or cornering at high speeds, the white noise model

is inadequate. For example, if there is signi�cant side slip, explicit slip modeling

may be required.

Experimental results of this implementation are provided in Chapter 5.

4.7 Multiple Aiding of an Inertial System

This section will discuss the aiding of an inertial unit with three forms of external ob-

servations; 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 deliv-

ered asynchronously and at di�erent rates. It is shown that the additional sensors

signi�cantly improve the quality of the location estimate. This is of fundamental

importance since it makes the inertial system less dependent on external information.


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,

zvelocityV (k) = Cnb

0BB@

Vx(k)

Vy(k)

Vz(k)

1CCA (4.43)

=

0BB@

Vx(k) cos� cos

Vx(k) cos � sin

�Vx(k) sin�

1CCA : (4.44)

Thus the observation vector is

z(k) = zinertialV (k)� zvelocity

V (k):

The observation model is given by

Hvelocity

V =�03�3 I3�3 03�3

�(4.45)


and the observation covariance matrix is

Rvelocity

V =

0BB@�2x 0 0

0 �2y 0

0 0 �2z

1CCA : (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

�Rvelocity

= CnbR

velocityCnbT (4.47)

When the position and velocity are obtained from the GNSS receiver the observation

vector is

z(k) =

24 zinertialP (k)� zGNSS

P (k)

zinertialV (k)� zGNSSV (k)

35 (4.48)

The observation model is

HGNSS(k) =

0@I3�3 03�3 03�3

03�3 I3�3 03�3

1A ; (4.49)

and the observation noise matrix is

RGNSS(k) =

0BBBBBBBBBBB@

�2PN 0 0 0 0 0

0 �2PE 0 0 0 0

0 0 �2PD 0 0 0

0 0 0 �2V N 0 0

0 0 0 0 �2V E 0

0 0 0 0 0 �2V D

1CCCCCCCCCCCA

(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 nav-

igation 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 com-

prises 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 con-

verting 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. Fur-

thermore, 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 pri-

mary concerns associated with such an implementation are processing power and

latency. The real issue of latency occurs with the delay in processing the GNSS in-

formation within the GNSS receiver. Velocity latencies of up to a second can occur.

Both the hardware and software implementation to address these e�ects was detailed.

4.8 Conclusion 103

The second navigation system implemented consisted of an inertial navigation sys-

tem 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 suÆcient 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. Con-

straint equations were developed along with the non-linear Kalman �lter implementa-

tion 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 veloc-

ity 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 naviga-

tion system implementation involves the aiding of an inertial navigation system using

the vehicle model constraints and speed provided by a drive shaft encoder. Further-

more, 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 infor-

mation 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 signi�cant 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.

5.2 Experimental Setup

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 con-

tainers 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


Figure 5.1: The utility is used as a test bed for the sensors. It houses the Transputerswhich 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 andhence a high amount of integrity is required.


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 andelevation of the inertial unit.

The GNSS equipment implemented includes:

� An Ashtech G12 GPS in standard di�erential 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


Speci�cation Value

Rate Range �50o=sRate Bias 2% FS

Rate Resolution < 0:05o=s

Acceleration Range �2gAcceleration Accuracy < 0:5% FS

Acceleration Bias < 0:5% FS

Acceleration Resolution < 2 mG0s

Sensor Alignment < 0:25o

Accelerometer Frequency 300 Hz

Gyro Frequency 70 Hz

Table 5.1: The speci�cation 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


Figure 5.5: The Novatel RT2

the �lter is modi�ed 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.


Figure 5.6: Outside the University campus showing extensive tree foliage and build-

ings which will a�ect the performance of the GPS receiver.

Figure 5.7: A major road outside of the University campus where multipath occursdue to the building structures.


Figure 5.8: The utility was driven in a relatively open area where sparse buildingstructures are found. Since the sky is relatively open to the receiver, minimal faultswill 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.


Figure 5.10: The port where the straddle carrier is located comprises mainly of con-

tainers and quay cranes. Although the containers have no a�ect on the GNSS signalthe quay cranes do and hence fault detection is particularly important.


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

5.3 Inertial/GNSS Results

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 oc-

curred. 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.


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 oc-

curred. 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 o�set in acceleration causes the velocity of the vehicle to drift

before being corrected by the next GPS �x. This o�set 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.


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 suÆciently 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 matrixQ(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

5.4 Vehicle Model Constraint Results 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 a�ects the velocity corrections even though the position of

the vehicle is judged acceptable.

5.4 Vehicle Model Constraint Results

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 con-

ducted 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 sim-

ulations 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 prede�ned trajec-

tory 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 e�ect 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 diÆcult 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

5.4 Vehicle Model Constraint Results 118

expected, any excitation due to !y and !z results in a zero error in predicted vehicle

speed whereas motion in !x has no e�ect 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 e�ect 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 accept-

able limits and in turn allow the constrained system to be comparable with the

Inertial/GNSS navigation system.

5.5 Multiple Aiding Results

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 im-

plemented 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 as-

sumed 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.

5.5 Multiple Aiding 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 atti-

tude 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

5.6 Inertial/GNSS Plots

−2000 −1500 −1000 −500 0−500

0

500

EAST (m)

NO

RT

H (

m)

POSITION

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 ontothe vehicle.

−500 −400 −300 −200 −100 0 100 200

−300

−200

−100

0

100

EAST (m)

NO

RT

H (

m)

POSITION

Figure 5.14: Fusion of inertial and GPS data with no multipath rejection.


−200 −190 −180 −170 −160 −150 −140 −130 −120 −110 −100−360

−350

−340

−330

−320

−310

−300

−290

EAST (m)

NO

RT

H (

m)

POSITION

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 multi-

path 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.

−100 −50 0 50 100

−150

−100

−50

0

50

100

150

EAST (m)

NO

RT

H (

m)

POSITION

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 inertialnavigation solution closely follows the GPS multipath.


−200 −190 −180 −170 −160 −150 −140 −130 −120 −110 −100−360

−350

−340

−330

−320

−310

−300

−290

EAST (m)

NO

RT

H (

m)

POSITION

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 thisis due to the accuracy of the GPS receiver.

−100 −50 0 50 100

−150

−100

−50

0

50

100

150

EAST (m)

NO

RT

H (

m)

POSITION

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.


−300 −250 −200 −150 −100 −50 0 50−50

0

50

100

150

200

250POSITION

EAST (m)

NO

RT

H (

m)

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.


−69 −68 −67 −66 −65 −64 −63 −62 −61 −60 −5955

56

57

58

59

60

61POSITION

EAST (m)

NO

RT

H (

m)

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 thevehicle on the return towards the end of the run. In this example the initial heading

is given the correct value.

−69 −68 −67 −66 −65 −64 −63 −62 −61 −60 −5955

56

57

58

59

60

61POSITION

EAST (m)

NO

RT

H (

m)

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.


1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.5

0

0.5ACCELERATION

Nor

th (

m/s

2 )

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.5

0

0.5

Eas

t (m

/s2 )

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.5

0

0.5

iteration

Dow

n (m

/s2 )

Figure 5.22: Enhanced view of the acceleration of the vehicle at the end of the runwith attitude correction.

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.2

−0.1

0

0.1

0.2VELOCITY

Nor

th (

m/s

)

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.2

−0.1

0

0.1

0.2

Eas

t (m

/s)

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.2

−0.1

0

0.1

0.2

iteration

Dow

n (m

/s)

Figure 5.23: Enhanced view of the velocity of the vehicle at the end of the run with

attitude correction.


1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.5

0

0.5ACCELERATION

Nor

th (

m/s

2 )

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.5

0

0.5

Eas

t (m

/s2 )

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.5

0

0.5

iteration

Dow

n (m

/s2 )

Figure 5.24: Enhanced view of the acceleration of the vehicle at the end of the runwithout attitude correction.

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.2

−0.1

0

0.1

0.2VELOCITY

Nor

th (

m/s

)

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.2

−0.1

0

0.1

0.2

Eas

t (m

/s)

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−0.2

−0.1

0

0.1

0.2

iteration

Dow

n (m

/s)

Figure 5.25: Enhanced view of the velocity of the vehicle at the end of the runwithout attitude correction.


1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5PITCH − CBN (dark) and Pendulum Gyro (light)

iteration

Ang

le (

deg)

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 lighterline is the pitch as determined by the tilt sensors.

1.33 1.335 1.34 1.345 1.35 1.355 1.36 1.365 1.37 1.375 1.38

x 104

−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5PITCH − CBN (dark) and Pendulum Gyro (light)

iteration

Ang

le (

deg)

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. Thelighter line is the pitch as determined by the tilt sensors.


−140 −120 −100 −80 −60 −40 −20 0 20 40−120

−100

−80

−60

−40

−20

0

20POSITION

EAST (m)

NO

RT

H (

m)

Figure 5.28: Position of the straddle carrier as it manoeuvres around containers beforedriving under a quay crane. The vehicle starts at position 0m North; 0m East and

moves in a counter clockwise direction.


−60 −50 −40 −30 −20 −10 0 10 20−120

−110

−100

−90

−80

−70

−60

−50POSITION

EAST (m)

NO

RT

H (

m)

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 thereare no GPS �xes. During this period the inertial errors are not corrected however, theon-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 areaccurate.

−40 −35 −30 −25 −20 −15 −10 −5 0−110

−105

−100

−95

−90

−85

−80POSITION

EAST (m)

NO

RT

H (

m)

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.


0 100 200 300 400 500 600 700−2

−1

0

1

2POSITION INNOVATIONS

Nor

th (

m)

0 100 200 300 400 500 600 700−2

−1

0

1

2

Eas

t (m

)

0 100 200 300 400 500 600 700−2

−1

0

1

2D

own

(m)

iteration

Figure 5.31: The position innovations of the �lter show that the �lter places moreemphasis on the inertial position evaluations. This is due to the large uncertainty of

the position �xes delivered by this GPS unit.

0 100 200 300 400 500 600 700

−0.2

−0.1

0

0.1

0.2

VELOCITY INNOVATIONS

Nor

th (

m/s

)

0 100 200 300 400 500 600 700

−0.2

−0.1

0

0.1

0.2

Eas

t (m

/s)

0 100 200 300 400 500 600 700

−0.2

−0.1

0

0.1

0.2

Dow

n (m

/s)

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 theposition determination delivered by the inertial data will also be accurate. Hence the

greater certainty in the position evaluations as illustrated in the position innovations.


−70 −60 −50 −40 −30 −20 −10 0 10 20 30

−100

−90

−80

−70

−60

−50

−40

−30

−20

POSITION

EAST (m)

NO

RT

H (

m)

Figure 5.33: With the same tuning parameters but with no fault detection routinesthe inertial solution closely follows the GPS multipath errors. During this period the

inertial errors are inaccurately corrected and hence the position estimates overshootthe correct path taken by the vehicle.

−40 −35 −30 −25 −20 −15 −10 −5 0−110

−105

−100

−95

−90

−85

−80POSITION

EAST (m)

NO

RT

H (

m)

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.


1000 1050 1100 1150 1200 1250 1300−15

−10

−5

0

5

POSITION INNOVATIONS

Nor

th (

m)

1000 1050 1100 1150 1200 1250 1300−20

−10

0

10

Eas

t (m

)

1000 1050 1100 1150 1200 1250 1300

−5

0

5

10

15

Dow

n (m

)

iteration

Figure 5.35: The position innovations show where multipath errors occur.

1000 1050 1100 1150 1200 1250 1300

−2

0

2

VELOCITY INNOVATIONS

Nor

th (

m/s

)

1000 1050 1100 1150 1200 1250 1300

−2

−1

0

1

2

Eas

t (m

/s)

1000 1050 1100 1150 1200 1250 1300

−1

0

1

Dow

n (m

/s)

iteration

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.


−60 −50 −40 −30 −20 −10 0 10 20−120

−110

−100

−90

−80

−70

−60

−50POSITION

EAST (m)

NO

RT

H (

m)

Figure 5.37: Keeping with no GPS fault detection, the path of the vehicle can bemade 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.


0 200 400 600 800 1000 1200 1400−20

−10

0

10POSITION INNOVATIONS

Nor

th (

m)

0 200 400 600 800 1000 1200 1400−20

−10

0

10

20

Eas

t (m

)

0 200 400 600 800 1000 1200 1400−10

0

10

20

Dow

n (m

)

iteration

Figure 5.38: The position innovations show that the �lter is behaving inconsistentlyeven when it is tuned to place little emphasis on the GPS �xes.

200 300 400 500 600 700 800 900 1000 1100 1200−1

−0.5

0

0.5

1VELOCITY INNOVATIONS

Nor

th (

m/s

)

200 300 400 500 600 700 800 900 1000 1100 1200−1

−0.5

0

0.5

1

Eas

t (m

/s)

200 300 400 500 600 700 800 900 1000 1100 1200−1

−0.5

0

0.5

1

Dow

n (m

/s)

iteration

Figure 5.39: The velocity innovations further magnify the inconsistency of the �lterwhen it is tuned to seemingly reject multipath errors.


0 2000 4000 6000 8000 10000 12000−10

−5

0

5

10VELOCITY

Nor

th (

m/s

)

0 2000 4000 6000 8000 10000 12000−5

0

5

10

Eas

t (m

/s)

0 2000 4000 6000 8000 10000 12000−0.4

−0.2

0

0.2

0.4

iteration

Dow

n (m

/s)

Figure 5.40: Illustration of the velocity of the vehicle when it is properly tuned andwith GPS fault detection.

0 2000 4000 6000 8000 10000 12000−10

−5

0

5

10VELOCITY

Nor

th (

m/s

)

0 2000 4000 6000 8000 10000 12000−5

0

5

10

Eas

t (m

/s)

0 2000 4000 6000 8000 10000 12000−1

0

1

2

iteration

Dow

n (m

/s)

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 arounditeration 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

5.7 Vehicle Model Constraint Plots

200 400 600 800 1000 1200 1400 1600 1800

−30

−20

−10

0

10

20

30

40

50

Time (sec)

Err

or in

Vel

ocity

(m

/sec

)

Constrained INS Direct Integration

Figure 5.42: Errors in vehicle speed when the vehicle is moving at a constant velocityof 10 m/s while the angular velocity !x is non-zero in the time interval 700� 1300s.

200 400 600 800 1000 1200 1400 1600 1800

−30

−20

−10

0

10

20

30

Time (sec)

Err

or in

Vel

ocity

(m

/sec

)


Figure 5.43: Errors in vehicle speed when the vehicle is moving at a constant velocity

of 10 m/s while the angular velocity !y is non-zero in the time interval 700� 1300s.


200 400 600 800 1000 1200 1400 1600 1800

−20

−10

0

10

20

30

Time (sec)

Err

or in

Vel

ocity

(m

/sec

)


Figure 5.44: Errors in vehicle speed 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.

0 200 400 600 800 1000 1200 1400 1600 1800 2000−0.05

0

0.05

0.1

0.15

Time (sec)

Err

or in

pitc

h(ra

d)

0 200 400 600 800 1000 1200 1400 1600 1800 2000−0.1

−0.05

0

0.05

0.1

Time (sec)

Err

or in

rol

l(rad

)

0 200 400 600 800 1000 1200 1400 1600 1800 2000−0.1

0

0.1

0.2

0.3

Time (sec)

Err

or in

yaw

(ra

d)


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 interval700-1300 sec.


0 200 400 600 800 1000 1200 1400 1600 1800 2000−0.15

−0.1

−0.05

0

0.05

Time (sec)

Err

or in

pitc

h(ra

d)

0 200 400 600 800 1000 1200 1400 1600 1800 2000−0.4

−0.2

0

0.2

0.4

Time (sec)

Err

or in

rol

l(rad

)

0 200 400 600 800 1000 1200 1400 1600 1800 2000−0.3

−0.2

−0.1

0

0.1

Time (sec)

Err

or in

yaw

(ra

d)


Figure 5.46: Errors in roll, pitch and yaw when the vehicle is moving at a constantvelocity of 10 m/s while the angular velocity !z is non-zero in the time interval

700� 1300s. A constant unestimated bias of 10�4 rad/s is introduced to all angularvelocity observations

200 400 600 800 1000 1200 1400 1600 1800

−100

−80

−60

−40

−20

0

20

40

60

Time (sec)

Err

or in

Vel

ocity

(m

/sec

)


Figure 5.47: Errors in the vehicle speed when the vehicle is moving at a constant

acceleration of 0.05 m=s2 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.


−300 −250 −200 −150 −100 −50 0 50

−50

0

50

100

150

200

POSITION

EAST (m)

NO

RT

H (

m) <−−−IMU/GPS

Raw data<−−−−−

Constrained data<−−−

Figure 5.48: The result of the three di�erent 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 inaccurateposition results. The di�erence between the position obtained by the Inertial/GPS

fusion and the proposed algorithm is too small to be clearly seen in this �gure.


0 5000 10000 15000−1000

−500

0

500POSITION ERROR

Nor

th (

m/s

) |

Raw data

Constrained data

v

−>

0 5000 10000 15000−200

0

200

400

Eas

t (m

/s)

|

Raw data

Constrained data

−>

0 5000 10000 15000−20

0

20

40

iteration

Dow

n (m

/s)

||

Raw data Constrained data

v

−>

Figure 5.49: The position error of the constrained and raw data. The referenceposition 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 theerror bounded.

0 5000 10000 15000−20

−10

0

10VELOCITY ERROR

Nor

th (

m/s

)

Constrained data

Raw data −>

|v

0 5000 10000 15000−5

0

5

10

Eas

t (m

/s)

Constrained data

Raw data −>

|

0 5000 10000 15000−1

0

1

2

iteration

Dow

n (m

/s)

Constrained data

Raw data|v

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.


1.36 1.38 1.4 1.42 1.44 1.46

x 104

−2

−1.5

−1

−0.5

0

ROLL

iteration

Ang

le (

deg)

Roll tilt sensor<−−

Constrained data

Raw data

IMU/GPS|

|

−−−−

Figure 5.51: The roll data from the three di�erent cases. The tilt information is pre-sented for comparison. As can be seen the constrained data follows the Inertial/GPSsolution quite well. The raw data provides good results as well.

1.34 1.36 1.38 1.4 1.42 1.44

x 104

−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

PITCH

iteration

Ang

le (

deg)

Raw data

IMU/GPS

Constrained data

Pitch tilt sensor<−−

|

||

||

|

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 datalie 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 velocityevaluation.


1.39 1.395 1.4 1.405 1.41 1.415 1.42 1.425 1.43 1.435 1.44

x 104

−0.65

−0.6

−0.55

−0.5

−0.45

−0.4

−0.35

−0.3

−0.25

−0.2

ROLL ERROR

iteration

Ang

le (

deg)

IMU/GPS

Constrained data

Raw data

|

|

|

v

Figure 5.53: Error in roll at the end of the trajectory for the three cases. The tiltsensor information is used as the reference.

1.36 1.37 1.38 1.39 1.4 1.41 1.42 1.43 1.44

x 104

0

0.2

0.4

0.6

0.8

1

1.2

PITCH ERROR

iteration

Ang

le (

deg)

Raw data

Constrained data

IMU/GPS<−−

<−−

|v

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 bene�t of using theconstraint equations.

5.8 Multiple Aiding Plots 144

5.8 Multiple Aiding Plots

−100 0 100 200 300 400 500 600 700−50

0

50

100

150

200

250

300POSITION

EAST (m)

NO

RT

H (

m)

Figure 5.55: Position plot of the path taken by the vehicle. This path was obtained

by using the Inertial/GPS navigation system.


0 20 40 60 80 100 120 140 160 180 2000

50

100

150

200

Nor

th E

rror

(m

)

Time (s)

Position Error

| Constrained Inertial

Free Inertial −

0 20 40 60 80 100 120 140 160 180 2000

100

200

300

400

500E

ast E

rror

(m

)

Time (s)


Free Inertial −

Figure 5.56: These two plots show the error growth in position of the inertial unit freeof any external observations and when it is fused with the vehicle model constraints.

0 20 40 60 80 100 120 140 160 180 2000

0.5

1

1.5

2

2.5

3

Nor

th E

rror

(m

/s)

Time (s)

Velocity Error


Free Inertial −

0 20 40 60 80 100 120 140 160 180 2000

2

4

6

8

Eas

t Err

or (

m/s

)

Time (s)

Free Inertial −


Figure 5.57: Plots of error growth in velocity of the inertial unit when it is perform-

ing free of any external observations and when it is fused with the vehicle model

constraints.


0 20 40 60 80 100 120 140 160 180 2000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Rol

l Err

or (

deg)

Time (s)

Attitude Error

Free Inertial −


0 20 40 60 80 100 120 140 160 180 2000

0.1

0.2

0.3

0.4

0.5

0.6

0.7P

itch

Err

or (

deg)

Time (s)

Free Inertial −


Figure 5.58: Plots of attitude error. These errors cause the velocity and hence positionerror growth shown in the previous plots. The constrained inertial data has been

bounded extremely well when compared to the free inertial data.

0 20 40 60 80 100 120 140 160 180 2000

1

2

3

4

5

6

Nor

th E

rror

(m

)

Time (s)

Position Error


| Constrained + GPS

0 20 40 60 80 100 120 140 160 180 2000

5

10

15

20

25

30

35

Eas

t Err

or (

m)

Time (s)


| Constrained + GPS

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 15seconds. Since position is unobservable when only using the vehicle model constraints,

the GPS dramatically improves the result.


0 20 40 60 80 100 120 140 160 180 2000

0.2

0.4

0.6

0.8

1

1.2

1.4

Nor

th E

rror

(m

/s)

Time (s)

Velocity Error


| Constrained + GPS

0 20 40 60 80 100 120 140 160 180 2000

0.5

1

1.5

2E

ast E

rror

(m

/s)

Time (s)

| Constrained + GPS


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 bene�ts the velocity estimate only slightly.

0 20 40 60 80 100 120 140 160 180 2000

0.1

0.2

0.3

0.4

0.5

Rol

l Err

or (

deg)

Time (s)

Attitude Error

| Constrained + GPS


0 20 40 60 80 100 120 140 160 180 2000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Pitc

h E

rror

(de

g)

Time (s)

| Constrained + GPS


Figure 5.61: The attitude estimation only bene�ts slightly from the GPS information

since the velocity information provided by the vehicle model constraints delivers a

signi�cant 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 pro-

vided 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 re-

jection 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 di�erent vehicles and in di�erent en-

vironments without any modi�cation 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 naviga-

tion 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 iner-

tial 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 eÆciency of aiding an iner-

tial 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 ob-

servable. Furthermore, the aiding by vehicle model constraints allows periodic loss of

GPS signal without a signi�cant 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. Al-

though 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 a�ects 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 simi-

lar redundant data, and a better conceptual understanding of how accuracy and fault

detection are a�ected by di�erent sensor con�gurations. This forms one of the main

contributions of this chapter.

A second contribution comes in the development of a redundant inertial measure-

ment 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 detec-

tion 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 di�erent

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

6.3 Con�guration of Redundant Inertial Sensors 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.

6.3 Con�guration of Redundant Inertial 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 Euro�ghter 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 or-

thogonal 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 re-

dundant 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


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 con�gure 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 con�guration for any

number of sensors was developed. The work considers the situation where any num-

ber 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 con�guration. 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

� = cos�1 1p3

(6.1)

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

� = cos�1

rn� 3

3n� 3(6.2)

where n is the number of sensors. Figures 6.1 and 6.2 illustrate the situation where

�ve sensors are con�gured in these two di�erent cases.

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 di�er-


X

Z

Y

72 deg.

α

Figure 6.1: One of the optimal con�gurations for �ve sensors. The cone's half angle

is 54:74Æ and the sensors are separated equally by 72Æ.

X

Z

Y

90 deg.

α

Figure 6.2: Another optimal con�guration 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Æ.


ent con�gurations 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 clari�cation.

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 obser-

vation 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 depend-

ing 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 observa-

tion 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�1(k)z�1(k) (6.4)

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

matrix is given by

I(k) = HT (k)R�1(k)H(k): (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 matrixR(k) is diagonal with equal variances, that


is,

R(k) =

0BBBBB@

�21 0 0 0

0 �22 0 0

0 0. . . 0

0 0 0 �2n

1CCCCCA

(6.6)

where n is the number of sensors employed. Thus both the information vector and

information matrix become

i(k) =1

�2HT (k)z(k) (6.7)

I(k) =1

�2HT (k)H(k) (6.8)

Since the information matrix is positive semi-de�nite, its corresponding eigenvec-

tors are orthogonal to one another. These eigenvectors represent the axes of an ellip-

soid in information space. If the eigenvalues of these eigenvectors are di�erent, 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 in-

formation 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 con�guration

for equal information in all directions and maximum information will be obtained.

Thus a suÆcient measure of information is

max[jI(k)j] = max[jH(k)TH(k)j]: (6.9)

Note that 1�2

is not required since it is simply a scaling factor.

In the two dimensional case, the optimal con�guration for n identical sensors is

when they are equally spaced on a unit circle. That is, the con�guration takes on the


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 con�guration, the con�guration can take

any orientation. However, for 4, 6, 8, 12 and 20 sensors, the corresponding optimal

sensor con�gurations 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 con�gurations is new and provides an

immediate visual understanding of the distribution of information. For the remaining

non-regular sensor con�gurations the results obtained were taken from [45]. From

these results it was found that regardless of the number of sensors, once an optimal

con�guration was found, the eigenvalues equal the number of sensors divided by three

(the number of dimensions). That is,

�i =n

3

for all i = 1:::n (6.10)

This suggests that, in information space, each sensor contributes a third of its infor-

mation 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 con�guration and comparing it to the standard orthog-

onal con�guration. Furthermore, when the con�guration is optimal, the determinant

is equal to the product of the eigenvalues, that is,

D = (n

3)3: (6.11)

thus the gain in information by adding in an extra sensor can be easily evaluated.


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 con�guration. 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)TH(k)

=

0BB@2 cos2 � 0 0

0 2 cos2 � 0

0 0 4 sin2 �:

1CCA (6.12)

The determinant, representing the volume of information, is

J = det(I(k))

= 16 cos4(�) sin2(�): (6.13)

The maximum determinant is obtained by di�erentiating and setting to zero,

� = tan1p2

2

= 54:74o; (6.14)

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� I2;2(k) = I3;3(k);


a solution for the cone angle of

� = tan�1

p6

2

= 39:23o: (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 require-

ment 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 con�gure 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 con�gured 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 con�guration 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 con�guration. As the noise

variance is increased on one of the sensors, then the remaining three sensors recon-

�gure themselves so that they approach an orthogonal con�guration (the optimal for

three sensors).

Situations requiring non-equal variances will arise when the acceleration or rota-

tion rate along the three axes need to cover dramatically di�erent ranges. Examples


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

X

Z

Y

Figure 6.3: The cones in this plot show the area de�ned 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 con�guration of the inertial suite does not need to bealtered.

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 applica-

tion 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


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 sec-

ond 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

di�erence between the range as obtained by the satellites and the predicted

range forms the residual. The sum of the residuals forms a �2 distribution,

assuming the noise on the range is zero mean Gaussian. Thus standard �2 tests

can be performed to determine if there is a fault.

3. Finally the \parity space" method as proposed in [50] forms its detection rou-

tines in parity space which is orthogonal to the observation space and hence has


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 iner-

tial units. These have ranged from standard voting schemes to maximum likelihood

strategies [61] to arti�cial 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 con�guration 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 con�gurations are optimal and are

shown in Figures 6.1 and 6.2, and have an information content value of 4:6296 as eval-

uated by Equation 6.9. If a sensor fails in either con�guration, then the information

content will be less than that for �ve sensors, 1:8519 (which is signi�cantly less than

the optimal con�guration for four sensors, 2:3704). However, although the informa-

tion content may be equal in these two con�gurations, the information distribution

is not. This is easily con�rmed by removing the central cone sensor as the failed one

from Figure 6.2, and removing a sensor from the con�guration represented by Figure

6.1, and visualising the �nal con�guration in both with respect to their information

6.5 Truncated Tetrahedron 164

distribution.

Hence fault detection and isolation, with the consideration of the resulting con�g-

uration's a�ect on navigation performance is of paramount importance.

6.5 Truncated Tetrahedron

The remaining part of this chapter will discuss the development of a low cost redun-

dant inertial unit.

The primary objective is to develop a low cost inertial unit which provides an in-

crease 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 con�gured in two di�erent optimal con�gurations, namely the tetrahedron or

a half-octahedron. These two con�gurations 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 de-

sign and manufacture of these sensors. Tables 6.1 and 6.2 provide the speci�cations

of the gyros and accelerometers respectively.

The sensors were con�gured 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 modi�ed so that the


No.63372 No.63363 No.50093 No.63352

Range Æ=s �500 �200 �200 �200Scale Factor mV=Æ=s

Temperature +20 10.065 24.829 24.721 24.882-30 9.656 23.862 23.719 23.956

+60 9.585 23.574 23.240 23.647

Linearity Æ=s

Temperature +20 0.050 0.020 0.04 0.020-30 -0.300 0.040 -0.82 -0.060

+60 0.050 0.020 -0.08 0.040

Bias Æ=s

Temperature +20 0.041 - 0.070 -0.013 0.028

-30 -1.226 -0.912 0.04 1.241+60 0.147 -0.893 -0.08 0.441

Bias Repeatability Æ=s

Temperature +20 0.003 0.018 0.008 0.005

Table 6.1: Speci�cations 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: Speci�cations of the accelerometers implemented. The top row represents

the model numbers of the individual sensors.


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 temper-

ature compensation can be used, the switch-on to switch-on variability is too great

to be of any bene�t. The more appropriate method is to heat the unit to a �xed

temperature. Attached to the block are three heating devices which raise the tem-

perature 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-


proximately 70Hz the sampling rate is suÆcient 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 Speci�cation

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 andthe 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 silvercylinders are the accelerometers.

6.6 Results

This section provides limited results to show that Tetrad functions according to spec-

i�cations. 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 di�erence 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 di�erent from

that as provided by the Tetrad data. An example of sideslip during this test is shown

in Figure 6.17. This does not a�ect 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 signi�cantly 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 boththe 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 120sare 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 di�erence between the Tetrad and GPS data. This is due to in-

terpretation; the GPS heading data is provided by GPS velocity while the Tetradheading data is provided by the integration of the gyros. Thus during sideslip thetwo results will be di�erent. The �lter is not a�ected 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 di�erence 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 onlybeen 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 hasbeen 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 con�guration of any number of

sensors, not only veri�es previous work, but also adds to it by allowing the visual in-

terpretation of how the information is spread. This is bene�cial for the determination

of the correct con�guration to implement once factors such as resolution, sensitivity

and the resulting con�guration 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 con�guration, thus providing a complete de�-

nition.

Combining the information structure with the cone con�guration, the theory also

provides for a method of obtaining a con�guration with directed information. It was

also shown that by adding an extra sensor and �nding the optimal con�guration re-

sults in a 33% increase in information. Furthermore, given the optimum con�guration

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 stabili-

sation 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 e�ect of error propaga-

tion, 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 signi�cantly 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 uctua-

tion 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, mis-

alignment and scale factor errors; recti�cation of these errors will improve the

navigation results.

The main contributions of this chapter are

� Providing a detailed understanding of the errors e�ecting 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 uncer-

tainty 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 combina-

tion of these two sensors along with the fault detection techniques proved to be


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 posi-

tion 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 situ-

ation 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 ap-

proach would be ideal for land vehicle applications since a single satellite could

provide valuable aiding information. Such a structure would be bene�cial in

situations such as mining where signi�cant 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 satis�ed 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

satis�ed. 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-


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 per-

formance for extended periods of time without GNSS. Furthermore, by using

the information �lter, multiple observations can be applied with less compu-

tational 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 en-

sured 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 iner-

tial 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.


� 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 con�guration of redundant iner-

tial components and also evaluating the appropriate con�guration 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

a�ected by di�erent sensor con�gurations. The culmination of the previous

chapters coupled with the theory developed in this chapter led to the devel-

opment 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


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 iner-

tial unit, then one not only encapsulates the total movement of the vehicle,

but also ensures that no single node controls the complete navigation archi-

tecture, thus increasing the integrity of the navigation suite. For example,

12 sensors con�gured on a dodecahedron form the optimal con�guration.

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 con�guration, then

their total information will be optimal.

� Finally, a more ambitious scheme would be to develop an inertial unit

that is recon�gurable 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 con�gured

to a con�guration 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.


� 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

Journal of Guidance and Control, 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 Journal of Guidance and Control,

pages 590{592, November 1988.

[3] Y. Bar-Shalom and X. Li. Estimation and Tracking - Principles, Techniques and

Software. Artech House, Inc, 1993.

[4] D.O. Benson. A Comparison of Two Approaches to Pure-Inertial and Doppler-Inertial Error Analysis. In IEEE Transactions on Aerospace and Electronic Sys-

tems, pages 447{455, July 1975.

[5] J. Borenstein. Experimental Evaluation of Fibre Optic Gyroscopes for Improving

Dead Reckoning Accuracy in Mobile Robots. In IEEE International Conference

on Robotics and Automation, pages 3456{3461, May 1998.

[6] J. Borenstein, H.R. Everett, L. Feng, and D. Wehe. Mobile Robot Positioning

Sensors and Techniques. In Journal of Robotic Systems, volume 14, No.4, pages231{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 IEEE Position, Location and

Navigation Symposium, pages 74{82, 1990.

[8] K.R. Britting. Inertial Navigation System Analysis. 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 IEEE Position, Location and Navigation

Symposium, pages 399{406, 1990.

[10] A.B. Chat�eld. Fundamentals of High Accuracy Inertial Guidance. American

Institute of Aeronautics and Astronautics, Inc, 1997.

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 IEEE

International Conference on Robotics and Automation - Planetary Rover Tech-

nology and Systems Workshop, 1996.

[12] S. Clark. Autonomous Land Vehicle Navigation using Millimetre Wave Radar.

PhD thesis, Department of Mechanical and Mechatronic Engineering, The Uni-

versity of Sydney, 1999.

[13] S. Cooper. A Frequency Response Method for Sensor Suite Selection with an

Application to High-Speed Vehicle Navigation. PhD thesis, Department of Engi-

neering Science, University of Oxford, 1996.

[14] B.S. Davis. Using Low-Cost MEMS Accelerometers and Gyroscopes as Strap-

down IMUs on Rolling Projectiles. In IEEE Position, Location and Navigation


[15] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durrant-Whyte. A New Algo-

rithm for the Alignment of Inertial Measurement Units without External Ob-servation for Land Vehicle Applications. In IEEE International Conference on

Robotics and Automation, volume 3, pages 2274{2279, May 1999.

[16] R.E. Ebner and J.G. Mark. Redundant Integrated Flight-Control/Navigation

Inertial Sensor Complex. In Journal Guidance and Control, volume 1, pages143{149, 1978.

[17] G. Fetzer, W. Golderer, and J. Gerstenmeier. Yaw Rate Sensor in Silicon Mi-

cromachining Technology for Automotive Applications. In Symposium of Gyro

Technology, pages 8.0{8.8, 1998.

[18] D.J. Flynn. A Discussion of Coning Errors Exhibited by Inertial NavigationSystems. In Royal Aircraft Establishment. Technical Memorandum Rad-Nav 243,1984.

[19] J.P. Gilmore and R.A. McKern. A Redundant Strapdown Inertial Reference Unit(SIRU). In Journal of Spacecraft and Rockets, volume 9, pages 39{47, January

1972.

[20] G. Giralt, L. Boissier, and L. Marechal. The Iares Project: Rovers for the HumanConquest of the Moon and Mars. In IEEE International Conference on Robotics

and Automation - Planetary Rover Technology and Systems Workshop, 1996.

[21] R.L. Greenspan. Gps and inertial navigation. In Global Positioning System:

Theory and Applications, volume 2, chapter 7. American Institute of Aeronautics

and Astronautics, Inc, 1996.

BIBLIOGRAPHY 189

[22] M.S. Grewal and A.P. Andrews. Kalman Filtering - Theory and Practice.

Prentice-Hall, Inc, 1993.

[23] G. Hyslop, D. Gerth, and J. Kraemer. GPS/INS Integration on the Stando� Land

Attack Missile (SLAM). In IEEE Position, Location and Navigation Symposium,

pages 407{412, 1990.

[24] M. Jeerage and K. Boettcher. Fault Tolerant Highly Reliable Inertial Navigation

System. In IEEE Position, Location and Navigation Symposium, pages 456{460,

1986.

[25] M. Koifman and I.Y. Bar-Itzhack. Inertial Navigation System Aided by AircraftDynamics. In IEEE Transactions on Control Systems Technology, pages 487{493,

July 1999.

[26] X. Kong. Development of a Non-Linear Psi-Angle Model for Large Misalign-

ment Errors and its Application in INS Alignment and Calibration. In IEEE

International Conference on Robotics and Automation, pages 1430{1435, May

1999.

[27] U. Krogmann. Arti�cial Neural Networks for Inertial Sensor Fault Diagnosis. In

Symposium of Gyro Technology, 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 IEEE International

Conference on Robotics and Automation - Planetary Rover Technology and Sys-

tems Workshop, 1996.

[29] W.J. Kubat. Application of Strapdown Inertial Navigation to High PerformanceAircraft. In AGARD Lecture Series No.95, Strap-Down Inertial Systems, pages

7.1{7.16, 1978.

[30] A. Lawrence. Modern Inertial Technology. 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 Proceedings of the Annual

Meeting of the Institute if Navigation, pages 1{4, June 1986.

[32] A. Leick. GPS Satellite Surveying. John Wiley and Sons, Inc, 1995.

[33] A.H. Lewantowicz. Architectures and GPS/INS Integration: Impact on MissionAccomplishment. In IEEE Position, Location and Navigation Symposium, pages

284{289, 1992.

[34] P. Maybeck. Stochastic Models, Estimation and Control, volume 1,2. Academic

Press, 1982.

BIBLIOGRAPHY 190

[35] P. Maybeck. Stochastic Models, Estimation and Control, volume 1. Academic

Press, 1982.

[36] A. Medvedev. Fault Detection and Isolation by a Continuous Space Method. In

Automatica, 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 IEEE Position, Location and Navigation


[38] G. Minkler and J. Minkler. Aerospace Coordinate Systems and Transformations.

Magellan Book Company, 1990.

[39] A.G.O Mutambara. Decentralised Estimation and Control with Applications to

a Modular Robot. 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 Journal of Autonomous Robots, volume 6, pages 147{

164, May 1999.

[41] E.M. Nebot, S. Sukkarieh, and H. Durrant-Whyte. Inertial Navigation Aided

with GPS Information. In M2VIP-97 Mechatronic and Machine Vision in Prac-

tice, pages 169{174, September 1997.

[42] E.M. Nebot, S. Sukkarieh, and H. Durrant-Whyte. Inertial Navigation Appliedto a High Speed Vehicle. In CONTROL-97, pages 61{66, October 1997.

[43] B.W. Parkinson and P. Axelrad. Autonomous GPS Integrity Monitoring Using

Pseudorange Residual. In Navigation, volume 35, pages 255{274, 1988.

[44] B.W. Parkinson and Jr. J.J. Spiker. Global Positioning System: Theory and

Applications. American Institute of Aeronautics and Astronautics, Inc, 1996.

[45] A.J. Pejsa. Optimum Skewed Redundant Inertial Navigators. In AIAA Journal,

volume 12, pages 899{902, July 1974.

[46] G.R. Jnr. Pittman. Inertial Guidance. John Wiley and Sons, Inc, 1962.

[47] R.M. Rogers. Integrated INU/DGPS for Autonomous Vehicle Navigation. In

IEEE Position, Location and Navigation Symposium, pages 471{476, 1996.

[48] S. Scheding. High Integrity Navigation. PhD thesis, Department of Mechanicaland Mechatronic Engineering, The University of Sydney, 1997.

[49] W. Sohne, O. Heinze, and E. Groten. Integrated INS/GPS System for HighPrecision Navigation Applications. In IEEE Position, Location and Navigation


BIBLIOGRAPHY 191

[50] M.A. Sturza. Navigation System Integrity Monitoring Using Redundant Mea-

surements. In Navigation, 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 FSR99 Field Service

Robotics, 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 FSR97 Field Service Robotics, pages 278{285,

December 1997.

[53] S. Sukkarieh, E.M. Nebot, and H. Durrant-Whyte. Achieving Integrity in anINS/GPS Navigation Loop for Autonomous Land Vehicle Applications. In IEEE

International Conference on Robotics and Automation, 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 IEEE Transac-

tions, 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 The 4th International Symposium

on Satellite Navigation Technology and Applications - Session 7, July 1999.

[56] S. Sukkarieh, G. Nebot, E.M. Dissanayake, and H. Durrant-Whyte. The Aiding ofa 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 FSR99 Field Service Robotics, pages 395{400, August1999.

[57] D.H. Titterton and J.L. Weston. Strapdown Inertial Navigation Technology.Peter Peregrinus Ltd, 1997.

[58] R. Volpe, J. Balaram, T. Ohm, and R. Ivlev. The Rocky 7 Mars Rover Proto-

type. In IEEE International Conference on Robotics and Automation - Planetary

Rover Technology and Systems Workshop, 1996.

[59] T. Weber. A Unique Solution to Provide A High Integrity Inertial MeasurementUnit (IMU) for the EF 2000 Flight Control System. In Avionics Conference and

Exhibition, 1996.

[60] R. Weiss and I. Nathan. An Ultrareliable Sensing System for Vehicles with

Limited Sparing Capability. In Journal of Spacecraft and Rockets, volume 4,pages 1151{1158, September 1967.

BIBLIOGRAPHY 192

[61] J.C Wilcox. Maximum Likelihood Failure Detection for Redundant Inertial Sys-

tems. In AIAA Guidance and Control Conference - AIAA Paper, number 72-864,

August 1972.

Documents

O DER E ·MM M UT A S·EA D · PDF fileno material previously published or written b y another p erson nor material whic h to a ... e. Thanks also go es to Dr.Gamini Dissana y ak e