Inertial Navigation System Algorithms For Low Cost Imu

Inertial Navigation System Algorithms
for Low Cost IMU

Xiaoying Kong
Department of Mechanical and Mechatronic Engineering

The University of Sydney
August 27, 2000
August 2000
Australian Centre for Field Robotics
Department of Mechanical and Mechatronic Engineering
The University of Sydney
This thesis is submitted to The University of Sydney in fulllment of the requirements for the degree of Doctor of Philosophy. The thesis is entirely my own work
and, except where otherwise stated, describes my original research.
Xiaoying Kong
c 2000 Xiaoying Kong

Copyright
All rights reserved
Abstract
This thesis describes the development of strapdown Inertial Navigation System (INS)
algorithms for low cost Inertial Measurement Units (IMU). The term \low cost IMU"
is used to describe an IMU built with standard low grade gyros and accelerometers
which cannot conduct self-alignment. These algorithms motivated the development of
the INS error models and the Global Positioning System (GPS) error models for low
cost aiding in autonomous navigation.
This thesis has three principle contributions. The rst is the development of strapdown
INS velocity, position and attitude error propagation models for large angle errors in
the computer frame approach. There are two sets of the models. The rst set uses
psi angles to describe attitude errors. The other uses quaternions. These models dier
from other INS error models that they do not require small angle assumptions. The
second contribution is the development of low cost INS algorithms using the INS error
models developed in this thesis. There are two algorithms which use the two sets of INS
models. The main contribution of these algorithms is the in-motion alignment approach
with unknown initial conditions. The implementation of the algorithm using the psi
angle model involves the Extended Kalman Filter (EKF). The quaternions algorithm
uses the Distribution Approximation Filter (DAF). GPS measurements are used to aid
INS. It is argued that the quaternions approach gives better accuracy and requires less
computation. The third contribution is the GPS modelling in the frequency domain.
The equations of GPS position errors are derived to be identical second order systems
in the frequency domain. Feedback and feedforward lters for GPS error de-correlation
using INS information are presented.
The theoretical work is veried by a set of experiments using real data. A standard
GPS is used to verify the GPS modelling. The experimental results using a low cost
IMU and an aiding DGPS have shown that position, velocity and attitude accuracy
can be achieved using the algorithms presented in this thesis.
Acknowledgments
I would like to thank all the people who encouraged and supported me during the
undertaking of this thesis.
Firstly I would like to thank Professor Hugh Durrant-Whyte, the leader of the Australia
Centre of Field Robotics, for giving me the opportunity to pursue PhD study at the
University of Sydney. Hugh's support and expertise have proved invaluable. I would
like to thank my supervisor, Dr. Eduardo Nebot, for his unwavering enthusiasm and
support. Eduardo's expertise and support have also been invaluable.
There are many people I would like to thank for their many and varied inputs. Thanks
goes to the entire Sydney University mechatronics crowd: Kee-Choon Wong, David
Rye, Gamini Dissanayake, Julio Rosenblatt, Ha Quang, Chris Mifsud, Michael Stevens,
Raj Madhavan, Jose Guivant, Paul Newman, Som Majumder, Miguel Santos, Stefan
Baiker, Monica Louda, Eric Nettleton, Keith Willis, Adrian Bonchis, Tim Bailey, Steve
Clark, Mihaly Csorba, Mohamad Bozorg and Daniel Pagac. Special thanks to Steve
Scheding and Stefan Williams for frequently correcting my English, to Simon Julier for
discussing the lters, to Salah Sukkarieh for hardware support, to K.Y. Lee for xing
my computer, to Nguyen Hong Quang and Anh Tuan Le for being friends, and to Grace
Wong and Lynda Brown for their spiritual encouragement.
I owe my greatest debt to my parents who have been looking after my baby girl overseas.
I would like to express my deepest gratitude to my husband, without his love and
encouragement and moral support, I could not have nished the study.
ii
Contents
Abstract
Acknowledgments
Contents
List of Figures
1 Introduction
1.1
1.2
1.3
1.4
1.5
1.6
The Objectives of this Thesis . . . . . . . . . . . .

Common Coordinate Systems Used in Navigation .
INS Error Models . . . . . . . . . . . . . . . . . . .
Integrated INS Algorithms Using Error Models . .
Main Contributions of this Thesis . . . . . . . . . .
The Structure of this Thesis . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 INS Error Models Review . . . . . . . . . . . . . . . . . . . . . . .
2.2.1 INS Error Models Review . . . . . . . . . . . . . . . . . . .
2.2.2 INS Error Models for Azimuth Uncertainty . . . . . . . . .
2.2.3 Quaternion Error Models Review . . . . . . . . . . . . . . .
2.2.4 Psi Angle Approach Review . . . . . . . . . . . . . . . . . .
2.2.5 Phi Angle Approach . . . . . . . . . . . . . . . . . . . . . .
2.2.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3 Development of Psi Angle Model for Large Errors . . . . . . . . . .
2.3.1 Velocity Error Propagation Model . . . . . . . . . . . . . .
2.3.2 Position Error Propagation Model . . . . . . . . . . . . . .
2.3.3 Psi Angle Error Model . . . . . . . . . . . . . . . . . . . . .
2.4 Development of INS Error Models Using the Quaternion Approach
2.4.1 Quaternions and Coordinate Systems . . . . . . . . . . . .
2.4.2 Velocity Error Propagation Model . . . . . . . . . . . . . .
2.4.3 Position Error Propagation Model . . . . . . . . . . . . . .
2.4.4 Quaternion Error Propagation Model . . . . . . . . . . . .
2.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
2 INS Error Propagation Models
iii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
i
ii
v
ix
1
1
1
3
6
7
8
11
11
12
12
13
15
16
19
22
22
23
25
25
31
31
33
34
34
36
CONTENTS
iv
3 GPS Modelling in Frequency Domain
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 GPS Frequency Domain Modelling Review . . . . . . . . . . . .
3.2.1 GPS Receiver Clock Oset Model . . . . . . . . . . . .
3.2.2 GPS Correlated Error Model . . . . . . . . . . . . . . .
3.3 GPS Position Model . . . . . . . . . . . . . . . . . . . . . . . .
3.4 GPS Position Error Modelling Using PSD . . . . . . . . . . . .
3.4.1 Power Spectral Density and Autocorrelation of Signals .
3.4.2 Error Modelling Using PSD and Autocorrelation . . . .
3.5 De-correlate GPS Noise Using INS . . . . . . . . . . . . . . . .
3.5.1 Shaping Filter . . . . . . . . . . . . . . . . . . . . . . .
3.5.2 De-correlation of GPS Noise Using Feedforward Filter .
3.5.3 De-correlation Using Indirect Feedback Filter . . . . . .
3.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4 INS Algorithm for Low Cost IMU in Psi Angle Approach
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.1 Self-alignment Review . . . . . . . . . . . . . . . . . . .
4.2.2 In-motion Alignment Review . . . . . . . . . . . . . . .
4.2.3 Large Misalignment Problem Review . . . . . . . . . . .
4.3 Coarse Alignment . . . . . . . . . . . . . . . . . . . . . . . . . .
4.3.1 Raw Data Process . . . . . . . . . . . . . . . . . . . . .
4.3.2 Coarse Leveling - Initial Direction Cosine Matrix . . . .
4.4 In-motion Alignment: Solution of Initial Attitude Uncertainty .
4.4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . .
4.4.2 Filter States . . . . . . . . . . . . . . . . . . . . . . . . .
4.4.3 Filter Equations . . . . . . . . . . . . . . . . . . . . . .
4.4.4 Discrete Filter and Jacobian Matrix . . . . . . . . . . .
4.4.5 Filter Implementation Using the EKF . . . . . . . . . .
4.5 Navigation Stage: Continue Alignment and Calibration . . . .
4.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5 INS Algorithm for Low Cost IMU in Quaternion Approach

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2 INS Navigation Using Quaternions . . . . . . . . . . . . . .
5.2.1 Overview . . . . . . . . . . . . . . . . . . . . . . . .
5.2.2 Raw Data Process . . . . . . . . . . . . . . . . . . .
5.2.3 Quaternions Initialization . . . . . . . . . . . . . . .
5.2.4 Transformation Matrix Using Quaternions . . . . . .
5.2.5 Quaternion, Velocity and Position Update . . . . . .
5.3 INS Algorithm for Low Cost IMU in Quaternion Approach
5.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . .
5.3.2 Filter Models . . . . . . . . . . . . . . . . . . . . . .
5.3.3 Filter Process Structure in Continuous Time . . . .
5.3.4 Observation Equations . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
38
38
39
40
41
43
48
48
49
53
53
56
64
66
70
70
71
72
74
75
76
78
83
85
85
87
87
97
101
104
104
105
105
106
106
108
109
110
110
112
112
114
118
121
CONTENTS
5.3.5 The Discrete-Time Filter . . . . . . . . . . . . . . .

5.3.6 Data Fusion - the Distribution Approximation Filter
5.3.7 Filter Implementation Using DAF . . . . . . . . . .
5.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6 Experimental Results
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . .
6.2 GPS Position Modelling Results . . . . . . . . . .
6.2.1 Model Results . . . . . . . . . . . . . . . .
6.2.2 Model Validation . . . . . . . . . . . . . . .
6.3 Experimental Results of the Psi Angle Approach .
6.4 Experimental Results of the Quaternion Algorithm
6.5 Summary . . . . . . . . . . . . . . . . . . . . . . .
7 Summary and Conclusions
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2 Summary of Each Chapter . . . . . . . . . . . . . . . . . . . . . . . . . .
7.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.3.1 INS Error Modelling in Psi Angle Approach . . . . . . . . . . . .
7.3.2 INS Error Modelling Using the Quaternion Approach . . . . . .
7.3.3 GPS Modelling in Frequency Domain . . . . . . . . . . . . . . .
7.3.4 INS Algorithm for Low Cost IMU Using Psi Angle Approach . .
7.3.5 INS Algorithm for Low Cost IMU Using Quaternion Model . . .
7.4 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.1 DGPS Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.2 Experimental Implementation of the INS Algorithms with Three
Unknown Attitudes . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.3 Self Tuning Filters . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.4 Phi Angle Model for Large Attitude Errors . . . . . . . . . . . .
Bibliography
122
124
128
131
132
132
133
133
136
144
153
159
160
160
160
164
164
164
165
165
166
166
166
167
167
167
168
List of Figures
1.1 Inertial Navigation System . . . . . . . . . . . . . . . . . . . . . . . . .
2.1
2.2
2.3
2.4
Computer frame, platform frame and three psi angles.

True frame and platform frame. . . . . . . . . . . . . .
Computer frame, platform frame and true frame. . . .
Quaternions and frames. . . . . . . . . . . . . . . . . .
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
3.13
3.14
Satellites, receiver and ranges. . . . . . . . . . . . . . . . . . . . . . . . .

GPS pseudo range and errors. . . . . . . . . . . . . . . . . . . . . . . . .
Raw data of GPS position in x-axis. . . . . . . . . . . . . . . . . . . . .
GPS correlated noise in x-axis . . . . . . . . . . . . . . . . . . . . . . . .
PSD curve : x . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
PSD curve tting. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Shaping lter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Shaping lter generating measurement corruption noise. . . . . . . . . .
Feedforward lter with GPS and INS. . . . . . . . . . . . . . . . . . . .
Bode plot of the feedforward lter using a poor INS. . . . . . . . . . . .
Time domain performance of the feedforward lter using a poor INS. . .
Bode plot of the feedforward lter using a good INS. . . . . . . . . . . .
Time domain performance of the feedforward lter using a good INS. . .
Bode plots of the transfer functions using the feedforward lter. The
magnitude of the Bode plots, the frequency in rad=sec unit and the INS
noise in m=s2 unit are logarithmically scaled. The magnitude unit in
the plots represent the power numbers of the logarithm units. The units
for the frequency and INS noise represent the negative power numbers
of the logarithm units. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Indirect feedback lter . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Bode plot of the feedback lter with a poor INS. . . . . . . . . . . . . .
Time domain performance with a poor INS using a feedback lter. . . .
Bode plot of the indirect feedback lter with a good INS. . . . . . . . .
Time domain performance of the indirect feedback lter using a good INS.
Bode plots of the transfer functions using indirect the feedback lter.
The magnitude of the Bode plots, the frequency in rad=sec unit and the
INS noise in m=s2 unit are logarithmically scaled. The magnitude unit in
the plots represent the power numbers of the logarithm units. The units
for the frequency and INS noise represent the negative power numbers
of the logarithm units. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.15
3.16
3.17
3.18
3.19
3.20
vi
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4
17
19
21
31
43
44
50
50
53
54
54
57
57
60
61
62
62
63
64
65
66
67
67
68
LIST OF FIGURES
vii
4.1 Resolution of Watson IMU-1. . . . . . . . . . . . . . . . . . . . . . . . .

4.2 Bank and elevation of Watson IMU. . . . . . . . . . . . . . . . . . . . .
4.3 These six plots show the turn-on biases for the Waston IMU-1. The 2000
samples are taken over a period of 34 seconds. The units for the y axes
in the top three plots are m=s2 . The units for the y axes in the bottom
three plots are rad=s. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.4 Quaternion errors due to gyro biases. The 1650 samples are taken over
a period of 28 seconds. The unit for the quaternions is 1. . . . . . . . .
4.5 Errors of acceleration, velocity and position due to turn-on biases of
IMU. The units for y axes in the top two plots, the middle two plots and
the bottom two plots are m=s2 ; m=s and m respectively. Along x axes
are data samples over time taken at 84Hz in 20 seconds. . . . . . . . . .
4.6 Quaternions after removal of turn on biases of gyros. The unit for the
quaternions is 1. Along x axis are data samples over time taken at 84Hz.
4.7 Acceleration, velocity and position after removal of turn-on biases. Along
x axes are data samples over time taken at 84Hz. The units for the y
axes in the top two plots, the middle two plots and the bottom two plots
are m=s2 , m=s and m respectively. . . . . . . . . . . . . . . . . . . . . .
4.8 Heading error denition. . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.9 Flow chart of heading correction and lter estimation using the psi angle
approach. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.1
5.2
5.3
5.4
5.5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
79
80
80
83
84
85
88
.
.
.
.
.
107
108
112
113
125
Position output from Ashtech Sensor II. . . . . . . . . . . . . . . . . . .

Position noise in axes x, y and z . . . . . . . . . . . . . . . . . . . . . . .
Power spectral density of position error in axis x. . . . . . . . . . . . . .
Power spectral density of position error in axis y. . . . . . . . . . . . . .
Power spectral density of position error in axis z . . . . . . . . . . . . . .
Measured PSD and estimated PSD in axis x. . . . . . . . . . . . . . . .
Measured PSD and estimated PSD in axis y. . . . . . . . . . . . . . . .
Measured PSD and estimated PSD in axis z . . . . . . . . . . . . . . . .
Bode plot in axis x. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Bode plot in axis y. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Bode plot in axis z . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Time domain performance in axis x using the estimated model. . . . . .
Time domain performance in axis y using the estimated model. . . . . .
Time domain performance in axis z using the estimated model. . . . . .
Time domain performance in axis y when the poles of the model change.
Bode plot of the transfer function for axis y when the poles of the model
change. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.17 Time domain performance in axis x when r is changed. . . . . . . . . .
134
134
135
135
136
137
137
138
138
139
139
140
141
141
142
6.1
6.2
6.3
6.4
6.5
6.6
6.7
6.8
6.9
6.10
6.11
6.12
6.13
6.14
6.15
6.16
INS ow chart . . . . . . . . . . . . . . . . . . . . . . .
Raw data of accelerometer x and gyro x. . . . . . . . . .
Quaternions' orthogonality . . . . . . . . . . . . . . . .
Flow chart of the quaternion approach. . . . . . . . . . .
The principle of the Distribution Approximation Filter .
77
78
142
143
LIST OF FIGURES
6.18
6.19
6.20
6.21
6.22
6.23
6.24
6.25
6.26
6.27
6.28
6.29
6.30
6.31
6.32
6.33
6.34
6.35
6.36
6.37
6.38
6.39
6.40
Time domain performance in axis z when three parameters are changed.

The vehicle trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Outputs of three accelerometers. The units for the x axes are seconds. .
Outputs of three gyros. The units for the x axes are seconds. . . . . . .
INS starts with a wrong heading. The lter corrects the heading in the
rst 20 seconds. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Heading error diminishes to small angles in 15 seconds. . . . . . . . . . .
Position outputs of the INS and the GPS after attitude correction. The
INS continually outputs navigation data between two consecutive GPS
sampling times. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Tilt and heading errors diminish to small angles. . . . . . . . . . . . . .
At the end of the run, the acceleration of the INS output in the p;frame
is very close to zero with DCM correction. The unit of the acceleration
is m=s2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
The acceleration of the INS output in the end is not zero without DCM
correction. The unit in the plot is m=s2 : . . . . . . . . . . . . . . . . . .
Random walk of the gyro x in 3 minutes. . . . . . . . . . . . . . . . . .
The noise added to the three accelerometers and the three gyros was
approximately 3 times larger than the standard noise of the Watson
IMU. The unit of the acceleration noise is m=s2 . . . . . . . . . . . . . .
The INS rotation rate at the end of the run when the vehicle is stationary
should be zero. The solid curve is the rotation rate after calibration by
the lter. It is close to zero. The 'plus +' curve is the rotation rate
without calibration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
With perturbed acceleration, at the end of the run, the acceleration in
the platform frame is close to zero after calibration by the lter. . . . .
At the beginning, the INS is uncertain within 180 . After 60 iterations
of the lter, the INS heading error is corrected to less than 5 . . . . .
The INS heading error is diminished to small angles after 60 iterations
of the lter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Quaternions are corrected after each lter time. . . . . . . . . . . . . . .
The enhanced view of the quaternion errors between the lter iteration
100 to 160 which correspond to the vehicle running time 25 seconds to 45
seconds. The four elements are very close to [1; 0; 0; 0]T which represents
quaternions of a zero rotation from the computer frame to the platform
frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Accelerations in the three axes of the platform frame. The unit of the
plot is m=s2 : . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
At the end of the run, the vehicle is stationary. The accelerations in the
three axes of the platform frame are very close to zero. . . . . . . . . . .
Tilt errors are less than 0.04 . Heading errors are within 0.5 . . . . .
The velocity errors are within 0.1 m=s. The position errors are within
0.02 m. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
The solid curve is the INS position output. Curve \o" is the GPS position
output. INS outputs position data with the frequency of 84Hz between
two GPS position outputs. . . . . . . . . . . . . . . . . . . . . . . . . . .
viii
143
145
146
146
147
147
148
149
149
150
150
151
152
152
154
154
155
155
156
157
157
158
158
LIST OF FIGURES
ix
6.41 INS Position and heading corrections can be seen in the curve between
two GPS position outputs. . . . . . . . . . . . . . . . . . . . . . . . . . . 159
Chapter 1
Introduction
1.1 The Objectives of this Thesis
This thesis develops algorithms for low cost inertial navigation systems (INS) based on
INS error propagation models. The algorithms are intended for use by a low cost inertial measurement unit (IMU) to provide accurate navigation information, as position,
velocity and attitude, of the vehicle which carries the INS. The term \low cost IMU"
is used to describe an IMU built with standard low grade gyros and accelerometers.
To achieve the goal of algorithm development, three issues are addressed in this
thesis:
Development of INS error models appropriate for low cost IMUs which determine
the accuracy and behaviour of the INS.
Designing and tuning of the lter algorithms for such INS .

Identifying sensor models which will be used in the lter algorithms.
1.2 Common Coordinate Systems Used in Navigation

Fundamental to the process of inertial navigation is the precise denition of a number
of Cartesian coordinate reference systems or frames [1]. Each frame is an orthogonal,
1
1.2 Common Coordinate Systems Used in Navigation
right handed axis set.

The following INS frames are dened and used in this thesis.
Inertial frame ( i-frame) is the frame in inertial space. Its origin is at the centre
of the Earth and axes are non-rotating with respect to the xed stars.
Earth frame (e-frame) is an earth-xed frame whose origin is at the Earth centre,
x axis points to the north pole, y axis points to the Greenwich meridian in the
equatorial plane and the z axis completes the system to a right-hand coordinate
system.
Body frame ( b-frame) is a frame xed to the vehicle. The accelerations and
angular rates generated by the strapdown accelerometers and gyros are measured
in b;frame.
Computer frame ( c-frame) is the local level frame at the INS computed position.
Platform frame ( p-frame) is the frame in which the transformed accelerations
from the accelerometers and angular rates from the gyros are solved.
True frame ( t-frame) is the true local level frame at the true position.
Navigation frame ( n-frame) is a user dened frame for navigation output. Any
frame dened above can be chosen as the navigation frame.
The schema of these frames are shown in the following chapters.
The Global Positioning System (GPS) is another navigation source used in this
thesis. The World Geodetic System WGS-84 is a coordinate system for GPS. WGS-84
is an Earth-centred Earth-xed reference frame dened as follows [2]:
Origin: Earth's centre of mass.
z -axis: Parallel to the direction of the conventional international origin for polar
motion, as dened by the Bureau International De L'HEURE (BIH) on the basis of the
latitudes adopted for the BIH stations.
1.3 INS Error Models
x-axis: Intersection of the WGS-84 reference meridian plane and the plane of the
mean astronomic equator, the reference meridian being parallel to the zero meridian
dened by the Bureau International De L'HEURE on the basis of the longitudes adopted
for the BIH stations.
y-axis: Completes a right-handed Earth-centred, Earth-xed orthogonal coordinate
system, measured in the plan of the mean astronomic equator 90 degrees east of the
x-axis.

An IMU usually contains a set of three orthogonal-installed accelerometers and three
orthogonal-installed gyros. When the accelerometers and the gyros are directly installed
in the vehicle body, the INS is called a strapdown INS (SINS). An inertial navigation
system is a real time algorithm to calculate the position, velocity and attitude of the
vehicle which carries the INS by integrating the acceleration and rotation rate signals
from an IMU.
The velocity and the position are calculated by double integration of the sum of
the gravitational acceleration and the non-gravitational acceleration from the three
accelerometers. For a strapdown INS, these integrations are performed in the coordinate
system in which the three accelerometers are installed. By integrating the rotation
rate signals from three gyros, the angular orientation of the three accelerometers is
determined. The calculated acceleration, velocity and position are transformed into
the desired navigation coordinate system by this orientation information.
Figure 1.1 shows the INS process. There are two main phases for INS operation:
the alignment phase and the navigation phase. The integration sequence starts from
the initial velocity, position and attitude. The process for determining these INS initial
conditions is called alignment. Any INS has to undergo this initial alignment phase
before it starts to navigate.
Any errors in either the alignment phase or the navigation phase will be integrated
Figure 1.1: Inertial Navigation System

and will propagate over time. These errors determine the performance and the navigation accuracy of the INS. Error analysis is based on error models. Error models also
serve for real-time failure detection and for the implementation of a data fusion lter
in the INS algorithms [3].
Most of the literature on INS is concerned with INS error propagation analysis and
models. There are two approaches to the derivation of INS error models [4]. One is
known as the phi-angle approach or the true frame approach. The other is known as
the psi-angle approach or the computer approach. The phi-angle approach perturbs
the INS equations in the local-level north-pointing Cartesian coordinate system that
corresponds to the true geographic location of the INS. The psi-angle approach perturbs the INS equations in the computer frame which is the local-level north-pointing
coordinate system that corresponds to the geographic location computed by the INS.
It has been shown that the models from these two approaches are equivalent and yield
identical results [5, 6, 7]. Modelling procedures and the perturbation rules have been
unied in reference [3].

Most of the literature for INS modelling makes the assumption that all perturbation
attitude errors are small angles. The coordinate transformation in the modelling is
therefore presented by some approximation matrices.
However, in many cases, this small angle assumption will not hold. IMUs with low
accuracy cannot measure the earth rate which is the key vector for INS alignment. The
initial heading will be unknown. The corresponding attitude error in both the phi-angle
model and the psi-angle model could be up to 180 degrees making the small angle
approximation not valid.
In recent literature, attempts to model INS propagation errors with an unknown
heading were presented. Stepanov et al. presented a model considering a large heading
error and small tilt errors using the true frame [8]. Scherzinger developed a modied psi
angle model with arbitrary heading and small tilt errors using the psi-angle approach
[9, 10]. Four states are used to describe three psi angles. The psi angle z which
represents the correspondent heading error is split into two states sin( z ) and cos( z ).
Other INS models related to land vehicles and robotics applications are presented
in references [11, 12].
So far, an accurate psi angle model for three large error angles has not been discussed
in the literature. Development of such a psi angle model for all large attitude errors
using three single psi angles is one of the main contributions of this thesis.
An INS may compute the attitude using Euler angles, the direction cosine matrix
or the quaternions. The quaternions method is advantageous since it requires less
computation, gives better accuracy and avoids singularity [13]. Some INS error models
using quaternions have been developed using the innitesimal angle assumption [13,
14, 15].
Error models using quaternions for three large errors have not been discussed in
the literature. The development of a quaternion based INS error propagation model for
large errors is another contribution of this thesis.
1.4 Integrated INS Algorithms Using Error Models
1.4 Integrated INS Algorithms Using Error Models

The purpose of INS error modelling is to develop a model for INS algorithms. Based
on the two sets of models developed in this thesis, two INS algorithms are developed for
low cost IMU.
The term \low cost INS" in this thesis is used to describe an INS built with low
cost IMU components whose sensitivity is not enough to measure the earth rate. The
earth rate is a key input to INS initial alignment using gyrocompassing. In a case when
the earth rate cannot be measured by an INS, the alignment will not be performed
properly. To obtain the initial attitude, external sensors have to be used [16]. An
in-motion alignment algorithm makes use of other navigation information to determine
the attitude of the IMU. The Global Positioning System and other navigation sensors
are commonly used as aiding information for INS alignment purposes. The Kalman
lter is the most commonly used algorithm for fusing INS and other navigation data in
both INS alignment and navigation phases.
The Kalman lter is a linear statistical algorithm used to recursively estimate the
states of interest [17]. In INS algorithms, the states of interest usually consist of the
errors of the vehicle velocity, position, attitude, IMU sensor biases, errors from other
navigation systems and other relevant vehicle parameters. To aid in the estimation of
the states, the Kalman lter requires two models: the process model and the observation
model. The process model describes the propagation of the states. The observation
model describes the information supplied by a sensor as a function of the states, together
with a model of measurement noise. INS error propagation models are the main process
models used in most of the literature.
Many common processes cannot be represented adequately using linear models.
The psi angle model and the quaternions model developed in this thesis are based
on nonlinear models. The Extended Kalman Filter (EKF) is the most widely used
approach for a nonlinear lter algorithm [18]. The EKF predicts the states of the system
under the assumption that its process model and observation model are locally linear.
1.5 Main Contributions of this Thesis
The lter models are expanded using Jacobians. However, there are some problems
with the EKF [19]. The implementation diculty of Jacobians is one shortcoming of
the EKF. The problems of the EKF motivated the development of a new lter algorithm
called the Distribution Approximation Filter (DAF) [19, 20, 21, 22, 23].
In this thesis, the EKF and DAF are used for the two INS algorithms respectively.
The algorithm with the psi angle model as the process model will use the EKF. The
quaternions model algorithm will use the DAF.
INS provides high frequency information and is considered a dead reckoning sensor
with an error that grows unbounded with time. External navigation sensors commonly
aid INS. GPS can provide low frequency navigation information without drift over time.
The integration of INS and GPS has been widely used for navigation. In this thesis,
GPS is used for observation to aid the INS.
There are important error sources in GPS measurement. GPS errors should be
modelled and estimated in the lter. This thesis presents new GPS shaping lters
which describe the GPS frequency characters.
1.5 Main Contributions of this Thesis

The main contributions of this thesis are:
The development of psi angle models in computer frame for large attitude errors.
The development of quaternion models for large attitude errors.
GPS modelling in the frequency domain.
The development of an INS algorithm for low cost IMU using the psi angle approach.
The development of an INS algorithm for low cost IMU using the quaternion
approach.
1.6 The Structure of this Thesis
These contributions are veried by a set of experimental implementations using a

low cost IMU and a GPS.

This thesis does not have one central literature review. Rather, the literature relevant
to a particular topic will be cited at the beginning of each chapter.
Chapter 2 addresses the rst two contributions of this thesis: development of
generic INS error propagation models for large attitude errors in the psi angle approach
and the quaternion approach.
The INS error models in the literature are reviewed at the beginning of this chapter. The psi angle approach, the phi angle approach and the quaternion model in the
literature are also discussed. None of the models assumes three large attitude errors.
The INS error propagation models for large errors in the psi angle approach are
developed in this chapter. There are three models: the velocity error model, the position
error model and the attitude model. The models in three large errors, one large heading
error and two tilts errors, and three small errors are also examined.
Another set of INS models in quaternions for large errors is derived. The computer
approach is also used in these models. The quaternions and the frames used in this approach are discussed. The quaternion error propagation models for large misalignment
of the platform frame and the computer frame are extended.
Chapter 3 details the modelling of the GPS errors in the frequency domain. GPS
is the aiding navigation source for the INS algorithm. The chapter begins by reviewing
the previous work on GPS modelling in the frequency domain.
A GPS position error model is derived. It is proved that the transfer function of the
GPS position error in any frame has the identical poles and zero as the pseudo-range
error. The modelling method using power spectral density of the noise is detailed.
The de-correlation of the GPS coloured noise is also described. A shaping lter is
introduced to take into account the coloured noise. An example is also presented using
feedforward and feedback lters.

Chapter 4 addresses the design of an INS algorithm for low cost IMUs with unknown initial conditions using the psi angle approach in the computer frame.
The analytic coarse alignment method and gyrocompassing, which are the major
methods for self-alignment, are discussed. A new in-motion alignment is proposed.
In this chapter, a new algorithm is developed in the computer frame using the psi
angle approach with an unknown initial attitude using a low cost INS aided by a GPS.
The coarse alignment on ground for raw data process and IMU turn-on biases
estimation is discussed. A method to determine the coarse initial cosine direction
matrix is also presented.
Chapter 5 develops an INS algorithm for low cost IMUs in the computer frame
using quaternions. Quaternions errors are exploited using the misalignment of the
computer frame and the platform frame. The entire lter process model structure and
the process noise are discussed. The process noise vectors are reconstructed by a linear
combination of the white noise on the accelerometers and gyros in the body frame.
The Distribution Approximation Filter (DAF) is used to implement this algorithm.
The principle and the benet of the DAF are brie y described.
Chapter 6 presents the experimental results for the INS algorithms using the psi
angle approach and the quaternion approach and GPS modelling.
Results of GPS modelling in the frequency domain are discussed. Modelling results
are shown. Power spectral density (PSD) of raw GPS position data and the calculated
PSDs by using the model parameters are compared by using a set of gures from the
experiments using a GPS receiver. The GPS model is validated with a set of plots in
the frequency domain and the time domain using the feed back de-correlation lter.
The experimental results are presented to verify the psi angle models, the quaternion
models and the INS algorithms for low cost IMU. The experiment uses a low cost IMU
aided by a DGPS. The results show how the heading errors are corrected from 180
to 0.1 .
Finally, Chapter 7 summarizes the main contributions of this study and provides

suggestions for further research.
10
Chapter 2
INS Error Propagation Models

2.1 Introduction
This chapter develops generic INS error propagation models for large attitude errors
using the psi angle approach and the quaternion approach.
Two approaches have been adopted in the literature: the psi angle approach and the
phi angle approach. The psi angle model and the phi error model are reviewed brie y
in Sections 2.2.4 and 2.2.5. In recent years, some models have been developed for large
heading errors in dierent frames. So far, there are no generic error propagation models
for large attitude errors. All quaternion error models in the literature use the small
angle assumption.
The INS error propagation models for large errors in the psi angle approach are
developed in Section 2.3. There are three models: the velocity error model, the position
error model and the attitude model. The velocity error model is developed in Section
2.3.1. The position error model has no dierence between small angles and large angles.
Section 2.3.3 presents the psi angle model with all large error angles. The models for
three large errors, one large heading error and two tilt errors, and three small errors
are also discussed.
Another type of INS model using quaternions for large errors is developed in Section
2.4. The computer approach is also used in these models. The quaternions and the
11
2.2 INS Error Models Review
12
frames used in this approach are discussed in Section 2.4.1. The velocity error model
in the quaternion approach is developed in Section 2.4.2. The quaternion error propagation models for large misalignment of the p;frame and the c;frame are extended in
Section 2.4.4.

2.2.1 INS Error Models Review
In the design and operation of an INS, navigation errors determine INS performance
and accuracy.
Error models are developed by perturbing the nominal dierential equations whose
solution yields the INS output of velocity, position and orientation. The nominal equations are based on Newton's law. The basic equations can be expressed in dierent
coordinate systems or frames. In 1992, Goshen-Meskin and Bar-Itzhack presented a
unied approach and systematic methodology for developing INS error models [3]. It
was demonstrated that the error models developed in the dierent forms are equivalent.
A developer's tool was presented in their work which indicated the model developing
steps to follow. Their INS error models had methodologies that produced equivalent
results [3].
In 1988, Bar-Itzhack and Berman approached the analysis of INS from a control
theory point of view [4]. The dierential equations that describe INS error behaviour
were divided into equations describing the propagation of the translatory errors and
equations describing the propagation of the attitude errors. Both the translatory and
attitude error equations can be expressed in two dierent ways that yield two versions
of the translatory error equations and two versions of the attitude error equations. The
two versions of the translatory equations depend on whether the equations are position error components or velocity error components. The two versions of the attitude
equations depend on whether the equation variables are components of the platform
to computer frame attitude dierence, or components of the platform to true frame
13
attitude dierence. All of these versions are equivalent [6, 7].

These two approaches for INS error analysis are known as the phi angle approach
and the psi angle approach. The phi angle approach is also called the perturbation
approach or true frame approach. In the phi angle approach, the navigation equations
and error models are solved in the true frame (t;frame). The psi angle approach is
also called the computer frame approach and solves the navigation equations in the
computer frame (c;frame).
Benson [5] derived error equations for INS using the perturbation approach and
the psi angle approach. This work shows the underlying assumptions and allows direct
comparison of the two methods. The two approaches are equivalent and yield identical
results.
The psi-angle error equation in the strapdown INS developed by Bar-Itzhack [24]
proved that apart from a sign change, the psi-angle dierential equation in the error
analysis of the strapdown INS is identical to the one used in a conventional gimbled
INS.
There are some other INS error models but all are based on the assumption that
the angle errors are small [25, 26, 27].
2.2.2 INS Error Models for Azimuth Uncertainty

Both the psi angle and the phi angle approaches assume that the INS attitude errors
are small.
Analytic coarse alignment and gyrocompassing based on the measurement of the
gravity vector and the earth rate vector are the main principles for INS alignment. High
quality IMUs with high resolution can measure the earth rate. This aids the INS initial
alignment to reduce the initial attitude error to satisfy the small angle assumption. This
method cannot be used with low cost IMUs because they are not sensitive enough to
determine the earth rate. External heading sensors and tilt sensors have to be employed
to obtain the INS initial attitude.
A general error model for large attitude errors in all dimensions is required to avoid
14
the use of external heading and tilt sensors.

So far, only a few works have attempted to model large angle errors and to consider,
for example, large heading uncertainty of IMU orientation.
Pham introduced a Kalman lter mechanization for the INS airstart system in 1992
[28]. This approach used two nonlinear states to describe one heading angle. The
attitude errors were not modelled. Dmitriyev, Stepanov and Shepel [8] presented an
INS error model considering large heading uncertainty and small tilt misalignment
errors using a true frame perturbation approach. The heading uncertainty is solved
with a piecewise-Gaussian approximation of a posterior density function. The error
model is built in the true frame with the assumption of two small tilt misalignments
and one large misalignment. The nonlinear characteristic of the problem is considered
in a short period. As a result, the heading misalignment rate is assumed to be zero. In
the misalignment error models, the rates of misalignment angles are coupled with the
velocity errors and the velocity errors are coupled with misalignment angles.
An approximate extended psi-angle model with large heading misalignment is presented in [9, 10] by Scherzinger using four states to describe the three psi-angles. The
model extension is very involved.
In Scherzinger's model, an extended misalignment vector e is dened as:
e = [ x ; y ; sin z ; cos z ; 1]T
(2.1)
with the psi angle [ x ; y ; z ]T being the misalignment of the computer frame and the
platform frame.
The psi angle model for large azimuth misalignment is derived as:
2
_ e = ;
6
6
6
6
4
(!icc )e+
7
7
7
7
5
6
6
6
6
4
021
; ; ; e + !x y ; !y
014
7
7
7
7
5
6
6
6
6
4
"p
; ;;;
0
3
7
7
7
7
5
(2.2)
where (!icc )e+ is the extended 1 4 vector of the rate between the computer frame.
"p is gyro error in p-frame and the inertial frame. [!x; !y ; !z ]T is the vector of the
15
misalignment angular rate. For small azimuth misalignment, the error model turns to:
2
6
(!icc )e+
"p
3
7
7
7
6
6
(2.3)
_ e = ; 66 ; ; ; 77 e ; 66 ; ; ; 77
5
5
4
4
014
0
which has a similar form to the psi angle model developed in the literature by Benson
[5] and Bar-Ithzack [24].
General INS error propagation models for all large angles have not been investigated
in the past. Models in the quaternion form have not been thoroughly investigated either.
In this chapter, general error models for three large attitude errors will be developed.
Two approaches are used in the error models: the psi angle approach and the quaternion
approach.
2.2.3 Quaternion Error Models Review

INS attitude calculation is based on the methods of Euler angles, direction cosine matrix
and quaternions. The psi angle model is applicable to the Euler angles and direction
cosine matrix methods. The quaternion method is more eective as it provides more
accuracy, requires less computation and avoids singularity in computation.
Error models in quaternions are consequently important when quaternion methods
are used.
Friedland [14] presented a number of models for a strapdown INS based on quaternions including dynamics and error propagation models. Small attitude errors assumption is implied. Vathsal [15] derived error equations for quaternion propagation in
spacecraft navigation also with the small angle assumption. Roumeliotis and Sukhatme
[29] modelled quaternions in attitude as a constant plus a white noise using the innitesimal angle assumption. Another quaternion error dierential equation was developed
by Crassidis and Markley [30] where the Riccati trajectories were introduced into error
quaternion trajectories. A numerical solution form of the quaternion error was given
by Lovren and Pieper [25]. The scale, skew and drift errors of INS were presented in
the numerical quaternion form.
16
In 1998, Lee, Roh and Park [13] proposed equivalent tilt error models which were
applicable to the analysis of the terrestrial strapdown inertial navigation system based
on quaternions. In this work the computer approach was used. To derive the velocity and the attitude error models, small angle assumptions were considered to satisfy
equivalent tilt angle denition.
In situations where initial attitudes are unknown or large attitude perturbation
occurs, the previous INS error models will not satisfy the small angle assumption. So
far, to the best of the author's knowledge there are no large error models employing
quaternions.
In this chapter, an INS velocity error model, a position error model and an attitude
error model are developed in the quaternion method in the computer frame. The
velocity error propagation is presented using quaternions. Attitude errors are presented
by the misalignment of the computer frame and the platform frame. Quaternion errors
are solved in the computer frame. No small angle assumption is made in the model
development. The models are suitable for both three small and large attitude errors
cases.
2.2.4 Psi Angle Approach Review

In the psi angle approach introduced by Benson [5], the navigation equations are solved
in the computer frame (c;frame). Error propagation models are derived from perturbation of the c;frame solution. See Figure 2.1.
The computer frame (c;frame) is dened as the local level frame located at the
INS computed position. The platform frame (p;frame) is the frame in which the
transformed accelerations and angular rates from the accelerometers and gyros are
resolved. Psi-angle ( ) is the angle between the c;frame and the p;frame. =
[ x ; y ; z ]T :
The velocity and attitude error models of the psi angle approach for small angles
are brie y introduced here.
17
Computer Zc Up
Frame
Zp
y
Platform
Frame
Yp
x
Yc
North
z
East
Xc
Xp
Figure 2.1: Computer frame, platform frame and three psi angles.
Velocity Error Model

The true INS velocity equation in the c;frame is given by:
V_tc = ftc + gtc ; (2

cie +
cec)Vtc
(2.4)
where Vtc = [Vtxc ; Vtyc ; Vtzc ]T is the true velocity in the c-frame, ftc is the true specic force
in the c-frame, gtc is the true gravity in the c-frame,
cie is the earth rate in the c-frame
and
cec is the rotation rate from the c-frame to the earth frame. In the following b
represents observation and represents error.
cie and
cec are the skew symmetric matrices of !iec and !ecc . In the east-north-up
frame with the local latitude ' :
2
!iec =
6
6
6
6
4
!ie cos(')
!ie sin(')
3
7
7
7
7
5
(2.5)
18
2
;Vyc=Ry
Vxc=Rx
Vxctg(')=Rx
6
!ecc = 66
4
7
7
7
7
5
(2.6)
where V c = [Vxc; Vyc ; Vzc ]T is the INS velocity in the computer frame. R = [Rx ; Ry ; Rz ]T
is the vector from the earth centre to the vehicle position.
The INS solves the following velocity V^ c :
c
V^_ = ftp + rp + g^c ; (2

cie +
cec)V^ c
(2.7)
where ftp is the true specic force in the p-frame and rp is the specic force error due
to accelerometers' errors.
Assuming the psi angles are small, the velocity errors are solved in the c-frame by
subtracting (2.7) from (2.4) :
V_ c = ; ftc + rp + gc ; (2
cie +
cec)V c
is the skew symmetric matrix of angles given by:
2
6
6
6
6
4
z
y
(2.8)
3
7
7
7
7
5
(2.9)
The gravity vector error is given by:

gc = [;!s2 Rxc ; ;!s2 Ryc ; ;!s2 Ryc ]
(2.10)
!s is the Schuler frequency dened as !s = g=re , being g the gravity constant and re
the earth radius.
Position Error Model

The position error model in the c-frame is derived from the velocity error model and is
given by Benson [5]:
R_ c = ;
cecRc + V c
(2.11)
19
Up
Zt
True
Frame
Zp
y
Platform
Frame
Yp
x
Yt
North
z
East
Xt
Xp
Figure 2.2: True frame and platform frame.
Attitude Error Model - Psi Angle Model

The psi angle model was introduced by Benson [5] and Bar-Itzhack [24]:
_p = ;!ipp p + p
(2.12)
where _p is the angular velocity of the p;frame with respect to the c-frame, p is the
gyro drift rate in the platform frame. !ipp is the angular velocity of the p;frame in the
p;frame with respect to the inertial frame (i;frame).
2.2.5 Phi Angle Approach

In the phi angle approach, see Figure 2.2, the frame in which the navigation equations
are solved is the true frame (t-frame).
The true frame is the true local level frame at the true position.
The phi angle = [x ; y ; z ]T is the angle between the true frame and the platform
frame.
20
Velocity Error Model

Similar to the navigation equations in the computer frame, the true velocity equation
in the true frame is:
V_tt = ftt + gtt ; (2

tie +
tet)Vtt
(2.13)
where Vtt = [Vxt ; Vyt ; Vzt ]T is the true velocity in the t-frame, ftt is the true specic force
in the t-frame, gtt is the true gravity in the t-frame,
tie is the earth rate in the t-frame
and
tec is the rotation rate from the c-frame with respect to the earth frame. Also
tie
and
tec are the skew symmetric matrices of !iet and !ect .
Considering the same sources of errors, the INS velocity is:
t
V^_ = (ftp + rp) + gb ; (2

tie +
tet )V^ t
(2.14)
The skew symmetric matrix of the phi angle is:

2
=
6
6
6
6
4
;z y
0 ;x
z
;y x
3
7
7
7
7
5
(2.15)
For small phi angles, the direction cosine matrix between the p;frame and the
t;frame holds the relationship:
2
Cpt =
6
6
6
6
4
;z y 7
7
1 ;x 77 = I33 ;
z
;y x
(2.16)
where I33 is the 3 3 identity matrix.

The velocity error model for small phi angles is then derived by subtracting (2.13)
from (2.14):
V_ t = ; ftp + rp + gt ; (2
tie +
tec)V t
(2.17)
21
Figure 2.3: Computer frame, platform frame and true frame.
Attitude Error Model - Phi Angle Model

Let !tpp be the angular velocity of the platform frame with respect to the true frame
and let _ be the small angle approximation of !tpp and dene the dierence of the psi
angle and the phi angle as = [x ; y ; z ]T : See Figure 2.3.
= ;
(2.18)
Identify ! as:
! = + !itt
(2.19)
2.3 Development of Psi Angle Model for Large Errors
22
The phi angle equation is given by [5, 3, 9]:
!tpp = _ = ;!ipp + ! + "p
(2.20)
where "p is the gyro drift in the p;frame.

Compared with the psi angle model (2.12), the phi angle model (2.20) contains the
item ! = + !itt :
Considerable comparison of the psi angle model and the phi angle model has been
conducted in the literature [6, 7, 3, 5]. The equivalence of these two models has been
proven by Benson and other authors in [6, 7, 3, 5].
2.2.6 Summary
The INS error models have been categorized into two identical approaches: the psi
angle approach and the phi angle approach. Most of the models make the small angle
assumption. Recently, there has been some work on error models assuming small tilts
and large heading errors. To the best of the author's knowledge, INS error propagation
models with three large attitude errors have not been presented before. The goal of
this chapter is to develop an INS velocity error propagation model, a position error
propagation model and a psi angle error model in the computer frame for all large
attitude errors. The error models in quaternion form will also be developed in this
chapter. The applications of these models will be discussed in the following chapters.

The INS velocity and position error models and the psi angle model for large angle
errors in the computer approach will be developed in this section using the computer
frame (c-frame) and the platform frame (p-frame).
23
2.3.1 Velocity Error Propagation Model

The true velocity equation in the c-frame is given by:

cie +
cec)Vtc
(2.21)
The INS computer solves the following velocity V^ c:

c
V^_ = ftp + rp + gbc ; (2

cie +
cec)V^ c
(2.22)
where ftp is the true specic force in the p-frame and rp is the specic force error due
to the errors of the accelerometers.
Subtracting (2.22) by (2.21), the velocity error V c = V^ c ; Vtc solved in the c-frame
is given by
V_ c = ftp ; ftc + rp + gc ; (2
cie +
cec)V c
(2.23)
The specic forces in the c-frame (ftc) and the p-frame (f pt) have the following
relationship:
ftc = Cpc f pt
(2.24)
ftp = Ccp f ct
(2.25)
where Cpc is the direction cosine matrix (DCM) between the p-frame and the c-frame
and Ccp = (Cpc );1 . Therefore the velocity error model is:
V_ c = (Ccp ; I33 )ftc + rp + gc ; (2
cie +
cec)V c
(2.26)
= (I33 ; Cpc)ftp + rp + gc ; (2

cie +
cec)V c
(2.27)
Let
sx = sin( x ); cx = cos( x )
sy = sin( y ); cy = cos( y )
sz = sin( z ); cz = cos( z )
(2.28)
24
For large angle the matrix Cpc is given by:

2
6
6
6
6
4
7
7
7
7
5
cy cz ; sy sx sz ;cxsz sy cz + cy sxsz
Cpc = cy sz + sy sxcz cx cz sy sz ; cy sxcz
;sy cx
sx
cy cx
(2.29)
The specic force is transferred to the p;frame using (2.27) instead of (2.26).
Equations (2.27) and (2.29) present the general velocity error propagation model
for large attitude errors.
In the case when x , y and z are all small, Cpc can be simplied.
sx = sin( x ) = x; cx = cos( x) = 1
(2.30)
sy = sin( y ) = y ; cy = cos( y ) = 1
sz = sin( z ) = z ; cz = cos( z ) = 1
and
sin( x ) sin( y ) = sin( y ) sin( z ) = sin( z ) sin( x ) = 0
(2.31)
Cpc = I33 +
(2.32)
Ccp = I33 ;
(2.33)
Thus:
where the skew symmetric matrix is given by (2.9).

Under the three small angles assumption, the velocity error Equation (2.27) becomes
V_ c = (I33 ; Cpc)ftp + rp + gc ; (2
cie +
cec)V c
= ftp + rp + gc ; (2
cie +
cec)V c
(2.34)
= ; ftc + rp + gc ; (2
cie +
cec)V c
(2.35)
Equations (2.34) or (2.35) and (2.32) or (2.33) are identical to the velocity error
model usually presented in the literature.
25
It is important to study the case of large uncertainties in heading and low uncertainties in tilt angles, that is large z and small x and y angles. With this assumption,
Cpc in (2.27) can be approximated by [31]:
2
Cpc =
6
6
6
6
4
;sz
;cz
cz
sz
y cz + x sz
y sz ; x cz
3
7
7
7
7
5
(2.36)
Then the matrix I33 ; Cpc in the velocity error model is given by:
2
I33 ; Cpc =
6
6
6
6
4
1 ; cos z sin z
; x sin z ; y cos z
; sin( z ) 1 ; cos( z ) + x cos( z ) ; y sin( z )
; x
0
y
3
7
7
7
7
5
(2.37)
The velocity error model with large heading error and small tilt errors is:
2
1 ; cos( z ) sin( z )
; x sin( z ) ; y cos( z ) 7
7
V_ c = ; sin( z ) 1 ; cos( z ) + x cos( z ) ; y sin( z ) 77 ftp
5
; x
0
y
+ rc + gc ; (2
cie +
cec)V c
6
6
6
6
4
(2.38)
2.3.2 Position Error Propagation Model

The position error model is the same as for the small angle case:
R_ c = ;
cecRc + V c
(2.39)
2.3.3 Psi Angle Error Model

A new general psi-angle model that can be used with large angle errors is derived in
this section.
The true transformation matrix Cbc, which is the direction cosine matrix from the
b-frame to the c-frame, is obtained from [27]:
C_bc = Cbc
bib ;
cicCbc
(2.40)
26
The INS solves Cbp , which is the direction cosine matrix from the b-frame to the
p-frame, using measured gyro rates
^ bib provided by the IMU:
C_bp = Cbp
^ bib ;
cicCbp
(2.41)
The rates
^ bib contain gyro drift errors 2b :
^ bib =
bib + 2b
(2.42)
2b is the skew symmetric matrix of the gyro drift vector [bx; by ; bz ]T in the body frame:
2
2b=
6
6
6
6
4
bz
;bz
0
;by bx
by
;bx
0
3
7
7
7
7
5
(2.43)
Let
C = Cbp ; Cbc
(2.44)
Then
C = Cbp ; Cbc
(2.45)
= Cbp ; Cpc Cbp
= (I33 ; Cpc)Cbp
(2.46)
_C can be derived from (2.46) and (2.41). Dierentiating equation (2.46), we have:
_C = (I33 ; Cpc)C_bp ; C_pcCbp
= (I33 ; Cpc)(Cbp
^ bib ;
cicCbp) ; C_pcCbp
(2.47)
= Cbp
^ bib ;
cicCbp ; CpcCbp
^ bib + Cpc
cic Cbp ; C_pcCbp
_C can also be obtained directly from C = Cbp ; Cbc. Dierentiating Cbp and Cbc
we get:
_C = C_bp ; C_bc
= Cbp
^ bib ;
cic Cbp ; Cbc
bib +
cic Cbc
= Cbp
^ bib ;
cic Cbp ; CpcCbp
bib +
cic CpcCbp
(2.48)
(2.49)
27
Substituting (2.47) and (2.48):
Cbp
^ bib ;
cicCbp ; Cpc Cbp
^ bib + Cpc
cicCbp ; C_pcCbp
= Cbp
^ bib ;
cicCbp ; CpcCbp
bib +
cic CpcCbp
Therefore
C_pcCbp + Cpc Cbp

^ bib ; Cpc
cicCbp ; CpcCbp
bib +
cic CpcCbp = 0
Consider
^ bib =
bib + 2b
(2.50)
post multiply both sides by Cpb gives:
C_pc + CpcCbp 2b Cbp ; Cpc

cic +
cicCpc = 0
(2.51)
Let 2p and 2c be the skew symmetric matrices of the gyro drift error vector in the
platform frame and the computer frame respectively:
2
;pz py 7
;cz cy
6
6 0
6
7
6
2p= 66 pz 0 ;px 77 ; 2c= 66 cz 0 ;cx
4
5
4
;py px 0
;cy cx 0
0
3
7
7
7
7
5
(2.52)
The computed skew symmetric matrix

b ppb is the sum of the true
ppb and 2p:
b ppb =
ppb + 2p
(2.53)
And the computed

b bpb is the sum of the true
bpb and 2b :
b bpb =
bpb+ 2b
(2.54)
For the computed

b ppb and
b bpb we have [27]:
b ppb = Cbp
b bpbCpb
(2.55)
Substituting (2.54) and (2.53) into (2.55) :
ppb + 2p= Cbp(

bpb + 2b )Cpb
(2.56)
28
For the true

ppb and
bpb we also have:
ppb = Cbp
bpbCpb
(2.57)
2p= Cbp 2b Cpb
(2.58)
Subtracting (2.57) from (2.56):
Similarly, it can also be proved that:
2p= Ccp 2c Cpc
(2.59)
Pre multiplying Cpc to the above equation, we have:
Cpc 2p = Cpc(Ccp 2c Cpc) =2c Cpc
(2.60)
Replacing the term CpcCbp 2b Cbp in (2.51) with (2.59) and (2.60):
CpcCbp 2b Cbp =
(2.61)
= Cpc(Cbp 2b Cbp )
= Cpc 2p
=2c Cpc
Consequently (2.51) can be simplied as:
C_pc+ 2c Cpc ; Cpc

cic +
cicCpc = 0
(2.62)
Replacing C_pc = Cpc

pcp; and left multiplying Ccp (2.48) gives:
Cpc
pcp+ 2c Cpc ; Cpc
cic +
cic Cpc = 0
(2.63)
pcp + Ccp 2c Cpc ;

cic + Ccp
cicCpc = 0
(2.64)
Notice that in (2.64):
Ccp 2c Cpc =2p
(2.65)
29
And
Ccp
cicCpc =
pic
(2.66)
pcp+ 2p ;
cic +
pic = 0
(2.67)
Then (2.64) changes to:

p + p ; !c + !p .
pcp+ 2p ;
cic +
pic are the sum of skew symmetric matrices of !cp
ic
ic
Equation (2.67) can then be written in the following 31 vector form:
!cpp + p ; !icc + !icp = 0
(2.68)
!cpp + p ; !icc + !icp = !cpp + p ; (I33 ; Ccp)!icc
(2.69)
=0
(2.70)
with !icp = Ccp!icc :
That is:
!cpp = (I33 ; Ccp)!icc ; p
(2.71)
p = _ being the angular rate between the c;frame and the p;frame solved in
with !cp
the p;frame. Therefore:
_ = (I33 ; Ccp )!icc ; p
(2.72)
Equation (2.72) is the general psi-angle error model that can be used for large or
small angle errors.
When x ; y and z are all small,
2
Ccp =
6
6
6
6
4
;
y
z
z
x
x
7
7
7
7
5
(2.73)
Equation (2.71) can be converted to small psi-angle model which is usually presented
in the literature:
_ = !icc ; p
(2.74)
30
As can be seen, it is a special approximation of the more general form given by

equation (2.72).
Another important case to consider is when x ; y are small and z is large, I33 ;
Ccp is simplied to :
I33 ; Ccp = I33 ; (Cpc);1

2
;sz
y
6 1 ; cz
6
= 66 sz
1 ; cz
; x
4
; xsz ; y cz + xcz ; y sz 0
3
7
7
7
7
5
(2.75)
(2.76)
In this case, the psi-angle model is given by:

_ = ;p +
2
6
6
6
6
4
1 ; cz
+ sz
where !icc is given by:
;sz
1 ; cz
; xsz ; y cz + xcz ; y sz 0
2
6
6
!icc = 66
4
(2.77)
;Vyc=Ry
Vxc=Rx + !ie cos(')
Vxctg(')=Rx + !ie sin(')
7
7
7
7
5
!icc
3
7
7
7
7
5
(2.78)
In this section, the INS propagation error models in velocity, position and attitude
for large attitude errors have been developed in the computer frame in the psi angle
approach. The models are identical to their corresponding small angle models when
the attitude errors are all small. INS error models using the quaternion approach will
be developed in the next section.
2.4 Development of INS Error Models Using the Quaternion Approach 31
Figure 2.4: Quaternions and frames.
2.4 Development of INS Error Models Using the Quaternion Approach

2.4.1 Quaternions and Coordinate Systems
In this section, INS error models for large attitude errors will be developed using quaternions. The computer approach is used here. The coordinate systems used in these
models are the computer frame, the body frame and the platform frame. The body
frame is at the IMU whose axes are the IMU body axes. See Figure 2.4.
INS navigation computes the attitude transformation from the body frame to the
platform frame. Quaternion Qpb = [qb0 ; qb1 ; qb2 ; qb3 ]T represents this rotation from the
body frame to the platform frame. Direct accelerometer outputs in the body frame are
transformed to the platform frame by left multiplying C (Qpb) to the accelerations in the
body frame.
2
6
6
C (Qpb) = 66
4
qb20 + qb21 ; qb22 ; qb23 2(qb1 qb2 ; qb0 qb3) 2(qb1 qb3 + qb0 qb2 )
2(qb1 qb2 + qb0 qb3 ) qb20 ; qb21 + qb22 ; qb23 2(qb2 qb3 ; qb0 qb1 )
2(qb1 qb3 ; qb0 qb2 ) 2(qb2 qb3 + qb0 qb1 ) qb20 ; qb21 ; qb22 + qb23
3
7
7
7
7
5
(2.79)

Qpb is normalized in each computation at INS sampling time. The four elements
satisfy the constraint [32]:
qb20 + qb21 + qb22 + qb23 = 1
(2.80)
The computer frame is the local level frame at the INS computed position. The
rotation from the platform frame to the computer frame is represented by quaternion
Qcp :
Qcp = [q0; q1;q2;q3]T
(2.81)
The transformation from the platform frame to the computer frame is given by the
matrix C (Qcp):
2
2 2 2 2 2(q1 q2 ; q0 q3 )
2(q1 q3 + q0 q2 )
6 q0 + q 1 ; q 2 ; q 3
6
C (Qcp ) = 66 2(q1 q2 + q0 q3 ) q02 ; q12 + q22 ; q32 2(q2 q3 ; q0 q1 )
4
2(q1 q3 ; q0 q2 )
2(q2 q3 + q0 q1 ) q02 ; q12 ; q22 + q32
3
7
7
7
7
5
(2.82)
The transformation from the body frame to the computer frame is represented by
the quaternion Qcb : This rotation can be considered as two consecutive rotations from
the body frame to the platform frame, then to the computer frame. Thus:
Qcb = Qcp
Qpb
(2.83)
where
represents the quaternion multiplication. The transformation matrix C (Qcb )
is equal to the product of two transformation matrices.
C (Qcb) = C (Qcp) C (Qpb)
(2.84)
The inconsistencies of the platform frame and the computer frame cause the navigation quaternion error Qpb . Likewise the psi angles which are the misalignment angles between the platform frame and the computer frame, quaternion Qcp consequently stands
for the inconsistency of the p;frame and the c;frame. The attitude error model is
therefore transferred to the modelling of the quaternion Qcp .
2.4.2 Velocity Error Propagation Model

In the computer approach, INS velocity is solved in the computer frame. The true
velocity in the computer frame Vtc is given by:

cie +
cec)Vtc
(2.85)
where ftc is the true specic force resolved in the computer frame, gtc is the gravity
vector resolved in the computer frame,
cie is the matrix of the earth rate resolved in
the computer frame and
cec is the matrix representing the rotation from the computer
frame to the earth frame.
Due to the existence of the error sources, the INS computes the following velocity:
c
V^_ t = (ftp + rp) + gbc ; (2

cie +
cec)Vbtc
(2.86)
where ftp is the true specic force resolved in the platform frame. rp is the bias of the
accelerometers in the platform frame and bgc is the computed gravity vector.
Let V c = Vbtc ; Vtc be the velocity error. The velocity error propagation model is
obtained by subtracting (2.85) from (2.86):
Then:
c
V_ c = V^_ t ; V_tc
(2.87)
= [(ftp + rp) + bgtc ; (2

cie +
cec)Vbtc ] ; [ftc + gtc ; (2
cie +
cec)Vtc ]
= (ftp ; ftc) ; (2
cie +
cec)V c + (gbtc ; gtc ) + rp
= (I33 ; C (Qcp ))ftp ; (2
cie +
cec)V c + rp + gc
where gc = (gbc ; gtc ) and ftc = C (Qcp)ftp. The transformation quaternion matrix
C (Qcp ) is shown in (2.82). Therefore the velocity error model in quaternion form is
V_ c = (I33 ; C (Qcp))ftp ; (2
cie +
cec)V c + rp + gc
(2.88)
This model is applicable to the full range of misalignment angles without small
angle errors assumption.
2.4.3 Position Error Propagation Model

The position error model is de-coupled from the attitude errors [5]. It has the same
general form as the psi angle form. The psi angle and the quaternions do not appear
directly in the position error equation:
R_ c = ;
cecRc + V c
(2.89)
2.4.4 Quaternion Error Propagation Model

Modelling the quaternion error is required to correct and update the navigation quaternions in real time at the sampling time.
The inconsistencies of the platform frame and the computer frame cause the navigation quaternion error Qpb . The quaternion error is modelled using Qcp which represents
the misalignment of the platform frame and the computer frame.
The quaternion Qcp satises the following dierential equation:
Q_ cp = 21
cppQcp
(2.90)
p which is the rotation rate between the

where the 44 matrix
cpp is a function of !cp
c;frame and the p-frame solved in the p-frame. !cpp = [ _ x ; _ y ; _ z ]T . And the psi angle
= [ x ; y ; z ]T is the angle between the p-frame and the c-frame.
2
cpp =
6
6
6
6
6
6
6
4
0 ; _x ; _y ; _z
_x 0
_z ; _y
_y ; _z 0
_x
_z _y ; _x 0
3
7
7
7
7
7
7
7
5
(2.91)

Extending (2.90) :
Q_ cp = 21
cppQcp
2
= 12
6
6
6
6
6
6
6
4
2
= 12
6
6
6
6
6
6
6
4
2
76
76
76
76
76
76
76
54
3
; _ xq1 ; _ y q2 ; _ z q3
_ x q0 + _ z q2 ; _ y q3
_ y q0 ; _ z q1 + _ x q3
_ z q0 + _ y q1 ; _ x q2
;q1 ;q2 ;q3

q0 ;q3 q2
= 12
q3 q0 ;q1
;q2 q1 q0
6
6
6
6
6
6
6
4
Let
32
0 ; _x ; _y ; _z
_x 0
_z ; _y
_y ; _z 0
_x
_z _y ; _x 0
_x
_y
_z
;q1 ;q2 ;q3

6
6
6 q
;q3 q2
B = 12 666 0
q0 ;q1
6 q3
4
;q2 q1 q0
Then
2
6
6
Q_ cp = B 66
4
_x
_y
_z
7
7
7
7
7
7
7
5
7
7
7
7
7
7
7
5
2
7
76
76
76
76
74
7
5
q0
q1
q2
q3
3
7
7
7
7
5
(2.92)
3
7
7
7
7
7
7
7
5
(2.93)
3
7
7
7
7
5
=B _
(2.94)
The general psi angle model when all the psi angles are large is given by:
_ = (I ; Ccp)!icc ; p
(2.95)
2.5 Summary
36
The direction cosine matrix Ccp which is the transformation matrix from the computer frame to the platform frame can be converted into the quaternions form as C (Qpc) :
2
6
6
C (Qpc ) = 66
4
q02 + q12 ; q22 ; q32 2(q1 q2 + q0 q3 ) 2(q1 q3 ; q0q2 ))

2(q1 q2 ; q0 q3 ) q02 ; q12 + q22 ; q32 2(q2 q3 + q0 q1 )
2(q1 q3 + q0 q2 )
2(q2 q3 ; q0 q1 ) q02 ; q12 ; q22 + q32
3
7
7
7
7
5
(2.96)
Therefore, the quaternion model is derived from the psi angle model:
Q_ cp = B _
= B [(I ; C (Qpc ))!icc ; p]
(2.97)
Q_ cp = B (I ; C (Qpc ))!icc ; Bp
(2.98)
That is
Equations (2.98), (2.96) and (2.93) give the general quaternion error model without
small angle assumption. The term p is the gyro error resolved in the platform frame,
!icc is given by:
2
!icc =
6
6
6
6
4
7
7
7
7
5
6
6
6
6
4
!ie cos(') +
!ie sin(')
;Vyc=Ry
Vxc =Rx
Vxc tg(')=Rx
3
7
7
7
7
5
(2.99)
with !ie being the earth rate, ' the local latitude. Rx and Ry are the earth radius elements in the east and north direction in the c-frame. The INS velocity vector
[Vxc ; Vyc ; Vzc]T is resolved in the computer frame.
In this section, the INS velocity, position and attitude error propagation models for
large attitude errors were developed in the quaternion approach in the computer frame.
2.5 Summary
In this chapter INS error propagation models of velocity, position and attitude for
large attitude errors were developed. The models proposed are based on the psi angle
2.5 Summary
37
method and the quaternion method. The error models using psi angles are for the
navigation using the direction cosine matrix. The quaternion models are for navigation
using quaternions.
There were no small angle assumptions in the development of these two sets of
models. These models become essential when the INS attitude errors are large. In the
later chapters, two INS algorithms using these models for INS in-motion alignment and
calibration are presented.
Chapter 3
GPS Modelling in Frequency

Domain
3.1 Introduction
The calibration and alignment of low cost IMUs requires an external aiding sensor. GPS
is an attractive sensor to aid INS in outdoor environment. To improve the operation
of the full system, it is important to model the main sources of GPS errors. This
chapter presents the determination of the GPS error model in the frequency domain.
The frequency domain is an appropriate method to capture, model and understand the
correlated errors that arise in GPS systems.
The navigation problem is generally split into two components: creating a process
model of the host vehicle and understanding or modelling the sensors to be used. The
process model of a navigation system describes the prediction of states which are typically the position, velocity, attitude and related parameters aecting these variables.
The INS error propagation models developed in this thesis are process models.
Section 3.2 overviews previous work on GPS modelling in the frequency domain.
GPS position error models are derived in Section 3.3. It is also proved in this section
that the transfer function of the GPS position error in any frame has identical poles
and zeros to the pseudo range error model. The modelling method using power spectral
38
3.2 GPS Frequency Domain Modelling Review
39
density of noise is described in Section 3.4. Section 3.5 describes the de-correlation of
GPS coloured noise. A shaping lter is introduced to facilitate the de-correlation task
using an additional sensor. Dierent types of feedforward and feedback lter structures
are proposed and the results are presented in the frequency and time domains.

The Global Positioning System provides three-dimensional position and velocity information to users anywhere in the world. Position determination is based on the
measurement of the transit time of signals from at least four satellites. Accuracy in
the order of 1cm-100m may be achieved depending on the type of GPS conguration.
The baseline constellation operates in 12-hour orbits. It provides visibility of 6 to 11
satellites at 5 degrees or more above the horizon to users. Signals are transmitted at
two L-band frequencies to permit corrections to be made for ionospheric delays in signal
propagation time. The signals are modulated with two codes: P , which provides for
precision measurement of time, and C=A, which provides for easy lock-on to the desired
signal [33]. Four satellites are normally required for navigation purposes, and the four
oering the best geometry can be selected using ephemeris information transmitted by
the satellites. Ranges to the four satellites are determined by scaling the signal transit
time by the speed of light. The transmitted message contains ephemeris parameters
that enable users to calculate the position of each satellite at the time of transmission
of the signal. Operation of the system requires precise synchronization of space vehicle
clocks with \GPS system time". The requirement for users to be equipped with precision clocks is eliminated by the use of range measurements from four satellites. The
four satellites permit an estimate of the user's clock error. The user position equations
contain four unknowns consisting of position in three dimensions and the error in the
user's imprecise clock.
The measurement of range to the satellites is called \pseudo range". It is dened
40
as
R = % + c t + n
(3.1)
where R is the pseudo range to the satellite, % is the true range, c is the speed of
light, t is the receiver clock error and n is the correlated noise. Errors contained
in the pseudo range measurement can be divided into the following main categories
[33]: space vehicle clock errors, atmospheric delays, group delay, ephemeris errors,
multipath, receiver noise and resolution, receiver vehicle dynamics. The Dierential
GPS (DGPS) is a method to correct the eects of measurement errors. This method can
help increase receiver accuracy from 100 metres to metres. The dierential corrections
are calculated at a base station away from the user receiver. In a case where the DGPS
correction is unavailable or in between the correction update time, it is necessary for
the characteristics of the errors to be modelled. Some error modelling methods have
appeared in the literature [34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44]. To the best of the
author's knowledge, there is no GPS error modelling theory using the receiver position
directly in the frequency domain.
3.2.1 GPS Receiver Clock Oset Model

The accuracy of the GPS range measurement relies on the accuracy of the transmitted
time measurement. Each GPS satellite clock is monitored and constantly adjusted to
be synchronous with GPS time. Each GPS receiver uses a low cost quartz oscillator
as its timing reference. The receiver clock oset is usually modelled by users. Cooper
[45] modeled this oset using a constant velocity model, because the receiver clock has
a reasonably constant frequency:
2
6
4
ct_(t)
ct(t)
7
5
= 64
0 1
0 0
32
76
54
ct(t) 7 6 1 0
5+4
c_ t(t)
0 1
32
76
54
wp (t)
wv (t)
3
7
5
(3.2)
where c is the light speed, t(t) is the receiver clock oset at time t, wp (t) and wv (t)
are process noise.
41
3.2.2 GPS Correlated Error Model

Cooper and Durrant-Whyte introduced a modelling technique in the frequency domain
[45]. The correlated error in the pseudo range is modelled using power spectral density
(PSD). It was shown that all the satellites have the same correlated noise (or coloured
noise) statistics. The power spectral density i (s) of the correlated noise of each
satellite has the form of a fourth order system:
+ ) ]2
i (s) = [ s2 +r(2sks
+ 2
(3.3)
The transfer function (3.3) can be converted to state space form so that the correlated
noise can be estimated along with the original lter states. For a single satellite, the
correlated noise in the pseudo range at time t is estimated as:
2
6
4
x_ 1 (t) 7 6 ;2k 1
5=4
x_ 2 (t)
;2 0
32
76
54
x1 (t) 7 6 r 7
5+4
5 wr (t)
x2 (t)
r
(3.4)
where x1 (t) is the state of interest, x2 (t) is the augmented state, wr (t) is a white noise
series, ; k; r and are the model parameters in (3.3).
Hence, the GPS error propagation model for a single satellite is:
x_ (t) = Fr x(t) + Gr ur (t)

where
ct(t) 7
6
6
7
6
c_ t(t) 77
6
x(t) =
7 ; Fr = 6
7
6
x1(t) 7
6
5
4
x2(t)
6
6
6
6
6
6
6
4
and
Gr =
6
6
6
6
6
6
6
4
1
0
0
0
0
1
0
0
0
0
r
r
3
7
7
7
7
7
7
7
5
0
0
0
0
1
0
0
0
(3.5)
0 0
0 0
;2k 1
;2 0
6 wp (t)
6
; ur (t) = 66 wv (t)
4
wr (t)
3
7
7
7
7
7
7
7
5
(3.6)
3
7
7
7
7
5
(3.7)
42
In the example an INS was used as an external sensor, the INS indicated position
was converted into "inertial-pseudo-range" by calculating the expected range to each
satellite using the inertially indicated position and the known satellite locations. The
converted inertial measurement Rins (t) is given by:
p
Rins(t) = (Xsat (t) ; Xins (t))2 + (Ysat (t) ; Yins (t))2 + (Zsat (t) ; Zins(t))2 (3.8)
where
[Xsat (t); Ysat (t); Zsat (t)] is the satellite position in WGS-84 reference.
[Xins (t); Yins (t); Zins (t)] is the INS indicated vehicle position in WGS-84 reference.
The GPS uses the World Geodetic System WGS-84 as a reference. WGS-84 is an
earth xed global reference frame, including an earth model. It is dened by a set of
parameters which dene the shape of an earth ellipsoid, its angular velocity, the earth
mass which is included in the ellipsoid reference and a detailed gravity model of the
earth [46].
There are a number of shortcomings with this modelling method. The rst is the
need to model the receiver's clock oset. Further, all the pseudo ranges to all the
satellites in use have to be modelled. Assuming that a maximum of ten satellites may
be tracked at a given time, the total error model can add an additional twenty two states
to the process model used in the fusion lter. Finally, the measurement of other sensors
has to be converted into an equivalent pseudo range to match the GPS parameters.
These problems motivated the development of an improved modelling method by
using the position measurement directly without modelling the receiver's clock oset
and the pseudo ranges of all the tracked satellites. The measurement of other sensors
may be used as position information without the need to convert into the equivalent
pseudo range. The number of the model states will be reduced to six. The improved
method is introduced in the following sections.
3.3 GPS Position Model
43
Figure 3.1: Satellites, receiver and ranges.

In this chapter the theory that the correlated position errors in any Cartesian coordinates system have an identical model structure is presented . They can be represented
with a second order system driven by a white noise similar to Equation (3.3).
As shown in Figure 3.1, at time t the coordinates of four visible satellites S1 , S2 , S3
and S4 in WGS-84 frame are (X1; Y1; Z1 ), (X2; Y2; Z2 ), (X3; Y3; Z3 ) and (X4; Y4;Z4 ). The
three axes of the WGS-84 frame are x, y and z . The position of the receiver in the
WGS-84 frame is (X0; Y0; Z0 ) and the true ranges from each satellite to the receiver are
1 , 2 , 3 and 4 with
p
i = (Xi ; X0 )2 + (Yi ; Y0 )2 + (Zi ; Z0 )2 ; i = 1; 2; 3; 4
(3.9)
The measured pseudo ranges Ri (i = 1; 2; 3; 4) are the combinations of the true

range, satellite clock oset range, receiver clock bias range and the correlated noise ni .
See Figure 3.2.
Ri = i + c (Dti ; Dt0 ) + ni ; i = 1; 2; 3; 4
(3.10)
where c is the light speed, Dti (i = 1; 2; 3; 4) is the satellite clock bias and Dt0 is the
unknown receiver clock bias.
44
Figure 3.2: GPS pseudo range and errors.

e Y
e; Z
e).
Assume the calculated receiver position from the pseudo ranges is (X;
Xe = X0 + Dx; Ye = Y0 + Dy ; Ze = Z0 + Dz
(3.11)
where (Dx ; Dy ; Dz ) is the correlated position error.

The pseudo range Ri (i = 1; 2; 3; 4) at the receiver true position (X0; Y0; Z0 ) with
increment (Dx ; Dy ; Dz ) is:
X0 D ; Yi ; Y0 D ; Zi ; Z0 D
Ri = i ; Xi ;
x
y
z

(3.12)
X0 ; b = ; Yi ; Y0 ; c = ; Zi ; Z0
ai = ; Xi ;
i
i

(3.13)
Let
i
Subtracting Equation (3.12) from (3.10) and leaving the terms containing position
errors on the left side:
aiDx + bi Dy + ci Dz = c(Dti ; Dt0) + ni
(3.14)
For the four individual satellites, we have
a1 Dx + b1 Dy + c1Dz = c(Dt1 ; Dt0 ) + n1
(3.15)
(3.16)
(3.17)
(3.18)
45
Subtracting Equation (3.15) from (3.16), (3.17) and (3.18) respectively, the receiver
clock bias Dt0 is removed:
(a1 ; a2 )Dx + (b1 ; b2 )Dy + (c1 ; c2 )Dz = c(Dt1 ; Dt2 ) + (n1 ; n2 )
(3.19)
(3.20)
(3.21)
The position error (Dx , Dy , Dz ) is consequently obtained from (3.19), (3.20) and
(3.21) with linear combination of satellite clock bias terms and range measurement
correlated noise ni (i = 1; 2; 3; 4).
Let

a1 ; a2 ; b1 ; b2 ; c1 ; c2
d = a1 ; a3 ; b1 ; b3 ; c1 ; c3
a1 ; a4 ; b1 ; b4 ; c1 ; c4

b1 ; b3 ; c1 ; c3
b1 ; b4 ; c1 ; c4
d12=
d

(3.22)
(3.23)

b1 ; b2 ; c1 ; c2
b1 ; b2 ; c1 ; c2
b1 ; b4 ; c1 ; c4
b1 ; b3 ; c1 ; c3
d13=
;
d
(3.24)
14=
d
d
where d; d12 ; d13 and d14 are related to the true ranges and true positions which can be
considered constants at that time.
Consider Dx rst, then at time t:
Dx (t) = d12 c(Dt1 (t) ; Dt2 (t)) ; d13 c(Dt1 (t) ; Dt3 (t)) +
+ d14 c(Dt1 (t) ; Dt4 (t)) + d12 (n1 (t) ; n2 (t))
; d13 (n1(t) ; n3(t)) + d14 (n1(t) ; n4(t))
(3.25)
The satellite clock bias ranges c(Dt1 (t) ; Dt2 (t)), c(Dt1 (t) ; Dt3(t)) and c(Dt1 (t) ;
Dt4(t)) are provided by the satellite ephemeris data in GPS message. After removing
46
these main clock bias ranges, the remaining error Dx cor(t) is the linear combination
of correlated noise (n1 (t) ; n2 (t)); (n1 (t) ; n3 (t)) and (n1 (t) ; n4 (t)):
Dx cor(t) = d12 (n1 (t) ; n2 (t)) ; d13 (n1 (t) ; n3(t)) + d14 (n1 (t) ; n4 (t))
(3.26)
In frequency domain:
Dx cor(s) = d12 (n1(s) ; n2(s)) ; d13 (n1(s) ; n3(s)) + d14 (n1 (s) ; n4(s))
(3.27)
The correlated noise of the pseudo range of all the satellites has an identical second
order model structure [45]. The model for each satellite has identical poles and zeros.
Therefore the linear combination of any of these correlated noises will not change the
poles and zeros. The position error will have an identical model structure with the
identical poles and zeros as the pseudo range model. The gain, however, is dierent
and can be obtained using the following method.
Assuming that the correlated noise ni (s) (i = 1; 2; 3; 4) of the pseudo range has the
following transfer function which is a second order system driven by a white noise series
!i (i = 1; 2; 3; 4) with unit strength:
+ ) !
ni (s) = s2 +ri2(sks
+ 2 i
(3.28)
where ; k; are identical to all the satellites. The gain ri is dierent for each satellite
and is related to the gain of the transfer function.
Then
+ ) ! ; r2 (s + ) ! ) ;
Dx cor(s) = d12 ( s2 +r12(sks
+ 2 1 s2 + 2ks + 2 2
+ ) ! ; r3 (s + ) ! ) +
; d13 ( s2 +r12(sks
+ 2 1 s2 + 2ks + 2 3
+ ) ! ; r4 (s + ) ! )
+ d14 ( s2 +r12(sks
+ 2 1 s2 + 2ks + 2 4
+ ) [(r ! ; r ! )d ;
= s2 +(2sks
+ 2 1 1 2 2 12
; (r1 !1 ; r3 !3)d13 + (r1 !1 ; r4 !4)d14 ]
+ ) [! r (d ; d + d ) ;
= s2 +(2sks
+ 2 1 1 12 13 14
; !2 r2d12 + !3r3 d13 ; !4r4d14 ]
(3.29)
47
We know that a linear combination of white noise is also white noise. Hence in
(3.29) [!1 r1 (d12 ; d13 + d14 ) ; !2 r2 d12 + !3 r3 d13 ; !4 r4 d14 ] is still a white noise series
with strength Gainx :
Gainx = E f[!1 r1 (d12 ; d13 + d14 ) ; !2r2 d12 +

+ !3 r3 d13 ; !4 r4 d14 ]T [!1 r1 (d12 ; d13 + d14 ) ;
+ !2 r2 d12 + !3 r3 d13 ; !4 r4 d14 ]g
= r12 (d12 ; d13 + d14 )2 + r22 d212 + r32 d213 + r42 d214
Therefore Dx cor(s) has the identical second order model structure

+
Dx cor(s) = s2 + 2sks
+ 2 !cor
(3.30)
driven by a white noise series !cor with strength Gainx :

The position errors in y and z also have a similar model with the same poles and
zeros and dierent gains.
The latitude and the longitude of the receiver can be linearly transferred from the
receiver's coordinates in axes x; y and z of the WGS-84 frame. Their correlated error
models also have identical structures. The position in any other Cartesian coordinate
system can be transferred to the WGS-84 frame by multiplying a direction cosine matrix. Hence, it can be concluded that in any other Cartesian coordinate system, the
position correlated errors have identical error models.
Therefore, the correlated error of positions can be modelled independently without
modelling the correlated errors of the pseudo ranges of an individual satellite and the
receiver clock error.
In this thesis, PSD techniques are used to model the error in x, y and z position
information provided by GPS.
3.4 GPS Position Error Modelling Using PSD
48

3.4.1 Power Spectral Density and Autocorrelation of Signals
Correlated signals can be examined in either the time domain or the frequency domain.
Frequency characteristics are shown in the power spectral density of the signals. Time
information is contained in the autocorrelation.
For an ergodic signal x(t) the expected value of x(t)x(t + ) is given by [47]:
1 Z T x(t)x(t + )dt
E fx(t)x(t + )g = xx( ) = Tlim
(3.31)
!1 2T ;T
where xx( ) is called the autocorrelation function . is deend as the delay variable.
The expected value of x(t) y(t + ) is given by
Z T
1
E fx(t)y(t + )g = xy ( ) = Tlim
x(t)y(t + )dt
(3.32)
!1 2T ;T
where xy ( ) is called the crosscorrelation function and yields information as to the
dependence or correlation of x(t) to y(t). y(t) is a second random signal.
Assuming that the time functions given by (3.31) and (3.32) are Fourier transformable, the Fourier transform of the autocorrelation and the crosscorrelation functions are dened as
Z 1
1
xx( )e;j! dt
xx(j!) = 2
;1
Z 1
xy (j!) = 21
xy ( )e;j! dt
;1
(3.33)
(3.34)
where xx(j!) is called the power spectral density (PSD) function and is xy (j!) called
the cross power spectral density function.
The autocorrelation is the Inverse Fourier Transform (IFT) of its PSD.
Consider the following linear time-invariant system with transfer function g1 (t):
x(t)9 9 K g1 (t) 9 9 Ky(t)
(3.35)
where the input and output are related by
y(t) =
;1
x(t ; )g1 ()d
(3.36)
49
The power spectral density function xx(j!) of the input x(t) and the power spectral
density function yy (j!) of the output y(t) have the following relationship:
yy (j!) =j G1 (j!) j2 xx(j!)
(3.37)
with G1 (j!) being the transfer function of this system in the frequency domain.
The transfer function of the correlated error model of GPS position has the structure:
+ )
G1 (s) = s2 +r(2sks
+ 2
(3.38)
The PSD cor (s) of the correlated error Xs (s) can be created by passing white noise
!(s) with a constant PSD white noise through the transfer function G1 (s) :
!(s)
99 K
G1 (s) 9 9 KXs (s)
(3.39)
And
cor (s) =j G1 (s) j2 white noise
(3.40)
G1 (s) can be used to create a shaping lter for use as an estimation error model.
3.4.2 Error Modelling Using PSD and Autocorrelation

The GPS correlated errors are modelled by the transfer function (3.38). The poles,
zeros and the gain of G1 (s) are determined using the PSD and the autocorrelation of
the GPS noise signals. In this section, the modelling techniques are presented.
To identify the GPS noise, raw data are collected from a known true position. For
example, Figure 3.3 shows the raw data in axis x collected during 12 hours at a xed
position. At least 50 groups of data have to be collected to obtain an average PSD.
The noise data are obtained by removing the true position data from the raw data
as shown in Figure 3.4. It can be seen that the noises of standard GPS position output
are within 100 metres.
The Hanning window is applied to the noise data before the calculation of its power
spectral density and autocorrelation [48].
4.6599
50
raw data x
x 10
4.6599
4.66
4.66
4.66
4.66
4.66
4.6601
4.6601
4.6601
4.6601
2000
4000
6000
8000
10000
12000
14000
16000
18000
Figure 3.3: Raw data of GPS position in x-axis.
noise x
100
80
60
40
20
20
40
60
80
2000
4000
6000
8000
10000
12000
14000
Figure 3.4: GPS correlated noise in x-axis
16000
18000
51
Least Square Method

Once the PSDs are obtained, a least square t method can be used to identify the
parameters of the transfer function.
Assume that the GPS correlated noise Xs is driven by a white noise of unit strength.
From Equation (3.40), in the frequency domain the PSD Xs of the noise Xs has the
following relation with the model:
+ )
2
j X (j!) j=j (j!)2 +r(j!
2k(j!) + 2 j
s
(3.41)
with the frequency series ! f0; +1g:

Obtaining the logarithm base 10 of the elements of (3.41):
r(j! + )
10 lg j Xs (j!) j= 20 lg j (j!)2 +
2k(j!) + 2 j
= 10 lg
(3.42)
r(!2 + 2 ) p
[(! + 1 ; k2 )2 + 2 k2 ][(! ; 1 ; k2 )2 + 2 k2 ]
Set k1 = 1 ; k2 ; and
fw =
r (! 2 + 2 ) p
[(! + 1 ; k2 )2 + 2 k2 ][(! ; 1 ; k2 )2 + 2 k2 ]
(3.43)
Then
r (! 2 + 2 )
fw = (!2 + 2 + 2!k
1 )(!2 + 2 ; 2!k1 )
2
2
= 2 r(!2 2 + )2 2 2
(! + ) ; 4! k1
(3.44)
Equation (3.42) can be simplied as
!2 r + y ; pfw ; 2q!2fw + 4u!2 fw = fw !4
(3.45)
y = r 2 ; zw = (!2 + 2 )2 ; u = 2 k12
(3.46)
when
p = 4 ; q = 2
52
That is:
2
!2 1 ;fw ;2!2 fw
r7
7
y 77

7
4!2 fw p 77 = fw !4
7
q 775
u
6
6
6
6
6
6
6
6
6
6
4
(3.47)
The least square equation (3.47) can be used to obtain the parameters r; y; p; q; u
which can be transformed to ; k; r; by (3.46). Back to (3.43) and (3.42), fw can be
obtained from the PSD of raw data 10 lg j Xs (j!) j:
fw = 10 lg j 10Xs (j!) j
(3.48)
Curve Fitting Method

Curve tting can also be used to obtain the parameters of the model.
Figure 3.5 shows the PSD plot of GPS correlated noise in axis x given in the WGS-84
frame. The curve has a cut-o frequency at approximately 0.0018Hz. The roll-o rate
after this cut-o is about ;40dB=decade. This roll-o shows that there are two poles in
the transfer function of the error model. There is also a slope change to ;20dB=decade
at the frequency 0:01Hz which indicates that there is a zero in the transfer function. In
the end of the curve, there is some white noise. This PSD curve shows the character
of the 2nd order system:
+ )
G1 (s) = s2 +r(2sks
+ 2
(3.49)
The PSD plot characteristics are identical to the system (3.30).

The parameters in (3.49) therefore can also be obtained from curve tting manually.
To model the errors accurately, 50 groups of raw data were collected to average
the PSD evaluation. Each group must contain the consecutive raw data for at least 30
minutes. The PSD mean curve which is the average PSD of these 50 groups of data is
shown in Figure 3.6. Also shown in this gure is the PSD curve using the estimated
3.5 De-correlate GPS Noise Using INS
53
PSD :x
60
cutoff frequency: 0.002HZ
50
40
rolloff rate: 40dB/decade
30
20dB/decade
20
10
white noise
10
20
4
10
10
10
10
10
Figure 3.5: PSD curve : x

parameters. The parameters are adjusted until the curve of the estimated PSD best
ts the raw PSD curve.
The best tting PSD curve is obtained by manually adjusting these curves and
visual inspection. The model parameters are consequently the best tted ; ; k and r
in that curve. This PSD curve tting technique can be applied to any position correlated
noise in any axis in any frame. Once the model is determined, a traditional shaping
lter can be introduced using the transfer function (3.49).

3.5.1 Shaping Filter
A shaping lter models a correlated noise as a linear system driven by white noise [49].
The output of the shaping lter is the observed measurement error. The PSD model of
GPS correlated noise can be expressed in block diagram form in Figure 3.7 as a shaping
lter.
The input of this shaping lter is a unity white noise !sp(t). The output Xs (t) is
54
PSD
50
40
30
20
10
10
20
30
4
10
10
10
10
Figure 3.6: PSD curve tting.
Figure 3.7: Shaping lter.
10
55
the GPS correlated noise. Its state space form is given by:
X_ s(t) = ;2kXs (t) + Xss (t) + r!sp(t)
(3.50)
X_ ss(t) = ;2 Xs (t) + r!sp(t)

That is:
2
6
4
X_ s(t)
X_ ss(t)
7
5
= 64
32
;2k 1 7 6 Xs(t) 7 6 r 7
54
5+4
5 !sp (t)
2
; 0
Xss (t)
r
(3.51)
Suppose there is a system with m 1 state vector x(t) and p 1 measurement vector
z(t):
x_ (t) = F (t)x(t) + G(t)!(t)
(3.52)
z(t) = H (t)x(t) + ns(t) + v(t)

The system process noise !(t) is a q 1 white noise vector. The measurement is
corrupted by white noise v(t) and nonwhite correlated noise ns (t) with:
ns (t) = Xs (t)
(3.53)
Dene the augmented state vector process xa (t) as:

2
6
6
6
6
4
7
7
7
7
5
x(t)
xa (t) = Xs (t)
Xss(t)
(3.54)
The augmented state process model can be written as

2
x_ (t) 7
7
x_ a (t) = X_ s (t) 77
5
_Xss (t)
(m+2)
2
F (t) 0m2 0m2
6
6
= 66 01m
;2k 1
4
;2 0
01m
6
6
6
6
4
(3.55)
32
76
76
76
76
54
x(t) 7 6 G
7 6
Xs (t) 77 + 66 01q
5 4
Xss(t)
01q
0q1
r
r
2
7
76
74
7
5
!(t)
!sp(t)
3
7
5
56
The augmented system output can be written as:

2
x(t) 7
7
z(t) = H (t) Ip 0p
Xs (t) 77 + v(t)
5
Xss (t)
= Ha (t) xa (t) + v(t)
6
6
6
6
4
(3.56)
with

Ha(t) = H (t) 0p2
(3.57)
The augmented system with shaping lter is depicted in Figure 3.8. It is noted that
the only driving noise in the augmented system is white noise. It consequently satises
the basic assumption for the Kalman lter that the non-model dynamics noise is white.
The coloured noise ns (t) will only be de-correlated from the GPS measurement by
external information.
3.5.2 De-correlation of GPS Noise Using Feedforward Filter

Description of the De-correlation Filter
When using a single GPS with correlated noise, the system will exhibit almost identical
behaviour regardless of whether the GPS error has been modelled. Further sensing is
needed to aid the de-correlation of the coloured noise to improve system accuracy [17].
Consider a lter in one dimension with the combination of a GPS and an INS as in
Figure 3.9.
Suppose that the acceleration, velocity and position of INS outputs and the position
output of GPS have been transformed into the same navigation frame. Assuming that
the acceleration is corrupted by white noise wins (t) only. The lter states x(t) are INS
position error dr(t), INS velocity error dv(t) and two GPS shaping states Xs (t), Xss (t) :
x(t) = [dr(t); dv(t); Xs (t); Xss (t)]T
(3.58)
Figure 3.8: Shaping lter generating measurement corruption noise.
Figure 3.9: Feedforward lter with GPS and INS.
57
58
and
dr(t) = Reins(t) ; Rtrue (t)
(3.59)
dv(t) = Veins(t) ; Vtrue (t)

where Rtrue (t) and Vtrue (t) are the true position and true velocity in the navigation
frame. Reins (t) and Veins (t) are the outputs of INS position and velocity. The lter
measurement z (t) is the dierence between the GPS output ReGPS (t) and the INS
output Reins (t):
z(t) = ReGPS (t) ; Reins(t)
(3.60)
GPS outputs are corrupted by the coloured noise Xs (t) and white noise v(t).
We have the relation:
dr_ (t) = dv(t)
(3.61)
dv_ (t) = wins(t)

and
z(t) = ReGPS (t) ; Reins(t)

= [Rtrue (t) + Xs (t) + v(t)] ; [Rtrue (t) + dr(t)]
= ;dr(t) + Xs (t) + v(t)
(3.62)
The lter process model is given by

2
6
6
6
6
6
6
6
4
dr_ (t) 7
6
7
6
7
6
dv_ (t) 7
6
7 = A6
7
6
X_ s(t) 7
6
5
4
_Xss (t)
dr(t) 7
2
7
7
dv(t) 7
6 wins (t)
7 + Gf 4
Xs (t) 77
!sp(t)
5
Xss (t)
3
7
5
(3.63)
with the process noise being a white noise sequence:

2
w(t) = 64
wins(t)
!sp(t)
3
7
5
(3.64)

where
A=
6
6
6
6
6
6
6
4
0
0
0
0
1 0 0
0 0 0
0 ;2k 1
0 ;2 0
2
Gf =
59
6
6
6
6
6
6
6
4
0
1
0
0
0
0
r
r
3
7
7
7
7
7
7
7
5
(3.65)
3
7
7
7
7
7
7
7
5
(3.66)
The observation model is given by
z (t) = Hf x(t) + v(t)
(3.67)
Hf = [;1; 0; 1; 0]
(3.68)
with
The process and observation noise covariance matrices are

2
Q = E [w(t)wT (t + )] = 64
2 0
qins
0 1
2 ( )
R = E [v(t)vT (t + )] = gps
3
7
5
( )
(3.69)
(3.70)
2 on the acceleration output and the variance gps

2 on
with the white noise variance qins
the GPS position observation.
De-correlation Feedforward Filter Performance in Frequency Domain and

Time Domain
In the frequency domain, the transfer function from the observation z (s) to the Kalman
lter states estimate xb(s) is calculated by:
xb(s) = (sI ; A + KH );1 K

44
f
z (s)
(3.71)
60
poor INS
100
dr
Gain dB
0
100
Xs
200
300 4
10
10
10
10
Frequency (rad/sec)
10
10
180
Phase deg
dr
0
dr
Xs
Xs
180
360
4
10
10
10
10
Frequency (rad/sec)
10
10
Figure 3.10: Bode plot of the feedforward lter using a poor INS.
with the lter gain K having been determined by the lter parameters and the process
and observation noise.
For a single standard GPS without dierential correction, the variance of position
error is about 20 20m2 . The Bode plot of a low quality INS with a variance of the
acceleration output of 0:1 0:1(m=s2 )2 is shown in Figure 3.10. From the plot it can be
seen that the gain for the shaping state estimate is very small. The performance of the
system in the time domain is shown in Figure 3.11. The GPS position output consists
of almost the entire correlated noise when the vehicle is stationary. The estimated
shaping state does not follow the shape of the GPS output. The estimate position
follows the INS output whose errors grow without bound due to no real-time correction
in this kind of lter structure. De-correlation of GPS coloured noise fails with a poor
INS of this level quality. These results were presented by Figure 3.10.
Figure 3.12 is the Bode plot of the feedforward lter using a high quality INS with
a variance of 1 10;6 (m=s2 )2 of the acceleration output. The gain for the shaping
state estimation in frequency band 10;4 (rad= sec) to 10;2 (rad= sec) is 0dB . Figure
3.13 shows the time domain performance of the system. The shaping state estimate
61
poor INS
100
GPS
meters
50
Xs
0
50
100
0
2000
4000
6000
8000
10000
12000
14000
16000
18000
14000
16000
18000
50
position estimate
meters
INS
50
100
150
0
2000
4000
6000
8000
10000
Time
12000
Figure 3.11: Time domain performance of the feedforward lter using a poor INS.
Xes has almost the same shape and magnitude as the GPS output which is almost the
entire coloured noise since the observation corresponds to a xed position. The position
estimate error has been considerably reduced to 4m only.
Figure 3.14 shows the Bode plot of the transfer function (3.71) when the variance
of the INS noise changes from 0:1 0:1(m=s2 )2 to 1 10;6 (m=s2 )2 :
The essential requirement for the variance of the acceleration noise for de-correlation
can be found from the Bode plot of the lter transfer function. Therefore, in order to use
an indirect feedforward Kalman lter to de-correlate GPS coloured noise with an INS,
the accuracy of the INS needs to be above a certain threshold which can be obtained
from the Bode plot.
62
good INS
50
Xs
Gain dB
0
dr
50
100 5
10
10
10
10
10
Frequency (rad/sec)
10
10
360
Phase deg
dr
Xs
180
dr
Xs
180
5
10
10
10
10
10
Frequency (rad/sec)
10
10
Figure 3.12: Bode plot of the feedforward lter using a good INS.
good INS
100
50
meters
Xs
0
GPS
50
100
0
2000
4000
6000
8000
10000
12000
14000
16000
14000
16000
meters
position estimate
2
1
0
INS
1
2
0
2000
4000
6000
8000
Time
10000
12000
Figure 3.13: Time domain performance of the feedforward lter using a good INS.
63
Magnitude of bode diagram : Xs/Zgps
50
0
Magnitude
50
100
150
200
250
0
20
40
Frequency
60
10
15
20
25
30
INS noise
Figure 3.14: Bode plots of the transfer functions using the feedforward lter. The
magnitude of the Bode plots, the frequency in rad=sec unit and the INS noise in m=s2
unit are logarithmically scaled. The magnitude unit in the plots represent the power
numbers of the logarithm units. The units for the frequency and INS noise represent
the negative power numbers of the logarithm units.
64
Figure 3.15: Indirect feedback lter
3.5.3 De-correlation Using Indirect Feedback Filter

Filter Description
Using a feedforward lter with INS to de-correlate GPS coloured noise can make the
position error grow without bound. An indirect feedback lter can overcome this problem by using estimated INS error to correct the state. Figure 3.15 shows the lter
structure.
The INS generates velocity and position at very high frequency. The lter processes
the measurement dierence between the GPS position and INS position at the GPS
sampling time. The lter estimates the position error dr, velocity error dv and two
GPS shaping states, feeds back the error estimate to the INS position and velocity to
correct the INS velocity and position outputs of the lter. The INS keeps generating
the output of the lter until a new GPS observation is made. The lter states are
still the INS position error dr, INS velocity error dv and two GPS shaping states Xs ,
Xss. The process model and the observation model remain the same as the indirect
feedforward lter as Figure 3.9.
65
Filter Performance in Frequency Domain and Time Domain

The performance of the indirect feedback lter is analyzed both in the frequency domain
and the time domain. It is found from the experiments that the quality of the INS still
aects the de-correlation in this lter structure.
Figure 3.16 shows the Bode plot of the transfer function from the lter observation
to the lter estimate using a low quality INS with a noise variance of 0:1 0:1(m=s2 )2 :
From the plot it can be seen that the gain for the GPS shaping state is very low.
The time domain performance is shown in Figure 3.17. The shaping state estimate
does not follow the shape and the gain of the GPS measurement which is almost the
entire coloured noise since the observation corresponds to a xed position. The INS
position output and position estimate are bound over time. The GPS coloured noise
de-correlation does not work appropriately with this quality of INS.
poor INS
0
Gain dB
dr
50
Xs
100
150 4
10
10
10
10
Frequency (rad/sec)
10
10
180
dr
Xs
Phase deg
Xs
0
dr
180
360
4
10
10
10
10
Frequency (rad/sec)
10
10
Figure 3.16: Bode plot of the feedback lter with a poor INS.
1 10;6 (m=s2 )2 is the threshold value of the noise variance of an accelerometer.
The gain of the transfer function for the shaping state Xes is 0dB in the Bode plot before
10;2 rad= sec as in Figure 3.18. This means that the shaping state estimate Xes tracks
3.6 Conclusion
66
poor INS
100
GPS
meters
50
Xs
0
50
100
0
2000
4000
6000
8000
10000
12000
14000
16000
18000
14000
16000
18000
100
position estimate
meters
50
0
INS
50
100
0
2000
4000
6000
8000
10000
Time
12000
Figure 3.17: Time domain performance with a poor INS using a feedback lter.
the observation which almost entirely consists of GPS coloured noise. Figure 3.19 shows
the performance in the time domain. The shaping state estimate Xes matches the GPS
measurement observations.
In order to generate de-correlation the quality of the INS can be obtained using
Figure 3.20. From the plot this essential variance requirement is still 3:16 10;6 (m=s2 )2
for a standard GPS with a variance of 20 20m2 :
Therefore, to use an indirect feedback lter to de-correlate the GPS coloured noise,
the essential quality requirement of the accelerometer is the same as for the feedforward
lter. The additional advantage with this approach is that the position estimate is
bound.
3.6 Conclusion
This chapter has introduced an improved GPS error modelling method in the frequency
domain. In previous work, the errors were modelled in the pseudo range and clock
oset. The measurement from other sensors has to be converted into an equivalent
3.6 Conclusion
67
good INS
0
Xs
Gain dB
20
dr
40
60
80 5
10
10
10
10
10
Frequency (rad/sec)
10
10
Phase deg
180
dr
Xs
Xs
180
dr
360
5
10
10
10
10
10
Frequency (rad/sec)
10
10
Figure 3.18: Bode plot of the indirect feedback lter with a good INS.
good INS
100
50
meters
Xs
0
GPS
50
100
0
2000
4000
6000
8000
10000
12000
14000
16000
14000
16000
20
meters
10
INS
0
10
position estimate
20
0
2000
4000
6000
8000
Time
10000
12000
Figure 3.19: Time domain performance of the indirect feedback lter using a good INS.
3.6 Conclusion
68
Magnitude of bode diagram : Xs/Zgps
Magnitude
50
100
150
200
250
0
20
40
Frequency
60
10
15
20
25
30
INS noise
Figure 3.20: Bode plots of the transfer functions using indirect the feedback lter. The
magnitude of the Bode plots, the frequency in rad=sec unit and the INS noise in m=s2
unit are logarithmically scaled. The magnitude unit in the plots represent the power
numbers of the logarithm units. The units for the frequency and INS noise represent
the negative power numbers of the logarithm units.
3.6 Conclusion
69
pseudo range to match the GPS measurement.

The model of the correlated noise in a GPS position has been derived using a second
order system driven by a white noise series.
The PSD model of the correlated position errors in any axis of any frame has been
proved to have an identical structure. The parameters of the position error model are
obtained from the PSD of the position raw data. By adjusting the poles, zero, damping
ratio and the magnitude of the estimate PSD curve against the PSD curve of raw data,
the best parameters can be found using the least square or the curve tting methods.
The PSD model has been used in a shaping lter. To de-correlate the GPS error,
another sensor has to be used. Two de-correlation lters using an INS have been
discussed in this chapter. To use an indirect feedforward lter with an INS, the position
estimate is not bound. The indirect feedback lter has better behaviour with a bound
position estimate. The quality of the INS in both the de-correlate lters is required for
the de-correlation. The essential variance of the INS has been found using the Bode
plot of the transfer function of the de-correlation lters.
The GPS model developed in this chapter will be used in the data fusion lter whose
observation contains GPS measurement.
Feedback and feedforward methods have been proposed to design a lter that decorrelates the GPS coloured noise. The minimal requirement for the external sensor is
also presented.
Chapter 4
INS Algorithm for Low Cost

IMU in Psi Angle Approach
4.1 Introduction
This chapter addresses the design of an INS algorithm for low cost IMUs with unknown
initial conditions using the psi angle approach in the computer frame.
Previous INS alignment algorithms are reviewed in Section 4.2. Self-alignment approaches are reviewed in Section 4.2.1. Analytic coarse alignment and gyrocompassing
are the major methods for self-alignment. They require measurements of the gravity
vector and the earth rate by three accelerometers and three gyros. In-motion alignment
is another approach for INS ne alignment which is reviewed in Section 4.2.2. Most
of the published applications of in-motion alignment are based on lters with known
initial attitudes. For low cost IMUs, the initial attitude errors are usually large. There
are very few works that have attempted to solve the problem.
In this chapter, a new algorithm is presented that does not require initial attitudes.
Section 4.3 describes the coarse alignment for raw data process and IMU turn-on biases
estimation. A method to determine the initial direction cosine matrix is formulated in
Section 4.3.2.
Unknown initial attitudes are solved in Section 4.4 by an in-motion alignment al70
4.2 Overview
71
gorithm. The lter process models and the measurement equations are developed in
Section 4.4.3. The non-linear lter is implemented using the extended Kalman Filter
(EKF). The discrete lter and Jacobian matrix are derived in Section 4.4.4.
Section 4.5 describes the navigation stage after the initial alignment. The continuing
alignment and the calibration of the system in the navigation are brie y discussed in
this section.
4.2 Overview
The INS algorithm generates velocity, position and attitude information. The inputs
to the INS algorithm are the measured gyro and accelerometer outputs. The INS
algorithm outputs navigation data in a desired navigation frame. The main functions
executed in a strapdown INS algorithm are the integration of IMU measured angular
rate into attitude and also the transformation and integration of IMU measured specic
force acceleration, Coriolis acceleration and modelled gravity to the desired navigation
frame [50, 51].
The INS integration assumes the initial values of velocity, position and attitude.
Therefore an initial alignment phase is required before navigation begins.
Initial misalignment is one of the major error sources of the INS. During the initial
alignment phase, the attitude dierence between the axes of the INS coordinate systems
and those of a chosen reference is estimated and removed. This attitude dierence, or
misalignment consists of two tilt angles and an azimuth angle which have distinct eects
on the propagation errors of the INS. The Kalman lter has been successfully used to
estimate the tilt and the azimuth errors of the INS. It is well known that the estimation
rate of the azimuth misalignment and the nal value it reaches are the two factors that
determine the performance of the whole alignment process [52]. The initial alignment
can be performed either when the INS is at rest on ground or while the vehicle is in
motion.
4.2 Overview
72
4.2.1 Self-alignment Review

One of the ground alignment methods is to obtain the INS initial attitude through
the use of external reference by optical techniques, magnetic heading sensors or other
external means. Most ground based applications consist of two phases: leveling and azimuth alignment [53]. Two self-alignment methods have been considered: gyrocompass
alignment and analytic alignment [27, 53, 54, 16, 55, 56, 57, 58, 4, 59] . The analytic
method is used for coarse alignment while the gyrocompass method is used for ne
alignment [58, 53].
The analytic coarse alignment method determines the transformation matrix which
relates vectors in the desired navigation frame to the same vectors expressed in the
geographic frame or in other equivalent frames using the knowledge of the gravity g
and the earth rate vectors wie .
Dene a vector v as
v = g wie
The alignment matrix Cgn is given by:
2
6
6
Cgn = 66
(gn )T
3;1 2
7
7
7
7
5
6
6
6
6
4
(4.1)
(gg )T
(wieg )T
(vg )T
3
7
7
7
7
5
(4.2)
(wien )T
(vn )T
where the gravity vector g and the earth rate wie in the desired navigation frame n and
the geographic frame g are gn , gg , wien and wieg .
Britting [27] analyzed the errors of the analytic coarse alignment scheme by taking
into account the eect of instrument uncertainties and that the base motions are not
readily amenable to analytic methods. This method requires the vectors g and wie to
be measured.
For a gimbled INS, physical gyrocompass alignment consists of nding the coordinate transformation between local geographic axes and navigation axes by physical
gyrocompassing. The transformation matrix Cgn is physically driven to be the unit
matrix.
4
4.2 Overview
73
For strapdown INS, the initial alignment matrix represents the transformation from
the body frame to the navigation frame. This analytic alignment method can be used
for high accuracy applications in only the most benign of environments [27], since the
performance deteriorates because of angular disturbance vibrations and accelerations.
The measurement of the gravity and the earth rate could also be corrupted. Some
ltering is introduced in order to reduce the eects of these vibrations. When a low-pass
lter is used to obtain the average values of the measured quantities, the instantaneous
position of the body frame can vary considerably from its average position. A signicant
misalignment could exist when the system is switched to the navigation operation mode.
A self-corrective alignment scheme is introduced to rene the initial estimate of the
transformation matrix by using the error angles between the known reference frame
and the corresponding computed frame.
Many gyrocompassing algorithms for both gimbled INSs and strapdown INSs have
appeared in literature. Jurenka and Leondes developed an optimum controller for driving the alignment in 1967 [58]. Huang and White presented self-alignment techniques
for an IMU considering the ne alignment of an INS platform whose base is subject
to vibration and whose sensors are subject to noise in 1975 [57]. The observability of
INS ground alignment was analyzed by Jiang and Lin in 1992 [53]. In the covariance
analysis of strapdown INS considering gyrocompass characteristics by Heung et al. in
1995 [56], it was found that cross-coupling terms in gyrocompass alignment errors can
signicantly in uence the strapdown INS error propagation. The initial heading error
has a close correlation with the east component of gyro bias error, while initial level tilt
errors are closely linked to the accelerometer bias error. Heung et al. also developed a
multiposition alignment method for INS stationary alignment on ground in 1993.
Bar-Itzhack summarized INS error models for the ground alignment in 1988 [4]. All
these models in the literature assume that the initial misalignments are small angles
with errors of only a few degrees. The measurement of the earth rate is indispensable
and an essential input for the ground alignment.
4.2 Overview
74
4.2.2 In-motion Alignment Review

If the initial alignment is carried out when the vehicle is in motion, the alignment is
called in-motion alignment. The initial condition can be obtained by transfer alignment.
Transfer alignment is the operation of aligning a slave INS with a master INS
comparing quantities computed by both INSs [60]. Direct transfer of the master INS
navigation states would not account for the fact that the slave INS is not pointed in
the same direction as the master INS. In the transfer alignment stage, the velocity
computed by the master INS is compared with the one computed by the slave INS and
the dierence, which is indicative of the slave misalignment with respect to the master
INS, is processed by a Kalman lter. This yields misalignment data as well as the
slave gyro and accelerometer error data. There are some other inertial measurement
matching methods such as velocity matching alignment and position update alignment
which achieve in-motion alignment by comparing estimates of velocity and position
generated by the aligning INS with estimates of the same quantities provided by the
master INS [1].
There are many issues concerning the transfer alignment algorithm design [52, 60,
61]. Bar-Itzhack investigated the azimuth observability enhancement that was accomplished by subjecting the INS to accelerations generated by manoeuvres of a combat
aircraft [52]. The eect of acceleration switching during INS in-motion alignment was
also analyzed [62].
To obtain high accuracy and a highly robust navigation system, inertial navigation
aided by other navigation references like star scanner, GPS, laser, radar, sighting devices
and Doppler have been widely used [63, 64, 65]. Complementary ltering is the one
mutual aiding method implemented in the frequency domain [65]. It is applied to the
fusion of a compass-aided heading gyro, Doppler-inertial ground speed estimation and a
baro-inertial altimeter. In the time domain, Kalman ltering with INS error models has
been the main tool for the implementation of aided INS in-motion alignment, navigation
and calibration stages. The assumption of small misalignment angles is implied in those
4.2 Overview
75
lterings and models.
4.2.3 Large Misalignment Problem Review

Ground alignment in the stationary stage using gyrocompassing, analytic alignment,
aiding attitude sensors and in-motion alignment using reference navigation information
have been the main methods for INS initial alignment. With the assumption of small
initial misalignments, Kalman lter mechanizations using the INS error propagation
models have been designed and implemented for many years.
Ground self-alignment is limited to high resolution inertial measurement units which
are able to measure the vectors of gravity and the earth rate. For low cost IMUs, with
resolution lower than the quantities of the gravity or the earth rate, self-alignment
principles will not work. External sensors have to be employed to obtain the initial
attitude [16]. If external sensors are not available, the initial misalignment will be
large. Several attempts have been made to solve this problem.
Jiang and Lin [66] suggested an error estimation algorithm in the true frame for
ground alignment for an arbitrary azimuth. The misalignment is estimated by a combination of leveling error rates. However the misalignment propagation was not modelled.
In 1992, Pham introduced a Kalman lter mechanization for strapdown INS airstart
with an unknown azimuth [28]. He assumed two small tilt misalignment errors and one
large heading error. Heading misalignment is considered as a wander angle in a
wander angle frame and separated into two states sin and cos in the lter. The
earth rate is still the required input in the lter process model. The misalignment was
not modelled.
In 1994 and 1996, Scherzinger developed inertial navigator error models for large
heading uncertainty [9, 10]. The expected application was for alignment with an unknown heading. The computer frame approach was used. Again, two states were used
to track one heading misalignment variable. An extended misalignment vector e was
4.3 Coarse Alignment
76
dened as:
e = [ x ; y ; sin z ; cos z ; 1]
(4.3)
where the psi angle [ x ; y ; z ] is the misalignment of the computer frame and the
platform frame. Scherzinger's model has a similar form of the psi angle model developed
in the literature by Benson [5] and Bar-Itzhack [24].
In 1997, Dmitriyev et al. published a nonlinear lter method application in INS
alignment which discussed the problem of coarse alignment [8]. Error equations with a
nonlinear characteristic have been obtained with a considerable level of a priori coarse
uncertainty. The error model is designed in the true frame with the assumption of two
small tilt misalignments and one large heading misalignment. The nonlinear characteristic of the problem was considered over a short period. As a result, the heading
misalignment rate was assumed to be zero. In the misalignment error models, the rates
of misalignment angles were coupled with the velocity errors and the velocity errors are
coupled with the misalignment angles.
In 1997, Rogers proposed another in-motion alignment method without the benet
of attitude initialization [67]. Similar to the methods of Pham and Scherzinger, Roger's
lter contained two sine and cosine states of the heading error. Small tilts and large
heading error were assumed.
The psi angle models and quaternion models for three large misalignment error
angles in the computer frame developed in the previous chapter of this thesis provide
a solution to the unknown initial attitude problem. The additional two tilt angles and
one heading angle could be all large using the proposed models.

An INS commonly uses the analytic alignment and the gyrocompass principle to perform self-alignment on ground. For a low cost IMU which cannot measure the earth
rate vector, external sensors must to be used.
77
In this thesis, a low grade inertial measurement system, made by Watson Industry
INC. is used. The resolution of gyros and accelerometers for the Watson IMU are shown
in the following table:
Watson IMU
IMU-1
IMU-2
gyros
1.077910;4 rad=sec 4.311510;4 rad= sec
accelerometers
0.0024 m=s2
0.0024 m=s2
m/s2
Figure 4.1 shows the raw data of the Watson IMU-1 in a stationary stage.
0.094
0.36
9.821
0.096
0.355
9.822
0.098
0.35
0.1
0.345
0.102
0.34
9.826
0.335
9.827
9.823
9.824
0.104
50
60
70
80
9.825
50
100
150
Resolution of accelerometer X Y Z
50
55
60
x 10
0.0105
0.0138
9.2
rad/sec
9.4
9.6
0.0139
0.011
0.014
0.0115
9.8
0.0141
10
10.2
0.012
0.0142
20
25
30
50
60
70
80
Resolution of gyro X,Y,Z
0.0125
100
200
Figure 4.1: Resolution of Watson IMU-1.

The quantity of the earth rate vector is 7:272210;5 rad=sec. Usually, the resolution
of accelerometers is high enough to measure the gravity vector. For this kind of IMU,
the coarse alignment can be partly performed using the acceleration data to detect the
tilt information. The coarse alignment algorithm is introduced here. The raw data
process which determines the average turn-on biases of the accelerometers and gyros is
included in the coarse alignment stage.
78
Figure 4.2: Bank and elevation of Watson IMU.
4.3.1 Raw Data Process

Once the IMU is turned on, there are random biases in the accelerometers and gyros.
They have a crucial eect on attitude, velocity and position. The integration of the
biases of the accelerometers cause the position errors to increase quadratically over
time. The attitudes, direction cosine matrix and quaternions will also have important
errors from integrating the gyro biases. Therefore, the gyro biases will accumulate the
position errors over time proportional to the cube of the time index.
The following table gives examples of the eects on velocity and position by the
Watson IMU.
axis (IMU body frame) gyro bias (rad=sec) accelerometer bias (m=sec2 )
x
0.004
0.008
y
0.008
0.2
z
0.003
0.03
Figure 4.3 shows the average biases of the three accelerometers and the three gyros
79
0.12
9.935
0.04
0.125
9.94
0.03
0.025
0.02
0
1000
0.13
0.135
0.14
0.145
0.15
2000
0
1000
3
0.002
raw gyro output:y
raw gyro output:x
0.004
0.006
0.008
0.01
0
1000
2000
x 10
9.95
9.955
9.96
9.965
2000
0
6,7,8
6
8
1000
1000
2000
1000
2000
10
0
9.945
raw gyro output:z
0.035
raw accelaration: z
0.045
raw accelaration: y
raw accelaration: x
of IMU-1 installed in the body frame over a period of 34 seconds.
2000
x 10
2
4
6
8
0
Figure 4.3: These six plots show the turn-on biases for the Waston IMU-1. The 2000
samples are taken over a period of 34 seconds. The units for the y axes in the top three
plots are m=s2 . The units for the y axes in the bottom three plots are rad=s.
Figure 4.4 shows the quaternion errors due to the gyro biases in a stationary position
over 28 seconds. The four curves of the four elements [Q0 ; Q1 ; Q2 ; Q3 ] show linear
divergence over time.
The rst order, the second order and the third order eects over time of the IMU
turn-on biases are shown in Figure 4.5. The acceleration \Vtx DOT" and \Vty DOT"
of axes x and y in the platform frame show a large linear divergence over time. The velocity \V tx" and \V ty" show a quadratic divergence over time squared. The curves of
the position coordinates \dx WGS84" and \dy WGS84" have a third order relationship
with time.
The gyro bias of 0.008 rad=second and the accelerometer bias of 0.2 m=second2 will
cause a velocity error of 20 m=s and a position error of 200m over 28 seconds.
These results demonstrate that before the raw data can be used, it is essential to
80
Quaternions
1
Q0
Q1
Q2
Q3
0.8
0.6
0.4
0.2
0.2
0
200
400
600
800
1000
1200
1400
1600
1800
Figure 4.4: Quaternion errors due to gyro biases. The 1650 samples are taken over a
period of 28 seconds. The unit for the quaternions is 1.
Vtx_DOT
Vty_DOT
0.08
0.2
0.06
0.15
0.04
0.1
0.02
0
0.05
0
50
100
150
V_tx
200
20
20
V_ty
40
60
20
0
10
20
0
0
500
1000
1500
2000
200
40
0
500
1000
1500
2000
1000
1500
dy_WGS84
2000
200
0
100
200
0
0
500
1000
1500
dx_WGS84
2000
400
0
500
Figure 4.5: Errors of acceleration, velocity and position due to turn-on biases of IMU.
The units for y axes in the top two plots, the middle two plots and the bottom two
plots are m=s2 ; m=s and m respectively. Along x axes are data samples over time
taken at 84Hz in 20 seconds.
81
remove the turn on biases. The method of bias determination is discussed here.
The frames of the Watson IMU are shown in Figure 4.2. The bank and elevation
information are dened as the angles with respect to the local level frame in Figure 4.2.
The average tilt angles from the tilt gyros are used to generate the coarse alignment
information.
Turn-on Biases of the Accelerometers

A time period during the stationary stage is selected. Dene the vector of the average
output of the three accelerometers in the body frame:
F^b = [f^xb; f^yb; f^zb ]T
(4.4)
This is the sum of the true acceleration vector Fb in the body frame and the bias
vector of the accelerometers rb (0) in the body frame:
F^b = Fb + rb(0)
(4.5)
The true acceleration vector Fb is transformed by the gravity vector in the platform
frame:
Fb = Cpb [0; 0; ;g]T = (Cbp);1 [0; 0; ;g]T
(4.6)
where the direction cosine matrix Cpb is given by [68]:

2
Cbp =
6
6
6
6
4
cos cos H ; sin sin EL sin H ; cos EL sin H

cos sin H + sin sin EL cos H cos EL cos H
; sin cos EL
sin EL
3
(4.7)
sin cos H + cos sin EL sin H 7

7
sin sin H ; cos sin EL cos H 77
5
cos cos EL
where BK , EL and H are the average bank, elevation and heading information during
the stationary stage and
BK )
= sin;1 ( sin
(4.8)
cos EL
82
Therefore the bias vector of the accelerometers rb (0) is subtracted from (4.5):
rb(0) = F^b ; Fb
= [f^xb ; f^yb ; f^zb ]T ; (Cbp );1 [0; 0; ;g]T

2
3 2
3
^
b
6 fx 7 6 ; sin cos EL 7
6
7 6
7
7g
= 66 f^yb 77 ; 66 sin EL
7
4
5 4
5
f^zb
cos BK cos EL
(4.9)
The heading does not appear here.
Turn-on Biases of the Gyros

Theoretically, the turn-on biases of the gyros are derived from the earth rate. During
the coarse alignment, the vehicle which is stationary, senses the earth rate only. In the
platform frame, the output of the gyros W^ p is the sum of the earth rate
ie and the
turn-on bias of the gyros rp:
W^ p =
ie + rp
(4.10)
For the convenience of removal of the gyro biases from the raw data in the body
frame, the bias vector is converted into the body frame:
rb = Cpb(0)rp = Cpb(0)(W^ p ;
ie)
(4.11)
The earth rate term is ignored for a low cost IMU, therefore:
rb = Cpb(0)W^ p
= (Cpb (0));1 (Cbp (0)W^ b )

2
^ xb
6 !
6
= W^ b = 66 !^ yb
4
!^ zb
3
7
7
7
7
5
(4.12)
To remove the turn on biases of the three gyros, the average gyro measurement

W^ b = !^ xb ; !^ by ; !^zb T is removed from the raw data.
83
quaternions after remove gyro turnon bias in static state
Q0
Q1
Q2
Q3
0.06
0.04
0.02
0.02
0.04
0.06
80
100
120
140
160
180
200
220
240
Figure 4.6: Quaternions after removal of turn on biases of gyros. The unit for the
quaternions is 1. Along x axis are data samples over time taken at 84Hz.
After the removal of the turn-on biases, the raw data from the IMU can be used
directly for navigation. Figure 4.6 is a plot of quaternions over 28 seconds. Compared
with the Figure 4.4 before the removal of turn on biases, the four curves of the four
elements [Q0 ,Q1 ,Q2 ,Q3 ] are almost at without divergence.
The errors in acceleration, velocity and position are reduced after this process.
Figure 4.7 is a plot of computed acceleration, velocity and position in the platform frame
while the Watson IMU was moving in an oscillation trajectory of 20cm around a xed
point for 320 seconds. As in Figure 4.7, the acceleration \Vtx DOT"and \Vty DOT"
of the axes x and y in the platform frame after 300 seconds are almost zero without
divergence. The velocity \V tx", \V ty" and position \dx WGS84", \dy WGS84" still
have some errors but are considerably smaller. The remaining biases are modelled and
will be removed by in-motion calibration algorithms.
4.3.2 Coarse Leveling - Initial Direction Cosine Matrix

The purpose of the leveling algorithm is to obtain the initial attitudes of the INS, that
is the initial direction cosine matrix or the initial quaternions. The turn-on biases of
84
Vtx_DOT
Vty_DOT
20
10
10
0
100
200
V_tx
300
400
10
0
100
200
V_ty
300
400
100
200
300
400
200
300
dy_WGS84
400
1
0
0
1
2
0
100
200
300
400
2
0
2
0
100
200
300
dx_WGS84
400
4
0
100
Figure 4.7: Acceleration, velocity and position after removal of turn-on biases. Along x
axes are data samples over time taken at 84Hz. The units for the y axes in the top two
plots, the middle two plots and the bottom two plots are m=s2 , m=s and m respectively.
the accelerometers and the gyros are also computed at this stage. The coarse leveling
alignment is conducted on-ground. The inertial cosine matrix for the psi angle approach
will be considered.
The direction of the axes x, y and z is dened as east-north-up of the local level
frame. The initial direction cosine matrix is constructed by the initial attitude and the
initial heading. In the coarse alignment mode, the vehicle remains stable for at least
15 seconds. Average bank and elevation are recorded as BK and EL. The heading is
unknown for a low cost INS and will be solved by the in-motion alignment algorithm.
Assuming an arbitrary heading input H , the initial direction cosine matrix Cbp between
the body frame and the platform frame is given by Equation (4.7).
The computation uses the average bank and elevation data during the stationary
stage. The vehicle position may be changed from its average position. Due to the noise
and the vibration of the vehicle, this estimation of leveling is only an approximation.
The ne alignment will be performed during the in-motion alignment stage when the
vehicle starts to move.
4.4 In-motion Alignment: Solution of Initial Attitude Uncertainty
85
Figure 4.8: Heading error denition.

4.4.1 Introduction
The coarse alignment takes about 15-20 seconds to obtain the initial values of the
navigation needs except the initial heading of the IMU. The purpose of the in-motion
alignment in this thesis is to solve heading uncertainty. The calibration of the direction
cosine matrix or the quaternions, the correction of the velocity and the position errors
and calibration of the accelerometers and the gyros are also done at this stage.
A low cost IMU cannot work alone due to the drift and nonlinearity of the inertial
sensors. External velocity or position information has to be employed to aid the INS.
GPS is an ideal source to aid an INS.
In this section, heading uncertainty is solved during in-motion alignment using
GPS information. The psi angle model for large angle errors developed in the previous
chapter is applied.
As illustrated in Figure 4.8, the true heading error is the misalignment between the
86
platform north and the true north of the true local level frame at the true position. The
position is aided by external position reference. The position error of the computed
position and the true position is small. Therefore the north of the computer frame
is very close to the true north of the true local level frame at the true position. The
misalignment z between the computer frame and the platform frame is approximately
the heading error. The correction of z consequently becomes the goal of the heading
uncertainty algorithm.
In-motion alignment commences after the coarse alignment. The initial heading
entry is arbitrarily set.
When the GPS x is not available, the INS performs vehicle navigation in the
platform frame at each IMU sampling time. The gyros of the IMU output the angular
rate of the vehicle in the body frame which can be used to calculate the direction cosine
matrix. The accelerometers output the acceleration of the vehicle in the body frame
which is transformed into the platform frame by the direction cosine matrix . The
INS navigation outputs the attitude, velocity and position in the platform frame by
integrating the angular rate, acceleration and velocity respectively.
When the GPS x becomes available, the velocity and position measurements of
the GPS x are processed. A nonlinear lter based on the computer approach (the psi
angle approach) estimates the psi angle errors, velocity errors and position propagation
errors of the INS in the computer frame and the drift errors of the accelerometers, the
gyros and the GPS errors. The nonlinearity of the lter is due to the nonlinearity of
the psi angle model for large heading errors between [;180 ; +180 ]. The estimates
of the errors are used to correct the velocity, position and attitude. The readings of
the accelerometers, gyros and the GPS measurement are also compensated by the lter
estimation of the sensor errors.
The algorithm runs until the errors are corrected. The three attitude errors are now
all satised to the small angle assumption. The psi angle model can be switched to
its small angle form. The algorithm is then switched from the alignment mode to the
navigation mode which still employs the same lter, only the psi angle model switches
87
to its small angle form.

The ow chart of the algorithm is shown in Figure 4.9.
4.4.2 Filter States

The lter in the heading correction stage is performing alignment and calibration. The
lter states include the propagation errors of the velocity, position and attitude errors
in the computer frame. Measurement errors are contained in the shaping lter which
is part of the alignment and calibration lter. X being the lter state vector, then:
X = [Vxc ; Vyc ; Vzc ; Rxc ; Ryc ; Rzc ;
x; y; z;
(4.13)
rbx;rby;rbz;bx; by ; bz ; Xsx; Xsy ; Xsz ; X2x ; X2y ; X2z ]T

where
V c = [Vxc ; Vyc ; Vzc ]T is the INS velocity error vector in the computer frame.
Rc = [Rxc ; Ryc ; Rzc ]T is the INS position error vector in the computer frame.
= [ x; y; z ]T is the psi angle vector, which is the angle between the computer frame
and the platform frame.
rb = [rbx;rby;rbz ]T is the vector of the accelerometer biases in the body frame.

b = [bx; by ; bz ]T is the vector of the gyro biases in the body frame.
Xs = [Xsx; Xsy ; Xsz ]T and Xss = [X2x ; X2y ; X2z ]T are the shaping states of the GPS
errors.
4.4.3 Filter Equations

The main lter equations are the error propagation models in the computer frame.
88
Figure 4.9: Flow chart of heading correction and lter estimation using the psi angle
approach.
89
Velocity Error Equation

The velocity error equation has been presented in the previous chapter. In a case where
the coarse alignment is not performed, there will be three large initial attitude errors.
The psi angle vector will have three large elements.
V_ c = (I ; Cpc)ftp + rc + gc ; (2
cie +
cec)V c
(4.14)
where Cpc is given by:

2
Cpc =
6
6
6
6
4
cos x cos z ; sin x sin y sin z ; cos y sin z

cos x sin z + sin x sin y cos z cos y cos z
; sin x cos y
sin y
sin x cos z + cos x sin y sin z

sin x sin z ; cos x sin y cos z
cos x cos y
3
7
7
7
7
5
(4.15)
In most cases, when the coarse alignment has been done with an unknown azimuth,
there will be two small initial tilt errors and one large heading error. The psi angle is
still large.
For most low cost INSs, the area of operation is close to the earth surface. The
gravity has very small variation. In this case, the gravity error model can be ignored
90
and the velocity error equation can be simplied to:

2
6
6
6
6
4
V_ c =
;[Vxctg(')=R0 + 2!ie sin(')]
Vxctg(')=R0 + 2!ie sin(') 0

;[Vxc=R0 + 2!ie cos(')] ;Vyc=R0
Vxc=R0 + 2!ie cos(')

Vyc=R0
0
7
7
7
7
5
6
6
6
6
4
Vxc
Vyc
Vzc
3
7
7
7
7
5
(4.16)
1 ; cos( z ) sin( z )
; x sin( z ) ; y cos( z )
+ ; sin( z ) 1 ; cos( z ) + x cos( z ) ; y sin( z )
; x
0
y
6
6
6
6
4
32
76
76
76
76
54
2
x sin( z ) + y cos( z ) 7 6
6 cos( z ) ; sin( z )
6
7
6
+ 66 sin( z ) cos( z ) ; x cos( z ) + y sin( z ) 77 Cbp 66
4
5
4
; y
0
x
fxp
fyp
fzp
rbx
rby
rbz
3
7
7
7
7
5
3
7
7
7
7
5
where
V c = [Vxc ; Vxc; Vxc]T is the velocity vector in the computer frame.

R0 is the earth radius. For a low cost INS, travel distance is limited and the earth
radius is considered to be a constant.
!ie is the earth rate.

' is the local latitude solved in navigation.
g is the gravity constant.
[fxp; fyp; fzp ]T is the specic force transformed to the platform frame.
Cbp is the direction cosine matrix with which INS actually transforms the vectors from
the body frame to the platform using measured INS angular rate in navigation
mode.
91
Position Error Equation

The position error model does not distinguish between small and large attitude errors:
R_ c = V c ;
cecRc
(4.17)
The extension of the position error equation is:

2
R_ c =
6
6
6
6
4
Vxc
Vyc
Vzc
7
7
7
7
5
6
6
6
6
4
Vxctg(')=R0
;Vxc=R0
;Vxctg(')=R0
0
;Vyc=R0
Vxc=R0
Vyc=R0
0
32
76
76
76
76
54
Rxc
Ryc
Rzc
3
7
7
7
7
5
The notation is as before.
Psi Angle Model

The attitude and heading errors are solved in the form of the psi angles. The psi angle
model developed in Chapter 2 is described here in detail.
The general angle model is given by equation (4.18):
_ = (I ; Ccp)!icc ; p
(4.18)
where Ccp is given by:
Ccp =
6
6
6
6
4
cos x cos z ; sin x sin y sin z

; cos y sin z
sin x cos z + cos x sin y sin z
cos x sin z + sin x sin y cos z ; sin x cos y

cos y cos z
sin y
sin x sin z ; cos x sin y cos z cos x cos y
3
7
7
7
7
5
(4.19)
When the three initial attitudes are all unknown, or in the large perturbation case
when the errors of the three attitudes are all large during the navigation stage, the
angle model with large angle assumption has to be used.
In most cases, the initial tilt angles can be solved within an error of 1 in the coarse
alignment stage, while the heading remains unknown. In this case, x and y are small
92
and z is large within [;180 ; +180 ]. The psi angle equation in this case is extended
to:
2
_=
6
6
6
6
4
2
1 ; cos( z )
; sin( z )
y
sin( z )
1 ; cos( z )
; x
; x sin( z ) ; y cos( z ) + x cos( z ) ; y sin( z ) 0
c
6 ;Vy =R0
6
6
6
4
Vxc =R0 + !ie cos(')

Vxc tg(')=R0 + !ie sin(')
7
7
7
7
5
6
; Cbp 66
6
4
bx
by
bz
3
7
7
7
7
5
3
7
7
7
7
5
(4.20)
Sensor Error Equations

In both the alignment and the navigation stages, the errors of the IMU sensors and
GPS can be estimated and calibrated in-motion. The error models are included in the
process model of the lter.
For a low cost IMU, the drift over a long period of time can be signicant. Internal
temperature variation could also cause bias changes.
The biases in the gyros and the accelerometers can be modelled as [45, 69]:
a1
r_ b = ;r
+
Ta Ta
b
_b = ; + b1
b
Tg
Tg
(4.21)
(4.22)
The model parameters Ta ; Tg ; a1 and b1 are obtained experimentally.

In a case where the IMU has slow internal temperature variation, the gyro and
accelerometer errors can be modelled as white noise plus a constant:
r_ b = 0
(4.23)
_b = 0
The white noise is to be added on top of these.
The error model for the GPS has been presented in the previous chapter. The
93
shaping states Xs and Xss of the GPS position error have the following relations:
X_ s = ;2kXs + Xss + r!sp
(4.24)
X_ ss = ;2 Xs + r!sp
That is:
2
6
4
X_ s
X_ ss
7
5
= 64
32
54
7
5
+ 64
;2k 1 7 6 Xs
2
Xss
r 7
5 [!sp ]
r
(4.25)
with the white noise !sp.
Measurement Equations
The lter observation is the dierence between the GPS measurement and the INS
output. It must be noted that these two dierent outputs are in two dierent frames.
When the dierential GPS with the position error of 2cm and the velocity error of
5cm=second is used as the navigation reference, the velocity and the position of the
DGPS are considered to be solved and transformed into the true local level frame.
Let the velocity dierence Vd and the position dierence Rd be the dierence between the INS outputs and measured GPS outputs:
c ; Vb t
Vd = VbINS
GPS
(4.26)
c ;R
bt
Rd = RbINS
GPS
(4.27)
where the superscripts c and t indicate whether the vectors are in the computer frame
c
c are the INS computed position and velocity.
or in the true frame. RbINS
and VbINS
t
t are the GPS position and velocity outputs respectively.
RbGPS
and VbGPS
When the GPS error model is proposed, the observation has the following form:
c ;R
bt
Z = Rd = RbINS
GPS
c + Rc) ; (Rt + X + ; )
= (Rtrue
s
gps
true
c ; Rt ) + (Rc ; Xs ; ;gps)
= (Rtrue
true
c + (Rc ; X ; ; )
= (I33 ; Cct )Rtrue
s
gps
(4.28)
94
t and Rc are
where ;gps = [;x ; ;y ; ;z ]T is the white noise of GPS measurement. Rtrue
true
the true position in the computer frame and the true frame respectively.
Since the position is aided, the INS computed position and the true position will
have small errors. The computer frame c; which is the local level frame at the computed
position, and the true local level frame t at the true position will have a small dierence
95
in orientation. Therefore Cct I33 in the observation equation. Consequently:
Rd = Rc ; Xs ; ;gps
2
3 2
3 2
c
6 Rx 7 6 Xsx 7 6 ;x
6
7 6
7 6
= 66 Ryc 77 ; 66 Xsy 77 ; 66 ;y
4
5 4
5 4
Rzc
Xsz
;z
3
7
7
7
7
5
= 033 I33 033 033 033
(4.29)
2
;I33 033
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
Vxc
Vyc
Vzc
Rxc
Ryc
Rzc
x
y
z
rbx
rby
rbz
bx
by
bz
Xsx
Xsy
Xsz
X2x
X2y
X2z
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
6
6
6
6
4
7
7
7
7
5
;x
; ;y
;z
(4.30)
When the DGPS is used, the observation can be considered as corrupted by white
96
noise only. The observation z is:

2
Z = 64
Vd
Rd
3
7
5
c ; Vb t
Vd = VbINS
GPS
c ;R
bt
Rd = RbINS
GPS
(4.31)
And
c ; Vb t
Vd = VbINS
GPS
c + V c ) ; (V t + gps)
= (Vtrue
true
c ; V t ) + (V c ; )
= (Vtrue
gps
true
c + V c ;
= (I33 ; Cct )Vtrue
gps
(4.32)
where
gps = [x ; y ; z ]T is the white noise of the DGPS velocity measurement.
c is the true velocity in the computer frame.
Vtrue
t is the true velocity in the true frame.
Vtrue
Again, Cct I33 in this case. Therefore
Vd = V c ; gps
(4.33)
And
c ; Rbt
Rd = RbINS
GPS
c + Rc) ; (Rt + ; )
= (Rtrue
gps
true
c + Rc ; ;gps
= (I33 ; Cct )Rtrue
= Rc ; ;gps
(4.34)

The observation equation will be:
2
Z = 64
V c ; gps
Vd 7 6
5=4
Rd
Rc ; ;gps
97
3
7
5
I
0
0
0
0
= 64 33 33 33 33 33
033 I33 033 033 033
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
36
6
6
76
56
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
Vxc
Vyc
Vzc
Rxc
Ryc
Rzc
x
y
z
rbx
rby
rbz
bx
by
bz
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
; 64
gps
;gps
3
7
5
(4.35)
4.4.4 Discrete Filter and Jacobian Matrix

The process model of the lter in the in-motion alignment stage is nonlinear. The
Kalman ltering process has been designed to estimate the state vector in a linear
model. It has many successful applications in INS alignment and navigation. If the
model turns out to be nonlinear, a linearization procedure is usually performed in
deriving the ltering equations. The linear Taylor approximation of the process model
at the previous state estimation and that of the observation model at the corresponding
predicted position is usually considered [18, 70]. The Kalman lter so obtained is called
the Extended Kalman Filter (EKF). Linearization is justied by the argument that
the estimate maintained by the EKF is close to the true state of the system. The
98
expected values of the second and higher-order terms in the Taylor series expansion
are small [19]. Jacobian matrices of the process and observation models have to be
evaluated. The Distribution Approximation Filter which is introduced by Julier and
Uhlman [19, 20, 21, 22, 23] provides another approach for nonlinear ltering.
The Extended Kalman lter is employed in this section for the psi angle approach,
where for the navigation update, direction cosine matrices are used for attitude update
purposes.
The update stage is performed at each data fusion step when a GPS x is available.
Usually, the INS sampling time is less than the sampling time of the GPS. For example,
the sampling frequency of the Watson IMU and the Ashtech GPS are 82Hz and 10Hz
respectively. During the period when a GPS x is not available, INS computes and
outputs the navigation data independently without the lter prediction.
The lter in continuous time is the combination of the equation in Section 4.4.3.
The whole lter can be written as:
X (t) = fc (t; X (t)) + G(t)U (t)
(4.36)
Z (t) = HX (t) + V (t)

The strengths of U (t) and V (t) are Q(t) and R(t) respectively.
The discrete-time model is:
X (tk ) = X (tk;1 ) + fc (tk ; X (tk ))dt + G(tk )U (tk )dt

Z (tk ) = H (tk )X (tk ) + V (tk )
(4.37)
where dt is the time interval between tk;1 and tk :

This compares with the standard form of the discrete-time lter:
Xk+1 = fk (Xk ) + Gk uk
Zk = Hk Xk + vk
(4.38)
99
with:
Gk = G(tk )dt
(4.39)
uk = U (tk )
(4.40)
vk = V (tk )
(4.41)
fk (Xk ) = X (tk;1 ) + fc (tk ; X (tk ))dt
(4.42)
The approximation is also given:

This relationship provides a shortcut to evaluate the Jacobian matrix Jf at time
tk in discrete time by using the Jacobian matrix Jc (t) at time tk in continuous time,
which can be directly derived from the lter models:
J (k) = @fk (Xk )
f
@Xk
k ; X (tk )) dt
I + @fc(t@X
k
= I + Jc (tk )dt
(4.43)
The derivation of the Jacobian matrix Jc of the lter at time tk is more complicated.
For example, when the noise of the accelerometers and the gyros is treated as white
noise plus a constant and the DGPS is used, the Jacobian matrix in continuous time is
derived as follows.
The lter can be separated into a linear part and a nonlinear part plus the noise:
3
2
2
32
3
c +
c ) 033 033 033 033
c
_c

V
;
(2

V
ec
ie
7
6
6
76
7
6
7
6
76
7
c 7 6
c
c
_
6 R
7
6
I33
;
ec 033 033 033 7 6 R 77
7
6
6
6
7
6
76
7
6 _
7=6
76
7+
0
0
0
0
0
33
33 33 33 33 7 6
6
7
6
7
6
6
6
4
rb
b
7
7
7
5
6
6
6
4
6
6
6
6
6
6
6
4
(I ; Cpc)ftp
031
(I ; Ccp)!icc
061
0615
7
7
7
7
7
7
7
5
6
6
6
6
6
6
6
4
rp
031
;p
061
76
76
76
54
3
7
7
7
7
7
7
7
5
rb
b
7
7
7
5
(4.44)
100
Then the Jacobian matrix is evaluated as:

2
Jc =
6
6
6
6
6
6
6
6
6
6
4
;(2
cie +
cec)
I33
033
033 F13 I33 033

;
cec 033 033 033
033 F33 033 I33
0615
3
7
7
7
7
7
7
7
7
7
7
5
(4.45)
When only the heading is unknown, the matrix elements F13 and F33 in Jc are:
2
; sin ( z ) fzp ; cos ( z ) fzp

F13 = cos ( z ) fzp ; sin ( z ) fzp
;fyp
fzp
6
6
6
6
4
fxp sin ( z ) + fyp cos ( z ) + ( y sin ( z ) ; x cos( z ))fzp

sin ( z ) fyp ; cos ( z ) fxp ; ( x sin ( z ) + y cos( z ))fzp
0
And
F33 =
6
6
6
6
4
;w3
3
7
7
7
7
5
(4.46)
w3
0
; sin ( z ) w1 + cos ( z ) w2 ; cos( z )w1 ; sin( z )w2
sin ( z ) w1 ; cos ( z ) w2
cos( z )w1 + sin( z )w2
( y sin( z ) ; x cos( z ))w1 ; ( y cos( z ) + x sin( z ))w2
3
7
7
7
7
5
(4.47)
where
w1 = ;Vyc=R0
w1 = Vxc =R0 + wie cos(')]
w3 = Vxc tan(')=R0 + wie cos(')
(4.48)
' is the local latitude. All the variables in the Jacobian matrices above are evaluated
at time tk .
101
The lter noise has the following attributes:

When the IMU noise is considered as white noise plus a constant and the noise is
estimated in the lter, G at time tk is evaluated as:
2
G(tk ) =
6
6
6
6
6
6
6
6
6
6
4
Cbp(tk )
033
033
066
Then in the continuous-time lter:

2
G(tk )U (tk ) =
6
6
6
6
6
6
6
4
rp
031
;p
061
3
7
7
7
7
7
7
7
5
033
033
;Cbp(tk )
6
6
6
6
6
6
6
6
6
6
4
Cbp(tk )
033
033
3
7
7
7
7
7
7
7
7
7
7
5
033
033
;Cbp(tk )
066
(4.49)
3
7
72
7
7
76
74
7
7
7
7
5
rb
b
3
7
5
(4.50)
where Cbp(tk ) is the direction cosine matrix from the body frame to the platform which
is solved in navigation at time tk .
In the discrete-time lter (4.38), uk and vk are white noise sequences with strength
Gk and Rk . They can be approximated as [71]:
Qk = G(tk )Q(tk )GT (tk )dt

Rk = R(tk )=dt
4.4.5 Filter Implementation Using the EKF

The lter real time implementation can be seen in Figure 4.9. The lter state X is
initially modelled as a Gaussian random variable X0;0 at the epoch when the coarse
alignment nished and the in-motion alignment starts. Take the zero-mean of the
Gaussian variable for initialization. The initial X is given as X0;0 :
X0;0 = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]T
(4.51)
102
The initial covariance matrix P0;0 provides a statistical measure of condence of the
states:
2
2 033 033 033

6 V
6
2
6 0
6 33 R 033 033
6
6
2 033
6 033 033
6
P0;0 = 666 033 033 033 r2 b
6
6
6
6
6
6
4
033
033
033
033
033
033
033
033
033
033
033
033
033
033
033
033 033 033 033 2b

033 033 033 033 033 X2 s
033 033 033 033 033 033 X2 ss
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(4.52)
P0;0 is assumed diagonal for lack of sucient statistical information to evaluate its
o-diagonal terms.
The initial dynamic process noise matrix is set as Q0 :
2
Q0 =
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
2
accx
0
0
0
0
0
2
accy
0
0
0
0
0
0
2
accz
0
0
0
0
0
0
2
gyrox
0
0
0
0
0
0
2
gyroy
The initial measurement noise matrix is set as R0 :

2
R0 =
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
2
rrx
0
0
0
0
0
0 0 0 0
rry2 0 0 0 0
2 0 0 0
0 rrz
2 0 0
0 0 rvx
2 0
0 0 0 rvy
2
0 0 0 0 rvz
0
0
0
0
0
2
gyroz
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(4.53)
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(4.54)
The diagonal terms of R0 are the variance of the noise on INS and GPS measurements.
103
At time tk ; the state estimation is Xk;k;1 , with the covariance matrix Pk;k;1 being
available. The Jacobian matrix of fk is then evaluated as:
Jf (k) = I + Jc(tk )dt
(4.55)
The lter gain Kk is calculated by:
Kk = Pk;k;1 HkT (Hk Pk;k;1HkT + Rk );1
(4.56)
The updated measurement is fused into the lter. The updated state estimation
Xk;k is:
Xk;k = Xk;k;1 + Kk (Zk ; Hk Xk;k;1 )
(4.57)
Since the errors have been compensated at each lter time, Xk;k;1 is zero. Therefore, the updated estimation of states which is used to correct navigation output and
calibration sensor errors is:
Xk;k = Kk Zk
(4.58)
Pk;k = (I ; Kk Hk )Pk;k;1
(4.59)
The covariance update is:
For the next iteration tk+1, the a priori state estimation will be:
Xk+1;k = fk (Xk;k )
(4.60)
with an a priori covariance matrix:
Pk+1;k = Jf (k)Pk;k JfT (k) + Gk+1Qk GTk+1
(4.61)
The estimation of the states is used to correct the system errors by subtracting each
estimated error from the INS velocity, position, attitude, accelerometer output data,
gyro output data and GPS position data, as shown in the ow chart in Figure 4.9.
4.5 Navigation Stage: Continue Alignment and Calibration
104
4.5 Navigation Stage: Continue Alignment and Calibration

Once the attitude errors diminish to a few degrees, the in-motion alignment stage
nishes. The velocity error model, position error model, psi angle model will be switched
to their small angle approximations.
The process models are replaced to their small angle forms, the lter process model
and the observation models are now all linear. A standard Kalman lter is applied to
estimate the navigation state.
4.6 Summary
In this chapter, the INS algorithm for low cost IMUs is developed using psi angle
models for large attitude errors. The main contribution of this algorithm is the inmotion alignment to solve the initial attitude uncertainty.
The alignment mode is separated into coarse alignment and in-motion ne alignment. Section 4.3 presented the determination of the initial direction cosine matrix,
turn-on biases of the accelerometers and the gyros in the coarse alignment stage.
Section 4.4 presented the in-motion alignment approach with an unknown initial
condition. The psi angle models developed in this thesis are used as the lter process
models. The Extended Kalman Filter is used to implement the nonlinear lter.
Chapter 5
INS Algorithm for Low Cost

IMU in Quaternion Approach
5.1 Introduction
This chapter addresses the INS algorithm design for low cost IMUs using quaternions
in the computer frame.
The INS navigation equations are based on a quaternion approach. The quaternion equations are formulated and an update based on the Adams-Bashford method is
presented.
The lter is composed of the INS velocity error model, the INS position error model
and the quaternion error model in the computer frame. The quaternion errors are exploited using the misalignment of the computer frame and the platform frame. The
entire lter process model structure and the process noise are discussed. The process
noise vectors are reconstructed by a linear combination of the white noise on the accelerometers and gyros in the body frame. A Distribution Approximation Filter (DAF)
is used instead of the EKF. The principle and the benet of the DAF are brie y described.
105
5.2 INS Navigation Using Quaternions
106

5.2.1 Overview
An INS computes the current velocity, position and attitude from the initial velocity,
position and attitude and the time history of the kinematic acceleration. The gyros of
the IMU provide angular rates of the body frame to the inertial frame. An orientation transformation matrix is calculated using outputs of gyros to transfer the specic
force measured by the accelerometers to the navigation frames. Velocity and position
are consequently integrated by acceleration and velocity respectively in the navigation
frame.
In the previous chapter, the vehicle attitudes, the misalignment of coordinate systems and the orientation of the coordinate frames were represented by a set of angles
and the direction cosine matrix. In this chapter, they are represented by quaternions.
Quaternions were introduced by Hamilton in 1843 and are a set of four parameters
that evolve in accordance with a simple dierential equation. Quaternions are less
amenable to direct physical interpretation. Although their elements have quadratic
forms, the quaternion approach is less computationally intensive, gives better accuracy
and avoids the singularity problem inherent in the Euler approach. It contains only four
parameters and uses the half angular increments [72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 14].
The algorithm ow chart is shown in Figure 5.1.
The INS algorithm processes the raw data to estimate the turn on biases of accelerometers and gyros. Coarse alignment is performed by setting up initial velocity,
position, attitude and quaternions. Initial quaternions are evaluated by the initial
attitude angles.
When external heading sensors are not available, ground coarse alignment cannot
be performed accurately. The psi angle approach described in the previous chapter
can be used to correct the attitude without initial attitude input. In this chapter,
another approach using quaternions instead of psi angles is developed. The velocity
error propagation model, position error model and quaternions propagation which are
Figure 5.1: INS ow chart
107
108
developed in this thesis are applied here.
5.2.2 Raw Data Process

The IMU provides raw data of acceleration and rotation rate in the body frame. Raw
data contain turn-on biases which can be estimated while stationary and removed at
each navigation step as described in the previous chapter.
An example of the raw data in axis x of the body frame is shown in Figure 5.2.
From time 15 seconds to 50 seconds, the vehicle is stationary. The average raw data
raw data of accelerometer x
1.5
1
m/sec2
0.5
0
0.5
1
1.5
2
20
40
60
80
100
120
80
100
120
raw data of gyro x

0.15
0.1
rad/sec
0.05
0
0.05
0.1
0.15
0.2
20
40
60
Time : second
Figure 5.2: Raw data of accelerometer x and gyro x.

of accelerometers and gyros are collected during this stage. Turn-on biases of three
accelerometers and three gyros are estimated and removed from the raw data. As in
the raw data plot, there is perturbation noise from time 38 seconds to 39 seconds which
cannot be estimated in real time. Therefore the calculated biases at this stage are not
accurate. The biases will also drift over time with temperature changes. They will be
estimated in the lter and removed at each update step.
109
5.2.3 Quaternions Initialization

To transfer the measured acceleration in the body frame to the platform frame, a
rotation transfer matrix has to be calculated to get the orientation of the vehicle at
each navigation step.
Quaternions are initialized while the vehicle is stationary. The initial rotation transfer matrix is calculated by averaging bank, elevation and heading. Let the transformation matrix T0 be the initial direction cosine matrix from the body frame to the
platform frame. In the case where the initial attitudes are unknown, T0 can be set
arbitrarily. If coarse tilt information can be obtained and the heading is unknown, T0
can be set by coarse tilt and arbitrary heading.
The quaternion integrations are updated from the initial quaternion vector Qpb(1) =
[qn0 (1); qn1 (1); qn2 (1); qn3 (1)]T .
2
2(q 2n0 (1) + q2n1 (1)) ; 1

2(q n1 (1)q n2 (1) ; qn0 (1)q n3 (1))
T0 = 2(q n1(1)q n2 (1) + qn0 (1)q n3(1))
2(q 2n0 (1) + q2n2 (1)) ; 1
2(q n1 (1)q n3 (1) ; qn0 (1)q n2 (1)) 2(q n2 (1)q n3 (1) + qn0 (1)q n1 (1))
3
2(q n1 (1)q n3 (1) + qn0 (1)q n2 (1)) 7
7
2(q n2 (1)q n3 (1) ; qn0 (1)q n1 (1)) 77
5
2(qn2 0 (1) + q2n3 (1)) ; 1
6
6
6
6
4
(5.1)
with the constraint
qn2 0(1) + qn2 1(1) + qn2 2(1) + qn2 3(1) = 1

Then the solutions of (5.1) are:
qn1 (1) = 21 1 + T0 (1; 1) ; T0 (2; 2) ; T0 (3; 3) sign(T0 (3; 2) ; T0 (2; 3))

p
qn2 (1) = 12 1 ; T0 (1; 1) + T0 (2; 2) ; T0 (3; 3) sign(T0 (1; 3) ; T0 (3; 1))

p
qn3 (1) = 12 1 ; T0 (1; 1) ; T0 (2; 2) + T0 (3; 3) sign(T0 (2; 1) ; T0 (1; 2))

p
qn0 (1) = 1 ; qn2 1(1) ; qn2 2 (1) ; qn2 3(1)
where sign(x) is +1 when x is positive, returns ;1 when x is negative.
(5.2)
110
5.2.4 Transformation Matrix Using Quaternions

At INS navigation time t, let C (Qpb ) be the rotation transformation matrix from the
body frame to the platform frame represented by Qpb (t) = [qn0 (t); qn1 (t); qn2 (t); qn3 (t)]T .
Then:
2
2(q2n0 (t) + q2n1 (t)) ; 1

2(q n1 (t)qn2 (t) ; qn0 (t)q n3 (t))
p
C (Qb ) = 2(q n1(t)qn2 (t) + qn0(t)qn3 (t))
2(q 2n0 (t) + q2n2 (t)) ; 1
2(q n1 (t)qn3 (t) ; qn0 (t)q n2 (t)) 2(q n2 (t)qn3 (t) + qn0 (t)q n1 (t))
3
2(q n1 (t)q n3 (t) + qn0 (t)qn2 (t)) 7
7
2(q n2 (t)q n3 (t) ; qn0 (t)qn1 (t)) 77
5
2(qn2 0 (t) + q2n3 (t)) ; 1
6
6
6
6
4
(5.3)
The evaluation of this matrix is less computationally expensive than using the direction cosine matrix.
5.2.5 Quaternion, Velocity and Position Update

In each step, the current quaternions are integrated from the information of the current
gyro outputs and the quaternions at the previous step. To update the quaternion vector
Qpb from time t to time t + 1, the Adams-Bashford integration is used.
At time t, Qpb satises the following dierential equation [32]:
Q_ pb = 21
nb Qpb
(5.4)
where the angular rate matrix

nb is:
2
nb =
6
6
6
6
6
6
6
4
0 ;wx ;wy ;wz

wx 0 wz ;wy
wy ;wz 0 wx
wz wy ;wx 0
3
7
7
7
7
7
7
7
5
(5.5)
and
b = [w ; w ; w ]T
wnb
x y z
(5.6)
111
is the body frame rate with respect to the platform frame solved in the body frame.
This is derived by dierencing the measured body frame rates wibb and the estimates
of the components of platform frame rate wipp . wipp is obtained by summing the Earth
rate with respect to the inertial frame wiep and the turn rate of the platform frame to
p . These rates have the following relation [27]:
the Earth frame wep
p
winp = wiep + wep
(5.7)
b = wb ; C b [wp + wp ]
wnb
p ie
ep
ib
(5.8)
and
Let t be the INS update interval and Qpb(t ; 2t) and Qpb (t ; t) be the quaternions
at time (t ; 2t) and (t ; t). The angular rate matrix of the previous two steps (t ; 2t)
and (t ; t) are
(t ; 2t) and
(t ; t). To integrate the quaternion dierential equation
(5.4), a 2-step Adams-Bashford method [82] is used. The quaternion at the current step
is given by [68]:
Qpb (t) = Qpb (t ; t) + (3

(t ; t)Qpb (t ; t) ;
(t ; 2t)Qpb (t ; 2t)) 0:25t (5.9)
Usually the INS sampling time is a constant. If there is a variation in t, Qpb and
are selected using the values of the last two steps. For the Watson IMU, the sampling
time is 0.008 seconds.
In the algorithm presented, the quaternion vector is normalized at each update
stage. Figure 5.3 is a plot of the actual norms of quaternions minus 1. The range is
within 5 10;16 . Therefore the quaternions are normalized at each INS navigation
time.

16
112
Quaternion orthogonality
x 10
50
60
70
80
90
100
110
120
Figure 5.3: Quaternions' orthogonality

5.3.1 Introduction
It is argued that attitude transformation and update using quaternions can provide
more accurate results than the direction cosine matrix and the Euler method.
When a low cost IMU is used, initial attitude and initial quaternions are unknown.
Quaternion models for large attitude errors developed in this thesis can be used to solve
the initial quaternion uncertainty without additional heading sensors.
In this section, the quaternion approach in the computer frame is applied to the INS
in-motion alignment to solve large azimuth uncertainty. A Distribution Approximation
Filter (DAF) instead of EKF is used to implement the nonlinear lter. One of the
advantages of this approach is that it does not require the evaluation of Jacobians.
The algorithm ow chart is shown in Figure 5.4.
During the quaternion initialization, the four elements of the quaternion vector
are evaluated under the assumption of an arbitrary attitude. The INS computes the
Figure 5.4: Flow chart of the quaternion approach.
113
114
navigation data at each INS sampling time starting with the rst quaternion vector.
When a GPS x is available, a lter based on the velocity, position and quaternion
error models updates the states. The state vector consists of the INS velocity error,
the position errors, the quaternion errors in the computer frame and the IMU and GPS
error states. These estimates are used to correct the INS navigation and calibrate IMU
biases.
The computer frame is selected as the local level frame in east, north and up at the
INS computed position.
5.3.2 Filter Models

The lter propagates the estimated errors of the INS. The lter states in this algorithm
are: the INS velocity and position errors in the computer frame and the quaternion
errors. To simplify the presentation of the algorithm, the accelerometer biases and
the gyro biases in the body frame are considered as zero-mean white noises. The
experimental data in this thesis was obtained using a DGPS system. The shaping lter
was not included in this case because the magnitude of the noise due to SA with this
type of dierential GPS implementation was neglibe.
The state vector is:
X = [Vxc ; Vyc; Vzc; Rxc ; Ryc ; Rzc ; q0 ; q1 ; q2 ; q3 ]T
(5.10)
where
V c = [Vxc ; Vyc ; Vzc ]T is the INS velocity error vector in the computer frame.
Rc = [Rxc ; Ryc ; Rzc ]T is the INS position error vector in the computer frame.
Qcp = [q0 ; q1 ; q2 ; q3]T is the quaternion vector which represents the rotation between
the platform frame and the computer frame.
115
The system model is:
X_ (t) = fc(X; t) + Gcu(t)
(5.11)
Z (t) = HX (t) + w(t)
(5.12)
where Equation (5.11) describes the continuous-time propagation equation. The lter
state equations are nonlinear and u(t) is white noise. The measurement equations
are linear in this case and w(t) is white noise. The measurement vector contains the
dierence between the INS velocity and position information and the GPS velocity and
position information.
Velocity Error Equation

Let V c = Vbtc ; Vtc be the INS velocity error in the computer frame. The general
velocity error model for large attitude errors in quaternion form is:
V_ c = (I33 ; C (Qcp))ftp ; (2
cie +
cec)V c + gc + rp
(5.13)
Most low cost INSs are used in level navigation applications and consequently the
gravity error gc can be ignored. The matrix C (Qcp ) transforms the true specic force
ftp = [fxp; fyp; fzp]T in the platform frame to the computer frame. It is given by:
2
6
6
C (Qcp ) = 66
4
q02 + q12 ; q22 ; q32 2(q1 q2 ; q0 q3)

2(q1 q3 + q0 q2 )
2(q1 q2 + q0 q3 ) q02 ; q12 + q22 ; q32 2(q2 q3 ; q0 q1 )
2(q1 q3 ; q0 q2 )
2(q2 q3 + q0 q1 ) q02 ; q12 ; q22 + q32
3
7
7
7
7
5
(5.14)
The earth rate matrix

cie and the matrix
cec of the rate of the computer frame in
respect to the earth frame are:
2
2
cie +
cec =
6
6
6
6
4
0
(5.15)
Vxc tg(')=Rx + 2!ie sin(')

;[Vxc=Rx + 2!ie cos(')]
;[Vxctg(')=Rx + 2!ie sin(')]

0
;Vyc=Ry
Vxc=Rx + 2!ie cos(')

Vyc=Ry
0
3
7
7
7
7
5
116
where ' is the local latitude. The earth rate !ie = 7:272205 10;5 rad=second. The
earth radius elements Rx in the east direction and Ry in the north direction can be
considered as equal to R = 6; 378; 393m. The INS velocity components Vxc and Vyc
are computed in the computer frame in east and north. Therefore the velocity error
equation must include the following transformation:
2
6
6
V_ c = 66
4
V_ xc
V_ yc
V_ zc
3
7
7
7
7
5
(5.16)
32
q22 + q32
;(q1q2 ; q0q3) ;(q1q3 + q0q2) 7 6 fxp 7
6
6
76
7
= 2 66 ;(q1 q2 + q0 q3 )
q12 + q32
;(q2q3 ; q0q1) 77 66 fyp 77 ;
4
54
5
;(q1q3 ; q0q2) ;(q2q3 + q0q1)
q12 + q22
fzp
2
;[Vxctg(')=Rx + 2!ie sin(')]
6 0
6
; 66 Vxctg(')=Rx + 2!ie sin(') 0
4
;[Vxc=Rx + 2!ie cos(')] ;Vyc=Ry
Vxc=Rx + 2!ie cos(')
Vyc=Ry
0
32
76
76
76
76
54
Vxc 7 6 rpx
7 6
Vyc 77 + 66 rpy
5 4
Vzc
rpz
3
7
7
7
7
5
where rp = [rpx ; rpy ; rpz ]T is the accelerometer error vector in the platform frame.
Position Error Equation

The position error vector in the computer frame is decoupled from the quaternions.
There are no dierences in the position error model between the two cases whether the
misalignments of the computer frame and the platform frame are small or large. Let the
dierence between the INS computed position and the true position in the computer
frame Rc be the position error in the computer frame. The position error equation

becomes:
2
6
6
R_ c = 66
4
R_xc
R_yc
R_zc
3
7
7
7
7
5
(5.17)
= V c ;
cecRc
2
6
6
6
6
4
117
Vxc 7 6 0
;Vxctg(')=Rx Vxc=Rx
7 6
Vyc 77 ; 66 Vxc tg(')=Rx 0
Vyc =Ry
5 4
Vzc
;Vxc=Rx
;Vyc=Ry
0
32
76
76
76
76
54
Rxc
Ryc
Rzc
3
7
7
7
7
5
Quaternion Error Model

The attitude error here is modelled using the quaternion vector Qcp = [q0 ; q1; q2; q3 ]T
which represents the misalignment of the platform frame and the computer frame.
Let
2
;q1 ;q2 ;q3

6
6
6 q
;q3 q2
B = 21 666 0
q0 ;q1
6 q3
4
;q2 q1 q0
3
7
7
7
7
7
7
7
5
(5.18)
118
Then the quaternion error model can be written:
Q_ cp = B [(I ; C (Qpc))!icc ; p]

2
3
;q1 ;q2 ;q3 7 2
6
q22 + q32
6
76
6 q
76
;
q
q
3 2 76
= 666 0
7 6 ;(q1 q2 ; q0 q3 )
q0 ;q1 77 4
6 q3
4
5
;(q1q3 + q0q2)
;q2 q1 q0
32
;(q1q2 + q0q3) ;(q1q3 ; q0q2) 7 6

7
76
7
7
6
7;
2
2
c
q1 + q3
;(q2q3 + q0q1) 7 6 !ie cos(') + Vx =Rx
7
54
5
;(q2q3 ; q0q1)
q12 + q22
!ie sin(') + Vxctg(')=Rx
2
;q1 ;q2 ;q3

6
6
6 q
;q3 q2
; 21 666 0
q0 ;q1
6 q3
4
;q2 q1 q0
2
7
76
76
76
76
74
7
5
px
py
pz
;Vyc=Ry
3
7
7
7
7
5
(5.19)
where p = [px ; py ; pz ]T is the gyro error vector in the platform frame.
5.3.3 Filter Process Structure in Continuous Time

The error models and the lter equations are presented in continuous time.
X_ (t) = fc(X; t) + Gcu(t)
(5.20)
X being the function of the velocity error, the position error and the quaternions, the
process model becomes:
2
3
2
3 2
p
c
c
c
c
c
p
_
6 V 7
6 (I33 ; C (Qp ))ft ; (2
ie +
ec )V 7 6 r
6
7
6
7 6
6 R_ c 7 = 6
7+6 0
V c ;
cecRc
6
7
6
7 6 3 1
4
5
4
5 4
p
Q_ cp
B (I ; C (Qc ))!icc
;Bp
where
2
fc (X; t) =
6
6
6
6
4
(I33 ; C (Qcp ))ftp ; (2

cie +
cec)V c
V c ;
cecRc
B (I ; C (Qpc))!icc
3
7
7
7
7
5
(5.21)
3
7
7
7
7
5
(5.22)

and
Gcu(t) =
6
6
6
6
4
rp
031
;Bp
119
3
7
7
7
7
5
(5.23)
Rather than model rb and b in the body frame as in the psi angle approach, this
method handles the IMU errors directly in the platform frame. rp = [rpx; rpy ; rpz ]T
and p = [px ; py ; pz ]T are the accelerometer and the gyro error vectors in the platform
frame. They are related to the IMU biases in the body frame by:
rp = C (Qpb) rb
p = C (Qpb ) b
(5.24)
(5.25)
where the matrix C (Qpb ) transforms the vectors from the body frame to the platform
frame. It must be noted that the quaternions Qpb in (5.24) and (5.25) do not contain
the lter state estimate Qcp .
Let
2
D1p
6
6
6 Dp
p
p
D = ;B = 666 2p
6 D3
4
D4p
3
7
7
7
7
7
7
7
5
(5.26)
and
E = ;B C (Qpb)
(5.27)
Dp = ;Bp = E b
(5.28)
Then
The turn-on biases of the accelerometers and the gyros in the body frame are removed in the coarse alignment and during the zero velocity stage when the vehicle
stops. The remains of the bias rb of the accelerometers and the bias b of the gyros in
the body frame are considered as white Gaussian noise.
120
The linear combinations of jointly white Gaussian 2random3 variables are also white
rp
Gaussian random variables [49]. Therefore rp, Dp and 64
Dp
noises.
Let us set the noise u(t) as
2
u(t) = 64
rp
Dp
rpx
6
6
6 rpy
6
6
6
p
6 rz
6
3
7
5
= 666 D1p
6
6
6
6
6
6
4
D2p
D3p
D4p
7
5
are also white Gaussian
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.29)
This can be projected into the state space using the transformation matrix Gc:
2
Gc =
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
1
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
1
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.30)
121
5.3.4 Observation Equations

The observations are the position and the velocity information dierences of the INS
and the DGPS. Let Z be the measurement vector:
2
Z = 64
2
= 64
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
RINS ; RGPS
c ; Vb t
VbINS
GPS
bc
bt
Rc ; ;gps
V c ; gps
Rxc
Ryc
Rzc
Vxc
Vyc
Vzc
7
5
(5.31)
3
7
5
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
;x
;y
;z
x
y
z
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
c and Vb c are the INS computed position and velocity. R

t
bt
where RbINS
INS
GPS and VbGPS
are the GPS position and velocity outputs respectively. ;gps = [;x ; ;y ; ;z ]T is the
white noise of the DGPS position measurement and gps = [x ; y ; z ]T is the white
noise of the DGPS velocity measurement. The observation matrix H is obtained by
the relationship between the states and the measurement:
2
2
c
6 V 7
6
7
Z = H 66 Rc 77 ;
Qcp
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
;x
;y
;z
x
y
z
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.32)
122
H is given by:
2
H=
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
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
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.33)
5.3.5 The Discrete-Time Filter

The lter process model in continuous time is:
X_ (t) = fc(X; t) + Gcu(t)
(5.34)
Z (t) = HX (t) + w(t)

where
fc (X; t) =
and
6
6
6
6
4
(I33 ; C (Qcp ))ftp ; (2

cie +
cec)V c
V c ;
cecRc
B (I ; C (Qpc))!icc
2
rpx
6
6
6 rpy
6
6
6
p
6 rz
6
Gcu(t) = Gc 666 D1p

6
6 Dp
6
2
6
6
p
6 D3
4
D4p
3
7
7
7
7
5
(5.35)
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.36)
with the constant matrix Gc .

The lter accuracy is achieved by using a rst order Euler integration scheme at
each lter time. Let dt be the data fusion interval. At time tk+1 and tk , dt = tk+1 ; tk ,
123
we have:
X_ (tk ) t X (tk+1 )dt; X (tk )
(5.37)
t fc(X (tk ); tk ) + Gcu(tk )
Therefore the discrete-time process model is:
X (tk+1) = X (tk ) + dt fc(X (tk ); tk ) + Gcu(tk )dt
(5.38)
= fk (X (tk )) + Gk uk
where
fk (Xk ) = X (tk ) + dt fc(X (tk ); tk )

2
c p
c
c
c
6 (I3X 3 ; C (Qp ))ft ; (2
ie +
ec )V
6
= X (tk ) + dt 66
V c ;
cecRc
4
B (I ; C (Qpc ))!icc
(5.39)
3
7
7
7
7
5
X (tk );tk
and
Gk = Gcdt
(5.40)
uk = u(tk )
The measurement is taken at the GPS sampling time. The discrete observation
equation is:
Z (tk ) = HX (tk ) + w(tk )
(5.41)
with the constant observation matrix H .

For convenience, let Xk = X (tk ), Zk = Z (tk ) and wk = w(tk ). The discrete lter is
written as
Xk = fk (Xk ) + Gk uk
Zk = HXk + wk
(5.42)
124
5.3.6 Data Fusion - the Distribution Approximation Filter

Introduction
The lter process models of the quaternion approach are nonlinear.
The EKF predicts the state of the system under the assumption that its process and
observation models are locally linear. The process model and the observation models
are expanded as a Taylor series about the lter's estimated trajectory using Jacobians.
The cyclic computation of the state prediction, measurement prediction, innovation,
updated state estimation and the state error covariance, state prediction covariance,
innovation, lter gain and the updated state covariance are similar to those derived
from the procedure using the linear Kalman lter [83, 71].
There are a number of problems with the EKF. The rst is the need to analytically
evaluate the Jacobian matrices of the process and observation models. The Jacobian is
not guaranteed to exist (e.g. at discontinuity), or might not have a nite value. Further,
there can be considerable implementation diculties when the system is composed of
many states and is highly non-linear. Finally the linearization can introduce signicant
errors.
These problems motivated the development of a new lter algorithm called the
Distribution Approximation Filter (DAF) jointly by Julier and Uhlmann [21, 19]. The
DAF takes a mid-course between the analytical and numerical approaches. Like the
numerical methods, it approximates the state distribution rather than the process or
observation model.
The DAF uses the intuition that it is easier to approximate a distribution than it
is to approximate an arbitrary nonlinear function or transformation. The approach is
illustrated in Figure 5.5.
125
*
*
Nonlinear
Transformation
*
*
*
*
Figure 5.5: The principle of the Distribution Approximation Filter
DAF Principle
If the discrete lter process model and observation model are:
Xk = fk (Xk ) + Uk
(5.43)
Zk = HXk + wk
it is assumed that the process noise Uk is a Gaussian-distributed, zero mean random
variable with covariance Qk . The process noise at any time is independent of the state
of the system.
E [U ] = 0
(5.44)
E [Ui UjT ] = Qi ij

E [Ui XjT ] = 0 8i; j
where ij is the Kronecker delta function.
The observation noise wk encompasses all the unmodelled eects which act on the
observations, but not on the underlying state of the system itself. It is assumed to be a
zero-mean Gaussian distributed random variable with variance Rk and it is independent
126
of the observation noises at all previous time steps:
E [w] = 0
(5.45)
E [wi wjT ] = Ri ij

E [wiXjT ] = 0 8i; j
The problem of how to solve can be summarized as follows. We have an n;dimensional
vector random variable X with mean x and covariance Pxx and we would like to predict
the mean y and covariance Pyy of an m;dimensional vector random variable Y where
Y is related to X by the non-linear transformation
Y = g[X ]
(5.46)
Following the intuition that it is easier to approximate a Gaussian distribution than

it is to approximate an arbitrary nonlinear function or transformation, a parameterization could be found which captures the mean and covariance information while at the
same time permitting the direct propagation of the information through an arbitrary set
of nonlinear equations. This can be accomplished by generating a discrete distribution
having the same rst and second order moments, where each point in the discrete approximation can be directly transformed. The mean and covariance of the transformed
ensemble can then be computed as the estimate of the nonlinear transformation of the
original distribution.
The DAF operational principle is explained as follows [19]:
Assume an n;dimensional Gaussian distribution having covariance P , a set of 2n
points having the same sample covariance from the columns (or rows) of the matrices
p
nP (the positive and negative roots). This set of points is zero mean, but if the
original distribution has mean x , then simply adding x to each of the points yields a
symmetric set of 2n points having the desired mean and covariance. Because the set is
symmetric, its odd central moments are zeros, so its rst three moments are the same as
the original Gaussian distribution. These points are transformed nonlinearly using the
transition equation. The transformed points can be used to compute the predicted mean
127
and covariance. With this approach, there is no need to evaluate Jacobian matrices to
compute the estimation of mean and covariance as in the EKF method.
The DAF is summarized as follows:
The set of translated points is computed from the n n matrix Pk;k as

q
k;k ; 2n columns from (n + )Pk;k
(5.47)
ok;k = Xk;k
ik;k = ik;k + Xk;k
which assures that
Pk;k = n +1
2n
X
i=1
[ik;k ; Xk;k ][ik;k ; Xk;k ]T
(5.48)
The predicted mean is computed as

Xk+1;k = n +1 [ok+1;k + 21
2n
X
i=1
ik+1;k ]
(5.49)
The predicted covariance is computed as

Pk+1;k = n +1 f[ok+1;k ; Xk+1;k ][ok+1;k ; Xk+1;k ]T +
+ 21
2n
X
i=1
(5.50)
[ik+1;k ; Xk+1;k ][ik+1;k ; Xk+1;k ]T g + Qk
where Qk is the dynamic noise covariance.
The predicted observation:

Zk+1;k = n +1 fZok+1;k + 12
2n
X
i=1
Zik+1;k g
(5.51)
The quantity is a scaling factor which provides an extra degree of freedom to ne

tune the higher order moments of the approximation.
128
5.3.7 Filter Implementation Using DAF

The lter states are the errors of the INS velocity, position and quaternions. The
discrete lter is:
Xk = fk (Xk ) + Gk uk
Zk = HXk + wk
where fk (Xk ), Gk uk and H are as equations (5.39), (5.40) and (5.33).
The initial lter state starts from X0;0 = [0; 0; 0; 0; 0; 0; q0 (1); q1 (1); q2 (1); q3 (1)]T
with zero INS velocity error, zero INS position error and arbitrary initial quaternion
error [q0 (1); q1 (1); q2 (1); q3 (1)]T . The initial covariance matrix is set by
2
P0;0 =
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
V2 x
0
0
0
0
0
0
0
0
0
2
Vy
0
0
0
0
0
0
0
0
0
0
V2 z
0
0
0
0
0
0
0
0
0
0
2
Rx
0
0
0
0
0
0
0
0
0
0
2
Ry
0
0
0
0
0
0
0
0
0
0
2
Rz
0
0
0
0
0
0
0
0
0
0
q20
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 q21
0 0 q22
0 0 0 q23
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.52)
The diagonal terms of P0;0 represent variances or mean-squared errors. The odiagonal terms are set to be zeros.
129
The initial dynamic process noise is set as Q0 .

2
Q0 =
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
2 x
acc
0
0
0
0
0
0
0
0
2 y
acc
2 z
acc
0
0
0
0
0
0
0
0
0
0
0
0
q2 0
0
0
0
0
0
0
0
q2 1
0
0
0
0
0
0
0
q2 2
0
0
0
0
0
0
0
q2 3
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.53)
where the tuning of the diagonal terms of Q0 determines the performance of the lter.
The initial measurement noise R0 is set by
2
r2 0 0 0
6 rx
6
6 0
rry2 0 0
6
6
6
2 0
0 rrz
6 0
6
R0 = 6
2
6 0
0 0 rvx
6
6
6
6
4
0
0
0
0
0
0
0
0
0
0
0
0
0
2
0 rvy
2
0 0 rvz
0
0
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(5.54)
The diagonal terms of R0 are initial measurement noises on INS and GPS position
and velocity.
The lter is implemented at time tk using the DAF. Following the DAF procedures
(5.47) to (5.51):
At the data fusion time tk;1, the state estimate is Xk;1. The projected covariance
matrix is Pk;1 :
At the current data fusion time tk , compute the points using the projected
covariance Pk;1 .
p
= nPk;1
where n is the size of Pk;1.
(5.55)
130
Form the matrix i

ik;k = [Xk;1 + ; Xk;1 ; ]
(5.56)
Transform each point through the nonlinear state process model as:
ik;k;1 = fk (ik;k )
(5.57)
Compute the predicted mean. Then the estimated errors are used to update the
states. After the correction, the predicted mean state Xk;k;1 of the lter should
be reset to zero:
Xk;k;1 = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0]T
(5.58)
The predicted covariance Pk;k;1 is computed as

Pk;k;1 = 21n
2n
X
i=1
[ik;k;1 ; Xk;k;1 ][ik;k;1 ; Xk;k;1]T + Qk
(5.59)
where Qk is the dynamic noise covariance.
Compute the lter gain Kk :

Kk = Pk;k;1H T [HPk;k;1H T + Rk ]
(5.60)
with Rk being the measurement noise.
Since the predicted state is zero, the update state estimate Xk is computed as:
Xk = Kk Zk
(5.61)
where Zk is the measurement of this time.
The update covariance estimate Pk is computed by:

Pk = [I ; Kk H ]Pk;k;1
(5.62)
5.4 Summary
131
Xk is then used to correct the INS velocity, position and quaternions.

The quaternion correction is given by:
Qpb = Qcp
Qpb
(5.63)
with
being the quaternion multiplication.
After this lter cycle, the INS will continue to generate navigation data at the IMU
sampling rate until the next GPS x becomes available.
5.4 Summary
In this chapter, the INS algorithm using quaternions for low cost IMUs has been presented.
INS provides navigation data with high frequency in between GPS updating. At
each INS navigation step, the INS velocity, position, attitude and quaternions are
updated independently. The quaternions are used to represent both the vehicle attitude
and the misalignment of the computer frame and the platform frame. Quaternion
initialization and update using a 2-step Adams-Bashford method are presented in this
approach.
The initial alignment method with unknown initial attitudes is developed using an
in-motion alignment lter aided by external velocity or position information. In this
approach, the GPS is used as the external information both for alignment and to bound
errors. The INS error propagation models developed in the previous chapter are used
as process models.
The Distribution Approximation Filter is used to solve the nonlinear lter and its
advantages with respect to the EKF are presented.
Experimental results will be presented in the next chapter.
Chapter 6
Experimental Results
6.1 Introduction
This chapter presents the experimental results for the INS algorithms using the psi
angle approach, the quaternion approach and the GPS modelling.
The results of the GPS modelling in the frequency domain are presented in Section
6.2. Model parameters are shown. PSDs of raw GPS position data and the calculated
PSDs using the model parameters are compared with the experimental data obtained
using a standard GPS receiver. Section 6.2.2 validates the GPS model by a set of plots
in the frequency domain and the time domain using the feed back de-correlation lter.
Section 6.3 shows the experimental results to verify the psi angle model and the INS
algorithm for a low cost IMU. The experiment uses a low cost IMU aided by a DGPS.
The results will display how the heading errors are corrected from 180 to 0.1 . INS
alignment and calibration results will be presented in this section.
Section 6.4 outlines the experimental results of the quaternion algorithm developed
in this thesis. INS errors in acceleration, velocity, position, attitude and quaternions
will be presented using this approach.
The experimental procedures were designed based on IEEE standards and other
publications [84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95].
132
6.2 GPS Position Modelling Results
133

This section presents the results of the GPS error modelling. The experimental data
were obtained using a standard GPS receiver (Ashtech Sensor II).
6.2.1 Model Results

The GPS receiver module processes signals from the Global Positioning System satellite constellation to provide real-time position, velocity and time measurement using
twelve dedicated separate and parallel channels for Clear/Acquisition (C=A) code-phase
measurement on the L1 (1575MHz ) band. The standard Sensor II is designed for
stand-alone range measurement applications. With four locked satellites, Sensor II
can determine a three-dimensional position with an accuracy of 100 metres rms. All
computations are accomplished relative to the World Geodetic System WGS-84 reference ellipsoid. The direct outputs of the antenna position data are in the WGS-84
earth-centred earth-xed reference frame.
In order to calculate the position error characteristics, 50 samples of position data
were collected from a known position. Each sample contains data of approximately 30
minutes duration. The known position in the WGS-84 frame is (;4:66106 , 2:571106 ,
;3:503106 ) meters. The sampling frequency is 10Hz.
Figure 6.1 shows the raw position output in three-dimension in the WGS-84 earthcentred earth-xed reference frame. The position error noise in each axis is calculated
by subtracting the known position from the raw data as in Figure 6.2.
The PSDs of 50 ensembles are averaged to obtain the mean PSD. The true models
are estimated by tting curves to these mean PSD curves. The mean PSDs in axes x,
y and z are shown in Figure 6.3, 6.4 and 6.5 respectively.
134
x 10
3.5034
3.5034
z meters
3.5034
3.5035
3.5035
3.5035
3.5035
2.5708
2.5707
4.6599
2.5707
x 10
4.66
2.5707
4.66
2.5707
y meters
x 10
4.66
2.5707
4.6601
x meters
Figure 6.1: Position output from Ashtech Sensor II.
x meters
100
100
0
100
200
300
400
500
600
700
800
900
100
200
300
400
500
600
700
800
900
100
200
300
400
500
600
700
800
900
y meters
50
50
0
z meters
100
50
0
50
0
time
Figure 6.2: Position noise in axes x, y and z .
135
PSD x
60
50
40
30
20
10
10
20 4
10
10
10
10
10
Figure 6.3: Power spectral density of position error in axis x.
PSD : y
50
40
30
20
10
10
20 4
10
10
10
10
10
Figure 6.4: Power spectral density of position error in axis y.
136
PSD : z
50
40
30
20
10
10
20 4
10
10
10
10
10
Figure 6.5: Power spectral density of position error in axis z .

The results of the PSD models for position errors in axes x, y and z are:
axis
x
y
z
0.0018 0.011 0.5 0.0987

0.00195 0.015 0.5 0.0523
0.00198 0.011 0.5 0.0937
The smooth curves in the Figure 6.6, 6.7 and 6.8 are the estimated PSDs in axes x, y
and z using the estimated models. The measured PSDs are plotted with the estimated
PSDs.
6.2.2 Model Validation

A feedback de-correlation lter using an INS with a noise variance of 1 10;6 (m=s2 )2
is used to validate the GPS model. The GPS measurement is basically the entire noise
when the vehicle is stationary. Using the estimated model parameters, the bode plots
of the transfer function from the GPS measurement to the estimated shaping states in
axes x, y and z are shown in Figure 6.9, 6.10 and 6.11 respectively. The gains are 0dB
for the shaping state estimate before the frequency of 10;2 rad= sec :
137
PSD x
60
50
40
30
20
10
10
20
4
10
10
10
10
10
Figure 6.6: Measured PSD and estimated PSD in axis x.
PSD : y
50
40
30
20
10
10
20
30
4
10
10
10
10
10
Figure 6.7: Measured PSD and estimated PSD in axis y.
138
PSD : z
60
50
40
30
20
10
10
20
30
4
10
10
10
10
10
Figure 6.8: Measured PSD and estimated PSD in axis z .
x
50
Gain dB
50
100 5
10
10
10
10
10
Frequency (rad/sec)
10
10
Phase deg
180
90
90
5
10
10
10
10
10
Frequency (rad/sec)
Figure 6.9: Bode plot in axis x.
10
10
139
y
50
Gain dB
50
100 6
10
10
10
10
10
Frequency (rad/sec)
10
10
10
Phase deg
180
90
90
6
10
10
10
10
10
Frequency (rad/sec)
10
10
10
Figure 6.10: Bode plot in axis y.
z
50
Gain dB
50
100 6
10
10
10
10
10
Frequency (rad/sec)
10
10
10
Phase deg
180
90
90
6
10
10
10
10
10
Frequency (rad/sec)
10
Figure 6.11: Bode plot in axis z .
10
10
140
x
100
80
60
meters
40
20
0
20
40
60
80
0
200
400
600
800
1000
1200
1400
1600
1800
Figure 6.12: Time domain performance in axis x using the estimated model.
The lter performance in the time domain is shown in Figure 6.12, 6.13 and 6.14.
It can clearly be seen that the estimated shaping states track the measured GPS noise.
The parameter values of the shaping lter are essential for the de-correlation process.
For instance, a change of the parameter , with = 0:003; will generate the degraded
performance shown in Figure 6.15 and 6.16. The dashdot curve in Figure 6.15 is the
estimated shaping state. The solid curve is the measured GPS noise. The change of
the poles in the model produces a large error in the shaping state estimate.
Similar changes can be seen by changing other parameters:
Figure 6.17
Figure 6.18
axis x
axis z
r = 0:05 b = 0:8; k = 1; r = 0:9
141
y
100
80
60
40
meters
20
0
20
40
60
80
0
200
400
600
800
1000
1200
1400
1600
1800
Figure 6.13: Time domain performance in axis y using the estimated model.
Z
100
80
60
meters
40
20
0
20
40
60
80
0
200
400
600
800
1000
1200
1400
1600
1800
Figure 6.14: Time domain performance in axis z using the estimated model.
142
y
100
80
60
40
meters
20
0
20
40
60
80
0
200
400
600
800
1000
1200
1400
1600
1800
Figure 6.15: Time domain performance in axis y when the poles of the model change.
50
Gain dB
0
Shaping state
50
100 5
10
10
10
10
10
Frequency (rad/sec)
10
10
Phase deg
180
90
90
5
10
10
10
10
10
Frequency (rad/sec)
10
10
Figure 6.16: Bode plot of the transfer function for axis y when the poles of the model
change.
143
x
100
80
60
meters
40
20
0
20
40
60
80
0
200
400
600
800
1000
1200
1400
1600
Figure 6.17: Time domain performance in axis x when r is changed.
z
150
100
meters
50
50
100
0
200
400
600
800
1000
1200
1400
1600
1800
Figure 6.18: Time domain performance in axis z when three parameters are changed.
6.3 Experimental Results of the Psi Angle Approach
144

This section presents the experiment results to verify the INS algorithm using the psi
angle approach for large attitude errors developed in this thesis. The experimental
platform consists of a low cost IMU (Watson IMU) aided by a DGPS (Novatel RT2
receiver). The experiment was conducted using a UTE in outdoor enviroment.
The IMU used in this experiment is a strapdown inertial measurement unit [96]
which contains three gyros, three accelerometers and two tilt gyros. The IMU is installed directly in the vehicles. The outputs of the IMU are in the body frame of the
vehicle whose origin is dened at the IMU mass centre. The vehicle body frame is
dened as the IMU body frame. Three accelerometers are called accelerometers x, y
and z which are installed in the axes x, y and z of the body frame. The three gyros x, y
and z which are installed in the axes x, y and z of the body frame provide the angular
rate of the vehicle with respect to the body frame. The accelerometers measure the
specic force in the body frame. The tilt gyros provide the bank and the elevation of
the vehicle. The data sampling frequency is 84Hz . The resolution of the IMU is shown
in the following table:
Angular Rate Gyros Accelerometers
4.311510;4 (rad= sec) 0:0024m=s2
The resolution and the sensitivity of the angular rate gyros is not enough to measure
the earth rate.
The DGPS provides the position and velocity information in the WGS-84 reference
with an accuracy of 2cm CEP and 2cm= sec CEP respectively. The sampling frequency
of the DGPS is 10Hz .
The vehicle trajectory in the experimentation is shown in Figure 6.19. It starts
from (0; 0) which is the origin of the local level frame. The vehicle moved from (0; 0)
after a stationary period of approximately 12 seconds and returned and stopped at the
starting position.
The outputs of the three accelerometers x, y and z are shown in Figure 6.20. Figure
145
Position
250
200
North (m)
150
100
50
50
300
250
200
150
East
100
(m)
50
50
Figure 6.19: The vehicle trajectory.

6.21 is the plot of the raw data of the three gyros. They are all in the body frame.
The noise of the DGPS can be modelled as white noise. The shaping lter is not
added to the algorithm. The lter state X using the psi angle approach is dened by:
X = [Vxc ; Vyc; Vzc; Rxc ; Ryc ; Rzc ;
rbx;rby;rbz;bx; by ; bz ]T
x; y; z;
(6.1)
where V c = [Vxc ; Vyc ; Vzc ]T is the INS velocity error vector in the computer frame,
Rc = [Rxc ; Ryc ; Rzc ]T is the INS position error vector in the computer frame,
= [ x; y; z ]T is the psi angle vector which is the angle between the computer frame
and the platform frame, rb = [rbx ; rby ; rbz ]T is the vector of the accelerometer biases in
the body frame and b = [bx ; by ; bz ] is the vector of the gyro biases in the body frame.
The lter is initialized with an arbitrary heading between 180 . The system runs
on prediction using IMU data until DGPS information becomes available. At this time
the IMU state is updated and the heading is corrected as shown in Figure 6.22.
Figure 6.23 shows how the heading error diminishes to 1 in approximately 15
seconds. After this stage, the system is switched to the small angle form of the lter.
The position outputs of the INS and the GPS are shown in Figure 6.24. The INS
continually outputs navigation data between two consecutive GPS sampling times. The
GPS information is used to correct the INS velocity, position and DCM at each GPS
146
Accelerometer x
4
m/s2
2
0
2
4
50
100
150
200
y
250
300
350
400
50
100
150
200
z
250
300
350
400
50
100
150
200
Time
250
300
350
400
m/s2
m/s2
10
15
Figure 6.20: Outputs of three accelerometers. The units for the x axes are seconds.
gyro x
0.2
rad/s
0.1
0
0.1
0.2
50
100
150
200
y
250
300
350
400
50
100
150
200
z
250
300
350
400
50
100
150
200
Time
250
300
350
400
0.2
rad/s
0.1
0
0.1
0.2
1
rad/s
0.5
0
0.5
1
Figure 6.21: Outputs of three gyros. The units for the x axes are seconds.
147
position
4
GPS
3.5
INS
North (m)
2.5
GPS
INS
GPS
2
start
INS
1.5
1
16
14
12
10
8
East (m)
Figure 6.22: INS starts with a wrong heading. The lter corrects the heading in the
rst 20 seconds.
heading error
100
degree
50
0
50
100
0
10
12
14
16
12
14
16
covariance of heading error

10
8
6
4
2
0
0
6
time
8
:
10
sec
Figure 6.23: Heading error diminishes to small angles in 15 seconds.
148
position
190
GPS
GPS
188
INS
186
INS
North (m)
184
182
180
178
176
174
172
170
95
90
85
80
East (m)
Figure 6.24: Position outputs of the INS and the GPS after attitude correction. The
INS continually outputs navigation data between two consecutive GPS sampling times.
sampling time.
Tilt and heading errors are within 0:05 as shown in Figure 6.25.
At the end of the run, the vehicle returned to the starting position and stayed
stationary for a few seconds. The acceleration and velocity predicted by the INS should
be zero during this period. The acceleration outputs in the p-frame are shown in Figure
6.26. It can be seen that the accelerations are very close to zero implying that the
alignment procedure is able to correct the transformation matrix or direction cosine
matrix (DCM).
Without DCM correction, only position and velocity errors are corrected. In this
case, the acceleration is not zero at the end of the run as shown in Figure 6.27.
The drift of one of the gyros used in the IMU is shown in Figure 6.28. It can be
appreciated that the random walk is about 0.1 degree=min. To degrade the accuracy
of the unit, an additional random noise plus drift was added to the IMU sensors. The
noise added to the three accelerometers and the three gyros was approximately 3 times
149
tilt error x
degree
0.01
0.01
108
108.5
109
109.5
110
110.5
111
tilt error y
111.5
112
112.5
113
0.05
108 3108.5
x 10
5
109
109.5
110
110.5
111
heading error z
111.5
112
112.5
113
109
109.5
110
110.5
111
time (seconds)
111.5
112
112.5
113
degree
0.05
degree
5
108
108.5
Figure 6.25: Tilt and heading errors diminish to small angles.
acceleration east
0.5
0
0.5
1
1.36
1.37
1.38
1.39
1.4
north
1.41
1.42
1.43
4
x 10
0.4
0.2
0
0.2
0.4
0.6
1.36
1.37
1.38
1.39
up
1.4
1.41
1.42
1.43
4
x 10
0.5
0
0.5
1.37
1.38
1.39
1.4
time
1.41
1.42
1.43
4
x 10
Figure 6.26: At the end of the run, the acceleration of the INS output in the p;frame
is very close to zero with DCM correction. The unit of the acceleration is m=s2 .
150
acceleration east
0.3
0.2
0.1
0
0.1
1.39
1.395
1.4
1.405
1.41
1.415
north
1.42
1.425
1.43
1.435
4
x 10
0.2
0
0.2
1.37
1.38
1.39
1.4
up
1.41
1.42
1.43
1.44
4
x 10
0.2
0
0.2
1.37
1.38
1.39
1.4
time
1.41
1.42
1.43
4
x 10
Figure 6.27: The acceleration of the INS output in the end is not zero without DCM
correction. The unit in the plot is m=s2 :
Figure 6.28: Random walk of the gyro x in 3 minutes.

noise added to gyros
151
noise added to accelarometers
0.025
0.04
0.02
0.03
0.015
0.02
0.01
degree/sec
0.01
0.005
0
0
0.01
0.005
0.02
0.01
0.03
0.015
0.02
0
500
1000
time
1500
0.04
0
500
1000
1500
time
Figure 6.29: The noise added to the three accelerometers and the three gyros was
approximately 3 times larger than the standard noise of the Watson IMU. The unit of
the acceleration noise is m=s2 .
larger than the standard noise of the Watson IMU. The drift rate of the perturbed gyro
noise is shown in Figure 6.29 and is now about 0.3 degree= min.
The alignment results can be seen in Figure 6.30 and Figure 6.31.
At the end of the run, the rotation rate and the acceleration should be close to
zero. In Figure 6.30, the rotation rate without lter calibration in 3 gyros is about
0.3 degree=sec while it is approximately zero with the alignment algorithm. Similar
results are obtained with acceleration as shown in Figure 6.31. The acceleration in the
platform frame at the end is close to zero with the calibration of the lter when the
acceleration is perturbed.
These results prove that the biases in gyros and accelerometers are properly estimated.
152
0.4
0.2
0
0.2
0.4
0.6
0.8
1.38
1.39
1.4
1.41
1.42
1.43
4
degree/sec
x 10
0.2
0
0.2
0.4
0.6
1.38
1.39
1.4
1.41
1.42
1.43
4
x 10
0.5
0
0.5
1.36
1.37
1.38
1.39
1.4
raw gyro data+added noise
1.41
1.42
Red:after filter
1.43
4
x 10
Figure 6.30: The INS rotation rate at the end of the run when the vehicle is stationary
should be zero. The solid curve is the rotation rate after calibration by the lter. It is
close to zero. The 'plus +' curve is the rotation rate without calibration.
Acceleration_East_North_Up
4
2
0
2
0
100
200
300
400
500
600
700
800
900
1000
100
200
300
400
500
600
700
800
900
1000
100
200
300
400
500
600
700
800
900
1000
0.5
0
0.5
1
0
1
0
1
2
0
Figure 6.31: With perturbed acceleration, at the end of the run, the acceleration in the
platform frame is close to zero after calibration by the lter.
153

The quaternion algorithm developed in this thesis is demonstrated using the experimental results in this section. The experimental data used in the previous section and
shown in Figure 6.19, 6.20 and 6.21 were used to test the algorithm.
The lter state X is given by:
X = [Vxc ; Vyc; Vzc; Rxc ; Ryc ; Rzc ; q0 ; q1 ; q2 ; q3 ]T
(6.2)
where
[Vxc ; Vyc ; Vzc ]T is the INS velocity error vector in the computer frame.
[Rxc ; Ryc ; Rzc ]T is the INS position error vector in the computer frame.
[q0 ; q1 ; q2 ; q3 ]T is the quaternion representing the error between the computer frame
and the platform frame.
The Distribution Approximation Filter starts with arbitrary initial quaternions.
The vehicle trajectory starts from (0; 0) of the local level frame which is at the right
bottom corner of the Figure 6.32. The INS outputs wrong navigation information at the
beginning of the run due to incorrect alignment. The position and heading correction
can be appreciated in this gure after 60 lter iterations, which is about 20 seconds. The
heading error is corrected to less than 2 . Figure 6.33 shows this heading correction.
The quaternion errors are shown in Figure 6.34. After 60 lter iterations which is
approximately 20 seconds, the quaternion errors become very close to [1; 0; 0; 0]T ; which
represents a zero rotation between the computer frame and the platform frame. Figure
6.35 is an enhanced view of the four elements of the quaternion errors at around 40
seconds of the vehicle running time.
The accelerations in the three axes of the platform frame are shown in Figure 6.36.
At the end of the run, the true acceleration in the platform should be zero. Figure
6.37 is the enhanced view of the calculated acceleration in the platform frame. The
154
Position
4
3.5
North (m)
2.5
1.5
18
16
14
12
10
East (m)
Figure 6.32: At the beginning, the INS is uncertain within 180 . After 60 iterations
of the lter, the INS heading error is corrected to less than 5 .
Heading Error
150
Degree
100
50
50
20
40
60
80
100
120
Figure 6.33: The INS heading error is diminished to small angles after 60 iterations of
the lter.

3
q0
1
155
q1
x 10
0.95
0.5
0.9
0.85
0
0.8
0.75
0.5
0.7
0.65
20
40
60
80
100
20
40
q2
x 10
60
80
100
60
80
100
q3
1
1.5
0.5
1
0.5
0
0
0.5
0.5
1
1.5
20
40
60
80
100
20
40
Figure 6.34: Quaternions are corrected after each lter time.
q0
q1
x 10
1.5
1.0003
1.0002
1.0001
0.5
0.9999
0.5
0.9998
0.9997
110
120
130
140
150
120
q2
x 10
140
160
q3
x 10
5
5
0
0
5
10
15
5
120
130
140
150
160
120
130
140
150
Iteration
160
170
Figure 6.35: The enhanced view of the quaternion errors between the lter iteration
100 to 160 which correspond to the vehicle running time 25 seconds to 45 seconds.
The four elements are very close to [1; 0; 0; 0]T which represents quaternions of a zero
rotation from the computer frame to the platform frame.
156
Acceleration
5
100
200
300
400
500
600
700
800
100
200
300
400
500
600
700
800
100
200
300
400
Iteration
500
600
700
800
4
2
0
2
4
10
5
0
5
Figure 6.36: Accelerations in the three axes of the platform frame. The unit of the plot
is m=s2 :
acceleration in axis x and axis z are very close to zero. The error in axis y is about
0.08 m=s2 :
The attitude errors in the three axes are shown in Figure 6.38. The tilt errors are
less than 0:04 . The heading errors are within 0.5 .
The velocity and position errors at the end of the run can be seen in Figure 6.39.
The velocity errors are within 0.5m=s2 : The position errors are within 0.2m.
Figure 6.40 shows the position curves of the INS and GPS outputs after 170 seconds
of the run. In this run the sampling frequency of the GPS is about 5Hz. The INS
position prediction matches the GPS position plot which is the label \o" in the plot.
Figure 6.41 is an enhanced view of this position match. The INS position output is
as the solid curve. The GPS position output is as the curve \o".
157
Acceleration
0.5
0
0.5
1.37
1.38
1.39
1.4
1.41
1.42
1.43
4
x 10
1
0.5
0
0.5
1
1.34
1.35
1.36
1.37
1.38
1.39
1.4
1.41
1.42
1.43
1.44
4
x 10
1
0
1
1.33
1.34
1.35
1.36
1.37
1.38
1.39
Iteration
1.4
1.41
1.42
1.43
4
x 10
Figure 6.37: At the end of the run, the vehicle is stationary. The accelerations in the
three axes of the platform frame are very close to zero.
Tiit Error x
Degree
0.02
0
0.02
628
630
632
634
636
638
640
Tiit Error y
642
644
646
648
0.04
Degree
0.02
0
0.02
625
630
635
640
645
Heading Error
650
655
660
Degree
0.5
0.5
630
635
640
Iteration
645
650
655
Figure 6.38: Tilt errors are less than 0.04 . Heading errors are within 0.5 .

3
Velocity Error
Position Error
x 10
5
xm
0.05
x m/s
158
0
0.05
0
5
660
665
670
675
680
685
660
665
3
x 10
0.3
670
675
680
685
20
0.1
0.2
10
0
0
0.1
640
650
660
670
680
690
660
665
670
675
680
685
0.03
0.02
0.1
0.01
0
0.1
0.01
640
650
660
670
680
690
650
660
670
680
690
Figure 6.39: The velocity errors are within 0.1 m=s. The position errors are within
0.02 m.
Position
50
45
40
35
North (m)
30
25
20
15
10
5
0
10
15
20
25
30
East (m)
35
40
45
50
Figure 6.40: The solid curve is the INS position output. Curve \o" is the GPS position
output. INS outputs position data with the frequency of 84Hz between two GPS
position outputs.
6.5 Summary
159
Position
189
North (m)
188
187
186
185
184
86
85
84
83
82
81
East (m)
Figure 6.41: INS Position and heading corrections can be seen in the curve between
two GPS position outputs.
6.5 Summary
The GPS modelling results have veried the modelling theory and techniques in the
frequency domain developed in this thesis.
The experimental results in this chapter have veried the psi angle and the quaternion models for low cost INS developed in this thesis.
These results have shown that for a low cost INS with aiding GPS information,
position, velocity and attitude accuracy can be achieved using the INS algorithms in
the psi angle approach and the quaternion approach presented in this thesis.
Chapter 7
Summary and Conclusions

7.1 Introduction
This chapter summarizes the major contributions of this thesis, draws conclusions and
makes suggestions for future research.
In Section 7.2, a chapter by chapter summary of this thesis is presented. Section
7.3 highlights the major contributions of this thesis. Finally, Section 7.4 reviews some
of the limitations of the work and future research in this area is suggested.
7.2 Summary of Each Chapter

Chapter 1 provided the primary motivation for the work carried out in this thesis. It
is argued that INS errors determine the behaviour and the accuracy of an INS. The
error model is the tool for error analysis. Advanced INS algorithms are based on error
modelling. For a low cost INS with less accurate resolution, INS errors are large. INS
error models for large attitude errors are needed.
A short survey of INS error models was given in this chapter. The psi angle approach
or the computer frame approach and the phi angle approach or the true frame approach
are two modelling approaches which were investigated.
This chapter introduced the main research theme of this thesis - the development of
160
161
INS error models for large attitude errors in the computer frame approach with both psi
angle form and quaternion form, and the development of two integrated INS algorithms
for a low cost INS. These two algorithms were aided by the GPS. GPS modelling in
the frequency domain was also discussed extensively.
In Chapter 2, the generic INS error propagation models for large attitude errors
in the psi angle approach and the quaternion approach were presented.
A short survey of INS error models was given in Section 2.2. This survey showed
that two approaches have been adopted in the literature: the psi angle approach (or the
computer frame approach) and the phi angle approach (or the true frame approach).
So far, there are no generic error propagation models for three large attitude errors.
All the quaternion error models in the literature are based on small angle assumption.
Section 2.3 developed INS error propagation models for large errors in the psi angle
approach. There are three models: the velocity error model, the position error model
and the attitude model. The attitude errors were presented using three psi angles. In
this case, the three attitude errors can be assumed to be large with uncertainties of
180 :
It was argued that INS attitude updating using quaternions provides more accuracy,
requires less computation and avoids singularity in computation. This motivated the
development of another set of INS models in quaternion form for large errors in Section
2.4. The computer frame approach was also used in this model. Quaternion errors
were presented using the quaternions between the platform frame and the computer
frame. Diering from other quaternion models, these models make no assumption of
small angle errors.
Chapter 3 presented the identication of GPS error models using frequency domain
techniques.
It is argued that the navigation problem can be generally split into two components:
creating a process model of the host vehicle and understanding or modelling the sensors
to be used. The process model of a navigation system describes the prediction of states
which are typically the position, velocity, attitude and related parameters aecting
162
these variables. This chapter detailed the modelling of the GPS measurement which is
used to aid INS navigation.
Section 3.2 reviewed previous work on GPS modelling in the frequency domain. This
review showed that a GPS position error model has not been theoretically developed
in the frequency domain. Section 3.3 derived a GPS position error model. It is proved
that the transfer function of the GPS position error in any frame has the identical poles
and zero as the pseudo-range error. A modelling method using power spectral density
of the noise was presented in Section 3.4. Section 3.5 examined the de-correlation of
the GPS coloured noise. A shaping lter was introduced to the de-correlation lter. An
additional sensor is required for de-correlation. Feedforward lter and feedback lter
with the INS and GPS measurements were analyzed in both the frequency domain and
the time domain to determine the quality of aiding sensors to perform de-correlation.
Chapter 4 designed a low cost INS algorithm with unknown initial conditions using
the psi angle approach in the computer frame.
The INS alignment algorithms were reviewed in Section 4.2. This review showed
that the analytic coarse alignment method and gyrocompassing are the major methods
for self-alignment. They require the measurements of the gravity vector and the earth
rate by three accelerometers and three gyros. In-motion alignment is also used for
INS ne alignment and was reviewed. It is shown that most of the applications in the
literature for in-motion alignment were based on lters with known initial attitudes. For
a low cost IMU with low resolution, the initial attitude errors are large. The algorithms
to solve this problem were reviewed.
In this chapter, a new algorithm was developed in the computer frame approach
that can be used with unknown initial attitudes.
Section 4.3 described the on ground coarse alignment for low cost IMUs. This
algorithm also takes into account the turn-on biases estimation. The coarse initial
direction cosine matrix was formulated.
The solution of unknown initial attitudes was presented in Section 4.4 using an
in-motion alignment algorithm. A lter with error propagation models for large error
163
angles in the computer frame was developed. The lter was implemented using the
Extended Kalman Filter (EKF).
Chapter 5 dealt with the issue of developing an INS algorithm for a low cost IMU
using quaternions in the computer frame.
Section 5.2 described independent INS navigation using quaternions. Quaternion
initialization was formulated. A 2-step Adams-Bashford method was introduced for
quaternion update.
The quaternion algorithm to deal with unknown initial attitudes was developed
in Section 5.3. The quaternion errors were exploited using the misalignment of the
computer frame and the platform frame. The entire lter process model structure
and the process noise were presented. The process noise vectors were reconstructed
using the linear combination of the white noise on the accelerometers and gyros in the
body frame. A Distribution Approximation Filter (DAF) was used to implement this
algorithm. The principle and the benet of the DAF were brie y described.
Chapter 6 presented the experimental results for the INS algorithms using the psi
angle approach, the quaternion algorithm and the GPS modelling.
The results of GPS modelling in the frequency domain were presented in Section
6.2. The GPS model was validated using a set of plots in the frequency domain and
the time domain using a feedback de-correlation lter.
Section 6.3 presented the experimental results to verify the psi angle models and
the INS algorithm for low cost IMUs using these models. The experiment used a low
cost IMU aided by a DGPS. The results demonstrated how the heading errors were
corrected. INS alignment and calibration results were also presented in this section.
Section 6.4 outlined the experimental results of the quaternion algorithm. INS
errors in acceleration, velocity, position, attitude and quaternions were examined.
These results have shown that for a low cost INS with aiding GPS information,
position, velocity and attitude accuracy can be achieved using the INS algorithms
presented in this thesis.
7.3 Contributions
164
7.3 Contributions
The major contributions of this thesis are the development of the error models and
the applications of the models to integrated INS algorithms. This section outlines the
individual contributions of this thesis.
7.3.1 INS Error Modelling in Psi Angle Approach

This thesis presents two new INS error propagation models for large angle errors.
Two approaches to generate INS models are investigated in the literature: the psi
angle approach or the computer frame approach and the phi angle approach or the true
frame approach. The two approaches yield identical results. Most of the models in the
literature have assumed that either the three attitude errors are small or the two tilt
errors are small and one heading error is large.
The psi angle approach was used in this thesis for the development of the INS
models. There are three equations in this psi angle model for large attitude errors: the
velocity error propagation equation, the position error propagation equation and the
psi angle equation. This model diers from other models in that it does not rely on
the small angle assumption and can accommodate three large attitude errors. Three
independent states were used to describe the three psi angles in the computer frame.
The model with three small attitude errors becomes a special form of this model. The
model for one large heading error and two small tilt errors is also a special case of the
model.
7.3.2 INS Error Modelling Using the Quaternion Approach

Another important contribution of this thesis is the INS model in quaternion form. The
model equations are also in the computer frame. There are three sets of equations: the
velocity error propagation equation using quaternions, the position error propagation
equation and the quaternion error propagation equation. The model diers from other
quaternion models in the literature in that it makes no assumption of small attitude
7.3 Contributions
165
errors. The model is suitable for both small and large angles.
7.3.3 GPS Modelling in Frequency Domain

GPS modelling in the frequency domain is another contribution of this thesis. Unlike
other GPS modelling in previous work that modelled the errors of the pseudo range
and the clock oset, this approach models the GPS position error reported by standard
GPS. The equations of GPS correlated errors in position were derived as second order
systems in the frequency domain. Power spectral density (PSD) plots were used as the
tool to obtain the model parameters.
This thesis also presented a feedback lter and a feedforward lter for GPS error
de-correlation using INS information. The essential requirement for the variance of the
INS noise for de-correlation was presented using frequency domain techniques.
7.3.4 INS Algorithm for Low Cost IMU Using Psi Angle Approach
An important contribution of this thesis is the INS algorithm using the psi angle approach. This algorithm was designed for a low cost INS whose resolution is low to
perform INS self-alignment. The main contribution is the in-motion alignment to solve
the initial attitude uncertainty.
The algorithm includes raw data process, coarse leveling, in-motion alignment and
calibration. A GPS shaping lter is introduced in the lter model. When a high
accuracy DGPS was used, the error of the DGPS was modelled as white noise. The
lter procedure using the EKF was formulated. After the attitude errors diminish to
small angles, the lter still works using the psi angle model in its small angle form
which is a special case of the psi angle model. The biases of the accelerometers and
gyros were modelled and calibrated in-motion. This algorithm combines INS alignment,
calibration and navigation under one lter.
7.4 Future Work
166
7.3.5 INS Algorithm for Low Cost IMU Using Quaternion Model
Another contribution of this thesis is the INS algorithm using the quaternion model
for low cost inertial systems with low resolution which are not able to conduct selfalignment.
It is argued that computing the INS attitude using quaternions has more advantages
than using Euler angles and the direction cosine matrix. The INS alignment and calibration were implemented in-motion using a nonlinear lter whose process models were
the INS error propagation models using quaternions. To avoid deriving the Jacobian
matrices of the lter, the Distribution Approximation Filter was used in this algorithm.
7.4 Future Work

7.4.1 DGPS Modelling
This thesis developed GPS modelling in the frequency domain. A shaping lter was
described. This GPS model is useful for a stand alone GPS with a position error of 20100m. In the experimental implementation of the INS algorithms in this thesis, a high
accuracy DGPS was used instead of a stand alone GPS. The accuracy of this DGPS
was 2cm. The error of this DGPS was modelled as white noise. Future work could
model the DGPS in the frequency domain to achieve higher accuracy of the navigation
system.
In this thesis, a \loose-coupling" mode is used to fuse the INS and GPS. In this
mode the INS and the GPS are treated as navigation systems. The calculated GPS
position and velocity are used. When a \tight-coupling" mode is used, the INS and GPS
receiver are treated as individual sensors. The raw data of the GPS pseudo range and
carrier phase are used directly. Further modelling of these raw data in the frequency
domain could be another interesting area.
7.4 Future Work
167
7.4.2 Experimental Implementation of the INS Algorithms with Three

Unknown Attitudes
The INS algorithms developed in this work are theoretically designed for all unknown
attitudes. In the real time experimental implementation, two tilt gyros were available
in the IMU to reduce initial tilt errors to small angles. Only one heading error was
large in the experiment.
Further experimental work could be extended without using tilt gyros.
7.4.3 Self Tuning Filters

In the experimental implementation for these INS algorithms, the performance of the
lters was in uenced by the choice of the process noise attributes. Currently, the noise
strengths were chosen by engineering judgement and experience. An important area
for future work could be to develop methods which are able to identify the strengths
of the process noise from the collected raw data. Consequently these future self-tuning
lters could lead to consistent and accurate performance in real time implementation.
7.4.4 Phi Angle Model for Large Attitude Errors

The psi angle approach and the phi angle approach are adopted in the literature. The
psi angle approach in the computer frame was used to develop the INS models and the
integrated algorithms for low cost INS in this thesis.
A phi angle model in the true frame for large attitude errors could be an interesting
research area for future work. The quaternion models for large attitude errors in the
phi angle approach could also be studied in the future.
Bibliography
[1] D. H. Titterton and J. L. Weston. Strapdown Inertial Navigation Technology . Peter
Peregrinus on behalf of the Institution of Electrical Engineers, London, 1997.
[2] SAGEM Australasia. Sensor II operating manual and interface guide, 1995.
[3] Drora Goshen-Meskin and Itzhack Y. Bar-Itzhack. Unied approach to inertial
navigation system error modeling. Journal of Guidance, Control and Dynamics,
15(3):648{653, 1992.
[4] Itzhack Y. Bar-Itzhack, Berman N. Control theoretic approach to inertial navigation systems. Journal of Guidance and Control, 11(3):237{245, May-June 1988.
[5] Donald O.Benson. A comparison of two approaches to pure-inertial and dopplerinertial error analysis. IEEE Aerospace and Electronic Systems, AES-11(4):447{
455, July 1975.
[6] Itzharck Y. Bar-Itzhack. Identity between INS position and velocity error models.
Journal of Guidance and Control, 4:568{570, Sept-Oct 1981.
[7] Itzharck Y. Bar-Itzhack. Identity between INS position and velocity error equations
in the true frame. Journal of Guidance and Control, 11(6):590{592, 1988.
[8] S. P. Dmitriyev, O. A. Stepanov and S. V. Shepel. Nonlinear ltering methods application in INS alignment. IEEE Aerospace and Electronic Systems, AES33(1):260{271, January 1997.
168
BIBLIOGRAPHY
169
[9] Bruno M. Scherzinger and D.Blake Reid. Modied strapdown inertial navigation
error models. Proceedings of 1994 IEEE Position, Location and Navigation Symposium - PLANS'94. Las Vegas, NV, USA. IEEE Aerosp. and Electron. Syst. Soc.
, pages 426{430, 1994.
[10] Bruno M. Scherzinger. Inertial navigation error models for large heading uncertainty. Proceedings of PLANS, pages 477{484, 1996.
[11] B. Barshan and H. F. Durrant-Whyte. Inertial navigation systems for mobile
robots. IEEE Trans. on Robotics and Automation, 11(3):328{342, June 1995.
[12] J. Vaganay and M. J. Aldon. Attitude estimation for a vehicle using inertial sensors. Proceedings the 1st IFAC International Workshop on Intelligent Autonomous
Vehicles, pages 89{94, April 1993.
[13] Hyung Keun Lee, Jang Gyu Lee, Yong Kyu Roh and Chan Gook Park. Modeling
quaternion errors in SDINS: computer frame approach. IEEE Transactions on
Aerospace and Electronic Systems, 34(1):289{297, January 1998.
[14] Bernard Friedland. Analysis strapdown navigation using quaternions. IEEE Transactions on Aerospace and Electronic Systems, AES-14(5):764{768, September 1978.
[15] S. Vathsal. Optimal control of quaternion propagation in spacecraft navigation.
Journal of Guidance, Control and Dynamics, 9(3):382{384, May-June 1986.
[16] Phillip J. Fenner. Requirements, application and result of strapdown inertial technology to commercial airplanes. Technical Report AD-P003 622, Boeing Commercial Airplane Company.
[17] Steven Scheding. High Integrity Navigation. PhD thesis, The University of Sydney,
1997.
[18] C. K. Chui and G. Chen. Kalman Filtering with real-time applications. SpringerVerlag, 1987.
BIBLIOGRAPHY
170
[19] Simon J. Julier. Process Models for the Navigation of High Speed Land Vehicles.
PhD thesis, University of Oxford, 1997.
[20] S. J. Julier, J. K. Uhlmann and H. F. Durrant-Whyte. A new approach for ltering
nonlinear systems. Proceedings of the 1995 American Control Conference, Seattle,
Washington, pages 1628{1632, 1995.
[21] S. J. Julier, J. K. Uhlmann and H. F. Durrant-Whyte. A new approach for the nonlinear transformation of means and covariances in linear lters. IEEE Transactions
on Automatic Control, 1996.
[22] S. J. Julier and J. K. Uhlmann. A new extension of the Kalman lter to nonlinear
systems. The Proceedings of AeroSense: The 11th International Symposium on
Aerospace/Defense, Session: Multi Sensor Fusion, Tracking and Resource Management II, 1997.
[23] S. J. Julier and J. K. Uhlmann. A consistent, debiased method for converting
between polar and cartesian coordinate systems. The Proceedings of AeroSense:
The 11th International Symposium on Aerospace/Defense Sensing, Simulation and
Controls, Orlando, Florida. Session: Acquisition, Tracking and Pointing XI, 1997.
[24] Abraham Weinred and Itzhack Y.Bar-Itzhack. The psi-angle error equation in
strapdown inertial navigation systems. IEEE Aerospace and Electronic Systems,
AES-14(3):539{542, May 1978.
[25] N. Lovren and J. K. Pieper. Error analysis of direction cosine and quaternion
parameters techniques for aircraft attitude determination. IEEE Transactions on
Aerospace and Electronic Systems, 34(3):983{989, July 1998.
[26] George Arshal. Error equations of inertial navigation.
control and dynamics, 10:351{358, July-Aug 1987.
Journal of Guidance,
[27] Kenneth R. Britting. Inertial Navigation System Analysis. Wiley-Interscience,

1971.
BIBLIOGRAPHY
171
[28] Tuan Manh Pham. Kalman lter mechanization for INS airstart. IEEE AES
System Magazine, pages 3{11, January 1992.
[29] Stergios I. Roumeliotis, Gaurav S. Sukhatme and George A. Bekey. Smoother
based 3d attitude estimation for mobile robot localization. Proceedings of the 1999
IEEE International Conference on Robotics and Automation, pages 1979{1986,
May 1999.
[30] John L. Crassidis and F. Landis Markley. Minimum model error approach for
attitude estimation. Journal of Guidance, Control and Dynamics, 20(6):1241{
1247, November-December 1997.
[31] Xiaoying Kong, E. Nebot and H. Durrant-Whyte. Development of a non-linear
psi-angle model for large misalignment errors and its application in INS alignment
and calibration. Proceedings of IEEE International Conference on Robotics and
Automation, pages 1430{1435, May 10-15 1999. Detroit, MI, USA.
[32] Chen Zhe. Principle of Strapdown Inertial Navigation System. Beijing University
of Aeronautics and Astronautics, 1985.
[33] R. J. Milleken and C. J. Zoller. Principle of operation of NAVSTAR and system
characteristics. Global positioning system : papers published in Navigation, 1:3{14,
1993.
[34] E. M. Nebot, H. Durrant-Whyte, S. Scheding. Frequency domain modeling of
aided GPS with application to high-speed vehicle navigation. IEEE International
Conference on Robotics and Automation, New Mexico, pages 1892{1897, April
1997.
[35] Institute of Navigation. Global positioning system : papers published in Navigation,
volume 1-4. Washington, D.C. Institute of Navigation, 1980-1993.
[36] Bertrand Merminod. The use of Kalman lters in GPS navigation. PhD thesis,
The University of New South Wales, Australia, 1989.
BIBLIOGRAPHY
172
[37] Cliord Meth. With GPS, you can get there from here. Electronic Design, November 1995.
[38] G. D'este R. Zito and M. A. Taylor. Global positioning system in the time domain:
How useful a tool for intelligent vehicle-highway systems. Technical Report 4,
University of South Australia, 1995.
[39] Bertil Ekstrand. Analytical steady state solution for a Kalman tracking lter. IEEE
Transactions on Aerospace and Electronic Systems, 19(6):815{819, November 1983.
[40] Steven R. Rogers. Alpha-Beta lter with correlated measurement noise. IEEE
Transactions on Aerospace and Electronic Systems, 23(4):592{594, July 1987.
[41] Steven R. Rogers. Continuous-time ECV and ECA track lters with colored
measurement noise. IEEE Transactions on Aerospace and Electronic Systems,
26(4):663{666, July 1990.
[42] C. C. Arcasoy, B. Koc. Analytical solution for continuous-time Kalman tracking
lters with colored measurement noise in frequency domain. IEEE Transactions
on Aerospace and Electronic Systems, 30(4):1059{1063, October 1994.
[43] Ralph Mason Joe M. Toth and Ken J. Runtz. Precise navigation using adaptive FIR ltering and time domain spectral estimation. IEEE Transactions on
Aerospace and Electronic Systems, 30(4):1071{1076, October 1994.
[44] R. A. Walker and K. Kubik. Parabolic equation modelling of GPS signal propagation and RF communication links in the open mining enviroment. Proceedings of
Fourth International Symposium on Mine Mechanisation and Automation, 2:B6{
47{B6{55, July 1997.
[45] Simon Cooper. A frequency response method for sensor suite selection with an
application to high-speed vehicle navigation. PhD thesis, Oxford University, June
1996.
BIBLIOGRAPHY
173
[46] H. Lichtenegger B. Hofmann-Wellenhof and J. Collins. Global Positioning System

: Theory and Practice. Wien ; New York : Springer, 1993.
[47] D. K. Anand. Introduction to Control Systems, volume 8 of International series
on system and control. Pergamon Press, University of Maryland,USA, 2nd edition,
1984.
[48] Benjamin C. Kuo. Automatic Control Systems. Englewood Clis, N.J. : PrenticeHall, 3rd edition, 1975.
[49] Peter S. Maybeck. Stochastic models, estimation and control, volume 141 of Mathematics in science and engineering. New York : Academic Press, 1979-1982.
[50] Paul G. Savage. Strapdown inertial navigation integration algorithm design Part
1: Attitude algorithm. Journal of Guidance and Control, 21(1):19{28, JanuaryFebruary 1998.
[51] Paul G. Savage. Strapdown inertial navigation integration algorithm design Part 2:
Velocity and position algorithm . Journal of Guidance and Control, 21(2):208{221,
March-April 1998.
[52] Itzhack Y. Bar-Itzhack. Azimuth observability enhancement during inertial navigation system in- ight alignment. Journal of Guidance and Control, 2(4):337{344,
July-August 1980.
[53] Yeon Fuh Jiang and Yu Ping Lin. Error estimation in ground alignment through
observation analysis. IEEE Transactions on Aerospace and Electronic Systems,
28(1):92{97, January 1992.
[54] Yeon Fuh Jiang. Error analysis of analytic coarse alignment methods. IEEE
Transactions on Aerospace and Electronic Systems, 34(1):334{337, January 1998.
[55] Heung Won Park, Jang Gyu Lee and Chan Gook Park. Multiposition alignment
of strapdown inertial navigation system. IEEE Transactions on Aerospace and
Electronic Systems, AES-29(4):1323{1328, October 1993.
BIBLIOGRAPHY
174
[56] Heung Won Park, Jang Gyu Lee and Chan Gook Park. Covariance analysis of
strapdown INS considering gyrocompass characteristics. IEEE Transactions on
Aerospace and Electronic Systems, AES-31(1):320{328, January 1995.
[57] J. C. Huang, H. V. White. Self-alignment techniques for inertial measurement
units. IEEE Transactions on Aerospace and Electronic Systems, AES-11(6):1232{
1247, November 1975.
[58] Frank D. Jurenka and Cornnelius T. Leondes. Optimum alignment of an inertial
autonavigator. IEEE Transactions on Aerospace and Electronic Systems, AES3(6):880{888, November 1967.
[59] Itzhack Y. Bar-Itzhack. Two misconceptions in the theory of inertial navigation
systems. Journal of Guidance and Control, 18(4):908{911, 1990.
[60] Itzhack Y. Bar-Itzhack. The enigma of false bias detection in a strapdown system
during transfer alignment. Journal of Guidance and Control, 8(2):175{180, MarchApril 1985.
[61] Cristopher C. Ross and Timothy F. Elbert. A transfer alignment algorithm study
based on actual ight test data from a tactical air-to-ground weapon launch. Proceedings of IEEE PLANS, pages 431{438, 1994.
[62] Boaz Porat, Itzhack Y. Bar-Itzhack. Eect of acceleration switching during INS
in- ight alignment. Journal of Guidance and Control, 4(4):385{389, July-August
1981.
[63] Itzhack Y. Bar-Itzhack. Optimal updating of INS using sighting devices. Journal
of Guidance and Control, 1(5):305{312, Sept.-Oct. 1978.
[64] Averil B. Chateld. Fundamentals of High Accuracy Inertial Navigation, volume
174 of Progress in Astronautics and Aeronautics. American Institute of Aeronautics
and Astronautics, Inc., 1997.
BIBLIOGRAPHY
175
[65] Shmuel Merhav. Aero Sensor System and Applications. Springer-Verlag New
york,Inc., 1996.
[66] Yeon Fuh Jiang, Yu Ping Lin. Error estimation of ground alignment to arbitrary
azimuth. IFAC, (AIAA-93-3823-CP):1088{1093, 1993.
[67] R. M. Rogers. IMU in-motion alignment without benet of attitude initialization.
Navigation, 44(3):301{311, Fall 1997.
[68] Xiaoying Kong, Eduardo Nebot and Hugh Durrant-Whyte. Use quaternions in a
low cost strapdown INS unit. Proceedings of the International Conference on Field
and Service Robotics, Canberra, Australia, pages 286{291, 1997.
[69] Fan Yaozu and Xu Dahua. Test and Modeling of Inertial Sensors. Aerospace Press:
Beijing, 1985.
[70] Richard P.Wishner Michael Athans and Anthony Bertolini. Suboptimal state estimation for continuous-time nonlinear system from discrete noisy measurements.
IEEE Transaction on Automatic Control, AC-13(5):504{514, October 1968.
[71] Robert Grover Brown and Patrick Y.C. Hwang. Introduction to random signals
and applied Kalman ltering : with Matlab exercises and solutions. New York :
Wiley, 1997.
[72] Jonh E. Bortz. A new mathematical formulation for strapdown inertial navigation. IEEE Transactions on Aerospace and Electronic Systems, AES-7(1):61{66,
January 1971.
[73] R. E. Mortensen. Strapdown guidance error analysis. IEEE Transactions on
Aerospace and Electronic Systems, AES-10(4):451{457, July 1974.
[74] James C. Wilcox. A new algorithm for strapped-down inertial navigation. IEEE
Transactions on Aerospace and Electronic Systems, AES-3(5):796{802, September
1967.
BIBLIOGRAPHY
176
[75] Yeon Fui Jiang and Yu Ping Lin. Error analysis of quaternion transformations.
IEEE Transactions on Aerospace and Electronic Systems, 27(4):634{638, July
1991.
[76] Francois Martel, Parimal K. Pal, Mark L. Psiaki. Three-axis attitude determination via Kalman Filtering of magnetometer data. NASA Technical Reports
N89-15951, 1989.
[77] Henderson D. M. Shuttle Program. Euler angles, quaternions, and transformation matrices working relationships. NASA Technical Reports NASA-TM-74839,
McDonnell Douglas Tech. Services Co., Inc., 1977.
[78] Paul G. Savage. Strapdown system algorithms. NASA Technical Reports
19840017626 N (84N25694), Strapdown Associates, Inc., 1984.
[79] J. L. Michelin and P. Masson. Strapdown inertial system for tactical missiles using
mass unbalanced two-axis rate gyros. NASA Technical Reports AD-P003 623.
[80] Edward Pervin and Jon A. Webb. Quaternions in computer vision and robotics.
CMU Report CMU-CS-82-150, Carnegie Mellon University, 1982.
[81] Dohyoung Chung, Jang Gyu Lee, Chan Gook Park and Heung Won Park. Equivalence of the quaternion error model for strapdown INS. Proceedings of Intelligent
Autonomous Control in Aerospace Conference. Beijing, PRC, pages 179{183, 1995.
[82] Arieh Iserles. A First Course in the Numerical Analysis. Cambridge University
Press, 1996.
[83] Anh Tuan Le. Modelling and Control of Tracked Vehicles. PhD thesis, The University of Sydney, 1999.
[84] Blaha R. J. and Gilmore J. P. Strapdown system performance optimization test
evaluation. NASA Technical Reports NASA-CR-135484, MIT, 1973.
BIBLIOGRAPHY
177
[85] Peter A. Grundy. Error source recovery INS test ight data. NASA Technical
Reports AD-A012 359, Intermetrics, Incorporated, 1975.
[86] Ebner R. Brown A. and Mark J. A calibration technique for a laser gyro strapdown
inertial navigation system. Proceedings of DGON symposium on Gyro technology,
Stuttgart, Germany, 1982.
[87] T. G. Smithson. A review of the mechanical design and development of a high
performance accelerometer. Proceedings of Institute of Mechanical Engineering
Conference on Mechanical Technology of Inertial Sensors, pages C49{87, 1987.
[88] The Institute of Electrical and Electronics Engineers, Inc. IEEE Standard Inertial
Sensor Terminology. (IEEE Std 528-1984).
[89] The Institute of Electrical and Electronics Engineers, Inc. IEEE Specication
Format for Single-Degree-of-Freedom Spring-Restrained Rate Gyros [Description].
(IEEE Std 292-1969).
[90] The Institute of Electrical and Electronics Engineers, Inc. IEEE Standard Specication Format Guide and Test Procedures for Linear, Single-Axis, Pendulous,
Analog Torque Balance Accelerometer [Description]. (IEEE Std 337-1972).
[91] The Institute of Electrical and Electronics Engineers, Inc. IEEE Standard Specication Format Guide and Test Procedure for Single-Degree-of-Freedom RateIntegrating Gyro [Description]. (IEEE Std 517-1974).
[92] The Institute of Electrical and Electronics Engineers, Inc. IEEE Supplement for
Strapdown Applications to IEEE Standard Specication Format Guide and Test
Procedure for Single-Degree-of-Freedom Rate-Integrating Gyros. (IEEE Std 5291980).
[93] The Institute of Electrical and Electronics Engineers, Inc. IEEE Standard Specication Format Guide and Test Procedure for Linear, Single-Axis, Digital, TorqueBalance Accelerometer [Description]. (IEEE Std 530-1978, R1992).
BIBLIOGRAPHY
178
[94] The Institute of Electrical and Electronics Engineers, Inc. IEEE Standard Specication Format Guide and Test Procedure for Single-Axis Laser Gyros[Description].
(IEEE Std 647-1995).
[95] The Institute of Electrical and Electronics Engineers, Inc. IEEE Recommended
Practice for Precision Centrifuge Testing of Linear Accelerometers [Description].
(IEEE Std 836-1991 (R1997)).
[96] Watson Industries INC. Inertial measurement unit owner's manual, 1995.

Inertial Navigation System Algorithms For Low Cost Imu

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Inertial Navigation System Algorithms For Low Cost Imu

Uploaded by

Copyright:

Available Formats

Inertial Navigation System Algorithms

for Low Cost IMU

Department of Mechanical and Mechatronic Engineering

c 2000 Xiaoying Kong

The Objectives of this Thesis . . . . . . . . . . . .

2 INS Error Propagation Models

3 GPS Modelling in Frequency Domain

4 INS Algorithm for Low Cost IMU in Psi Angle Approach

5 INS Algorithm for Low Cost IMU in Quaternion Approach

5.3.5 The Discrete-Time Filter . . . . . . . . . . . . . . .

7 Summary and Conclusions

Computer frame, platform frame and three psi angles.

Satellites, receiver and ranges. . . . . . . . . . . . . . . . . . . . . . . . .

4.1 Resolution of Watson IMU-1. . . . . . . . . . . . . . . . . . . . . . . . .

Position output from Ashtech Sensor II. . . . . . . . . . . . . . . . . . .

Time domain performance in axis z when three parameters are changed.

 Designing and tuning of the lter algorithms for such INS .

1.2 Common Coordinate Systems Used in Navigation

1.2 Common Coordinate Systems Used in Navigation

right handed axis set.

1.3 INS Error Models

1.3 INS Error Models

1.3 INS Error Models

Figure 1.1: Inertial Navigation System

1.3 INS Error Models

uni ed in reference [3].

1.4 Integrated INS Algorithms Using Error Models

1.4 Integrated INS Algorithms Using Error Models

1.5 Main Contributions of this Thesis

1.5 Main Contributions of this Thesis

1.6 The Structure of this Thesis

These contributions are veri ed by a set of experimental implementations using a

1.6 The Structure of this Thesis

1.6 The Structure of this Thesis

feedforward and feedback lters.

1.6 The Structure of this Thesis

INS Error Propagation Models

2.2 INS Error Models Review

2.2 INS Error Models Review

2.2 INS Error Models Review

attitude di erence. All of these versions are equivalent [6, 7].

2.2.2 INS Error Models for Azimuth Uncertainty

2.2 INS Error Models Review

the use of external heading and tilt sensors.

2.2 INS Error Models Review

2.2.3 Quaternion Error Models Review

2.2 INS Error Models Review

2.2.4 Psi Angle Approach Review

2.2 INS Error Models Review

Velocity Error Model

V_tc = ftc + gtc ; (2

2.2 INS Error Models Review

V^_ = ftp + rp + g^c ; (2

The gravity vector error is given by:

Position Error Model

2.2 INS Error Models Review

Figure 2.2: True frame and platform frame.

Attitude Error Model - Psi Angle Model

2.2.5 Phi Angle Approach

2.2 INS Error Models Review

Velocity Error Model

V_tt = ftt + gtt ; (2

V^_ = (ftp + rp) + gb ; (2

Designing and tuning of the lter algorithms for such INS .

unied in reference [3].

These contributions are veried by a set of experimental implementations using a

attitude dierence. All of these versions are equivalent [6, 7].

The skew symmetric matrix of the phi angle is:

!tpp = _ = ;!ipp + ! + "p

Consequently (2.51) can be simplied as:

!cpp + p ; !icc + !icp = 0

!cpp + p ; !icc + !icp = !cpp + p ; (I33 ; Ccp)!icc

!cpp = (I33 ; Ccp)!icc ; p

_ = (I33 ; Ccp )!icc ; p