Inertial Navigation System: Department of Control Engineering 10th Semester Aalborg University 2008

Inertial Navigation System
ie
i
y
, e
y
N, g
y
E, g
x
U, g
z
Equator
l
L
e
z
i
z
i
x
e
x
Department of Control Engineering
10th Semester
Aalborg University 2008
A A L B O R G U N I V E R S I T Y
Master project
Section of Automation and Control
Department of Electronic Systems
Faculty of Engineering, Science and Medicine
Fredrik Bajers Vej 7 C3
DK-9220 Aalborg st
Denmark
Telephone +45 96 35 87 02
Fax +45 98 15 17 39
http://www.control.aau.dk/
Title:
Theme:
Estimation and nonlinear control
Period:
Fall-Spring Semester 2007-2008
Project group:
08gr1030a
Group members:
Rolf Christensen
Nikolaj Fogh
Supervisor:
Anders la Cour-Harbo
Morten Bisgaard
Copies: 5
Pages: 122
Attachments: None
Finished: 4
th
of June 2008
Synopsis:
This thesis documents the development of algorithms
for an inertial navigation system (INS). In general,
inertial navigation consists of three parts: Alignment,
navigation and aiding. Algorithms for these parts are
derived in this thesis.
In order to create reproducable test data for veri-
cation and analysis of the algorithms, an earth simu-
lation model is developed. The algorithms were also
tested against a reference INS, a Kearfott T16 INS,
to verify that the algorithms also work on real-world
data with real noise sources.
The navigation equations needed for an INS for ter-
restrial navigation are developed and tested against
the reference INS. Tests showed that the developed
navigation equations were able to follow the reference
trajectory to a satisfactory degree, thus validating the
navigation equations.
The alignment algorithms make use of an indirect
Kalman lter, which estimates the errors in the INS
states. For this, an error model is developed, which
describes the propagation of errors within the INS.
An alignment algorithm is developed to estimate tilt
and heading of the system. The alignment algorithm
was veried against data from the reference system,
and was shown to be able to estimate tilt and heading
to a satisfactory degree.
Algorithms for aiding the INS navigation equations
were developed for a speedometer and a GPS. These
algorithms also use the indirect Kalman lter and the
error model. Using data from the reference system
together with GPS measurements, it was shown that
the developed algorithms were able to aid the naviga-
tion equations and produce better results than either
of the systems in a standalone setup. It was also
shown that the INS was able to track position during
GPS outages.
A A L B O R G U N I V E R S I T Y
8. Semester
Sektion for Automation og kontrol
Institut for Elektroniske Systemer
Det Ingenir-, Natur-, og Sundheds-
videnskabelige Fakultet
Fredrik Bajers Vej 7 C3
DK-9220 Aalborg st
Danmark
Telefon +45 96 35 87 02
Fax +45 98 15 17 39
http://www.control.aau.dk/
Titel:
Tema:
Estimation and nonlinear control
Periode:
Efterr-Forr 2007-2008
Projektgruppe:
08gr1030a
Gruppemedlemmer:
Rolf Christensen
Nikolaj Fogh
Vejleder:
Anders la Cour-Harbo
Morten Bisgaard
Kopier: 5
Sider: 122
Bilag: Ingen
Frdiggjort: 4. Juni 2008
Synopsis:
Denne afhandling dokumenterer udviklingen af de al-
goritmer der bruges i inertial navigation systemer
(INS). Generelt bestr inertial navigation af tre dele:
Alignment, navigering og aiding. Denne afhandling
indeholder algoritmer til alle disse dele.
En simuleringsmodel af jorden er blevet lavet for at
kunne lave reproducerbare test data til verikation og
analyse of de udviklede algoritmer. For at vericere
at algoritmerne ogs fungerer med data fra virkelighe-
den, er algoritmerne ogs blevet testet med et refer-
encesystem, en Kearfott T16 INS.
De navigeringsligninger som er ndvendige for nav-
igering i nrheden af jorden er blevet udviklet og
testet med referencesystemet. Testene viste at de ud-
viklede navigeringsligninger var i stand til at flge
referencesystemet til en tilfredsstillende grad, hvilket
gr at navigeringsligningerne er vericerede.
En alignment algoritme bruger et indirect Kalman l-
ter som estimerer fejlene i INS tilstandene. Derfor er
der blevet udviklet en fejlmodel, som beskriver udbre-
delsen af fejl i INS systemet. En alignment algoritme
er blevet udviklet som estimerer hldning og retning
af systemet. Alignment algoritmen er blevet veri-
ceret op imod data fra referencesystemet, og det er
blevet vist at den var i stand til at estimere hldning
og retning tilfredsstillende.
Aidingalgoritmer til navigeringsligningerne er blevet
udviklet for en hastighedsmler og en GPS. Disse al-
goritmer bruger ogs et indirect Kalman lter og en
fejlmodel. Ved brug af data fra referencesystemet
samt simulerede GPS mlinger blev det vist at de ud-
viklede algoritmer kunne aide navigeringsligningerne
og give bedre resultater end nogle af systemerne alene.
Det blev ogs vist at INS systemet var i stand til
at flge referencesystemet under manglende GPS sig-
naler.
Preface
This thesis is written by group 08gr1030a and documents a project concerning the
implementation of an Inertial Navigation System. The project was proposed by the
group themselves.
The thesis and project is made at the Section of Automation and Control under
the Deportment of Electronic Systems at Aalborg University, and is composed during
the period from the 3
th
of September 2007 to the 4
th
of June 2008 under the theme
Estimation and Nonlinear Control.
For a complete understanding of the report, a technical and scientic level corre-
sponding to that of 9
th
semester students at the Section of Automation and Control is
required.
Literature references are written according to the Harvard-method: [Last name of
author, year of publication]. If a literary work has more than two authors, the rst
authors last name is written followed by et al.. A complete bibliography is found at
page 121.
Throughout the project Matlab has been used for data processing and algorithm
implementation. The version of Matlab used is 7.4.0.287 (R2007a).
The report is copyrighted by the members of the group and Aalborg University.
Aalborg University, 31st of May, 2007
Rolf Christensen Nikolaj Fogh
Contents
I Introduction 2
1 Introduction 4
2 Nomenclature and Mathematical Techniques 7
3 Frames of Reference and Position Determination 12
4 Principle of a Strapdown Inertial Navigation System 17
II Inertial Navigation Systems 22
5 Introduction 24
6 Geodesy 26
6.1 Geodetic Variables and Constants . . . . . . . . . . . . . . . . . . . . 26
6.2 Gravity Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
7 Equations of Motion 29
8 Earth Simulation Model 32
8.1 Accelerometer Output . . . . . . . . . . . . . . . . . . . . . . . . . . 32
8.2 Gyro Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
9 Navigation Equations 35
9.1 General Navigation Equations . . . . . . . . . . . . . . . . . . . . . . 35
9.2 Vertical Channel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
9.3 Navigation Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
9.4 Discrete Integration Algorithms . . . . . . . . . . . . . . . . . . . . . 39
9.5 Validation of Navigation Algorithm . . . . . . . . . . . . . . . . . . . 40
9.6 Analysis of Long Term Error Propagation . . . . . . . . . . . . . . . 41
10 Error Estimation Using Indirect Kalman Filtering 49
10.1 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
10.2 Motivating Example . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
10.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
11 Error Equations 55
11.1 Derivation of Error Equations . . . . . . . . . . . . . . . . . . . . . . 55
11.2 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
12 Alignment 64
12.1 Comparison between Gimballed and Strapdown Alignment . . . . . . 65
12.2 Coarse Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
12.3 Fine Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
12.4 Indirect Kalman Filter Applied to Quasi-Stationary Fine Alignment 70
12.5 Verication of Alignment . . . . . . . . . . . . . . . . . . . . . . . . 75
12.6 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
8
12.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
13 Inertial Navigation with Aiding 86
13.1 Aiding with GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
13.2 GPS Error Model Analysis . . . . . . . . . . . . . . . . . . . . . . . . 90
13.3 Implementation of Navigation with Aiding . . . . . . . . . . . . . . . 94
13.4 Verication of Navigation with Aiding . . . . . . . . . . . . . . . . . 97
13.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
14 Inertial Sensors 102
14.1 Optical Angular Rate Sensors . . . . . . . . . . . . . . . . . . . . . . 102
14.2 Accelerometers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
14.3 Sensor Error Models . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
15 Calibration of Sensors 106
15.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
15.2 Calibration Equations . . . . . . . . . . . . . . . . . . . . . . . . . . 106
15.3 Smoothing Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
15.4 Calibration Example . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
16 Summary 113
III Thesis Summary 116
17 Conclusion 118
18 Improvements and Discussion 120
Bibliography 122
Part I
Introduction
Chapter 1
Introduction
Why INS
No matter how far we go back in time it has always been critical to know where one
is positioned. It being a neanderthal going hunting whom should be able to nd back
home to the camp or a cruise missile which should hit its target with high precision.
Through time several dierent approaches has been used to locate and navigate to a
destination. Using the stars, the sun or special marks in the landscape. Nowadays GPS
is the most widespread navigation system but in certain applications GPS is not a viable
solution. In certain areas, e.g. in mountainous surroundings, the GPS signal is either
not present or very inaccurate because of multipath interference. Likewise if the vehicle
is located under water or under ground the GPS signal is not present. Furthermore,
navigation using GPS is dependent on the satellites governed by the USA which might
not always be accessible during a war situation. Lastly the precision using GPS may be
very poor, and not suitable for precision navigation needed by e.g. a missile or a war
ship.
An INS is a navigation system which depends entirely on inertial measurements for nav-
igation. An INS consists of accelerometers which measure the translatory acceleration
and gyroscopes which measure the angular rotation of the system. This sensor array is
called an Inertial Measurement Unit (IMU). Using the measurements from the IMU, the
INS can calculate the current attitude, velocity and position of the system starting from
some known initial point. This means that INS does not depend on any third party
applications, like GPS, and thus will always work regardless of external inuences. This
makes it suitable for navigation where GPS signal is not present or if the signals are
being jammed. When relying only on inertial measurements, however, the accuracy
of the INS degrades with time due to measurement inaccuracies. But used in a short
time span, INS gives a much higher precision than GPS. This makes INS very suitable
for missiles where precision is crucial, but which are only airborne for a few seconds.
High precision INS are also suitable for submarines where high precision is needed and
external positioning aids are not readily available.
4
5 CHAPTER 1. INTRODUCTION
Historical Development of INS
The rst applications using inertial measurements for navigation purposes appeared at
the end of the 19th century. They were simple gyro compasses were able to determine the
direction of true north. Under WW2 the development of the gimballed INS was rened,
and the Germans used a gimballed INS to navigate their V2 rockets. In gimballed INS,
the IMU is isolated from vehicle rotations by the use of a gimbal. Further development
of the gyros lead to even more precise INS during the 1950s. Until the 1970s only the
gimbaled systems had been investigated but in the late 1970s the development of the
strapdown INS (SINS) began. In a SINS, the sensors are rigidly mounted to the body
of the vehicle, hence the name strapdown. The development of the SINS is primarily
due to the introduction of the Ring Laser Gyro (RLG) in the 1960s and the Fibre Optic
Gyro (FOG) in the 1970s. These gyros eventually enabled strapdown INS to obtain a
degree of accuracy comparable to low-end gimballed systems but with a lower price tag.
This made INS solutions applicable to military aircraft and the rst commercial aircraft
Boeing 757. The advantages of a non-mechanic system with low price and low weight
was the source of this development. The lack of computer processing power postponed
the introduction of SINS system until the 1980s. The gimbaled system still achieved
better precision but the SINS had an precision which made it applicable in lower-cost
applications.
Strapdown INS
The strapdown implementation oers numerous advantages over its gimballed counter-
part, and has made it the preferred type of inertial navigation systems today. The
principal advantage is apparent from the name strapdown. By disposing of bulky,
expensive and error-prone moving mechanical parts, the reliability of the system is in-
creased and the cost and size is reduced. With the invention and maturing of the laser
and optical gyro, SINS have even obtained an accuracy which at a much lower price tag
rivals high-end gimballed systems.
The problem with strapdown inertial navigation systems is, that the sensor assembly
is not mechanically isolated from the rotations of the body, and thus, the gyros must
be able to capture these rotations. This requires the strapdown gyros to have a much
higher dynamic range than the gyros employed in gimballed systems. The RLG and
the FOG both have both ne accuracy and a large dynamic range, making them ideally
suitable for strapdown INS.
Scope of This Project
This project seeks to implement algorithms for a low-cost FOG based INS. The main
part of this task is to derive the navigation algorithms which translate the IMU mea-
surements into attitude, velocity and position.
A general sensor model will be derived, and the properties of the selected sensors will
be determined using system identication.
Inserting the parameters in the developed algorithms will yield the nal software for
the INS.
6
Our Contributions
The authors have found it dicult to nd literature about INS which are both com-
prehensive and easily understandable. This thesis seeks to provide algorithms for all
the steps necessary to implement an INS, while still keeping the derivations relatively
simple. In particular, the application of the indirect Kalman lter to the alignment and
aiding problem will be explained, a topic which is missing in most literature.
To the authors knowledge, there have been conducted no analysis of the errors arising
from assuming the system to be quasi-stationary during the ne alignment process.
This thesis includes results from a simulation, where the quasi-stationary requirement
are violated, and an analysis of the errors that arise from these violations.
Thesis Structure
The thesis structure is as follows:
Part I gives an introduction to the subject and denes the goals and scope of the
project. Additionally, some of the mathematical techniques used throughout the
report will be introduced.
Part II derives the equations needed for the inertial navigation algorithms. First, the
subject of geodesy is introduced. Geodesy deals with the parameters concerning
the earth, and is a key aspect of inertial navigation, as parameters such as the
oblateness of the earth and gravity has major inuences on INS. Thereafter, the
equations of motion are derived, which describes the speed of a vehicle moving
relative to earth. These equations, along with the geodetic variables, are used by
the navigation equations to compute the position and velocity of a vehicle using
measured angular velocity and translatory acceleration. The following chapters
shows how the eects of measurement uncertainties aect the system, and an error
model is derived. This model is used during the initial alignment phase, which
will estimate the initial attitude, azimuth and position of the system. In the next
chapter inertial navigation with aiding is described. The error equations are used
to fuse the INS with an external aiding device. An indirect Kalman lter is
used both in alignment mode and to fuse the INS with an aiding device in an
optimal way. The last two chapters models the sensors used in this project and
the determines the errors they suer from.
Part III will include a system test, which veries the system against a reference system.
Lastly, a conclusion and discussion will review the results obtained in the system
test.
Chapter 2
Nomenclature and Mathematical
Techniques
The purpose of this chapter is to dene the notations used throughout this report and
some of the fundamental mathematical techniques which is essential in the derivation
of some of the equations.
Scalars are noted with non-bold letters, vectors as lower-case bold letters and matrices
as capital bold letters
v = scalar
r = vector
= matrix
Coordinate Frames and Transformations
To describe the kinematics of the IMU a set of frames and notations must be agreed
upon. All frames used in this report will span a three dimensional Euclidean space,
consisting of three orthogonal basis vectors. The basis vectors of the b frame will be
denoted by b
x
, b
y
and b
z
, with x, y and z being the principal axes of the frame. A
vector r referenced in the b frame will be denoted r
b
. The subscript notion will be used
to extract the x, y and z components of the vector.
r
b
=
_
r
b
x
r
b
y
r
b
z
T
(2.1)
It is often needed to represent a vector in dierent coordinate frames. One coordinate
frame is created from another by a series of rotations around the principal axes of the
frame. Such a rotation around the y axis is shown in Fig. 2.1. In the gure, there are
two coordinate frames: The i coordinate frame, spanned by i
x
, i
y
and i
z
, and the j
coordinate frame spanned by j
x
, j
y
and j
z
. The j coordinate frame has been rotated
relative to the i frame through the i
z
axis by an angle . The r
i
z
, r
j
z
, i
z
, j
z
vectors are
coincident and invisible, as they protrude out through the page. Using the nomenclature
7
8
described above one can derive the components of the r vector in the j frame as the
components of the r vector in the i frame, and the angle . The components of a vector
r in the i frame is found in the gure as the bold lines r
i
x
, r
i
y
, r
i
z
, and in the j frame as
the bold lines r
j
x
, r
j
x
, r
j
x
. Figure 2.1 can now be used to nd the equations transforming
the vector r from the i frame to the j frame.
i
y
r
j
x
r
i
y
r
i
x
r
j
x
r
j
y
r
j
x,1
r
i
z
, r
j
z
, i
z
, j
z
i
x
j
y
Figure 2.1: Rotation around the positive y axis
+ r
j
y
= r
i
y
cos()
= r
i
x
sin()
r
j
y
= r
i
y
cos() r
i
x
sin()
r
j
x,1
= r
i
x
cos()
= r
i
y
sin()
r
j
x
= r
j
x,1
+ = r
i
x
cos() + r
i
y
sin()
r
j
z
= r
i
z
It is customary to write these equations in matrix form as
_
_
r
j
x
r
j
y
r
j
z
_
_
=
_
_
cos() sin() 0
sin() cos() 0
0 0 1
_
_
_
_
r
i
x
r
i
y
r
i
z
_
_
r
j
= C
j
i
r
i
Here, C
j
i
is the rotation matrix transforming r from frame i to frame j. The rotation
matrix is also called the Direction Cosine Matrix (DCM).
The DCMs corresponding to rotations around all principal axis are stated here for
completeness
9 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES
Rotation around X axis
_
_
1 0 0
0 cos() sin()
0 sin() cos()
_
_
(2.2)
Rotation around Y axis
_
_
cos() 0 sin()
0 1 0
sin() 0 cos()
_
_
(2.3)
Rotation around Z axis
_
_
cos() sin() 0
sin() cos() 0
0 0 1
_
_
(2.4)
The rotation matrix from arbitrary frame i to arbitrary frame j is constructed by
multiplying the rotation around z with rotation around y and with rotation around
x and results in the following rotation matrix:
C
j
i
=
_
cos() cos() sin() cos()+cos() sin() sin() sin() sin()cos() sin() cos()
sin() cos() cos() cos()sin() sin() sin() cos() sin()
sin() sin() cos() cos() cos()
_
(2.5)
It is important to note, that the order in which the rotations around the principal are
applied are not arbitrary. An X-Y-Z rotation is not in general the same as an Y-X-Z
rotation.
As a general rule when a DCM transforms from one coordinate system which is or-
thogonal to another coordinate system which is also orthogonal the following property
exist:
C
j
i
= (C
i
j
)
T
Similarity Transformations
If an equation is wished to be expressed in another coordinate system this can be
achieved using the DCM. If the following equation which is expressed in frame i is
wished expressed in frame j this can be achieved as follows:
Ar
i
= v
i
C
j
i
AC
i
j
r
j
= v
j
Matrix A is said to be transformed under a similarity transformation.
Relative Angular Velocity of Frames
When describing the relative angular velocity of two frames, it is necessary to describe
both which frames are involved and in which coordinate system the angular velocity is
expressed. If e.g. the angular velocity of frame j relative to frame i is expressed in the
jth coordinate system this is written as:
j
ij
=
_

j
ij,x

j
ij,y

j
ij,z
_
T
10
Computed and Measured Quantities
In order to distinguish between actual measurements from sensors and computed quan-
tities used in the computer, they are denoted dierently. A physical vector measured
by a sensor is denoted with a tilde and a computed quantity is denoted by a hat:

j
ij
= measured angular velocity

k
ik
= computed angular velocity
Skew Symmetrix Matrices
Throughout the thesis, it is sometimes convenient to change between a vector rep-
resentation and a matrix representation. In doing so, the skew symmetric matrix is
introduced. Using the skew symmetric matrix, the cross product of two vectors reduces
to doing a matrix multiplication:
a b = Ab (2.6)
where A is the skew symmetric matrix form of a. The elements in the skew symmetric
matrix is as follows:
a =
_
a
1
a
2
a
3
T
A =
_
_
0 a
3
a
2
a
3
0 a
1
a
2
a
1
0
_
_
(2.7)
Time Derivative of the Direction Cosine Matrix
At time t, the relation of frame i and j is expressed by the DCM C
j
i
(t). During the
time t, the i frame is rotated to a new orientation so the DCM is now C
j
i
(t + t).
By denition, the time derivative of a DCM is:
C
j
i
= lim
t0
C
j
i
t
= lim
t0
C
j
i
(t + t) C
j
i
(t)
t
(2.8)
Using a geometrical interpretation the small angle rotation C
j
i
(t + t) is the same as
C
j
i
(I+
i
) where
i
is the skew symmetric matrix related to the small angle rotation
from of C
j
i
from time t to time t + t. The skew symmetric form is due to the small
angle simplications applied to the cosine and sine parts of the DCM from (2.5). The
result is:
i
=
_
_
0
0
_
_
(2.9)
where
is the small angles that the i frame has been rotated through.
Substituting (2.9) in (2.8) yields:
C
j
i
= C
j
i
lim
t0
i
t
(2.10)
11 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES
Inspecting the part

i
t
when t 0 reveals that it is the same as the angular velocity
of the ith frame with respect to the jth frame and this is denoted as
i
ji
. This vector
can be expressed as a skew symmetric matrix:
i
ji

i
ji
(2.11)
where
i
ji
is a skew symmetric version of
i
ji
of the form from (2.7).
These two rotational representations will be used interchangably throughout the report.
The frame of reference can be changed by transforming the matrix under the similarity
transform
i
ij
= C
i
j
j
ij
C
j
i
Using this property, (2.10) can be written as:
C
j
i
= C
j
i
i
ji
=
j
ij
C
j
i
(2.12)
Chapter 3
Frames of Reference and Position
Determination
Before explaining the principles of INS, it is important to dene the coordinate frames
used throughout this report as they are essential to the derivation of the INS equations.
An INS uses accelerometers and gyros that register known physical phenomena caused
by the acceleration or rotation of an Inertial Measurement Unit (IMU). An IMU in this
thesis dened as a collection of three gyroscopes and three accelerometers arranged as
an orthogonal triad.
In order to model the output of the sensors, an inertial frame is needed as a reference
frame, i.e. a frame in which there are no forces acting. Ideally, the inertial frame should
include all forces exerted on the IMU, but in practice this is impossible, as this would
require knowledge about the entire universe.
Fortunately, it can be shown, that as the resolution of most inertial sensors today
are limited, an adequate precision of the inertial frame for use in terrestrial inertial
navigation systems is one that includes the rotation of the earth but not the revolving
of the earth around the sun [Britting, 1971]. Also the gravitational pull of other celestial
bodies can be disregarded, as they are an order of 10
7
[g] or less.
Earth-Centered Inertial Frame
In INS applications, the inertial frame i is chosen as an earth-centered inertial frame
(ECIF) which has origin at the center of the earth, and one axis parallel to the direction
of earth rotation. The choice of elementary vector representing this axis (i
x
, i
y
or i
z
) is
a matter of preference, and in the literature, dierent ones are used. In this thesis, the
i
y
vector is chosen, with the i
z
vector going through the equator at the longitude where
navigation was started, while the i
x
completes the right-hand coordinate system. The
frame is seen in Fig. 3.1.
In terrestrial navigation, one is primarily interested in knowing longitude, latitude,
height, north velocity, east velocity, azimuth and orientation. These could be obtained
using only the inertial frame. However, most inertial navigation systems today use an
additional set of frames.
12
13 CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION
ie
i
y
, e
y
N, g
y
E, g
x
U, g
z
Equator
l
L
e
z
i
z
i
x
e
x
Figure 3.1: The inertial frame is spanned by ix, iy and iz. The earth frame is the
inertial frame rotated around the y axis through the angle . The geodetic frame
is the earth frame rotated around the y axis through the longitude angle L, and
thereafter rotated negatively around the new x axis through the latitude angle l.
14
Earth-Centered, Earth-Fixed Frame
The earth-centered, earth-xed frame (ECEF) e is dened, to be rotating with the earth
at the earth spin rate (
e
). At time t = 0 the ECEF and ECIF are coincident, but as
time progresses, the ECEF frame is rotated at an angle through the e
y
axis, where
=
e
t. The ECEF frame is seen in Fig. 3.1. Using (2.3), the rotation matrix from
ECIF to ECEF is
C
e
i
=
_
_
cos() 0 sin()
0 1 0
sin() 0 cos()
_
_
(3.1)
Local Geodetic/Geographic Frame
The Local Geodetic/Geographic Frame g dened as a frame which has origin at the
position of the IMU, with one axis pointing north, one pointing vertically down or up,
with the last completing the right-hand coordinate frame (pointing east or west). As
two of the axes are horizontal, the local geodetic frame is always locally level. The
choice of geodetic frame does not inuence navigation performance and can be freely
chosen as long as the constraints just mentioned are met. A common geodetic frame,
which is also the geodetic frame in this thesis, is the East, North, Up frame (ENU),
which is produced by rotating the ECEF frame around the e
y
axis at a longitude angle
L, and then through a latitude angle l through the new e
x
axis. This frame is seen in
Fig. 3.1. With the geographic frame, the x, y and z axes are also called e, n and u axes
in the case of an ENU frame, so g
x
= g
e
, g
y
= g
n
and g
z
= g
u
. One other popular
formulation is the North-East-Down (NED) frame.
Navigation Frame
As with the local geodetic frame, the navigation frame (NF) n is dened as a frame
which has origin at the position of the sensor frame and has one principal axis pointing
vertically up or down. The dierence from the geodetic frame is that the navigation
frame can be rotated about the vertical axis. The choice of navigation frame is also
known as a mechanization.
In [Britting, 1971], an NED frame is chosen as a navigation frame. Using this mecha-
nization, one can simply integrate the acceleration resolved in the North and East axis
to obtain velocity in north, and east respectively. Height is obtained by integrating
acceleration in the down axis. However, using the geodetic frame as a navigation frame
introduces a singularity at the North pole, where North is undened, and the South
pole, where North has innitely many solutions. To overcome this limitation, current
INS typically uses a mechanization known as wander angle (WA) [Savage, 2000]. This
mechanization is constructed from the geodetic frame by rotating the geodetic frame
through the g
z
axis through an angle known as the wander angle. This rotation is
seen in Fig. 3.2.
As will be evident later in this thesis, the wander angle mechanization also simplies
the navigation equations which reduces the computational demand of the system. As
with the ENU frame, one can still integrate the accelerations to obtain velocities, but
they must be rotated through to obtain it in North and East components. Height
is still a double integration of acceleration in the n
z
. Using (2.2), (2.3) and (2.4), the
rotation matrix from the ECEF to the NF is
15 CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION
n
y
g
x
n
x
g
y
Figure 3.2: The wander angle is the angle which the navigation frame is rotated
around the z axis with respect to the geodetic frame.
C
n
e
=
_
cos(L) cos()sin(L) sin(l) sin() cos(l) sin() sin(L) cos()cos(L) sin(l) sin()
cos(L) sin()sin(L) sin(l) cos() cos() cos(l) sin(L) sin()cos(L) sin(l) cos()
cos(l) sin(L) sin(l) cos(L) cos(l)
_
(3.2)
And the rotation matrix from the geodetic frame to the navigation frame is
C
n
g
=
_
_
cos() sin() 0
sin() cos() 0
0 0 1
_
_
(3.3)
Body Frame
The body frame (BF) is xed to the casing of the IMU, and rotated with respect to the
navigation frame by a roll, pitch and yaw angle.
Sensor frame
The sensors are not always perfectly aligned mechanically with respect to the body
frame. The sensor frame is dened so that the xy plane is spanned by the x and y
accelerometer, with the x accelerometer corresponding to the x-axis. The sensor inputs
are rotated from the sensor frame to the body frame prior to being used by the navigation
equations. The rotation matrix from the sensor frame to the body frame is found by
the sensor calibration algorithm.
Position Tracking
An INS could keep track on position by keeping track of longitude, latitude and height
of the system. However, this suers from the same problems as the NED navigation
frame mechanization. At the poles, the latitude is 90
, but longitude is not uniquely de-

ned. Therefore, it is not possible to restore longitude information after a pole traversal,
and the navigation information is destroyed. Although one might argue that naviga-
tion involving pole traversal are not common, and thus the singularity is not an issue,
INS today uses either quaternions parameters or DCMs internally to keep track of po-
sition. Having a singularity also decreases navigation performance close to the poles
due to numerical problems because of nite precision within the navigation computer.
Historically, quaternions have been used because they required less processing power
during navigation as compared to DCMs. However, they tend to be unintuitive and
signicant calculations are needed for outputting the variables of interest (latitude, lon-
gitude, etc.). Today, the choice between DCMs and quaternions is more a matter of
16
preference[Savage, 1997]. In this thesis, the C
n
e
DCM is used to keep track of the po-
sition on earth. From (3.2), longitude, latitude and wander angle can be extracted by
isolating the L, l and a terms. Assigning the entries of the C
n
e
matrix so C
ij
corresponds
to the ith column and the jth row of the C
n
e
matrix as follows:
C
n
e
=
_
_
C
11
C
21
C
31
C
12
C
22
C
32
C
13
C
23
C
33
_
_
(3.4)
Yields
L = arctan
_
C
13
C
11
C
22
C
21
C
12
_
(3.5)
l = arcsin(C
23
) (3.6)
a = arctan
_
C
21
C
22
_
(3.7)
As seen, only the rst two columns of C
e
n
are needed to obtain the wanted information.
Omitting the calculation of the third column reduces the processing power needed for
the navigation equation.
Chapter 4
Principle of a Strapdown Inertial
Navigation System
In order to get an idea of the concepts and principles of inertial navigation systems, a
simple example is given in two dimensions. Many of the concepts in the two dimensional
case can be directly transferred into the three dimensional case.
When navigating on the earth, we would like to know our current position relative to
some other point on the earth surface. In many applications, one uses the notion of
longitude, latitude and height above the earth surface to indicate where one is positioned
relative to the earth.
First, consider a 2 dimensional Y-Z IMU as depicted in Fig. 4.1. Here, it is assumed
that the sensor frame and the body frame are coincident. This system is allowed to tilt
by an angle at a rate , as well as accelerate in the b
y
and b
z
direction.
a
b
z
Accelerometers Gyro
a
b
y
b
z
b
x
b
y
Figure 4.1: Inertial Measurement Unit measuring in two dimensions.
Being a 2D INS, our system is only able to navigate around the meridional (north-south)
plane. Also, assume that the earth is not rotating, so the inertial frame is equal to the
earth frame. This two dimensional system is depicted in Fig. 4.2. For simplicity, the
navigation frame is chosen as a ENU geodetic frame.
In this system, the variables of interest is the latitude l, the height h, and the tilt of
the system. By introducing the navigation frame, it can be seen from the gure that
the derivative of l (in radians) is equal to the velocity of the body to the north divided
by the radius of the earth plus the height of the system. The derivative of h is equal to
the velocity of the body in the vertical direction.
17
18
i
y
, e
y
l
h
R
0
n
n
b
y
n
u
b
z
i
z
, e
z
Figure 4.2: INS system around the meridian plane. The platform is tilted by an
angle with respect to the earth surface.
l =
v
n
n
R
0
+ h
h = v
n
u
(4.1)
To nd these we need to nd the velocities of the system in the navigation frame, v
n
n
and v
n
u
. These are obtained using the measured accelerations. As these measurements
are given in the body frame, these needs to be rotated into the navigation frame as
follows
a
n
n
= a
b
y
cos + a
b
z
sin()
a
n
u
= a
b
y
sin + a
b
z
sin() (4.2)
Where the tilt of the platform needs to be calculated. The derivative of is equal to
the angular rotation of the body with respect to the inertial frame minus the rotation
of the navigation frame with respect to the earth frame.
=
v
n
n
R
0
+ h
(4.3)
The v
n
n
/(R
0
+ h) term is also called the transport rate. It is the rate at which one
needs to tilt the platform in order to keep it level on the ground. At start of navigation,
the attitude is initialized at some initial value (0 if the system is level on the ground
at the start of navigation), and the above equation can then be used to calculate it at
any point from there on.
Having obtained the accelerations in the navigation frame, one can obtain the velocity
as an integration of the acceleration. The acceleration measured by the accelerometers
is comprised of both gravity and mass movement. Because the navigation frame is level
on earth, gravity only inuences the u axis
As the navigation is rotating with respect to the inertial frame, one must take into
account the accelerations arising from this rotation. This is also known as the coriolis
force. Recognizing that the rotation rate of the navigation frame with respect to the
19
CHAPTER 4. PRINCIPLE OF A STRAPDOWN INERTIAL NAVIGATION
SYSTEM
inertial frame is the transport rate of (4.3), the derivative of the velocity of the system
is:
v
n
n
= a
n
n
v
n
n
R
0
+ h
v
n
u
(4.4)
v
n
u
= a
n
u
g +
v
n
n
R
0
+ h
v
n
n
(4.5)
These are integrated to yield the velocity of the system. At start of navigation, the
system must be stationary so the initial velocities are 0.
The job of the navigation computer is to integrate (4.1), (4.3), (4.4) and (4.5) to yield
the latitude, height and tilt of the system.
The principles explained in this chapter are used in the next part to derive the navigation
equations for a full three dimensional case.
Part II
Inertial Navigation Systems
Chapter 5
Introduction
Inertial navigation consists of three major parts: Alignment, navigation and aiding.
During the alignment phase, the alignment algorithm estimates the initial value of the
body frame with respect to the navigation frame, that is, the roll, pitch and yaw of the
system. This is used by the navigation algorithm to transform the measurements from
the body frame to the navigation frame. By requiring the IMU to be stationary during
alignment, this can be done by measuring gravity and earth rotation.
When the alignment phase has obtained a suciently accurate estimate of the roll, pitch
and yaw, the navigation phase can begin. During navigation, a navigation algorithm
continuously calculate the roll, pitch and yaw from inputs from the gyroscopes, so ac-
celeration measurements can be resolved in the navigation frame and integrated to yield
velocity and subsequently positions with respect to the earth. Ultimately, this results
in the latitude, longitude, height, velocity and attitude outputs, which are the primary
variables of interest in inertial navigation systems. However, because of sensor errors
and the double integration from acceleration to position, inertial navigation systems
tend to drift exponentially from the true values.
This shortcoming can be overcome by aiding the INS with various external sensors which
can bound the errors propagating in the INS algorithms. An aiding algorithm uses
knowledge about the errors in the INS and the aiding device to fuse the information
from the two systems in an optimal way. A good example is GPS aiding, which can
bound the position and velocity of the fused INS/GPS system whilst keeping the
precision of the INS.
The sensors used in INS all have errors, some of which are reproducible and hence cor-
rectable. Before inertial sensors are incorporated into an INS, these errors are identied
by a calibration algorithm.
In this part, mathematical formulae needed by the above algorithms are derived.
The structure of the part is as follows:
As terrestrial navigation occurs in the vicinity of the earth, the geometric aspects of the
earth, and the magnitude of gravity have a large inuence on the INS. The important
aspects of this topic is introduced in the rst chapter (Ch. 6).
The next chapter (Ch. 7) derives the equation of motion of an INS which are used by
24
25 CHAPTER 5. INTRODUCTION
both an earth simulation model and the navigation equations. The earth simulation
model translates longitude, latitude, height and attitude inputs to accelerations and
angular rates that the accelerometers and gyros would experience during navigation.
The navigation equations reverses the process as seen in Fig. 5.1.
model
INS
Longitude, latitude
height and attitude
trajectories
Longitude, latitude
height and attitude
trajectories
Accelerations
Angular Rates
Simulation
Figure 5.1: Simulation and INS
The earth simulation model and navigation equations are developed in the two following
chapters, Ch. 8 and Ch. 9. The earth simulation model is derived in order to test the
developed algorithms under controlled conditions.
Before developing the alignment and aiding algorithms, chapter 10 gives an introduction
to indirect Kalman ltering. Indirect Kalman ltering uses an error model of the INS
which is derived in Ch. 11. Using this error model, indirect Kalman ltering equations
and the navigation equations, the alignment algorithm is derived in Ch. 12.
Two dierent aiding approaches is presented in Ch. 13. GPS aiding and speedometer
aiding.
The last two chapters of this part present a somewhat dierent part of this project than
the preceding chapters. When implementing an INS in the real world the performance
can be drastically increased by determining sensor errors and correcting them before the
navigation algorithm. So the last two chapters describes how to calibrate the sensors.
The sensor calibration equations are based on sensor error models which are presented
in Ch. 14. Chapter 15 shows how the sensor errors are identied using a test xture to
rotate the IMU through a series of rotations.
Chapter 6
Geodesy
In order to develop the equations of motion of a vehicle in the vicinity of the earth,
knowledge is needed about the geometric aspects of the earth as well as the gravitational
eld surrounding the earth. These values are known as geodetic variables and constants.
The most widely used model of the earth is the World Geodetic System (WGS-84), which
denes many of the constants that are used in INS, as well as a gravity model.
6.1 Geodetic Variables and Constants
The rotational rate of the earth have been well known for many years, and can be
expected to be constant
e
= 7292115.0 10
11
[ rad/s ] (6.1)
Expressed in earth coordinates
e
ie
=
_
0
e
0
(6.2)
The equatorial radius of the earth, also known as the semimajor axis radius
r
e
= 6378137 [ m] (6.3)
The polar radius of the earth, also known as the semiminor axis radius
r
p
= 6399592 [ m] (6.4)
The eccentricity of the earth is dened as
=
1
r
2
p
r
2
e
= 0.0818191908426 (6.5)
Two important geometric aspects of geodecy is the meridional and the normal radius
of curvature. These are dened as the radius of the circle one would travel in, if one
continued ones current curve.
The meridional radius of curvature is the radius of the circle one would follow if one
travelled directly north or south. This is depicted in Fig. 6.1. The normal radius of
26
27 CHAPTER 6. GEODESY
Surface
r
mer
North
Circle of curvature
Earth
Figure 6.1: Figure showing the dierence between the circle one would follow if one
traveled directly north compared to the earths curvature.
curvature is the radius of the circle if one travelled directly east or west. The equations
giving the radii is a function of geographic latitude.
Meridional (north-south) radius of curvature
r
mer
=
r
e
(1
2
)
(1
2
sin
2
(l))
3/2
(6.6)
Normal (east-west) radius of curvature
r
nor
=
r
e
_
(1
2
sin
2
(l))
(6.7)
6.2 Gravity Model
From [Britting, 1971], the gravitational magnitude can be calculated approximately
using the following formula.
g =

r
2
_
1
3
4
J
2
(1 3 cos(2l))
_
r
2
e
cos
2
(l) (6.8)
Where J
2
, and are empirical constants calculated by geodetic measurements and r is
the radius from earth surface to the point of navigation. As apparent gravity is always
pointing vertically, the gravity in the navigation frame is
g
n
=
_
_
0
0
r
2
_
1
3
4
J
2
(1 3 cos(2l))
_
r
2
e
cos
2
(l)
_
_
(6.9)
From [Savage, 1997], r is approximated as:
r = r
e
_
1 sin
2
(l)
_
+ h (6.10)
The geodetic values and gravity model described in this chapter is used throughout this
thesis. It is used both in the earth simulation model and in the navigation algorithm
as sucient precision is not possible assuming a spherical earth and constant gravity.
6.2. GRAVITY MODEL 28
Before developing the earth simulation model and navigation algorithms the equations
of motion need to be derived. These equations are derived in the following chapter and
describe the motion, of a vehicle in the vicinity of the earth, relative to the earth.
Chapter 7
Equations of Motion
As shown in the example in Ch. 4, an INS is based on integrating the acceleration
experienced by the IMU to obtain the velocity and ultimately the position of the INS
relative to earth.
In this chapter, this is done by deriving a dierential equation of the speed of a vehicle
relative to earth. In later chapters, this equation is integrated to yield velocity and
subsequently position of the INS.
Derivation of Equations of Motion
Acceleration is comprised of two terms: The acceleration due to mass movement and
the acceleration due to gravity. Due to the principle of equivalence, these cannot be
distinguished and are both measured by the accelerometers [Britting, 1971]. Accelera-
tion due to mass movement is the double derivative of the position vector in the inertial
frame. Rotating this into the navigation frame through C
n
i
yields the acceleration in
the navigation frame. The acceleration due to gravity is given in the navigation frame
as g
n
.
a
n
= C
n
i
r
i
+g
n
(7.1)
Where a
n
is the acceleration in the navigation frame.
As the output from an INS is velocity and position relative to earth, it is convenient
to derive this equation in terms of the geographically referred earth referenced velocity,
also known as the ground speed. This is the velocity of r
i
seen from the earth frame
transformed into the navigation frame, dened as follows
v
n
C
n
e
r
e
(7.2)
The result of this chapter is an expression relating v
n
to the accelerometer measurements
given in the navigation frame a
n
.
The dependence of r
e
on r
i
is obtained as follows
29
30
r
e
= C
e
i
r
i
(7.3)
r
e
= C
e
i
r
i
+

C
e
i
r
i
= C
e
i
r
i
+C
e
i
i
ei
r
i
= C
e
i
_
r
i
i
ie
r
i
_
(7.4)
Notice the sign change due to
i
ei
=
i
ie
. Inserting into (7.2) and dierentiating v
n
with respect to time yields
v
n
= C
n
i
r
i
C
n
i
i
ie
r
i
(7.5)
v
n
=

C
n
i
r
i
+ C
n
i
r
i

C
n
i
i
ie
r
i
C
n
i
i
ie
r
i
= C
n
i
_
i
ie
r
i
+ r
i
i
ni
i
ie
r
i
i
ie
r
i
_
= C
n
i
__
i
ni
i
ie
_
r
i
+ r
i
i
ni
i
ie
r
i
_
(7.6)
By isolating r
i
from (7.4) one obtains
r
i
= C
i
e
r
e
+
i
ie
r
i
(7.7)
Inserting this into (7.6) and rearranging the terms yields
v
n
= C
n
i
__
i
ni
i
ie
_ _
C
i
e
r
e
+
i
ie
r
i
_
+ r
i
i
ni
i
ie
r
i
_
= C
n
i
_
i
ni
C
i
e
r
e
+
i
ni
i
ie
r
i
i
ie
C
i
e
r
e
i
ie
i
ie
r
i
+ r
i
i
ni
i
ie
r
i
_
= C
n
i
__
i
ni
C
i
e
i
ie
C
i
e
_
r
e
i
ie
i
ie
r
i
+ r
i
_
(7.8)
Knowing that C
i
e
= C
i
n
C
n
e
and
i
ni
=
i
ne
i
ie
and inserting (7.2) yields the following
v
n
= C
n
i
_
r
i
i
en
+ 2
i
ie
_
C
i
n
v
n
i
ie
i
ie
r
i
_
(7.9)
By isolating the inertial acceleration r
i
and multiplying by C
n
i
one obtains the rst term
of (7.1)
C
n
i
r
i
= v
n
+C
n
i
_
i
en
+ 2
i
ie
_
C
i
n
v
n
+C
n
i
i
ie
i
ie
r
i
(7.10)
Inserting into (7.1) and rearranging yields the equation that was sought after
v
n
= a
n
(
n
en
+ 2
n
ie
) v
n
C
n
i
i
ie
i
ie
r
i
g
n
(7.11)
It is customary to include the centripetal term
i
ie
i
ie
r
i
in the equation of gravity,
resulting in a so called apparent or plumb bob gravity term:
g
n
g
n
+C
n
i
i
ie
i
ie
r
i
(7.12)
Rearranging and inserting the expression for g
n
into (7.11) yields the ground speed of
a vehicle as a function of accelerometer measurements, the rotation rate of earth, the
rotation of the navigation frame relative to earth and the apparent gravity:
31 CHAPTER 7. EQUATIONS OF MOTION
v
n
= a
n
(
n
en
+ 2
n
ie
) v
n
g
n
(7.13)
In the following chapters, this equation is used both to develop a simulation model, and
the INS navigation equations.
Chapter 8
Earth Simulation Model
In this chapter, an earth simulation model is derived using the equations of motion
derived in Ch. 7. This model enables one to create repeatable test data as well as
test the system under sub-optimal, but well-dened conditions such as erroneous sensor
outputs.
This chapter is divided into two sections. As two sensors are used, accelerometer and
gyroscope, the earth simulation model should output both the acceleration and rate
sensed by the sensors when the vehicle is moving.
8.1 Accelerometer Output
In this section the accelerometer output from the earth simulation model is derived.
Restating the equation of motion (7.13):
v
n
= a
n
(
n
en
+ 2
n
ie
) v
n
g
n
(8.1)
Rearranging and rotating yields the wanted output
a
b
= C
b
n
( v
n
+ (
n
en
+ 2
n
ie
) v
n
+g
n
) (8.2)
The plumb-bob gravity term g
n
was calculated in Ch. 6 as
g
n
=
_
_
0
0
r
2
_
1
3
4
J
2
(1 3 cos(2l))
_
r
2
e
cos
2
(l)
_
_
(8.3)
The
n
ie
term is obtained by rotating the rotation rate matrix in earth coordinates into
the navigation frame using the similarity transform
32
33 CHAPTER 8. EARTH SIMULATION MODEL
n
ie
= C
n
e
e
ie
C
e
n
(8.4)
n
ie
=
_
cos(l) sin()
e
cos(l) cos()
e
sin(l)
e
T
(8.5)
The wander angle (Fig.3.2) is initialized at 0 and follows the following equation[Rogers, 2007].
=

Lsin(l) (8.6)
The C
b
n
matrix is calculated using the body rotations
n
bn
supplied by the user
C
b
n
= C
b
n
n
bn
(8.7)
At start of simulation, the C
b
n
matrix is initialized by the user.
Left is to nd the v
n
and
n
en
terms.
First, the horizontal parts of v
n
is found. This is done by rst observing the horizontal
parts of the ground speed in the geodetic frame as a function of the horizontal angular
rates of the geodetic frame. By doing this, one can nd a relationship between the
horizontal angular rates and the horizontal velocity of the geographic frame. Figure 8.1
shows the relationship for the north velocity.
g
eg,e
h
v
g
n
r
mer
Figure 8.1: Relationship between north velocity and east angular rate of the geo-
graphic frame. The bold ellipse is the earth surface, the dashed line is the meridional
radius of curvature.
v
g
e
=
g
eg,n
(r
nor
+ h) (8.8)
v
g
n
=
g
eg,e
(r
mer
+ h) (8.9)
Where [Savage, 2000]
g
eg
=
_

l

Lcos(l)

Lsin(l)
T
(8.10)
As the navigation frame is locally level, the vertical term of v
g
is equal to the derivative
of the height h.
8.2. GYRO OUTPUT 34
v
g
u
=

h (8.11)
Hence,
v
g
=
_

L(r
nor
+ h) cos(l)

l(r
mer
+ h)

h
T
(8.12)
This is then rotated into the navigation frame through the wander angle using (3.3).
v
n
= C
n
g
v
g
=
_
_
L(r
nor
+ h) cos(l) cos() +

l(r
mer
+ h) sin()
L(r
nor
+ h) cos(l) sin() +

l(r
mer
+ h) cos()
h
_
_
(8.13)
The remaining term is
n
en
. In the wander azimuth conguration, the vertical part is
set to 0, and the horizontal terms are obtained by rotating the horizontal parts of the
geographic rotation rate through the wander angle.
n
en
= C
n
g
(
g
eg
+
g
gn
) (8.14)
With
n
gn
=
_
0 0
Lsin(l)
T
(8.15)
Yields
n
en
= C
n
g
_

l

Lcos(l) 0
T
=
_
_
l cos() +

Lcos(l) sin()
l sin() +

Lcos(l) cos()
0
_
_
(8.16)
To get the outputs from the accelerometers, (8.3), (8.5), (8.13) and (8.16) is inserted
into (8.2), and the rotation matrix C
b
n
is calculated using (8.7).
8.2 Gyro Output
The outputs from the gyros are the components of the
b
ib
matrix which is given as
b
ib
= C
b
n
(
n
ie
+
n
en
+
n
nb
) (8.17)
Which is calculated by inserting (8.5), (8.16) and the body rotations
n
nb
given by the
user.
To use this earth simulation model L, l and h need to be dierentiable. This can
cause problems, as longitude changes instantaneously at pole traversals. Hence, this
earth simulation model is limited to navigation away from the poles. Also, longitude is
generally dened as being within 180
to 180
, which will also cause a sudden jump.

This jump, however, can be eliminated, by removing the longitude constraint so it can
attain any value. Latitude is also dened as being within 90
to 90
, but it does not

exhibit this jump in value during navigation. Height is also not expected to have jumps.
One way of removing this problems from the earth simulation model is to used DCMs
instead of longitude/latitude. But it has been chosen to use longitude/latitude as it
is more intuitive for the user than inputting a DCM. Furthermore, it is assumed that
simulations will not include pole traversals.
Chapter 9
Navigation Equations
During this chapter the navigation equations needed for inertial navigation are derived.
The navigation equation continuously integrate accelerometer and gyro measurements
and output attitude, heading, velocity and position. Many of the expressions needed
have already been derived in the previous chapter.
The rst section derives the general navigation equations. These equations describe
how the velocity of the vehicle changes which can be integrated to give the position of
the INS. The next section describes the problem with the unstable vertical channel of
the INS and proposes a solution to stabilize this channel. The last section describes the
idea of dividing the integration of the attitude matrix into a fast and slow part. This
is used if the processor power is not sucient to integrate the whole part with the high
sample rate.
9.1 General Navigation Equations
In order to determine the velocity and position of the INS, this section seeks to derive
expressions for the terms in the equation of motion (9.1).
v
n
= C
n
b
a
b
(
n
en
+ 2
n
ie
) v
n
g
n
(9.1)
At start of navigation, the IMU is not moving relative to the earth surface, so v
n
is
initialized at 0.
The measured acceleration a
b
, gravity g
n
(8.3) and earth rotation
n
ie
(8.5) are all
known. The C
n
b
matrix is calculated from gyro inputs as follows:
C
n
b
= C
n
b
b
nb
= C
n
b
(
b
ib
C
b
n
(
n
en
+
n
ie
)C
n
b
) (9.2)
Where
b
ib
is the input from the gyros, and earth rotation is known. The remaining
term in both (9.1) and (9.2) is an expression for the transport rate
n
en
.
Apart from being used to calculate the derivative of the ground speed, the transport
rate is also used to calculate the C
n
e
matrix. As explained in Ch. 3, this matrix is used to
35
9.1. GENERAL NAVIGATION EQUATIONS 36
calculate latitude, longitude and wander angle of the system. At the start of navigation
the C
n
e
matrix is given initial values by the initial alignment algorithm. From there on
it follows the following equation
C
n
e
=
n
en
C
n
e
(9.3)
which is continuously integrated by the INS.
The remainder of this section deals with the derivation of the transport rate.
The transport rate matrix
n
en
is obtained from v
n
by rotating v
n
into the geographic
frame and using the relationship between ground speed and rotation rate in the geo-
graphic frame, and then rotating back to the navigation frame, so
v
g
= C
g
n
v
n
(9.4)
By rearranging (8.8) and (8.9), and inserting it into the two rst positions of (8.10), one
obtains
g
eg
=
_
v
g
n
(rmer+h)
v
g
e
(rnor+h)
Lsin(l)
_
T
(9.5)
This is used to calculate
n
en
using
n
en
= C
n
g
_
g
eg
+
g
gn
_
(9.6)
With
g
gn
=
_
0 0
Lsin(l)
T
(9.7)
Substituting (9.5) and (9.7) into (9.6) and isolating v
n
yields the relationship between
ground speed and transport rate:
n
en
=
_
_
sin() cos()(
1
rnor+h
1
rmer+h
)
sin
2
()
rnor+h
cos
2
()
rmer+h
cos
2
()
rnor+h
sin
2
()
rmer+h
sin() cos()(
1
rnor+h
1
rmer+h
) 0
0 0 0
_
_v
n
(9.8)
= Fv
n
(9.9)
Note that the z part of
n
en
is always zero because of the wander angle mechanization.
This reduces the computational demands of the system.
As can be seen, this expression contains the wander angle , which is not dened at
the poles. The rest of this section will rewrite F to use the entries in the C
n
e
matrix
instead, overcoming this limitation. This is done as follows:
Using the trigonometric identity sin
2
() + cos
2
() = 1 gives
F =
_
sin() cos()(
1
rnor+h
1
rmer+h
)
1
rnor+h
+(
1
rnor+h
1
rmer+h
) cos
2
() 0
1
rnor+h
(
1
rnor+h
1
rmer+h
) sin
2
() sin() cos()(
1
rnor+h
1
rmer+h
) 0
0 0 0
_
(9.10)
37 CHAPTER 9. NAVIGATION EQUATIONS
Using the entries in the C
n
e
matrix as done in Ch. 3, the sin() and cos() terms can
be denoted
sin() =
C
21
cos(l)
=
C
21
_
1 (C
23
)
2
cos() =
C
22
cos(l)
=
C
22
_
1 (C
23
)
2
(9.11)
Yielding the nal result, which can be used in navigation without the wander angle
constraint
F =
_
_
C
21
C
22
1(C
23
)
2
(
1
rnor+h
1
rmer+h
)
1
rnor+h
+(
1
rnor+h
1
rmer+h
)
(C
22
)
2
1(C
23
)
2
0
1
rnor+h
(
1
rnor+h
1
rmer+h
)
(C
21
)
2
1(C
23
)
2

C
21
C
22
1(C
23
)
2
(
1
rnor+h
1
rmer+h
) 0
0 0 0
_
_
(9.12)
9.2 Vertical Channel
The altitude (h) of the system can be obtained simply by noting that the Z axis of the
locally level navigation frame is by denition always pointing downwards, hence:
h = v
n
z
(9.13)
However, relying only on inertial measurements for calculation of the vertical channel
will render it exponentially unstable[Rogers, 2007]. This is because of the dependence
of g on h. As h increases, g decreases, yielding an increased upward acceleration. As a
general rule when navigation time exceed 10 minutes, external measurements must be
used to keep the vertical channel within acceptable values. [Savage, 2000]. A common
way of aiding the vertical channel is by implemented the following PID servo control
loop, which is depicted in Fig. 9.1. The purpose of the loop is to drive the error h
towards 0.
v
n
z,ins + + +
+
- - -
-
_
_ _
h
aid
h
h
v
n
z
c
1
c
3
c
2
v
n
z
Figure 9.1: Vertical channel control loop
h = v
n
z
c
1
(h h
aid
) (9.14)
v
n
z
= a
n
z
((
n
en
+ 2
n
ie
) v
n
)
n
z
g
n
z
c
2
(h h
aid
) c
3
_
(h h
aid
)dt (9.15)
9.2. VERTICAL CHANNEL 38
The PID gains c
1
, c
2
and c
3
are selected using classical servo control theory. To ana-
lyze the vertical channel control loop, the Laplace transform of the error forms of the
equations are derived as follows:
h = v
n
z
c
1
(h h
aid
) (9.16)
Assuming that the error of the aiding device is constant [Rogers, 2007], the second
derivative is:
h + c
1
(
h) = v
n
z
(9.17)
The v
n
z
term is obtained from (9.15) as follows: Assuming that earth rotation and
transport rate are negligibly small compared to the other terms, it simplies to
v
n
z
= a
n
z
+ g
n
z
c
2
(h h
aid
) c
3
_
(h h
aid
)dt (9.18)
The gravity model is approximated by an inverse square law as
g
n
z
(r)
_
g
0
r
2
0
r
2
_
(9.19)
where g
0
is the gravity at the surface of the earth, r
0
is the radius of the earth at the
point directly under the INS and r is the length from the center of the earth to the INS.
The error form is derived as
g
n
z
=
dg
n
z
(r)
dr
r =
2g
r
h (9.20)
where g is the gravity magnitude at the equator.
Assuming a
n
z
to be constant, dierentiating (9.18) yields
v
n
z

2g
r

h c
2
(
h h
aid
) c
3
(h h
aid
) (9.21)
which can be inserted into (9.17)
h + c
1
h +
_
c
2
2g
r
_
h + c
3
h = c
3
h
aid
(9.22)
Taking the Laplace transform
s
3
+ c
1
s
2
+
_
c
2
2g
r
_
s + c
3
= 0 (9.23)
With no control (c
1
= c
2
= c
3
= 0),
s
3
2g
r
= 0 (9.24)
which has the solutions 0 and
_
2g/r, yielding an unstable system because of the
positive root.
The aiding device is unable to capture fast changes in height, but have stable character-
istics, whereas the height measured by the INS are unstable, but includes fast changes.
The c
1
, c
2
and c
3
PID gains should be chosen so that the resulting h contains both the
stable characteristics of the aiding device, and the high dynamics of the height captured
by the INS. In order to obtain this, the PID gains are usually chosen as follows
s
3
+ 3s
2
+ 4
2
s + 2
3
= 0 (9.25)
Such that
c
1
= 3 c
2
= 4
2
+
2g
r
c
3
= 2
3
(9.26)
Where is usually chosen to be 0.01 [Rogers, 2007].
9.3 Navigation Output
The latitude, longitude and height can be extracted from the C
n
e
matrix as described
in Ch. 3:
L = arctan
_
C
13
C
11
C
22
C
21
C
12
_
(9.27)
l = arcsin(C
23
) (9.28)
a = arctan
_
C
21
C
22
_
(9.29)
The velocities in north and east can be expressed as
v
N
= v
n
y
cos() + v
n
x
sin() (9.30)
v
E
= v
n
x
cos() v
n
y
sin() (9.31)
9.4 Discrete Integration Algorithms
The navigation equations are integrated by discrete digital algorithms in the navigation
computer. However, the relatively high dynamics experienced during vehicle motion
causes some problems regarding the bandwidth of the system. The integration algo-
rithms must be executed at a rate fast enough to include all major dynamics of the
system. Conversely, the processing power needed to execute the relatively complex al-
gorithms must not exceed the available processing power. Earlier, systems either used
a rst order algorithm at a very high update rate (10 - 20 KHz), or a higher order algo-
rithm at lower update rates (50 - 100 Hz). In many systems today, digital integration is
accomplished by dividing the integration algorithm into two parts [Savage, 2000], one
9.5. VALIDATION OF NAVIGATION ALGORITHM 40
for the parts with high dynamics, which needs to be run at a high iteration rate and
one for the parts with low dynamics, which can be run at a lower rate. This approach
is known as a two-speed approach. The parts running with a high iteration rate (1 -
2 KHz) are lower order integration algorithms, which are run by specialized hardware
or highly optimized software algorithms. The parts running with a lower iteration rate
(50 - 100 Hz) are executed by the navigation computer software.
However, as the processing power available in modern microcomputers has increased
dramatically during the last decades, the integration problem have decreased, as it can
be solved by integrating all the terms in the navigation equations in the navigation
computer.
9.5 Validation of Navigation Algorithm
In order to validate the navigation algorithm derived in this chapter, the navigation
algorithmis compared to an already existing INS unit. This makes it possible to compare
the performance with respect to a commercial product. As a reference INS the Kearfott
T16 medium precision INS has been chosen.
The Kearfott unit makes it possible to log both longitude/latitude determined by the
Kearfott and the raw sensor data. It does not say at which sample rate the sensor data
is available to the INS algorithms running within the Kearfott, but the sensor data is
available to the user at 50Hz.
The test was performed by strapping the Kearfott unit to the backseat of a car and
driving a route of approximately 1.5km. The speed did not exceed 60km/h and modest
acceleration was utilized. Furthermore any sudden turns were avoided. The drive time
was approximately 5.6 minutes.
The same longitude/latitude and velocity was used for both systems. The initial rotation
matrix C
n
b
is the one determined by the Kearfott used by the navigation equations
derived in this chapter.
The test started with a 15 min alignment and immediately after alignment completion
followed by driving in order to minimize the drift of the Kearfott unit.
By applying the sensor outputs obtained from the Kearfott unit to the navigation algo-
rithms derived in this chapter, the latitude and longitude results was compared to those
of the Kearfott. The results are shown in Fig. 9.2.
It can be seen that the navigation developed in this chapter followed the reference
system quite accurately during the rst minutes of the navigation. After a while, the
navigation developed in this thesis drifts away from the reference system and at the end
the navigation developed in this chapter drifts South. This is quite possibly because of
the lower update rate available (50 Hz) compared to the operating speed of the reference
system (normally >2kHz), although this cannot be veried. The curve, however, seems
to follow the reference system good enough to justify a successful validation of the
navigation algorithm.
What is also interesting is that the car was stopped at the starting point, so the route
should end where it started. Looking at the Kearfott position it is clear that at the
end it makes the same hook-like turn. It goes South and turns East but does not stop
at the same point and furthermore starts to drift east. Investigating the position just
before it starts to drift East reveals that the position is 88m further East and 22 meters
2.086 2.085 2.084 2.083 2.082 2.081 2.08 2.079 2.078 2.077
57.182
57.183
57.184
57.185
57.186
57.187
57.188
57.189
57.19
57.191
57.192
Longitude
L
a
t
i
t
u
d
e
Our system vs. Kearfott T16

Our system
Kearfott T16
Figure 9.2: Comparison between our INS and the Kearfott INS.
further North than the start point. This dierence is due to the drift of the INS when
no aiding device is present.
It is more dicult to point out when the navigation from this thesis should be at the
starting point and thereby determining the precision. This is due to the high degree of
drift. But it is clear that the South drift is present from the last turn to the left and
this drift dwarf the small hook like turn at the end of the route.
9.6 Analysis of Long Term Error Propagation
The earth simulation model together with the navigation equations can be used to
evaluate navigation performance under various conditions. By introducing errors in the
initial INS states or sensor measurements, one can investigate how the errors propagate
through the system.
Figure 9.3 through Fig. 9.8 shows how various errors within an INS inuences the long-
term (48 [hour]) performance of the system. Gyro and accelerometer bias errors are
examined, as well as initial tilt, heading, velocity and position errors.
In the simulations, the gyro and accelerometer output is calculated by the earth simu-
lation model, where the INS was set to being stationary at 57.191 [deg] latitude 0 [deg]
longitude. The sensor frame was assumed equal to the navigation frame. Sensor errors
were introduced by adding a bias between the simulation model and the navigation
equations.
9.6. ANALYSIS OF LONG TERM ERROR PROPAGATION 42
Accelerometer bias error
0 10 20 30 40
5
0
5
10
15
x 10
3
Roll
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
0.1
0.05
0
0.05
0.1
E
r
r
o
r

[
m
/
s
]
Velocity

East
North
0 10 20 30 40
15
10
5
0
5
x 10
3
Pitch
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
50
0
50
100
Latitude
E
r
r
o
r

[
m
]
0 10 20 30 40
15
10
5
0
5
x 10
3
Yaw
E
r
r
o
r

[
m
r
a
d
]
Time [h]
0 10 20 30 40
100
0
100
200
Longitude
E
r
r
o
r

[
m
]
Time [h]
Figure 9.3: Error propagation with an accelerometer bias of 50 [g]. It can be seen
that apart from the heading, all errors are bounded. The heading drift is so small it
can be assumed negligible.
Gyro bias error
0 10 20 30 40
0.1
0.05
0
0.05
0.1
Roll
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
0.5
0
0.5
1
E
r
r
o
r

[
m
/
s
]
Velocity

East
North
0 10 20 30 40
0.1
0.05
0
0.05
0.1
Pitch
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
10000
5000
0
5000
Latitude
E
r
r
o
r

[
m
]
0 10 20 30 40
0
5
10
Yaw
E
r
r
o
r

[
m
r
a
d
]
Time [h]
0 10 20 30 40
0
5
10
x 10
4
Longitude
E
r
r
o
r

[
m
]
Time [h]
Figure 9.4: Error propagation with an gyro bias of 0.01 [
/hr]. It can be seen, that

longitude and heading error grow unbounded.
Initial heading error
0 10 20 30 40
2
1
0
1
2
x 10
3
Roll
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
0.04
0.02
0
0.02
0.04
E
r
r
o
r

[
m
/
s
]
Velocity

East
North
0 10 20 30 40
2
1
0
1
2
x 10
3
Pitch
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
200
100
0
100
200
Latitude
E
r
r
o
r

[
m
]
0 10 20 30 40
0.06
0.05
0.04
0.03
0.02
Yaw
E
r
r
o
r

[
m
r
a
d
]
Time [h]
0 10 20 30 40
1000
500
0
500
Longitude
E
r
r
o
r

[
m
]
Time [h]
Figure 9.5: Error propagation with an initial heading error of 25E-6 [rad]. All
errors are bounded.
Initial position error
0 10 20 30 40
1
0.5
0
0.5
1
x 10
5
Roll
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
2
1
0
1
2
x 10
4
E
r
r
o
r

[
m
/
s
]
Velocity

East
North
0 10 20 30 40
1
0.5
0
0.5
1
x 10
5
Pitch
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
2
1
0
1
2
Latitude
E
r
r
o
r

[
m
]
0 10 20 30 40
1
0.5
0
0.5
1
x 10
4
Yaw
E
r
r
o
r

[
m
r
a
d
]
Time [h]
0 10 20 30 40
4
2
0
2
4
Longitude
E
r
r
o
r

[
m
]
Time [h]
Figure 9.6: Error propagation with an initial position error of 1 [m]. All errors are
bounded.
Initial pitch error
0 10 20 30 40
0.05
0
0.05
Roll
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
0.5
0
0.5
E
r
r
o
r

[
m
/
s
]
Velocity

East
North
0 10 20 30 40
0.05
0
0.05
0.1
0.15
Pitch
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
1000
500
0
500
1000
Latitude
E
r
r
o
r

[
m
]
0 10 20 30 40
0.02
0
0.02
0.04
0.06
Yaw
E
r
r
o
r

[
m
r
a
d
]
Time [h]
0 10 20 30 40
2000
1000
0
1000
2000
Longitude
E
r
r
o
r

[
m
]
Time [h]
Figure 9.7: Error propagation with an initial pitch and roll error of 25E-6 [rad].
All errors are bounded.
Initial velocity error
0 10 20 30 40
0.02
0.01
0
0.01
0.02
Roll
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
0.1
0.05
0
0.05
0.1
E
r
r
o
r

[
m
/
s
]
Velocity

East
North
0 10 20 30 40
0.02
0.01
0
0.01
0.02
Pitch
E
r
r
o
r

[
m
r
a
d
]
0 10 20 30 40
100
50
0
50
100
Latitude
E
r
r
o
r

[
m
]
0 10 20 30 40
0.02
0.01
0
0.01
0.02
Yaw
E
r
r
o
r

[
m
r
a
d
]
Time [h]
0 10 20 30 40
200
100
0
100
200
Longitude
E
r
r
o
r

[
m
]
Time [h]
Figure 9.8: Error propagation with an initial velocity error of 0.1 [m/s]. The
heading drift is so small it can be assumed negligible.
One general characteristic on all the gures are oscillations with a period of 84.4 [min].
The Schuler oscillations apparent in the gures arise from the torquing of the platform
in order to keep it level. Figure 9.9 shows an INS with an exaggerated initial tilt error.
Figure 9.9(a) is the initial time where navigation is started, and Fig. 9.9(b) is at a later
time. In (a), is the system initiated with an tilt error of . This will result in gravity
being resolved wrongly, and will give an acceleration in the east direction. As the INS
assumes that the system is moving east, it will torque the C
n
e
matrix (the platform)
around the north axis using the transport rate, although the system is actually not
moving. At a time between (a) and (b), the platform will be level relative to earth, but
the INS will assume it still has an east velocity component, so it will continue to torque
the platform. In (b), it is seen that the INS will measure an acceleration to the west,
but will still have a velocity to the east. Because of the acceleration, the velocity will
decrease, and the result is an oscillation with the Schuler period of 84.4 [min]. This can
be recognized as being the frequency of a pendulum with the same length as the earth
radius.
Interesting is it to see, that initial tilt errors and gyro bias are by far the biggest
contributor to the errors in the system. From Fig. 9.4 and Fig. 9.7 is it seen that even
small gyro bias and tilt error give rise to position errors in kilometers. All the other
errors give rise to a position error with a maximum of 500m and most of them below.
What is also very important to notice is that the unbounded yaw error is also mainly
due to gyro bias. Although both accelerometer bias and initial velocity error contribute
to an unbounded yaw error, their contributions are very small compared to that of
the gyro bias. This shows the importance of good alignment of an INS. It is also the
main cause of the requirement of 1 [nm/h] INS to have a gyro bias of 0.01 [
/h] or
less[Savage, 1997]. Accelerometer bias does give rise to errors, but not as signicant
as gyro errors. Position errors are not very signicant, whereas initial velocity errors
should be kept to a minimum.
Assumed velocity
g
n
g
n
u
g
n
e
g
n
g
n
e
g
n
u
Torquing due to transport rate
Assumed acceleration
Torquing due to transport rate
Assumed acceleration
(a) (b)
Figure 9.9: Schuler oscilation. The platform is tilted with respect to the earth.
This gives rise to an oscilation because of gravity being resolved incorrectly.
An INS continuously integrates the equations derived in this chapter, to obtain attitude,
velocity and position of an INS. However, before navigation, one needs to know the initial
attitude and heading of the system. This is done in a process called alignment, which
will be explained in the following chapters.
Chapter 10
Error Estimation Using Indirect
Kalman Filtering
The process of alignment is to determine attitude and heading of the INS. As the
attitude and heading is not directly measurable these are to be estimated. The Kalman
lter technique proposes an optimal way of estimating states why this approach is also
used in this thesis. However it is not a standard direct Kalman lter but an indirect
Kalman lter which is used in this thesis. Where the normal direct Kalman lter seeks
to estimate the states directly the indirect Kalman lter estimates the error in these
states and corrects the real states. For this reason is the model used by the Kalman
lter an error model, describing the error propagation of the system. This chapter
introduces the reader to the principles of and the notation used in indirect Kalman
ltering. The dierences between the normal direct approach and the indirect approach
are outlined. Last, a simple one dimensional example is presented in order to better
explain the technique.
10.1 Kalman Filtering
The main dierence between a normal Kalman lter and the indirect Kalman lter
approach is how the system is modeled. Normally the system model describes how the
states, e.g. position, velocity, etc., develops over time. In indirect Kalman ltering the
system model describes how the errors in the states develop over time. This means that
instead of estimating the states directly, only the errors in the states are estimated by
the Kalman lter. The states in an indirect Kalman lter is hence called error states.
With this in mind the system model is described as follows:
x = A(t)x(t) +G
p
(t)n
p
(t) (10.1)
where
x = Error state vector
A = Error state dynamic matrix
n
p
= Process noise vector
G
p
= Process noise dynamic matrix
49
10.1. KALMAN FILTERING 50
with the measurement equation as follows:
z(t) = H(t)x(t) +G
m
(t)n
m
(t) (10.2)
where
z = Measurement vector
H = Measurement matrix
n
m
= Measurement noise vector
G
m
= Measurement noise dynamic matrix
These equations describe the system used by the Kalman lter to estimate the error
states x. As the lter estimates error states, the input to the lter must also be errors.
Hence, the system must be aided by some external state measurements which can be
compared to the states of the system to generate measurements of the errors in the
system.
The measurement equation is a linearized version of the general non-linear observation
equation z
obs
which describes the actual, non-linear observation which is dened as
follows:
z
obs
= f(
ins
,
aid
) (10.3)
where
ins
= INS navigation states (position, velocity, etc.)
aid
= Aiding device navigation states (position, velocity, etc.)
f() = Function which compares the parameters from the INS and the aiding device.
It is constructed such that with an error free INS and an error free aiding device
this function is equal to zero
An example of the observation equation could be the position estimated by the INS
minus the position estimated by a GPS. With these estimates the observation equation
is:
z
obs
= r
ins
r
gps
(10.4)
With error free measurement this observation equation is indeed zero.
An example of an indirect Kalman lter is depicted in Fig. 10.1. The INS system
calculates the INS states (position, velocity, attitude etc.), where some are compared to
the aiding device states to generate the observations for the Kalman lter. The Kalman
lter outputs the optimal estimate of the errors in the INS states.
Looking at the gure, it can be seen that the indirect Kalman lter is allowed to adjust
the states of the system through a process called control reset. As the estimate from
the Kalman lter is in fact the best estimate of the errors in the states, it seems obvious
to use this information to adjust the states and thus also the error states (as they depend
directly on the states). The error states are reset as follows:
x
n
(+
c
) = x
n
(+
e
) +u
nc
(10.5)
where
u
c
= Control vector used to control the error build up in the states
(+
c
) = Designation of the corrected error state just after the states have been reset
(+
e
) = Designation of the estimated error state just before the control reset vector is applied.
51
CHAPTER 10. ERROR ESTIMATION USING INDIRECT KALMAN
FILTERING
The control reset operation will prevent errors from being build up at the integrators,
as the errors are set to 0 at each time step. The implementation of correcting the INS
parameters involves applying u
n
to the INS parameters in the following way:
insn
(+
c
) = g
ins
(
insn
(), u
cn
) (10.6)
where
g
ins
() = Functional operator to apply u
c
to the INS parameters
The g
ins
() operation diers depending on which type of state that should corrected. In
this report two dierent types of states are to be corrected. One type of correction is
to simply subtract the two states from each other. This correction is applicable when
correcting states like position and velocity. But when correcting the rotation matrices
subtraction is not applicable and they are corrected as seen in (12.41).
The idea of resetting the error states by control resetting the actual states is illustrated
in Fig. 10.1. The measurements are parsed through the INS algorithms in order to get
the lter input in the correct form. With the INS error model the lter input should
also be an error. This be either position error, velocity error, etc. The Kalman lter
estimates the error states and these are used to correct the INS states and thus indirectly
resetting the error states.
INSSystem
Filter
Input
ErrorState
Estimate
KalmanFilter
ControlResetofINSStates/IndirectResetofErrorState
ForegroundEquations
+
-
AidingDevice
Measurements
Figure 10.1: Indirect Kalman lter with control reset
The ideal control vector u
c
is of course - x
n
(+
e
) as it can be seen from (10.5). This
control vector would reset the errors to zero at every kalman lter cycle, as x
n
(+
e
) is
the best estimate of the error.
In order to point out the dierences between a normal direct Kalman lter and an
indirect Kalman lter the Kalman equations for both lters are listed. First of all the
normal direct Kalman equations are listed:
Measurement update
K
n
= P
n
()H
T
n
(H
n
P
n
()H
T
n
+R
n
)
-1
x
n
(+) = x
n
() +K
n
(z
n
z
n
)
P
n
(+) = (I K
n
H
n
)P
n
()
Time update
x
n
() =
n
x
n-1
(+)
z
n
= H
n
x
n
()
Secondly the indirect Kalman lter equations are listed. The parts which are special for
the indirect Kalman lter is indented in order to compare it with the normal Kalman
lter.
10.2. MOTIVATING EXAMPLE 52
z
obsn
= f(
insn
,
aidn
)
Measurement update
K
n
= P
n
()H
T
n
(H
n
P
n
()H
T
n
+R
n
)
-1
x
n
(+
e
) = x
n
() +K
n
(z
obsn
z
n
)
P
n
(+) = (I K
n
H
n
)P
n
()
u
cn
= x
n
(+
e
)
x
n
(+
c
) = x
n
(+
e
) +u
nc
insn
(+
c
) = g
ins
(
insn
(), u
cn
)
Time update
x
n
() =
n
x
n-1
(+
c
)
z
n
= H
n
x
n
() (10.7)
where
(+
e
) = Designation for parameter after estimation control but before control reset
(+
c
) = Designation for parameter after control reset
K
n
= Kalman gain
10.2 Motivating Example
For this example a one dimensional accelerometer is used. The accelerometer delivers
noisy acceleration measurements and the system consist of an integrator in order to
determine the velocity. Furthermore another speedometer is used which delivers velocity
measurements. The Kalman lter is to estimate the errors in the velocity in order to
get a better estimate of the velocity.
The indirect Kalman lter is presented in Fig. 10.2.
z-1
Kalman
filter
Measured
acceleration
a
v
Velocitymeasuredby
aidingdevice
v
ref
Observationsetfor
theKalmanfilter
z =v-
obs
v
ref
Kalmanfilter
estimate
v
Controlreset
+
-
Ts
Figure 10.2: One dimensional indirect Kalman lter
As seen in the gure, the measured acceleration is integrated to give the velocity. The
reference velocity measured by the aiding speedometer is subtracted from the systems
estimated velocity to give the observation set. The reason for this is to get the observa-
tion set to contain velocity errors which is the driving parameter for the Kalman lter.
The Kalman lter then estimates errors as the system model is an error model. This
error is of course fed back to the integrator to correct the velocity. The correction inside
the integrator is shown below.
53
CHAPTER 10. ERROR ESTIMATION USING INDIRECT KALMAN
FILTERING
1
z
+
+
T
n
+
+
Measured
acceleration
a
Add_1
Add_2
Controlreset
v
v
Errorcorrected
velocity
Discretetime
delay
Figure 10.3: Illustration of correction of states inside a discrete integrator
As seen in Fig. 10.3 the integrator is a discrete integrator with an addition block inside
the integration loop. This method ensures that the correct value is store, thus avoiding
residual error buildup in the integrator.
10.2.1 Choice of Kalman Filter
When using Kalman ltering it is normally quite simple to choose between e.g. a normal
linear, extended or unscented Kalman lter. It might not be quite as intuitive when
it could be advantageous to do the extra work to derive an error model in order to
implement the indirect Kalman lter. Two guidelines can be used to when the IKF
should be considered.
If the system has high dynamics and one is trying to estimate the states, then the
Kalman lter cycle has to be very fast. This sets demands to the processor in order to
do the Kalman calculations before the next Kalman cycle. This might pose a problem
when a system should be implemented on a microprocessor. It might not be possible
to do all the calculations within each Kalman cycle. Secondly, if the system is also
non-linear then a non-linear Kalman lter should be chosen. This leads to even more
complicated calculation which sets even higher demands to the processor chosen.
As mentioned earlier the use of indirect Kalman ltering requires the system model to
be an error model. This error model can be approximated to be linear and in INS,
normally has much lower dynamics than the real system. For these reasons a normal
linear Kalman lter can be used with a much lower Kalman cycle setting much lower
demands to the processor.
The reasons for choosing the IKF for this project is that the model of the INS has
very high dynamics. In order to describe the motion with high precision, the model
should be run at more than 2kHz and furthermore the is model non-linear. Choosing
a standard direct Kalman lter approach requires the use of an extended Kalman lter
which should be run at 2kHz. This might pose a problem when implementing on a
microprocessor. Using the indirect Kalman lter, it is only necessary to run when new
data is available from the aiding device (1 Hz for typital GPS receivers). For this reason,
the indirect Kalman lter is chosen.
10.3. SUMMARY 54
10.3 Summary
During this chapter, the indirect Kalman lter technique has been described and the
dierences and advantages with respect to the direct Kalman lter have been outlined.
As described, the indirect Kalman lter is advantageous if the system is characterized
by high dynamics and/or non-linearity. In order to utilize the indirect Kalman lter
an error model of the system needs to be developed and this error model of the INS is
developed in the next chapter.
Chapter 11
Error Equations
During this chapter is the error model of the INS developed which describes the prop-
agation of errors within the INS. These errors are position, velocity, rotation, attitude
and heading error of the INS. This error model is developed as it is required as model
for the indirect Kalman lter.
11.1 Derivation of Error Equations
The purpose of the error model is to describe the propagation of the errors in the
navigation equations. The errors are dened as the angle between the actual DCMs and
the DCMs computed in the navigation computer and the velocity dierence between
the actual velocity of the INS and the velocity used by the navigation computer. The
errors to be dened are the error angles associated with the DCM rotating from body
frame to navigation frame () the error angles associated with the DCM rotating from
navigation frame to earth frame () and lastly the velocity error v.
The navigation equations derived in Sec. 9.1 describe the motion of a vehicle relative
to the earth instrumented in a strapdown inertial navigation system. The navigation
equations derived in Sec. 9.1 are simplied for use in the error equations. The earth is
considered a sphere instead of an ellipsoid which yields the following equations:
C
n
b
= C
n
b
b
ib
(
b
ie
+
b
en
)C
n
b
n
ie
= C
n
e
e
ie
n
en
=
1
R
(u
n
zn
v
n
) +
R
u
n
zn
v
n
= C
n
b
a
n
+g
n
(
n
en
+ 2
n
ie
) v
n
C
e
n
= C
e
n
n
en
h = v
n
u
n
zn
(11.1)
Where u
n
zn
=
_
0 0 1
is a vertical unit vector and

R
is the magnitude of the
transport rate around the vertical.
Certain approximations and assumptions has been made deriving the navigation equa-
55
11.1. DERIVATION OF ERROR EQUATIONS 56
tions above which lead to one type of errors. These errors can be minimized by making
a suciently accurate model.
Secondly, another type of errors is introduced. These errors arise when the INS computer
evaluates the navigation equations. The equations used in the computer are not exactly
equivalent to the equations in (11.1) due to several sources of error. These errors arise
because of measurement inaccuracies from the sensors and computer errors due to nite
precision and integration errors. In this chapter, only the errors from the sensors is
considered as the computer errors can be minimized by choosing sucient length of
word.
The navigation equations in the navigation computer are equal to (11.1), but with a
hat to indicate that they include the second type of errors:
C
n
b
=

C
n
b
b
ib
(
b
ie
+

b
en
)
C
n
b

n
ie
=

C
n
e

e
ie

n
en
=
1
R
(u
n
zn
v
n
) +
R
u
n
zn
v
n
=

C
n
b
a
n
+ g
n
(
n
en
+ 2
n
ie
) v
n
C
e
n
=

C
e
n
n
en
h = v
n
u
n
zn
(11.2)
In order to develope the error mode an error is dened as follows:
a = a a (11.3)
Where a is the computed state and a is the actual state.
Using this error denition an error model of the INS can be developed. This is achieved
by subtracting the true navigation equations (11.1) from the navigation equations used
by the onboard navigation computer (11.2) as dened by (11.3).
Before deriving the error model the errors of interest are stated. As the INS is to be
used in two modes, alignment mode and navigation mode, two sets of errors are of
interest. During alignment, the primary error of interest is the error in the C
n
b
matrix.
In navigation mode it is mainly the position error which is of interest. This means that
the error in C
n
e
matrix and the error in height is important. Furthermore, the errors
in the velocity of the INS is necessary in order to estimate the position of the vehicle
when navigating. This leads to the following errors which are of interest:
C
n
b
=

C
n
b
C
n
b
v
n
= v
n
v
n
C
e
n
=

C
e
n
C
e
n
h =

h h (11.4)
The error equations can be obtained by subtracting (11.1) from (11.2) and neglecting
the parts which are a multiplication of two errors as they are assumed to be very small.
This yields the following expression for the errors of interest:
57 CHAPTER 11. ERROR EQUATIONS

C
n
b
= C
n
b
b
ib
+C
n
b
b
ib
(
n
ie
+
n
en
)C
n
b
(
n
ie
+
n
en
)C
n
b
v
n
= C
n
b
a
b
+C
n
b
a
b
(2
n
ie
+
n
en
) v
n
(2
n
ie
+
n
en
) v
n
+ g
n

C
e
n
= C
e
n
n
en
+C
e
n
n
en
n
ie
= C
n
e
e
ie
n
en
=
1
R
(u
n
zn
v
n
)
R
R
2
(u
n
zn
v
n
) +
R
u
n
zn
h = v
n
u
n
zn
(11.5)
Although these equations describe the error propagation it does not directly describe
the errors of interest. These equations show the derivative of the error DCM and not the
derivative of the error angle of the DCM. By introducing small angle approximations to
(11.5) these equations can be transformed into a suitable form. These approximations
are, of course, only applicable when the error angles are small, so the error equations
derived here can only be used in such cases. The small angle approximation is dened
as follows:
C
n
b
= (I
n
)C
n
b
C
n
b
=
n
C
n
b
C
n
e
= (I E
n
)C
n
e
C
n
e
= E
n
C
n
e
C
e
n
=

C
n
e
T
= C
e
n
(I +E
n
) C
e
n
= C
e
n
E
n
(11.6)
where:
, E = Skew symmetric form of
n
and
n
.
n
= The rotation vector associated with C
n
b
n
= The rotation vector associated with C
e
n
(11.7)
With this small angle approximation the error equations are rewritten. Using (11.6)
n
ie
is rewritten:
n
ie
= E
n
C
n
e
e
ie
=
n
n
ie
(11.8)
The gravity error model in the v
n
term from (11.5) is the variation from the true
gravity to the one estimated by the computer. This gravity error is inuenced both by
errors on the altitude estimation and errors in the actual gravity to the gravity model
in the computer. Gravity can be expressed as a simple inverse square law model plus a
term taking into account the deviations from the simple model:
g
n
= g
0
R
2
0
R
2
u
n
zn
+ g
n
(11.9)
where
g
0
= Gravity magnitude on the earth surface
R
0
= Earths radius
R = Distance from the earth center to the INS
g
n
= Correction to inverse square model to take into account
the centripetal acceleration and earth mass distribution.
The gravity error g
n
is found using a special approach. As gravity is dependent on the
parameter R, variation of gravity is dened to be g
n
dierentiated with respect to R.
This denition is used to dene the error in gravity. The error in the gravity is then
simply the variation of gravity multiplied with the error in the position as follows:
g
n
var
=
dg
n
dR
g
n
=
dg
n
dR
R (11.10)
Using this approach and the gravity model from (11.9) g
n
is as follows:
g
n
= 2g
0
R
2
0
R
3
Ru
n
zn
+ g
n
2g
0
R
hu
n
zn
+ g
n
M
(11.11)
where
g
n
M
= Unmodeled gravity error
Using (11.6), (11.8) and (11.11) v
n
from (11.2) is rewritten:
v
n
=
n
f
n
+C
n
b
f
b
(2
n
ie
+
n
en
) v
n
(
n
en
2
n
n
ie
) v
n
+
2g
R
hu
n
zn
+ g
n
M
(11.12)
In order to rewrite

C
e
n
(11.6) is substituted into

C
e
n
from (11.5) which yields:
C
e
n
E
n
+C
e
n
E
n
= C
e
n
E
n
n
en
+C
e
n
n
en
(11.13)
C
e
n
E
n
= C
e
n
E
n
n
en
C
e
n
n
en
E
n
+C
e
n
n
en

E
n
= (E
n
n
en
n
en
E
n
) +
n
en
(11.14)
In order to describe this equation in vector form and in a somewhat simpler form the
terms in the brackets can be reduced using the triple vector product identity, here shown
for three arbitrary vectors:
v
1
(v
2
v
3
) = (v
1
v
3
)v
2
(v
1
v
2
)v
3
(11.15)
Multiplying the term in brackets from (11.14) with an arbitrary vector (v) and switching
to vector form:
(E
n
n
en
n
en
E
n
)v =
n
(
n
en
) v)
n
en
(
n
) v) (11.16)
Applying the triple vector product identity yields:
(E
n
n
en
n
en
E
n
)v = (
n
v)
n
en
(
n

n
en
)v (
n
en
v)
n
+ (
n
en

n
)v
= (
n
v)
n
en
(
n
en
v)
n
(11.17)
Using the triple vector product to collect term again:
(E
n
n
en
n
en
E
n
)v = (
n
n
en
) v (11.18)
Converting this into matrix form where the term in the bracket should be represented
by the skew symmetric counterpart:
(E
n
n
en
n
en
E
n
)v = (
n
n
en
)
v (11.19)
Where the ()
operator brings the vector enclosed in the brackets to skew symmetric

form. As the vector v is an arbitrary vector this will be chosen as identity:
(E
n
n
en
n
en
E
n
) = (
n
n
en
)
E
n
= (
n
n
en
)
+
n
en
(11.20)
which can be represented in vector form:

n
=
n
n
en
+
n
en
(11.21)
In the same manner as with

C
e
n
,

C
n
b
is rewritten by using the expression from (11.6)
and then replacing with the expression from (11.5):

C
n
b
=
n
C
n
b
n

C
n
b
=
n
C
n
b
b
ib
+C
n
b
b
ib
(
n
ie
+
n
en
)C
n
b
+ (
n
ie
+
n
en
)
n
C
n
b
(11.22)
In order to get rid of the derivative of

C
n
b
this is substituted with the expression from
(11.1) yielding:
n
C
n
b
=
n
_
C
n
b
b
ib
(
n
ie
+
n
en
)C
n
b
_
+
n
C
n
b
b
ib
C
n
b
b
ib
+ (
n
ie
n
en
)C
n
b
(
n
ie
+
n
en
)
n
C
n
b
=
n
(
n
ie
+
n
en
)C
n
b
(
n
ie
+
n
en
)
n
C
n
b
C
n
b
b
ib
+ (
n
ie
+
n
en
)C
n
b
(11.23)
Which can be simplied further by right side multiplying with (C
n
b
)
1
n
=
n
(
n
ie
+
n
en
) (
n
ie
+
n
en
)
n
C
n
b
b
ib
C
b
n
+
n
ie
+
n
en
= [
n
(
n
ie
+
n
en
)]
C
n
b
b
ib
C
b
n
+
n
ie
+
n
en
(11.24)
Using the similarity transform simplies the expression to:
n
= [
n
(
n
ie
+
n
en
)]
(C
n
b
b
ib
)
+
n
ie
+
n
en
(11.25)
which can be written in vector form:

n
=
n
(
n
ie
+
n
en
) C
n
b
b
ib
+
n
ie
+
n
en
(11.26)
Equation (11.12), (11.21), (11.26) along with
n
en
,

h from (11.5) and the approximation
R = h constitute the error model for (11.1) in terms of the sensor errors a
b
and
b
ib
:

n
=
n
(
n
ie
+
n
en
) C
n
b
b
ib
+
n
ie
+
n
en
v
n
=
n
a
n
+C
n
b
a
b
(2
n
ie
+
n
en
) v
n
(
n
en
2
n
n
ie
) v
n
+
2g
R
hu
n
zn
+ g
n
M

n
=
n
n
en
+
n
en
n
en
=
1
R
(u
n
zn
v
n
)
h
R
2
(u
n
zn
v
n
) +
R
u
n
zn
h = C
n
e
e
ie
(11.27)
However, this error model, described by the equations (11.27), suer from one disad-
vantage. The position error is described in form of four parts:
n
and h. This means
that there is some redundant information as the position error can be described by three
parts. The redundant information is in
n
where the vertical part equals the azimuth
error of the navigation frame. During the navigation mode where it is the position error
vector which is of interest, this model is inconvenient as it does not described this po-
sition error directly. This disadvantage disappears if the error model is to be used in a
situation where the position error is not important and can therefore be neglected. This
situation is e.g. when aligning with the use of a Kalman lter as the position is assumed
to be known precisely. But when using the error model in a navigation situation it is
convenient to have the position error described only by three parts thus removing the
redundant information.
Error Equation Model with Simple Position Error Part
In order to represent the position error by three elements and thus getting rid of the
redundancy the attitude, velocity and position error is dened now in the earth frame:
C
e
b
= (I
e
)C
e
b
C
e
b
=
e
C
e
b
v
e
= v
e
v
e
r
e
=

r
e
r
e
(11.28)
where
= The skew symmetric counterpart of
which is equal to the angular error in C
e
b
r = The position vector from the earths center to the INS
The task is now to relate these new earth-frame error equations to the navigation-frame
error equation already developed.
First of all it should be noted that:
C
e
b
= C
e
n
C
n
b
+C
e
n
C
n
b
(11.29)
which makes it possible to describe the part of (11.28) as:
e
= (C
e
n
C
n
b
+ C
e
n
C
n
b
)C
e
b
T
= (C
e
n
C
n
b
E
n
C
n
b
C
e
n
n
C
n
b
)(C
n
b
)
T
(C
e
n
)
T
= C
e
n
(
n
E
n
)C
n
e
(11.30)
This can be rearranged into:
(C
e
n
)
T
e
C
e
n
=
n
E
n
(11.31)
Using the similarity transformation yields the following:
n
=
n
n
(11.32)
In order to relate v
e
with v
n
it is rst recognized that:
v
e
= C
e
n
v
n
v
e
=

C
e
n
v
n
C
e
n
= C
e
n
+ C
e
n
(11.33)
Substituting (11.33) into v
e
from (11.28), using the denition of v
n
from (11.4) and
neglecting any second order errors as they are assumed insignicant yields:
v
e
=

C
e
n
v
n
C
e
n
v
n
= (C
e
n
+ C
e
n
) v
n
C
e
n
v
n
= C
e
n
( v
n
v
n
) + C
e
n
v
n
= C
e
n
v
n
+ C
e
n
v
n
(11.34)
Using small angle approximation yields:
v
e
= C
e
n
(v
n
+E
n
v
n
) = C
e
n
(v
n
+
n
v
n
) (11.35)
As the error model should be expressed in navigation frame the velocity error in navi-
gation frame is dened:

C
n
e
v
e
(11.36)
With denition (11.35) it becomes:
n
= v
n
+
n
v
n
(11.37)
A relation from r
e
with
n
and h is obtained by rst dening r
e
:
r
e
= Ru
e
ze
(11.38)
Where R is the length of r
e
, and u
e
ze
is the unit vector pointing from the center of the
earth to the INS in earth frame. The error is dened in the standard way and neglecting
second order errors yield:
r
e
=

R u
e
ze
Ru
e
ze
= (R + R)(C
e
n
+ C
e
n
)u
n
zn
RC
e
n
u
e
ze
RC
e
n
u
n
zn
+ RC
e
n
u
n
zn
(11.39)
Transforming this into the navigation frame yields:
r
n
= RE
n
u
n
zn
+ Ru
n
zn
(11.40)
which can be represented in vector form:
r
n
= R(
n
u
n
zn
) + Ru
n
zn
(11.41)
From this position error vector the horizontal and vertical parts are easily found. The
vertical parts is the dot product of (11.41) with u
n
zn
and as expected it is equal to R.
The vertical part is simply found by subtracting the vertical part from (11.41) and is
equal to:
r
n
H
= R(
n
u
n
zn
) (11.42)
To this point the old error model has been related to the new error states and is sum-
marized here:
n
=
n
n
= v
n
+
n
v
n
r
n
= R(
n
u
n
zn
) + Ru
n
zn
r
n
H
= R(
n
u
n
zn
)
R = h (11.43)
In order to obtain the dierential equation for the new error model the equivalent
parameters from (11.27) is substituted and dierentiated.
The

n
equation is obtained by subtracting
n
from
n
and substituting the parts with
their equivalent from (11.43):
n
=
n
(
n
ie
+
n
en
) C
n
b
b
ib
(11.44)
The
n
dierential equation is derived by dierentiating the
n
expression from
(11.43):

n
= v
n
+
n
v
n
+
n
v
n
(11.45)
Substituting v
n
, v
n
and
n
with expressions from (11.1) and (11.27) and substituting
v
n
with an rearranged
n
expression from (11.43) yields:

n
= C
n
b
a
b
n
a
n
(2
n
ie
+
n
en
) (
n
n
v
n
)
(
n
en
2
n
n
ie
) v
n
+
2g
R
hu
n
zn
+ g
n
M
+ (
n
n
en
+
n
en
) v
n
+
n
_
C
n
b
a
b
(2
n
ie
+
n
en
) v
n
+g
n
_
= C
n
b
a
b
(
n
n
) a
n
+
n
g
n
(2
n
ie
+
n
en
)
n
+
2g
R
hu
n
zn
+ g
n
M
+ (2
n
ie
+
n
en
) (
n
v
n
) + [
n
(2
n
ie
+
n
en
)] v
n
[(2
n
ie
+
n
en
) v
n
] (11.46)
The three last parts can be shown to sum to zero by using the triple vector product
from (11.15). Dening = 2
n
ie
+
n
en
the triple vector product is used on the three
last parts and summarized with the following result:
( v) + ( ) v ( v)
( v) v( ) ( v) +( v) ( v) +v( )
v( ) = 0 (11.47)
Furthermore (
n
n
) can be substituted with
n
. Lastly the g
n
part from g
n
from
(11.9) can be neglected as it is very small compared to g
n
. As g
n
point the same way
as u
n
zn
the following expression can be substituted using r
n
H
from (11.43):
n
g
n
= g(
n
u
n
zn
)
=
g
R
r
n
H
(11.48)
All of these rearrangements result in the nal expression for
n
:

n
= C
n
b
a
b
n
a
n
(2
n
ie
+
n
en
)
n
+
2g
R
Ru
n
zn
g
R
r
n
H
+ g
n
M
(11.49)
What remains is to relate the new position vector r
n
to the old states
n
and h. By
denition:
r
e
= v
e
(11.50)
and furthermore:
r
n
= C
n
e
r
e
(11.51)
Dierentiating (11.51):
r
n
=

C
n
e
r
e
+C
n
e
r
e
(11.52)
Substituting (11.50) and (11.51) into (11.52) with the expression of

C
n
e
from (11.1)
yields:
r
n
=
n
en
C
n
e
r
e
+C
n
e
v
e
=
n
n
en
r
n
(11.53)
The new error model with the new error states

n
,
n
and r
n
is summarized below:
n
=
n
(
n
ie
+
n
en
) C
n
b
b
ib

n
= C
n
b
a
b
n
a
n
(2
n
ie
+
n
en
)
n
+
2g
R
Ru
n
zn
g
R
r
n
H
+ g
n
M
r
n
=
n
n
en
r
n
(11.54)
This new error model is of course equivalent to the one from (11.27) with the relation
found in (11.43). The -error model is shown below in matrix form:
_
x
y
z
vx
vy
vz
rx
ry
rz
_
_
=
_
_
0
c

b
0 0 0 0 0 0
c
0
a
+
0 0 0 0 0 0
b
+
0 0 0 0 0 0 0
0 a
n
U
a
n
N
0 2
c
2
b
+
g/r 0 0
a
n
U
0 a
n
E
2
c
0 2
a
+
0 g/r 0
a
n
N
a
n
E
0 2
b
+
2
a
0 0 0 2g/r
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1
0
_
_
_
_
x
y
z
vx
vy
vz
rx
ry
rz
_
_
(11.55)
where
a
= cos(l) sin()
e
b
= cos(l) cos()
e
c
= sin(l)
e
l cos() +

Lcos(l) sin()
=

l sin() +

Lcos(l) cos()
l = v
g
N
/(r + h)
L = v
g
E
/(r + h) cos(l)
11.2 Summary
During this chapter has the error model of the INS been developed. Two dierent
approaches has been used which leads to two dierent error models. These two error
models describe the error propagation of the INS but has dierent characteristics as
they describe dierent error states of the INS. One example is the position error where
one error model describes the position error with rotation matrices and the other error
model describe the position error directly by a vector. The reason for developing two
error models is that each of them are preferable in two dierent situations. The rst
error model, () error model, describe the errors in C
n
b
along with velocity and position
errors which makes this error model preferable when doing alignment as alignment
tries to estimate these states. When in navigation mode the second error model, ()
error model, is simpler to calculate enabling a faster Kalman lter and thus making it
preferable.
With the error model developed and the indirect Kalman lter technique described the
alignment algorithm is ready to be developed.
Chapter 12
Alignment
As mentioned earlier an INS simply integrates measured acceleration and rotation rate
to determine the position of the body frame. But the position is always relative to
the starting position and for this reason the INS needs to know both the position,
attitude and heading before navigation can begin. The position is assumed known but
the attitude and heading needs to be determined. This is the process of alignment.
The idea behind alignment is to use the measurements from the accelerometers and
gyroscopes to determine the orientation of the body frame with respect to a reference
frame, in this thesis the navigation frame. The Alignment process is divided into two
parts: Coarse alignment and ne alignment. Coarse alignment is an analytical alignment
which suers from a lack of precision as measurement errors and other noise sources are
not considered. It does, however, estimate the attitude and heading accurately enough
to justify the small angle approximations made in the error model, and hence allow
ne alignment to use an indirect Kalman lter using the error model to obtain a more
precise alignment. This chapter will explain the alignment procedure.
Alignment is achieved using sensor measurements when the platform is in a quasi-
stationary situation. Quasi-stationarity is a state where the body is not stationary, but
close to with small variations. This is e.g. a boat at dockside pitching with the waves,
and aeroplane on the ground which vibrates due to fueling, passengers or gust of wind.
In this thesis is the navigation frame and geodetic frame the same during alignment.
This corresponds to forcing the wander angle of the navigation frame to 0. Choosing
the navigation frame equal to the geodetic frame simplies the coarse alignment as the
earth rotation is only present on two axes.
The rst section will describe the coarse alignment. The second section describes the
ne alignment procedure which uses the indirect Kalman ltering technique and the
error model developed in the preceding chapter. The last section is a verication of the
alignment where it is tested against both simulated and real-world measurements.
64
65 CHAPTER 12. ALIGNMENT
12.1 Comparison between Gimballed and Strapdown
Alignment
Understanding the concept of alignment when using a strapdown INS is easier when
visualizing the gimballed situation. When aligning a gimballed INS the gimbal is phys-
ically rotated until two of the the sensor axes are level with respect to the earth. This
means that if a plane is tilted 45
with respect to the earth the gimbal will need to

be rotated 45
to be level. The gimbal is simply rotated/tilted until the accelerometer

measures [0 0 g]
T
which means that the INS is now level with the earth. When using
a strapdown INS the sensors are mounted directly to the body and can not be rotated
so this is achieved analytically. By adjusting C
n
b
until the accelerometer measurements
equals [0 0 g]
T
in the navigation frame, results in the same as physically rotating the
gimbal. So throughout this thesis the concept of adjusting C
n
b
will be called leveling in
order to gain greater understanding.
12.2 Coarse Alignment
The gravity vector along with the Earths rotation rate is known in the navigation
frame and is compared to the measured acceleration and rotation rate in body frame.
Comparing these quantities makes it possible to determine the direction cosine matrix
relating the navigation frame and body frame.
The objective of this section is to determine the direction cosine matrix C
b
n
relating the
navigation frame with the body frame. In the n-frame gravity has only one component
which is the vertical, and earth rotation is also known due to the knowledge of the
latitude. The measured quantities are g
b
and
b
ie
which are the measurements from the
accelerometer and gyroscope. These quantities are related in the following way:
g
b
= C
b
n
g
n
b
ie
= C
b
n
n
ie
where g
n
=
_
0 0 g
T
and
n
ie
=
_
0
ie
cos(l)
ie
sin(l)
T
. Knowing the quan-
tities g
b
=
_
g
b
x
g
b
y
g
b
z
T
and
b
ie
=
_

b
x

b
y

b
z
T
from the measurements of
the accelerometer and gyroscope, this system is easily solved.
C
31
=
g
x
g
C
21
=

x
ie
cos(l)

g
x
g
tan(l) (12.1)
C
32
=
g
y
g
C
22
=

y
ie
cos(l)

g
y
g
tan(l) (12.2)
C
33
=
g
z
g
C
13
=

z
ie
cos(l)

g
z
g
tan(l) (12.3)
where C
21
, C
22
, ... C
33
are the elements of the direction cosine matrix C
b
n
. The last
three elements are easily determined with the use of the orthogonality property of the
direction cosine matrix, (C
b
n
)
1
= (C
b
n
)
T
.
12.3. FINE ALIGNMENT 66
C
21
= C
12
C
33
+ C
13
C
32
(12.4)
C
22
= C
11
C
33
C
31
C
13
(12.5)
C
23
= C
11
C
32
+ C
31
C
12
(12.6)
As seen from these equations this DCM is only uniquely dened if the latitude is not
equal to 90
. This means that this alignment procedure is not possible if the INS is
positioned at the North- or South Pole.
12.3 Fine Alignment
Fine alignment uses an indirect Kalman lter with the result from the coarse alignment
as the initial C
n
b
. At ne alignment completion, the attitude and azimuth heading is
estimated with greater accuracy than possible with coarse alignment. The reason for
this is that the coarse alignment is a one shot alignment procedure which suers from
measurement noise which prevents it from calculating the true value of C
n
b
. However,
the errors in the C
n
b
matrix can be assumed to be small, which enables the use of the
previously derived error equations as this error model uses small angle approximations.
After coarse alignment, the remaining errors in the C
n
b
matrix consists of small roll, pitch
and yaw angles. Roll and pitch errors are also denoted tilt errors, as they correspond
to the platform being not truly level. The yaw error is the deviation of the y axis of the
navigation frame from pointing true North.
During ne alignment, the estimates of roll and pitch errors are used as control resets
to correct the C
n
b
matrix, so the platform is leveled. The yaw error can be calculated
from the horizontal parts of the estimated earth rotation
n
ie
. The vertical part of
earth rotation is known analytically as a function of latitude, and does not need to be
estimated. A precise estimate of earth rotation is also needed in order to cancel the
eects of earth rotation on platform tilt. This will become evident later in this section.
Fine alignment uses the navigation equations to obtain position information of the INS.
As the system is quasi-stationary, any position dierent from 0 will be an error. This
error is used as the driving parameter for the indirect Kalman lter. The indirect
Kalman lter then estimates the errors in the rotation matrix from body frame to
navigation frame C
n
b
, error in earth rotation
n
ie
, error in velocity v
n
and error in the
dierence in position from the initial position r
n
.
The navigation equations from Sec. 11.1 is restated below.
C
n
b
= C
n
b
b
ib
(
n
ie
+
n
en
)C
n
b
n
ie
= C
n
e
e
ie
n
en
=
1
R
(u
n
zn
v
n
) +
R
u
n
zn
v
n
= C
n
b
a
n
+g
n
(
n
en
+ 2
n
ie
) v
n
C
e
n
= C
e
n
n
en
h = v
n
u
n
zn
(12.7)
As ne alignment is performed when the INS is in a quasi-stationary situation the
navigation equations can be simplied as both
n
en
and v
n
are zero. Applying these
simplications gives the following simplied navigation equations:
C
n
b
= C
n
b
b
ib
n
ie
C
n
b
n
ie
= C
n
e
e
ie
v
n
H
= (C
n
b
a
n
)
H
r
n
H
= v
n
H
(12.8)
where
H
indicate the horizontal parts and r
n
H
is the position divergence dened as the
dierence in position from the initial position.
Equation (12.8) is called the foreground equation as it is not within the Kalman lter.
This equation is the mapping from inertial measurements to velocities and positions
which is called INS System in Fig. 10.1. The quantities calculated in this foreground
equation is not to be mistaken for the error states estimated within the Kalman lter.
In order to level the body frame analytically C
n
b
is torqued so that no components are
present in v
n
H
, which correspond to horizontal tilt of C
n
b
. This means that the INS
is torqued only in the horizontal plane but with no rotation around the vertical axis.
Furthermore, in order to bring C
n
b
to steady state, the estimated earth rotation
n
ie
needs to correspond to the measurements from the gyroscope
b
ib
=
b
ie
.
As mentioned before, the driving parameter for the Kalman lter is the position diver-
gence vector r
n
H
. This vector is a measure of the attitude error
n
and estimation
error of
n
ie
and can thus be used to estimate these two parameters. As any tilt from
horizontal will give rise to a component of the earth gravitation in the horizontal plane,
any tilt error will thus lead to a non-zero r
n
H
. Furthermore, an estimation error of
n
ie
will tilt the INS from the correct leveled attitude and thus again lead to a non-zero
r
n
H
.
Using the approach described in Ch.10, the Kalman lter is used not only to estimate the
error states, but also to correct errors in the INS states. As mentioned two parameters
needs to be corrected in order to bring C
n
b
in steady state. First of all, C
n
b
needs to
be leveled in order to cancel out the gravitation force in the horizontal plane. This
is performed by introducing a quantity called
n
tilt
used to correct C
n
b
. Secondly, the
earth rotation rate needs to be estimated. The earth rate is divided into horizontal and
vertical parts as the vertical part is known due the knowledge of the latitude of the INS.
Both
n
ie
and
n
tilt
are directly connected to r
n
H
as any error in r
n
H
is either due to an
attitude error, estimation error of
n
ie
or both. Last, the Kalman lter is allowed to reset
the errors in the last two INS states, v
n
and r
n
H
. In order to interlink r
n
H
with the four
parameters which are to be corrected, the Kalman gains are introduced. The Kalman
gains describe how r
n
H
is correlated with the system states. Four Kalman gains are
needed to correct the four parameters. Kalman gains k
1
and k
2
are used to estimate
n
ie
and
n
tilt
and k
3
and k
4
are used to estimate velocity v
n
and position divergence
r
n
H
. The navigation equations (12.8) are now expanded into the form utilized during
ne alignment:
C
n
b
= C
n
b
b
ib
(
n
ie
+
n
tilt
)C
n
b
n
tilt
= k
2
u
n
zn
r
n
H
n
ie
=
e
ieH
+u
n
zn
ie
sin l

n
ie
H
= k
1
u
n
zn
r
n
H
v
n
H
= (C
n
b
a
n
)
H
k
3
r
n
H
r
n
H
= v
n
H
k
4
r
n
H
(12.9)
12.3. FINE ALIGNMENT 68
where
u
n
zn
= Unit vector along the n-frame vertical axis (z)
k
1
, k
2
, k
3
, k
4
= Kalman gains
n
tilt
= The angular rate feedback used to correct the attitude error associated with C
n
b
The cross product with u
n
zn
has been introduced in order to assign the correct sign and
gain to the correction. It can be realized by inspecting Fig. 12.1. In gure Fig. 12.1(a)
the body has not been rotated and in Fig. 12.1(b) the body has been rotated positive
around x.
z
y
x
z
x
y
g=9.82
a b
Figure 12.1: Illustration of the accelerometer measurements due to an rotation of
the body.
As can be seen in Fig. 12.1 if the frame is rotated positive about the x-axis then the
accelerometer with sensitivity axis y will measure a positive acceleration. This means
that an positive acceleration on the y-axis should result in an negative rotation about
the x-axis. Looking at the vector product this property will be revealed.
u
n
zn
= [0 0 1]
T
u
n
zn
[x y z]
T
= [-y x 0]
T
(12.10)
A block diagram of the ne alignment procedure can be seen in Fig. 12.2.
Figure 12.2 shows the ne alignment scheme. As can be seen the input to the INS
system is the measured accelerations and rotation rates. The accelerations are double
integrated in order to get the divergence position which is the lter input. This lter
input is multiplied with the Kalman gains in order to get estimates of the errors states.
These error states are then used to correct the states in the INS system.
Having established the initial understanding of ne alignment a further description of
the derivation of the Kalman gains is done.
b
n ib
Cb
b
ib
n nn
n
C
b
n
n
C
b
n n n
C
b
n
a
b
v
n
H
n
(C)
b H
a
b
b
v
n
H
FilterInput
1
s
1
s
1
s
r
n
H
K
1
K
3
K
4
u
zn
n
n
tilt
ie
K
2
Kalman
Filter
Verticalpart
of
n
ie
n
ie
H
INSSystem
v
n
H
n
H
r
n
tilt
) (
Figure 12.2: Illustration of the ne alignment procedure.
12.4. INDIRECT KALMAN FILTER APPLIED TO QUASI-STATIONARY FINE
ALIGNMENT 70
12.4 Indirect Kalman Filter Applied to Quasi-Stationary
Fine Alignment
In order to describe ne alignment within the Kalman lter framework as described in
Ch. 10 the observation set needs to be dened. As there is no aiding apart from the
knowledge of the quasi-stationarity, the observation set is:
z
obs
= r
n
H
r
n
refH
(12.11)
where r
n
ref
H
is the reference position for the position divergence which is approximated
to zero due to quasi-stationarity.
As mentioned earlier, ne alignment uses an indirect Kalman lter to estimate the error
states which means that the model used by the Kalman lter should be the error model
which is derived in Ch. 11. The error model used when deriving the Kalman gains for
ne alignment is the model from (11.27). This error model is restated below:

n
=
n
(
n
ie
+
n
en
) C
n
b
b
ib
+
n
ie
+
n
en
v
n
=
n
a
n
+C
n
b
a
b
(2
n
ie
+
n
en
) v
n
(
n
en
2
n
n
ie
) v
n
+
2g
R
hu
n
zn
+ g
n
M

n
=
n
n
en
+
n
en
n
en
=
1
R
(u
n
zn
v
n
)
h
R
2
(u
n
zn
v
n
) +
R
u
n
zn
h = C
n
e
e
ie
(12.12)
This error model is simplied as certain parameters are not relevant when performing
alignment. Using the fact that the platform is quasi-stationary zeroes the velocity v
n
and the transport rate
n
en
giving the following expression:

n
=
n
n
ie
C
n
b

ib
b
+
n
ie
v
n
=
n
a
n
+C
n
b
a
b
2
n
ie
v
n
+
2g
R
hu
n
R
+ g
n
M
(12.13)
These equations should be expanded to include the position divergence as this is the
driving parameter of the Kalman lter. This is easily included by recognizing that the
error in the position divergence vector is:
r
n
= v
n
(12.14)
As can be seen from (11.8) when
n
is constant, the rate of the earth rotation error is
zero:

n
ie
=
d
dt
(
n
e
ie
) = 0 (12.15)
Remembering that it is only the horizontal parameters which is of interest, the error
model for the Kalman lter is simplied to only contain the horizontal parts. This is
done as follows:
First of all the quasi-stationary error model from Sec. 12.4 is repeated below:

n
=
n
n
ie
C
n
b

ib
b
+
n
ie
v
n
=
n
f
n
+C
n
b
f
b
2
n
ie
v
n
+
2g
R
hu
n
R
+ g
n
M
(12.16)
Certain assumptions are made when reducing the error model to only containing hori-
zontal parts. First of all, the horizontal tilt/earth rate product from the rst equation
is assumed negligibly small[Savage, 2000]. Secondly, the vertical part of
n
is dened
to be zero at the beginning of ne alignment. This can be achieved as the local level
navigation frame n has an arbitrarily chosen heading which can be chosen to coincide
with C
n
b
heading at ne alignment initialization. As the ne alignment is performed
within a short time period, the vertical error in
n
does not grow large and thus can be
neglected. Lastly, the earth rate error/velocity error product is also assumed negligibly
small[Savage, 2000]. Furthermore, the last two terms of the second equation can be
removed as they are of no interest when looking at the horizontal parts. With these
assumptions and with the augmentation with the model of
n
ie
from Sec. 12.4, the error
model is presented in matrix form:
_
_

n
ie

n
v
n
_
_
=
_
_
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
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0
0 0 0 0 f
n
U
f
n
N
0 0 0
0 0 0 f
n
U
0 f
n
E
0 0 0
0 0 0 f
n
N
f
n
E
0 0 0 0
_
_
_
_
n
ie
n
v
n
_
_
(12.17)
+
_
_
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
c
11
c
12
c
13
0 0 0
c
21
c
22
c
23
0 0 0
c
31
c
32
c
33
0 0 0
0 0 0 c
11
c
12
c
13
0 0 0 c
21
c
22
c
23
0 0 0 c
31
c
32
c
33
_
_
_

b
ib
f
b
ib
_
(12.18)
This model is reduced to the horizontal version simply by removing the third parameter
of each variable. The horizontal version is presented below:
_
_

n
ie

n
v
n
_
_
H
=
_
_
0 0 0 0 0 0
0 0 0 0 0 0
1 0 0 0 0 0
0 1 0 0 0 0
0 0 0 f
n
U
0 0
0 0 f
n
U
0 0 0
_
_
_
_
n
ie
n
v
n
_
_
H
(12.19)
+
_
_
0 0 0 0
0 0 0 0
c
11
c
12
0 0
c
21
c
22
0 0
0 0 c
11
c
12
0 0 c
21
c
22
_
_
_

b
ib
f
b
ib
_
H
(12.20)
ALIGNMENT 72
This yields the following model:
x = [
n
ieH
,
n
H
, v
n
H
, r
n
H
]
n
p
= [
b
ib
, f
b
, 0, 0, 0, 0]
A =
_
_
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 0 g 0 0 0 0
0 0 g 0 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
_
_
G
p
=
_
_
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
C
11
C
12
0 0 0 0 0 0
C
21
C
22
0 0 0 0 0 0
0 0 C
11
C
12
0 0 0 0
0 0 C
21
C
22
0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
_
_
(12.21)
where:
x = Error states of the system
n
p
= Process noise vector
A = Error states dynamic matrix
G
p
= Process noise dynamic matrix
The measurement equation is the error form of (12.11) in order to bring it to an error
form suited for the indirect Kalman lter:
z = z
obs
= r
n
H
r
n
ref
H
(12.22)
where
r
n
H
= The position divergence error vector
r
n
refH
= Is the error due to the assumption of r
n
refH
equal to zero
If r
n
ref
H
is dened to be the dierence between the true value and the value used by
the INS:
r
n
ref
H
= r
n
INS
H
r
n
true
H
(12.23)
where
r
n
INSH
= 0
The true value can be recognized as the vibrating part of the position. This means that
r
n
ref
H
can be written as:
r
n
ref
H
= r
n
vib
H
(12.24)
where
r
n
vib
H
= The horizontal position error due to the quasi-stationary situation of the INS
With these denitions, the measurement equation is written as:
z = r
n
H
+ r
n
vib
H
(12.25)
With both the state and measurement equation dened the rst thing to notice is that
the error state dynamic matrix is constant which enables an oine calculation. The
discrete version is calculated in matlab and shown here:
n
=
_
_
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
T
n
0 1 0 0 0 0 0
0 T
n
0 1 0 0 0 0
0
g
2
T
2
n
0 gT
n
1 0 0 0
g
2
T
2
n
0 gT
n
0 0 1 0 0
0
g
6
T
3
n
0
g
2
T
2
n
T
n
0 1 0
g
6
T
3
n
0 gT
2
n
T
n
0 0 0 1
_
_
(12.26)
where:
n
= Discrete version of the error state dynamic matrix
T
n
= Sampling time
The next thing to calculate is the integrated process noise matrix Q
n
which is used
in the covariance equations. From [Savage, 2000] a second order approximation matrix
can be found to be:
Q
1n
= G
p
Q
pDens
G
T
p
T
n
Q
n

1
2
Q
1n
+
1
2
n
Q
1n
T
n
(12.27)
where:
Q
1n
= A rst order approximation of the integrated process noise matrix
Q
pDens
= The process noise density associated with the process noise vector n
p
It is assumed that the densities for the sensor errors in n
p
are equal and the process
noise density is dened as:
Q
pDens
=
_
_
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 q
0 0 0 0 0
0 0 0 q
0 0 0 0
0 0 0 0 q
f
0 0 0
0 0 0 0 0 q
f
0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
_
_
(12.28)
First of all Q
1n
is calculated:
Q
1n
=
_
_
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 q
0 0 0 0 0
0 0 0 q
0 0 0 0
0 0 0 0 q
f
0 0 0
0 0 0 0 0 q
f
0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
_
_
T
n
(12.29)
ALIGNMENT 74
The reason for Q
1n
being equal to
Q
pDens
Tn
is due to the orthogonality principle of C
n
b
where C
n
b
C
n
b
T
= I. Due to this property the integrated process noise is also constant
and can therefore be calculated oine.
The nal task is to construct the output equations. Using the denition of the measure-
ment equation in Sec. 10.1, (12.25) and (12.21) the measurement matrix, measurement
noise vector and measurement dynamic noise matrix is as follows:
H
n
= [0 0 0 0 0 0 I
2x2
]
G
mn
= I
2x2
n
mn
= r
n
vib
H
If we dene the elements of n
mn
to have equal and uncorrelated variance the measure-
ment noise covariance matrix can be written as follows [Savage, 2000]:
R
n
= E(n
mn
n
T
mn
) = p
r
vib
H
I
2x2
(12.30)
where
p
r
vib
H
= Horizontal quasi-stationary position random motion variance
E = Is the expected value operator (12.31)
Again it should be noted that both H
n
, G
mn
and R
n
are independent of time, attitude
and position.
With both the state and measurement equations established, the Kalman lter gain and
error covariance matrices can be calculated according to the following general Kalman
lter equations:
P
n
=
n
P
n-1
(+)
T
n
+Q
n
K
n
= P
n
()H
T
n
_
H
n
P
n
()H
T
n
+G
mn
R
n
G
T
mn
_
1
(12.32)
P
n
(+) = (I K
n
H
n
)P
n
()
The Kalman gains are repetitively calculated allowing an estimation of the states and a
control reset in order to bound the error build up. As mentioned in Sec. 10.1 the ideal
reset is:
u
nc
= x
n
(
e
) (12.33)
Using this reset the discrete Kalman equations from (10.7) is as follows:
x
n
(+
c
) = x
n
(+
e
) + u
n
= 0
x
n
() =
n
x
n-1
(+
c
) = 0
z
n
= H
n
x
n
() = 0
x
n
(+
e
) = x
n
() +K
n
(z
obsn
z
n
) = K
n
z
obsn
(12.34)
From (12.11) the observation is:
z
obsn
= r
n
H
(12.35)
This leads to the calculation of the control vector:
u
nc
= K
n
r
n
H
(12.36)
Equation (12.36) using K
n
from (12.32) constitute the complete Kalman lter and
the last thing to develop is the method to apply the control vector to the foreground
equations from (12.9). This is done by dening the individual parts of the control vector:
u
c
= [
n
ie
Hc
,
n
Hc
, v
n
Hc
, r
n
Hc
]
T
(12.37)
where
c
= Designation for the u
c
component which are to be applied
to the foreground equations (12.38)
The last thing to determine is how to correct the INS states with the control vector
from (12.37).
As the control vector contains the errors of the INS states it is obvious to simply subtract
the control vector from the INS state to get the correct state. Subtracting the control
vector from the earth rotation, velocity and position is straightforward, as these error
states are directly accessible from the control vector. Applying horizontal attitude error
n
H
to C
n
b
is performed as follows. As dened in (11.6) errors in the rotation matrix C
n
b
is:
C
n
b
=
n
C
n
b
(12.39)
This error can also be interpreted as the correction to C
n
b
() why the corrected C
n
b
(+
c
)
is:
C
n
b
(+
c
) = C
n
b
() + C
n
b

C
n
b
(+
c
) = (I
n
H
)C
n
b
() (12.40)
With this correction to the rotation matrix the four INS states are corrected in the
following way:
n
ieHn
(+
c
) =
n
ieHn
() +
n
ieHcn
C
n
b
n
(+
c
) = [I
2x2
n
Hcn
]C
n
b
n
()
v
n
Hn
(+
c
) = v
n
Hn
() + v
n
Hnc
r
n
Hn
(+
c
) = r
n
Hn
() + r
n
Hnc
(12.41)
To sum up, the ne alignment process consist of integrating the foreground equations,
(12.8), while applying the control vector, (12.37), which has been calculated by the use
of the Kalman gain (12.32).
12.5 Verication of Alignment
As the coarse and ne alignment has been developed, they are to be veried. Verication
of alignment is divided into two steps which are described below.
Step One:
In step one, the alignment schemes are veried by manually inputting acceleration and
gyroscopic measurements which would be measured by gyroscopes and accelerometers
at a specic and known orientation in space. This way the coarse alignment should make
a perfect one shot alignment. Secondly to verify the ne alignment the same inputs are
used while selecting the initial rotation matrix C
n
b
with a small error. This resembles a
real situation where the coarse alignment estimates C
n
b
with errors. The ne alignment
12.5. VERIFICATION OF ALIGNMENT 76
lter should correct C
n
b
and at ne alignment completion it should once again resemble
the perfect rotation matrix from coarse alignment.
Step Two:
In step two, the alignment scheme developed in this report is tested against an already
existing INS system. The system tested against is a Kearfott INS system. With this
INS it is possible to get both the roll, pitch and yaw angles after it has performed
coarse and ne alignment while accessing the raw measurements from the accelerometer
and gyroscope. This way it is possible to compare the roll, pitch and yaw angles with
and already existing system. Although it is not possible to distinguish which of the two
systems is the most correct, it is possible to compare the results and indicate the system
developed in this thesis does work correctly.
12.5.1 Step One
Coarse Alignment:
The coarse alignment calculates the rotation matrix C
n
b
and by inputting accelerometer
and gyroscope measurement which resembles a leveled INS pointing north, C
n
b
should
be unity. This is of course a special case with lots of zeros in the solution so several
other rotations of the body has also been tested. The special case is with accelerometer
input [0 0 g]
T
and gyroscopic input [0
ie
cos(l)
ie
sin(l)] where the platform is
positioned level on the earth at latitude l. C
n
b
should be unity as the body frame is
coincident with the n-frame and a test showed that the coarse alignment worked. Testing
with other rotations of the body frame showed that the coarse alignment worked.
Fine Alignment:
With the same acceleration and gyroscopic inputs as with the special case under coarse
alignment, but with an initial error in C
n
b
, ne alignment should decrease this error and
estimate the C
n
b
with greater accuracy. The error in the initial C
n
b
has been chosen to
be [0.5
, 0.7
, 0.4
] in [roll, pitch, yaw].

As mentioned in Sec. 12.4 the resets and torquing signals to correct C
n
b
is driven by the
error in the horizontal position. This error should be driven to zero as C
n
b
and earth
rotation
n
ie
is estimated more and more precisely. This means that the acceleration
and position should be driven to zero. Due to the errors in the initial C
n
b
, horizontal
accelerations will be present due to the gravitationally force. It will be expected to
give a positive acceleration to the north and a negative acceleration to the east due to
the misalignment in 0.5
, and 0.7
in roll and pitch error. As can be seen in Fig. 12.3

the initial north acceleration due to the gravitation projected onto the x- axis can be
calculated as: sin(0.5
) g = .086 which coincides with the graph. The same goes for
the east acceleration. It can also be seen that it converges to zero an expected.
The same characteristics can be seen by investigating Fig. 12.4. The position starts out
being zero, as it is the best guess to where the platform is positioned. By due to the
misalignment, the position changes until a correct estimate of C
n
b
and
n
ie
is given, at
which point the position is zero. Due to the fact that the closer the position gets to
zero the smaller the torquing signal, it will converge towards zero slower and slower. It
is again seen that the north position starts out going positive north and negative east
due to the components of gravity.
The next thing to verify is that the rotation matrix C
n
b
converges to the expected value.
With the same accelerometer and gyroscopic measurement, the expected rotation would
be unity with a yaw angle error. This originate from the fact that ne alignment only
torque horizontally to level the platform. Simultaneously it estimates the earth rotation
which is used to estimate the yaw angle.
0 0.2 0.4 0.6 0.8 1
0.15
0.1
0.05
0
0.05
0.1
Time [min]
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]

East Acceleration
North Acceleration
Figure 12.3: Acceleration due to misalignment
0 0.2 0.4 0.6 0.8 1
6
5
4
3
2
1
0
1
2
3
4
x 10
4
Time [min]
P
o
s
i
t
i
o
n

[
m
]

East Position
North Position
Figure 12.4: Position divergence due to misalignment
0 0.2 0.4 0.6 0.8 1
0.5
0
0.5
1
E
r
r
o
r

[
]

Roll Error
0 0.2 0.4 0.6 0.8 1
0.5
0
0.5
1
E
r
r
o
r

[
]

Pitch Error
0 0.2 0.4 0.6 0.8 1
0.5
0
0.5
1
Time [min]
E
r
r
o
r

[
]

Yaw Error
Figure 12.5: Euler rotation associated with C
n
b
The Euler rotation associated with C
n
b
is shown in Fig.12.5. As can be seen, the rotation
around x (roll), y (pitch) and z (yaw) starts out being the initial errors in the C
n
b
. The
ne alignment then levels the platform by torquing the horizontal parts to zero. As can
be seen in Fig. 12.5 only the roll and pitch rotation is converging to zero whilst the yaw
error is not torqued.
The expected value of the horizontal earth rotation rate is to be mainly on the y axis
due to the small yaw angle of 0.4
but with a small component on the x axis. So

at latitude 57.19
, which is the latitude at the test site at Aberdeen, a rotation of

cos(57.19)
ie
= 3.95e5 is expected on the y axis and much smaller on the x axis. As
can be seen in Fig. 12.6
n
ie
starts out being initialized as zero. But as soon as there is
an error in the position divergence vector the horizontal earth rate is changed.
A closer inspection, which can be seen in Fig. 12.7, reveals that the rotation around y
converges to around 3.9e-5 and small rotation approximately zero around x.
In order to take the misalignment around z into account the yaw angle needs to be
calculated. It is expected that the yaw angle converges towards the yaw error in C
n
b
.
As can be seen in Fig. 12.8 the yaw angle is stationary at 0.4
.
Step Two:
Step two shows the result from the test against an already existing INS system. The
Kearfott system was positioned on a table and completed a 15min ne alignment. This
was compared to a 15min ne alignment with the equations developed in this thesis.
The results are compared directly in Tab. 12.1:
As can be seen the Kearfott and the ne alignment used in this thesis estimate C
n
b
to
somewhat the same values. Although the dierences are in tenth of a degree, which is
unacceptable for an actual usage, they cannot be compared directly as there is some
uncertainty to how the Kearfott data should be interpreted. But the results imply that
the ne alignment developed does function with real-world measurements. To test if the
0 0.2 0.4 0.6 0.8 1
3
2.5
2
1.5
1
0.5
0
0.5
1
x 10
3
Time [min]
A
n
g
u
l
a
r

R
a
t
e

[
r
a
d
/
s
]

Earth Rotation Rate Around X
Earth Rotation Rate Around Y
Figure 12.6: Estimate of the earth rotation rate
0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1
0
2
4
6
8
10
x 10
5
Time [min]
A
n
g
u
l
a
r

R
a
t
e

[
r
a
d
/
s
]

Earth Rotation Rate Around X
Earth Rotation Rate Around Y
Figure 12.7: Estimate of the earth rotation rate
0 2 4 6 8 10
100
50
0
50
100

Y
a
w

A
n
g
l
e

[
]
2 3 4 5 6 7 8 9 10
1
0.5
0

Y
a
w

A
n
g
l
e

[
]
Time [min]
Figure 12.8: Wander angle at ne alignment completion
Kearfott This report
Role -81.2219
-81.3166
Pitch -12.04109
-12.2752
Yaw 8.0859
8.0990
Table 12.1: Comparison of a 15min ne alignment

estimated C
n
b
is a good estimate the accelerometer data is rotated through C
n
b
in order
to verify that C
n
b
actually levels the INS analytically. If the INS is leveled, then the
rotated accelerations measured in body frame should be [0 0 9.8171]
T
as the n-frame
is leveled. The result are seen in Fig. 12.9. As can be seen from Fig. 12.9 the rotated
acceleration with x and y sensitive axes is located around zero with the z sensitive axis
around 9.8. A mean of the values from time 5min to 15min is presented below. The
reason for not including the time before 5min is because ne alignment needs some time
to estimate C
n
b
. The results are seen in Fig. 12.2. The gravity in Aberdeen, where the
f
n
x
-2.55347332e-05
f
n
y
6.11895069e-04
f
n
z
9.81699624
Table 12.2: Measured acceleration rotated into the n-frame
measurements is performed can be calculated with (6.9) to be [0 0 9.81710728]
T
. As seen
from Tab. 12.2 the accelerations is rotated so only a component is in the z-direction.
And this component is very close to the theoretical. This means that C
n
b
does actually
level the INS.
0 2 4 6 8 10
0.2
0
0.2
a
x n
0 2 4 6 8 10
0.2
0
0.2
a
y n
0 2 4 6 8 10
9
9.5
10
10.5
a
z n
Time [min]
Figure 12.9: Accelerations rotated into n-frame
12.6 Discussion
As has been shown in the verication section, the alignment scheme developed in the
preceding chapter works and the rotation matrices is converging to the correct values.
The test cases in step one where alignment is tested with simulated measurements can
of course have innite convergence rate as the Kalman gains can be innitely high. So
the important thing is not the convergence time but the fact that is actually converges
to the correct values which it does.
In step two when testing alignment on real-world measurements, it was shown that
the alignment developed in this thesis did not converge to the exact same value as the
Kearfott. Due to uncertainties with the data extraction from the Kearfott unit the best
test is to check if the accelerations measurements in the navigation frame are actually
leveled. The dierence on the theoretical gravity in Aberdeen and the one measured is
approximately 0.1mg. This dierence stems from dierent sources. The alignment is of
course nor perfect but also the theoretical gravity, calculated from (6.9), does not take
the local gravity anomalies into account. Looking at magnitude of the error reveals that
the error in the vertical gravity has the same magnitude as the errors in the horizontal
plane. This indicate that it stem from noise and ne alignment precision.
As with the navigation equations, it is interesting to investigate what might inuence
alignment. Two dierent error sources are dominant in the alignment phase: Sensor
errors and errors due to a non-quasi-stationary state when assuming quasi-stationary.
First, it is investigated what eects arise from non-quasi-stationarity. In practice, two
movements diers from the quasi-stationary state. Horizontal velocity and rotation
around yaw. This situation might be a boat aligning at sea. Due to sea currents, the
12.6. DISCUSSION 82
boat drifts horizontally and rotates around the yaw axis. It does not rotate around
horizontal axis and does not have a vertical drift. These two situations have been
investigated.
Horizontal Drift
Horizontal drift causes two changes. First of all the centripetal acceleration due to
the speed around the earth. This acceleration is only measurable on the vertical axis.
Furthermore, a Coriolis eect is present which is due to the moving navigation frame.
This eect causes accelerations on the horizontal axis.
The earth simulation model from Ch. 8 has been used to simulate the accelerations and
rotation rates sensed by the sensors when moving with a horizontal speed of 10m/s both
North and East. The sensor outputs are showed in Fig. 12.10.
0 5 10 15
6.4145
6.414
6.4135
6.413
x 10
6
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]
East Axis
0 5 10 15
3.4747
3.4748
3.4749
3.475
x 10
6
North Axis
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]
0 5 10 15
2.4
2.2
2
1.8
x 10
6
Up Axis
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]
Time [min]
0 5 10 15
8.5
8.4
8.3
8.2
x 10
9
R
o
r
a
t
i
o
n

R
a
t
e

[
/
s
]
East Axis
0 5 10 15
2.8
3
3.2
3.4
x 10
9
North Axis
R
o
r
a
t
i
o
n

R
a
t
e

[
/
s
]
0 5 10 15
6
7
8
9
10
x 10
10
Time [min]
R
o
r
a
t
i
o
n

R
a
t
e

[
/
s
]
Up Axis
Figure 12.10: Errors in accelerations and rotation rates due to horizontal velocity
when assuming the INS to be stationary. All sensors outputs are in body frame.
Figure 12.10 shows the errors in both acceleration and rotation rate. As expected
there are accelerations errors on both horizontal and vertical axes. The errors in the
horizontal axes are due to the Coriolis eect and the error in the vertical axis is due
to the centripetal acceleration. The errors in the rotation rate is caused by the INS
following the curvature of the earth when moving North-East.
In order to examine the alignment errors due to this horizontal velocity alignment has
been performed with both quasi-stationary and non-quasi-stationary sensor outputs
and the alignment result compared. The important quantities are the heading error
and attitude error. Table 12.3 shows the alignment errors:
Alignment Error Magnitude [
]
Heading 12 10
3
Pitch 42 10
6
Roll 13 10
6
Table 12.3: Comparison of alignment error when aligning with horizontal velocity.
As can be seen from Tab. 12.3, aligning with horizontal velocity causes a heading error
of 12 millidegree and attitude errors in the microdegree range.
Rotation Around Yaw
A rotation around yaw could be caused by aligning on a ship yawing. This yaw error
only causes errors in the rotation rates and not in the acceleration as the boat does not
have any translatory motion. The vertical yaw error is directly connected to the yaw rate
of the ship, and should be constant, whilst the horizontal yaw errors are the horizontal
earth rotation components projected into the two horizontal axes. It is assumed that
the yaw errors increase on both horizontal axes as the boat yaws more and more from
zero.
As with the horizontal velocity, rotation around yaw has been simulated. A yaw rate at
1/200

/s has been chosen, which results in 4.5
after 15 minutes. The sensor outputs

are shown in Fig. 12.11.
As can be seen in Fig. 12.11, the acceleration errors are all zero as expected. The
vertical yaw error is constant at 8.7267 10
5
rad/s which is 1/200
/s. The horizontal

yaw errors both start at zero and increases with time. As can be seen, the North axis
rotation error looks like a linear increase whilst the East axis rotation error looks like
an exponential increase. This is because the earth rotation which is around the North
axis is projected onto the rotated North and East axes. This is projected with a sine
and a sine at 90
change almost linear whilst a sine at 0
changes exponentially. As
expected, the North axis rotation should become smaller with a positive yaw rotation
and the East axis rotation becomes larger. This explains the negative North error and
positive East error.
Again the alignment error with yaw error is examined and compared to alignment
without any motion and with horizontal velocity. Table 12.4 shows the result:
]
Heading 46 10
6
Pitch 5 10
6
Roll 6 10
6
Table 12.4: Comparison of alignment error when aligning with a yaw rotation.
As can be seen from Tab.12.4, the error in both heading and attitude are in millidegrees.
What is interesting to see, is that the accelerations and angular rates arising from the
non-quasi-stationarity are in the order of 10
6
[m/s
2
] and 10
9
[rad/s] for nonzero
velocity. High-grade inertial sensors today experience bias stabilities of 10
3
[m/s
2
] and
10
8
[rad/s]. Hence, it can be concluded, that sensor errors have a larger inuence on
12.6. DISCUSSION 84
0 5 10 15
1
0.5
0
0.5
1
x 10
6
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]
East Axis
0 5 10 15
1
0.5
0
0.5
1
x 10
6
North Axis
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]
0 5 10 15
1
0.5
0
0.5
1
x 10
6
Up Axis
A
c
c
e
l
e
r
a
t
i
o
n

[
m
/
s
2
]
Time [min]
0 5 10 15
0
1
2
3
4
x 10
6
R
o
r
a
t
i
o
n

R
a
t
e

[
/
s
]
East Axis
0 5 10 15
1.5
1
0.5
0
x 10
7
North Axis
R
o
r
a
t
i
o
n

R
a
t
e

[
/
s
]
0 5 10 15
8.7267
8.7267
8.7267
8.7267
x 10
5
Time [min]
R
o
r
a
t
i
o
n

R
a
t
e

[
/
s
]
Up Axis
Figure 12.11: Errors in accelerations and rotation rate due to rotation around yaw
when assuming the INS to be stationary. All sensors outputs are in body frame.
the alignment, than a constant velocity during alignment. In order to see the alignment
error when using a high grade gyroscope an alignment has been performed with a
gyroscope with a bias of 0.01 [
/hour] which is equal to 8 10

8
[rad/s]. Table 12.5
shows alignment result:
]
Heading 71 10
3
Pitch 5.1 10
6
Roll 7.9 10
6
Table 12.5: Comparison of alignment error when aligning with a gyro bias of 0.01
[
/hour].
What can be derived from these test is that gyro bias causes larger alignment errors
than horizontal velocity. And vertical rotation does only inuence alignment marginal.
12.7 Summary
During this chapter, has the alignment procedure been developed and been tested with
simulated and real-world measurements.
The alignment procedure is divided into two phases. First, the coarse alignment is
performed which is a one shot approach where the gravity and earth rotation of a
known frame is compared to the acceleration and gyroscope measurement. In order
to get a better estimate of C
n
b
, the ne alignment procedure is performed afterwards.
Fine alignment uses an indirect Kalman lter in order to estimate attitude and heading
of the INS. In order to perform a precise ne alignment, the INS is assumed to be in
quasi-stationary state. Quasi-stationary means that over time the position is zero. This
knowledge along with the knowledge of the sensor errors is used with the Kalman lter
to estimate attitude and heading of the INS.
The ne alignment procedure has been tested with both simulated inputs and with real-
world sensor measurements. Both tests showed that the alignment procedure worked
and ne alignment was able to decrease the error in the one shot coarse alignment
estimate.
Chapter 13
Inertial Navigation with Aiding
Unaided INS navigation inevitably suer from unbounded errors in both velocity, po-
sition and attitude which may be unacceptable in many applications. For this reason
some kind of aiding is needed to either bound or reduce these errors. Many dierent
forms of aiding can be used giving various results. Some of the more popular aiding
devices are Doppler Velocity Logs (DVL), Global Positioning System (GPS) and baro-
metric measurements. The main idea is that with independent information at hand the
Kalman lter is able to estimate with greater accuracy. This report will describe the
use of two dierent aiding devices, a speedometer and GPS. Aiding with a speedometer
will not bound the position error but merely decrease the rate to which they increase
whilst aiding with GPS will bound the position error.
This chapter will start out with a description of the GPS system and describe some
dierent ways to implement GPS with an INS system. An error model of the GPS will
be derived leading to an error model of the INS/GPS system ready for Kalman ltering.
Secondly, the same procedure will be followed describing how to aid with a speedometer.
This chapter will end with a description of the approach used in this report to aid the
INS with a GPS.
13.1 Aiding with GPS
The idea behind aiding with GPS originate from the fact that although the two systems
produce the same outputs, position and velocity, they measure dierent quantities and
suer from dierent error characteristics. Where an INS system estimates position and
velocity by measuring acceleration and angular rate and afterward double integrating
these quantities to give the output, GPS measures position and velocity directly. So
where the INS will have an unbounded long-time error drift, the GPS is bounded, though
experiencing short time error drift. A comparison of the two systems is described below
in Tab. 13.1:
86
87 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
Advantages Disadvantages
INS Very precise estimates Unbound errors
Provide attitude estimates Knowledge of gravity required
Fast update rate
Noise almost white
GPS Bounded errors No attitude estimate
Slow update rate
Coloured noise
Table 13.1: Comparison of GPS and INS
These dierent error characteristics makes for a benecial combination of the two mea-
surements.
13.1.1 Description of GPS
The GPS system consists of at least 24 satellites orbiting the earth. The orbits of the
satellites are arranged in six planes, with at least four satellites in each plane. They
are arranged in such a way that at least four satellites are visible from anywhere on the
earth. With the use of at least four satellites both position and velocity of the GPS
antenna can be estimated.
Although it might seem peculiar why four satellites are needed to describe a position
in Cartesian space, where normally only three parameters are needed, the reason is the
clock oset in the GPS receiver. As the satellites are not in geostationary orbit the
GPS receiver has a clock which keep track on where the satellites are at a specic time.
As the clock in the receiver is not synchronized with the satellite clock this clock oset
also needs to be determined. For this reason, four satellites are needed to determine the
position.
A general GPS scheme is illustrated below.
Antennaand
RFHardware
Code
Correlation
Codetracking
Loop
Carrier
TrackingLoop
DeltaRange
PseudoRange
Figure 13.1: Illustration of a simplied GPS receiver scheme
[Brown and Hwang, 1997]
The rst block of Fig. 13.1 is the antenna and RF hardware which is used to receive the
GPS signals. As all the satellites transmits on the same frequency the received signals
are a mix up of all the visible satellites signals. The code correlation algorithm extracts
the signal from the correct GPS which is then sent to the code- and carrier tracking loop.
The code tracking loop is the algorithm which actually estimates the pseudo range. The
pseudo range is the designation for the time the signal has traveled from the dierent
satellites to the receiver. This is the reason for the name pseudo range as it is not a
distance, but the traveling time of the signals. The carrier loop is the algorithm which
13.1. AIDING WITH GPS 88
estimates the velocity of the receiver. It uses the fact that the carrier wave changes
frequency when either the satellite or receiver is moving. The velocity of the satellites
is known at any time so the receivers velocity can be estimated because of this Doppler
eect.
As mentioned earlier, in order to calculate the position of the receiver at least four
satellites are needed. With these four satellites, the position is estimated by solving the
following system[Brown and Hwang, 1997]:
1
=
_
(x
sat1
x
gps
)
2
+ (y
sat1
y
gps
)
2
+ (z
sat1
z
gps
)
2
+ ct
2
=
_
(x
sat2
x
gps
)
2
+ (y
sat2
y
gps
)
2
+ (z
sat2
z
gps
)
2
+ ct
3
=
_
(x
sat3
x
gps
)
2
+ (y
sat3
y
gps
)
2
+ (z
sat3
z
gps
)
2
+ ct
4
=
_
(x
sat4
x
gps
)
2
+ (y
sat4
y
gps
)
2
+ (z
sat4
z
gps
)
2
+ ct (13.1)
where
i
= Pseudorange from the ith satellite to the GPS receiver antenna.
x
sati
, y
sati
, z
sati
= Position of the ith satellite
x
gps
, y
gps
, z
gps
= Position of the GPS receiver antenna
c = Speed of light
t = Oset between the clock in the satellite and the clock in the receiver
The four equations in (13.1) describes four spheres with center at the ith satellite
where the radius is corrected with the clock oset. So by solving these four equations,
the intersection of these four spheres is the location of the receiver. So it is obvious that
the more satellites the more precise estimate of the position.
Aiding with GPS can be divided into three dierent approaches. Uncoupled, loosely
coupled and tightly coupled aiding. A short introduction will describe the advantages
and drawbacks of the dierent approaches.
13.1.2 Uncoupled
The uncoupled aiding scheme is the simplest method. It uses the GPS measurements
when these are available and uses the INS during GPS outages. With valid GPS data
these are used to reset the error states within the INS system so with this method
it is possible to bound the errors within the INS with regular GPS data. If GPS
measurements are missing over large time periods the errors within the INS will of course
grow. This approach does not involve any changes to the two systems and they will
function separately if one system fails. This means that it can always be implemented to
an already existing INS system and with any kind of GPS receiver. The disadvantages
is that it is not possible to gain the same precision as with the coupled integration as
will be described below.
13.1.3 Loosely Coupled
With the loosely coupled approach, the GPS system still functions separately whilst the
INS system uses the GPS measurements as a parameter in the measurement equation.
A simplied representation of this integration is seen below:
As can be seen in Fig. 13.2, the output from the GPS algorithms and INS measurements
are used as the input to the Kalman integration lter. The main advantages of this
AntennaandRF
hardware
Correlationand
trackingloops
GPSKalmanfilter
Inertialsensors
INScomputation
INS/GPSKalman
filterintegration
GPSreceiver
INS
GPSpositionand
velocityestimates
INSpositionand
velocityestimates
Errorestimations
Position
Figure 13.2: Lossely coupled INS/GPS approach
approach is that it can be implemented with any existing INS system and with any
GPS receiver. Furthermore, both systems will function separately although one of the
systems fails although this requires some kind of error detection. As can also be seen
the GPS receiver is also aided with INS states. This is due to the fact that the position
is used in the code tracking loop so the corrected position can be used to estimate the
receiver clock error within the GPS receiver.
The main disadvantage with the loosely coupled integration is the fact that this in-
tegration is a cascade of two Kalman lters. As the output from the GPS Kalman
lter is used as a measurement this is assumed to have white noise by the integration
Kalman lter. This is not necessary true as the output from the GPS may be time
correlated. This can happen if the integration lter samples data faster than the GPS
can supply uncorrelated data. It can also happen due to multipath interference which
happens when the same satellite signal is received several times due to reections. Yet
another problem is the fact that at least four satellites are needed to get position and
velocity output from the GPS. In situations where less than four satellites are available,
aiding is not possible. Furthermore is it a problem that the integration lter requires
knowledge of the covariance matrix of the GPS lter output. This covariance matrix
dier with satellite geometry and most GPS receivers does not give this information.
[Titterton and Weston, 2005].
13.1.4 Tightly Coupled
The tightly coupled aiding approach is shown in Fig. 13.3.
AntennaandRF
hardware
Correlationand
trackingloops
Inertialsensors
INScomputation
INS/GPSKalman
filterintegration
GPSreceiver
INS
INSpseudo-rangeand
delta-rangeestimates
Errorestimations
Position
GPSpseudo-rangeand
delta-rangeestimates
Figure 13.3: Tightly coupled INS/GPS approach
As can be seen in the gure, it is now the pseudo-range and delta-range which are
used in the integration lter. These quantities are the outputs from the GPS tracking
loops and can be used to model errors in the INS system. As with the loosely coupled
13.2. GPS ERROR MODEL ANALYSIS 90
integration, the GPS tracking loops are aided with the INS states in the same manner
as with the loosely coupled approach. The advantages of this approach is as follows:
The statistical problem with the output from the loosely coupled approach is
eliminated by integrating the two Kalman lters in one.
This approach will function although four satellites are not visible to the GPS
receiver though accuracy will decrease drastically.
As can be seen from Fig. 13.3 the GPS position and velocity is not present. These
estimates can be produced in a separate loop and error detection may be performed on
these estimates.
This approach is preferable to the loosely coupled approach but the implementation is
more complicated and the pseudo-range and delta-range might not be available from
normal GPS receivers.
13.2 GPS Error Model Analysis
If the tightly coupled INS/GPS integration is to be used, an error model of the GPS
is needed. A disadvantage with this approach is that it is not all GPS receivers which
can be used as they normally only output position and velocity where pseudo range
and delta range is needed. Using the loosely coupled integration any GPS receiver can
be used as only the position and velocity is needed along with the precision of these
quantities, which is described in the data sheet.
This section will present an error model for the tightly coupled integration followed by
an error model for aiding with a speedometer and lastly present an approach for the
loosely coupled integration which is the one used in this report.
13.2.1 Error Model for the Tightly Coupled INS/GPS Integra-
tion
The observation for the GPS receiver is the ith GPS measured range from the INS
to the satellites ( p
GPSi
) subtracted from the INS estimated range from the INS to the
satellite p
i
. It should be noted that the GPS range should be corrected for the lever
arm between the INS and the GPS receiver antenna. The observation is as follows:
z
obsi
= p
i
p
GPSi
(13.2)
The GPS range is calculated using the measured time interval between the time the
satellite sent the signal to the time the receiver received the signal. This can be expressed
as follows:
p
GPSi
= c(
t
reci

t
GPSi
) (13.3)
where
t
reci
= The time of the ith signal reception of the GPS antenna
t
GPSi
= The time of signal transmission from the ith satellite
c = The speed of light
The measurement equation for the Kalman lter needs to be in a form suitable for
the lter which is in error states. For this reason the measurement equation is the
dierenced version of (13.2), z = z
obs
z
obs
which is:
z
i
= p
i
c(t
reci
t
GPSi
) (13.4)
This is the measurement equation for the Kalman lter so an expression needs to be
derived for the three quantities p
i
, t
reci
, t
GPSi
The quantity p
i
is the error in the range from the INS to the ith satellite. This error
originate from two dierent error sources. First of all, the position estimated by the
INS is error-prone and secondly, the position of the satellite, which is estimated from
the ephemeris data, is error-prone. As the ephemeris data gives the satellite position in
earth xed coordinate system(E-frame) the range is also dened in the E-frame:
p
e
i
= r
e
si
r
e
l
e
(13.5)
where
r
e
si
= Position vector from the center of the earth to the ith satellite
r
e
= Position vector from the center of the earth to the INS
l
e
= Lever arm from GPS antenna to INS
In order to nd the range error the magnitude of (13.5) is dened as:
p
2
i
= ( p
e
i
)
T
p
e
i
(13.6)
In order to nd the range error p
i
the dierential of (13.6) is found:
2 p
i
p
i
= (p
e
i
)
T
p
e
i
+ ( p
e
i
)
T
p
e
i
= 2( p
e
i
)
T
p
e
i
(13.7)
which can be rearranged into:
p
i
=
( p
e
i
)
T
p
e
i
p
i
(13.8)
p
e
i
can be substituted with the dierential of (13.5) to give:
p
i
=
( p
e
i
)
T
(r
e
si
r
e
l
e
)
p
i
(13.9)
As it can be seen from (13.9), p
i
is calculated from a vector dot product. As a dot
product is invariant p
i
can be evaluated in the navigation frame[Savage, 2000]:
p
i
=
(C
n
e
p
e
i
)
T
(C
n
e
r
e
si
r
n
l
n
)
p
i
(13.10)
The lever arm error can be expressed as follows:
l
n
= (C
n
b
l
b
) = C
n
b
l
b
+C
n
b
l
b
(13.11)
Using (11.6) on (13.11) reformulates it to:
l
n
=
g
C
n
b
l
b
+C
n
b
l
b
= (C
n
b
l
b
) +C
n
b
l
b
(13.12)
With these rearrangement p
i
from (13.4) is:
p
i
=
(C
n
e
p
e
i
)
T
(C
n
e
r
e
si
r
n
(C
n
b
l
b
) +C
n
b
l
b
)
p
i
(13.13)
13.2. GPS ERROR MODEL ANALYSIS 92
The last thing to do before implementation with the Kalman lter is to derive expres-
sions for the lever arm error l
b
, position error of the ith satellite r
e
si
and the clock
error t
reci
,t
reci
.
The true lever arm is modelled as a constant and a exing term:
l
b
= l
b
const
+l
b
ex
l
b
const
= 0 (13.14)
As the lever arm is assume constant the lever used by the computer is a constant:
l
b
= l
b
const
l
b
const
= 0 (13.15)
The lever arm error is the estimated lever arm subtracted from the true lever arm:
l
b
= l
b
const
l
b
ex

l
b
const
= 0 (13.16)
For INS/GPS integration, it is customary to assume that r
e
si
and t
reci
are zero as
ephemeris data are very accurate and the atomic clock in the satellite is very precise.
The main source of error originate from the receiver clock error. This receiver clock
error is modelled as follows.
A clock suer from two kinds of errors. A random error caused by jitter and quantization
and an oscillatory error[Savage, 2000]. In order to describe these errors, it is dened
how a clock is normally constructed. Normally a clock is simply a matter of counting
ticks from a oscillatory source, such as a crystal. So the receiver clock time is given by:
t
recn
=

t
recn-1
+ t
osc0
=

t
recn-1
+
1
f
osc0
(13.17)
where
t
rec
= Clock time
t
osc0
= Nominal time between clock ticks
f
osc0
= Nominal clock frequency (13.18)
Rearranging this clock time equation and dividing with the actual time between clock
ticks t gives:
t
recn

t
recn-1
t
=
1
f
osc0
t
=
1
f
osc0
1
t
(13.19)
Recognizing that the left-hand term is the derivative of

t
recn
yields:
t
rec
=
f
osc
f
osc0
(13.20)
where
f
osc
= 1/t = The actual clock frequency
As it is the error in the clock time r
reci
from (13.4) which is needed this is derived as
the dierential of (13.20) plus a random term [Savage, 2000]:
t
f
=
f
osc
f
osc0
t
reci
= t
f
+ n
t
(13.21)
where
t
f
= Is the receiver clock error without the random part due to jitter and quantisation
n
t
= Is the random part of receiver clock error due to jitter and quantisation
f
osc
= Is the oscillatory part of the receiver clock error
The random part n
t
can be modelled as white noise as it is not correlated from satellite-
to-satellite or from cycle-to-cycle. The oscillatory part can be modelled as a random
varying parameter plus a rst order Markov process[Savage, 2000]:
f
osc
= f
osc,rand
+ f
osc,Mark
f
osc,rand
= n
osc,rand
f
osc,Mark
= C
osc,Mark
f
osc,Mark
+ n
osc,Mark
(13.22)
where
f
osc,rand
, f
osc,Mark
= Is the random and rst order Markov process error contribution
C
osc,Mark
, n
osc,Mark
= Is the correlation frequency and white noise error contribution
for the rst order Markov process
n
osc,rand
= Is the white noise error source for the oscillatory part of the receiver clock error
(13.23)
This section described the derivation of the GPS error model needed when using tightly
coupled integration. The last thing before implementation is to determine the numeric
values of the clock error model. As this involves modeling and testing the crystal of
the specic GPS receiver this is quite time consuming and is not within the scope of
this report. For this reason the loosely coupled integration is used which is described
in Sec. 13.3.
13.2.2 Error Model for the INS/Speedometer Integration
Aiding with a speedometer diers somewhat from aiding with a position. The lever arm
between the INS and aiding device needs to be introduced as the INS and aiding device
normally does not output velocity in the exact same position. This means that if the
INS rotate the aiding device would output an erroneous velocity if not correct for the
lever arm. So the velocity of the INS measured by the aiding device is the measured
velocity corrected for lever arm velocity in the following way:
v
n
INS,aid
= C
n
b
v
b
ref
+C
n
e
l
e
(13.24)
The observation equation for the Kalman lter is the INS estimated velocity ( v
n
) minus
the measured velocity (v
n
INS,aid
):
z
obs
= v
n
v
n
INS,aid
= v
n

C
n
b
v
b
ref

C
n
e
l
e
(13.25)
The velocity of the lever arm can be expressed as the rotation of the INS multiplied
with the lever arm by which the observation equation is expressed as:
z
obs
= v
n

C
n
b
v
b
ref

C
n
b
(
b
ib
l
b
) (13.26)
The associated measurement equation is the dierential of (13.26):
z = v
n
C
n
b
v
b
ref

C
n
b
v
b
ref

C
n
b
(
b
ib
l
b
) (13.27)
13.3. IMPLEMENTATION OF NAVIGATION WITH AIDING 94
where C
n
b
(
b
ib
l
b
) and C
n
b
(
b
ib
l
b
) have been neglected as they are assumed very
small [Savage, 2000]. Using (11.6) and the lever arm error model from (13.16) on (13.27)
reveals:
z =v
n
+ (
n
C
n
b
) v
b
ref

C
n
b
v
b
ref

C
n
b
(
b
ib
(l
b
const
l
b
ex
))
v
n
v
n
ref

C
n
b
v
b
ref

C
n
b
(
b
ib
l
b
const
) +

C
n
b
(
b
ib
l
b
ex
) (13.28)
An approximation for v
b
ref
derived from (13.24) gives the following:
v
n
ref
= v
n

C
n
b
(
b
ib
l
b
) (13.29)
Using this expression in (13.28) gives:
z = v
n
( v
n

C
n
b
(
b
ib
l
b
))
n

C
n
b
v
b
ref

C
n
b
(
b
ib
l
b
const
) +

C
n
b
(
b
ib
l
b
ex
)
(13.30)
As the parameter

l
b
can be set freely as long as l
b
const
is taken account,

l
b
is chosen
to be zero. Furthermore, the measurement equation is modied into containing -
error parameters rather than error parameters using (11.32) and (11.37). These two
properties relating the two error models are restated here:
n
=
n
n
= v
n
+
n
v
n
(13.31)
Using these properties gives the following:
z =
n
v
n

n

C
n
b
v
b
ref

C
n
b
(
b
ib
l
b
const
) +

C
n
b
(
b
ib
l
b
ex
) (13.32)
At this point, the integration of a speedometer is almost ready to be implemented. The
only thing missing is an expression for the velocity error experienced by the speedometer
v
b
ref
. As this expression diers according to which speedometer is used and it is quite
cumbersome to develop the error model, this is outside the scope of this report. But it
should be quite clear how to augment the IMU error model with the speedometer error
model and thus be ready for Kalman implementation.
13.3 Implementation of Navigation with Aiding
This section will describe the derivation of the Kalman equations used to implement
the loosely coupled INS/GPS integration.
13.3.1 Loosely Coupled INS/GPS Integration
As mentioned earlier, the loosely coupled integration combines the INS and GPS mea-
surements to estimate the velocity, position and attitude error of the INS. No error
model of the GPS is needed as it is the position and/or velocity from the GPS which
is used to form the observation set and the precision of the specic GPS which is used
+
-
INS
GPS
GPS
INS
INS
r v
n n
r v
n n
GPS
INS
Kalman
Filter
v
n
n
r
n
r
v
n
n
r
n
Figure 13.4: Illustration of the loosely coupled INS/GPS integration
as measurement noise. The precision of the GPS is described in the data sheet and
therefore, this approach can always be implemented in any INS system. A gure of the
INS/GPS integration is presented below.
As can be seen from Fig. 13.4 the GPS measurements are subtracted from the INS
measurements to form the observation set. The Kalman lter estimates the errors
which are used to reset the INS. If possible the GPS receiver can be aided with the
position which can be used to aid the code tracking loop in the GPS receiver.
Derivation of Kalman Filter Equations
The Kalman lter is developed using the same approach as in Sec. 12.4.
The observation equation is:
z
obs
=
_
r
n
INS
r
n
GPS
v
n
INS
v
n
GPS
_
(13.33)
The measurement equation is:
z = z
n
obs
z
n
obs

z =
_
r
n
true
+ r
n
INS
r
n
true
r
n
GPS
v
n
true
+ v
n
INS
v
n
true
v
n
GPS
_
=
_
r
n
INS
r
n
GPS
v
n
INS
v
n
GPS
_
(13.34)
where
r
n
INS
, v
n
INS
= Is the error in the INS estimation of position and velocity
r
n
GPS
, v
n
GPS
= Is the error in the GPS estimation of position and velocity
r
n
true
, v
n
true
= Is the true position and velocity
With the measurement equation derived, the model for the Kalman lter is:
x = Ax +G
p
n
p
z = Hx +G
m
n
m
(13.35)
where
x =
_
n
v
n
r
n
T
(13.36)
The error state dynamic matrix (A) is the time varying -error model from (11.54) and
13.3. IMPLEMENTATION OF NAVIGATION WITH AIDING 96
the process noise dynamic matrix is derived from (13.34) and is as follows:
H =
_
_
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
_
_
(13.37)
The measurement noise is derived in the same way as in Sec. 12.4 and is as follows:
G
mn
=
_
I
3x3
0
0 I
3x3
_
n
mn
=
_
v
n
GPS
r
n
GPS
_
(13.38)
As in Sec. 12.4 n
m
is assumed to have equal and uncorrelated variance which leads to
the following measurement covariance matrix:
R
n
= E(n
mn
n
T
mn
) =
_
I
3x3
GPS
velocity
0
0 I
3x3
GPS
position
_
(13.39)
where
GPS
velocity
, GPS
position
= GPS velocity and position variance
E = Is the expected value operator
The last thing before implementation is to nd the discrete error state dynamic matrix
and the integrated process noise matrix.
The error model which should be used when in navigation mode is the one from (11.54).
The discrete error state dynamic matrix is computed in Matlab shown below:
=
_
_
1 T(
c
) T(
b
) 0 0 0 0 0 0
T(
c
) 1 T(
a
+
) 0 0 0 0 0 0
T(
b
+
) T(
a
) 1 0 0 0 0 0 0
0 Ta
U
Ta
N
1 T(2
c
) T(2
b
) T
g
r
0 0
Ta
U
0 Ta
E
T(2
c
) 1 T(2
a
+
) 0 T
g
r
0
Ta
N
Ta
E
0 T(2
b
+
) T(2
a
) 1 0 0 2T
g
r
0 0.5T
2
a
U
0.5T
2
a
N
T 0 0 1 0
0.5T
2
a
U
0 0.5T
2
a
E
0 T 0 0 1
0.5T
2
a
N
0.5T
2
a
E
0 0 0 T
1
_
_
(13.40)
The integrated process noise matrix is found using the same approach as in Sec. 12.4
where:
Q
1n
= G
p
Q
pDens
G
T
p
T
n
Q
n

1
2
Q
1n
+
1
2
n
Q
1n
T
n
(13.41)
So the rst order process noise matrix is:
Q
1n
=
_
_
q
w
0 0 0 0 0 0 0 0
0 q
w
0 0 0 0 0 0 0
0 0 q
w
0 0 0 0 0 0
0 0 0 q
f
0 0 0 0 0
0 0 0 0 q
f
0 0 0 0
0 0 0 0 0 q
f
0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
_
_
T
n
(13.42)
The last thing to dene the control reset vector and how it is used correct the INS
states.
The control reset vector is dened as in Sec. 12.4 which gives:
u
c
= [
n
, v
v
, r
v
]
T
(13.43)
The states which are corrected are the body to navigation rotation matrix C
n
b
with
n
,
navigation to Earth rotation matrix C
e
n
with
n
and velocity v
n
with v
n
. If one look
at u
c
the only state which can be directly corrected is the velocity. In order to correct
C
n
b
and C
e
n
some transformations are necessary. As contain both the error in C
n
b
and
C
e
n
, r is needed as it describe the errors in C
e
n
. It has been dened earlier that the
navigation frame does not rotate with respect to the geodetic frame. This means that
all the heading error is in
n
. But in order to extract
n
from ,
n
is needed. Using
the r
n
relation from (11.43)
n
is calculated from r
n
in the following way:
r
n
= R(
n
u
n
zn
) + Ru
n
zn

_
_
r
x
/R
r
y
/R
r
z
/R
_
_
=
_
_
y
x
0
_
_
+ Ru (13.44)
Having constructed
n
,
n
can be calculated from the
n
relation from (11.43) in the
following way:
n
=
n
+
n
(13.45)
At this point, all the necessary parameters are available and the INS states are corrected
as described in (12.41):
C
n
b
(+
c
) = [I
3x3
n
]C
n
b
C
e
n
(+
c
) = [I
3x3
E
n
]C
e
n
v
n
(+
c
) = v(
c
) + v
n
(13.46)
With these equations derived, the Kalman lter is ready for implementation and veri-
cation.
13.4 Verication of Navigation with Aiding
In order to test the aiding scheme developed in the preceding section both a GPS and
an INS is needed. It has not been possible to get GPS data while navigating with an
INS and for this reason another approach has been used to verify the aiding scheme.
13.4. VERIFICATION OF NAVIGATION WITH AIDING 98
The test data used to verify navigation with aiding is the same as used in Sec. 9.5. With
a drive time of approximately 5.6 [min], the Kearfott does only experience minor drift
why the Kearfott track can be used as the reference track. In order to emulate the GPS
position a noise signal has been added to the Kearfott navigation data with a standard
deviation of 5 [m] and extracted with a sample time of one second. These data have
been used as the GPS position.
Two test scenarios are presented. One test scenario where the INS starts out with
15 [min] alignment followed by navigation without aiding. The track is the same track
used to verify the navigation equations in Sec. 9.5. The second test scenario is a scenario
where the INS again starts with 15 [min] alignment. The same track is followed as in
the rst test scenario but navigation is aided with the GPS data. A GPS outage of 40 [s]
duration is simulated after 3.3 [min] driving time. During this time, the INS navigates
without any aiding.
The two test scenarios should show how aiding is able to bound the position error, and
how the INS system keeps the system on track during GPS outages.
Figure 13.5 shows navigation without aiding.
2.086 2.084 2.082 2.08 2.078
57.186
57.187
57.188
57.189
57.19
57.191
57.192
57.193
57.194
Longitude []
L
a
t
i
t
u
d
e

[
]

Aided position
Real position
Figure 13.5: This gure shows navigation on real-world measurements without
aiding.
As can be seen from Fig. 13.5 the navigation starts out following the reference track but
drifts away from the reference track unbounded. Aiding with a GPS should bound the
position error. Navigation with aiding is shown in Fig. 13.6
Figure 13.6 shows the reference track, the GPS position and the aided navigation. As
can be seen the aided navigation follows the reference track. Just before the North turn,
in the bottom right of the track, the GPS data outage occurs. The INS then navigate
2.086 2.084 2.082 2.08 2.078
57.186
57.187
57.188
57.189
57.19
57.191
57.192
57.193
57.194
Longitude []
L
a
t
i
t
u
d
e

[
]

Aided position
GPS position
Real position
Figure 13.6: This gure shows navigation on real-world measurements aiding with
a GPS.
without aiding but the position starts to drift from the reference track. It does, however,
follow the turns made by the vehicle. As can be seen the x which indicate the GPS
position is absent. Just before the turn to the South the GPS data is valid again and
thus pulls the navigation towards the reference track.
What can not be seen in Fig. 13.6 is how the INS/GPS position variates around the
reference track. In order to investigate that Fig. 13.7 shows a 2d plot of the latitude
and longitude.
The rst row of Fig. 13.7 shows the latitude where the second row shows the longitude
of the INS. Although it is hard to see any details on the two plots on the left column
they show that the fused INS/GPS position has not been time shifted. But the plots on
the right column shows a detailed view of the latitude and longitude the rst minute of
navigation. What can be seen is that the fused position (dotted) does not suer from
the same sudden jumps to the same degree as the GPS (x).
13.5 Summary
As unaided INS inevitably suers from unbounded position error, some kind of aiding
is preferable if INS is to be used over longer time periods. During this chapter, two
dierent aiding devices have been presented, a GPS and a Doppler velocity log. Aiding
with a GPS, where both the velocity and position are available bounds the position
error. The precision of this position depends on how tightly the INS is fused with the
GPS. Three dierent approaches are described in this thesis where the loosely coupled
13.5. SUMMARY 100
0 1 2 3 4 5
57.186
57.187
57.188
57.189
57.19
57.191
57.192
L
a
t
i
t
u
d
e

[
]
0 0.2 0.4 0.6 0.8 1
57.1908
57.1909
57.191
57.1911
57.1912
0 1 2 3 4 5
2.086
2.084
2.082
2.08
2.078
Time [min]
L
o
n
g
i
t
u
d
e

[
]
0 0.2 0.4 0.6 0.8 1
2.0822
2.0821
2.082
2.0819
2.0818
Time [min]
Figure 13.7: This gure shows the positions of the fused INS/GPS (dotted), the
GPS positions (x) and the reference track (solid).
approach have been chosen for this project. A disadvantage when aiding with GPS is
that it is not possible when no GPS signal is present, e.g. under water. When in such
a situation aiding with a DVL is a possibility. A drawback from aiding with a DVL is
that the position error is not bounded but only decreased.
Chapter 14
Inertial Sensors
The next two chapters describe an approach which is necessary when implementing an
INS in the real world. The basis of inertial navigation is the accelerometers and angular
rate sensors which exist in a wide variety, each exploiting dierent physical phenomena
to measure specic force or angular rate. This chapter will give a description of the
sensors that are commonly used in strapdown inertial navigation systems. Also, the
errors that are expected to be experienced in these kinds of sensors are explained.
14.1 Optical Angular Rate Sensors
Optical angular rate sensors uses the fact that two beams of light travelling around a
closed circular path will travel dierent lengths if the path is rotated. This is illustrated
in Fig. 14.1
r
CCW beam
CW beam
Beam splitter/
Light
detector
source
Light path
Figure 14.1: The fundamental principle of optical angular rate sensors
Light is generated by the light source, and split into two beams travelling clockwise and
counterclockwise around the circular light path with the radius r. With no rotation,
is 0, and the transit time t for both beams to go around the light path is the same.
102
103 CHAPTER 14. INERTIAL SENSORS
However, if the light path is rotated, the path length for each of the beams are changed,
and the transit time dierence for each beam can be measured.
14.1.1 Ring Laser Gyro
The Ring Laser Gyro (RLG) is the most common type of gyro used for precision strap-
down inertial navigation systems. It works by letting a laser travel clockwise and coun-
terclockwise around in a continuous light path constructed from three or more mirrors.
One of the mirrors allows a small amount of light to exit the cavity, which can be
detected. As the gyro rotates, the frequency of the two beams change. A prism com-
bines the two light beams, and the frequency dierence forms an inference pattern. The
fringes of the inference pattern moves at a rate proportional to the rate of rotation.
By outputting a pulse each time a fringe moves past a photodiode, the RLG outputs a
pulse every time the gyro has rotated a certain angle. This makes the RLG inherently
digital and rate-integrating. Along with also being a solid state sensor, this makes it
ideal for strapdown inertial navigation systems.
14.1.2 Interferometric Fibre Optic Gyro
In an Interferometric Fibre Optic Gyro (IFOG), two broad-band light beams are trans-
mitted CW and CCW through a bre optic coil, which is spun around the sensitive
axis. As the IFOG is rotated, the length which the beams have to travel changes, and
the phase dierence can be measured as a change of intensity by photodetectors. This
makes the IFOG an analog rate sensor. As the IFOG uses bre optic coils instead of
a laser cavity, the need for high quality mirrors, and precision machinery is avoided,
which makes the IFOG cheaper than the RLG. The performance of the IFOG, however,
appeals more to medium accuracy applications. A comparison of typical accuracy of
RLG and IFOG sensors are shown in table Tab. 14.1[Titterton and Weston, 1997].
RLG IFOG
g-independent bias <0.001 10
/hour <0.5 50
/hour
g-dependent bias insignicant 1
/hour/g
g
2
-dependent bias insignicant 0.1
/hour/g
2
scale factor errors few parts per million to 0.01% 0.05 0.5%
bandwidth >200 Hz >100 Hz
maximum input rate >1000
/s >1000
/s
Table 14.1: Comparison of typical gyro accuracy
14.2 Accelerometers
An accelerometer uses Newtons Second Law of motion to measure specic force.
F = ma (14.1)
A gure of an accelerometer is shown in Fig. 14.2. By having a proof mass suspended
by springs, the displacement of the mass due to acceleration can be measured by the
pick-o.
14.3. SENSOR ERROR MODELS 104
Displacement pick-o
Spring
Spring
Proof mass
Acceleration
Case
Figure 14.2: Conceptual drawing of a accelerometer
It is important to notice that gravity inuences the proof mass by attraction, so the
acceleration measured by the accelerometer will be in the opposite direction of the
attraction.
Accelerometers working as shown in Fig. 14.2 is known as open-loop accelerometers.
These suers from non-linearities at large accelerations, as the springs are not linear.
To overcome this, the most used type of accelerometer in inertial navigation systems
today is the force-rebalance accelerometer. Here, the proof mass is held at the zero
position by an electromagnet. When the sensor accelerates, the current needed by the
electromagnet to hold the proof mass in place changes, and this change is proportional
to the acceleration experienced.
Accelerometers for INS applications are of the integrating type, so they output a pulse
when a certain change in velocity have been measured.
14.3 Sensor Error Models
Many sources of errors arise from sensor imperfections, both deterministic and non-
deterministic. This means that the input the sensor receives (angular rate, acceleration)
does not correspond to the output the sensor sends. These include
Bias The output from the sensor when no input is applied.
Scale Factor The scaling the sensor applies from input to output. I.e. the dierence
from a 1:1 input/output mapping.
Misalignment The inaccuracies in sensor alignment, when 3 1-dof sensors are mounted
in a frame.
g-sensitivity The inuence of acceleration on the output of the sensor.
Temperature sensitivity The inuence of temperature on the output of the sensor.
Quantization noise The noise introduced by converting an analog signal into a nite
precision digital signal.
Random noise/random walk Stochastic elements arising from other unmodelled noise
sources.
105 CHAPTER 14. INERTIAL SENSORS
All of these can be non-linear. Normally, bias, scale factor, misalignments, g-sensitivity
and temperature sensitivity can be regarded as random constants with a process model
of c = 0. Hence, these can be found by testing the sensor with various inputs. The most
dominant terms are the linear bias, scale-factor misalignments and temperature depen-
dency. Therefore, most gyro and accelerometer errors are modelled by the following
equation:
o(t) =
_
_
B
x
(T)
B
y
(T)
B
z
(T)
_
_
+
_
_
SF
x
(T) MA
xy
(T) MA
xz
(T)
MA
yx
(T) SF
y
(T) MA
yz
(T)
MA
zx
(T) MA
zy
(T) SF
z
(T)
_
_
i(t) (14.2)
Where
i
are the measured input, o are the output, the Bs are the biases, the SFs are
the scale factors, the MA are the misalignments (with the subscripts dening which
axes the misalignment applies to), and T is the temperature. This model will be used
in chapter 15 to calibrate inertial sensors.
Chapter 15
Calibration of Sensors
The performance of the INS is dependant on the errors that are present in the ac-
celerometers and gyros. The magnitude of the random errors depends on the quality of
the sensor. These errors cannot be compensated for without inuencing the signal itself.
The repeatable errors, however, can be identied through a process called calibration
and thus be compensated for. The term sensor error will throughout this chapter
signify only a repeatable sensor error.
15.1 Introduction
While it is possible to identify all the sensor errors, this can take much time and requires
expensive calibration equipment. Depending on the required accuracy of the nal INS,
a weighing between sensor compensation (cost and time) and INS accuracy must be
made.
In this project, it is chosen to identify constant but temperature dependent errors.
This is done by identifying the constant errors at various temperatures by rotating the
IMU through a series of 180
rotations[Diesel, 1987]. With no translatory motion, only

gravity and the rotation of the earth inuences the measurements. A rotation and a
measurement can be done in 5 to 20 seconds depending on environmental eects.
15.2 Calibration Equations
In order to nd some of the sensor misalignment errors, it is necessary to dene a sensor
frame, which might be rotated with respect to the body frame. As the roll, pitch and
yaw angles of this rotation is not known, these will be called misalignment angles and
must be estimated during calibration.
Thus, a simplied error model has been developed. In this error model is the earth
rotation neglected due to the short time between each rotation making the earth rotation
very small. This simplied error model of the system is:
106
107 CHAPTER 15. CALIBRATION OF SENSORS
v
n
= a
n
+
_
_
0
n
z

n
y
n
z
0
n
x
n
y

n
x
0
_
_
_
_
0
0
g
_
_
= a
n
+
_
_
g
n
y
g
n
x
0
_
_
(15.1)
Where
n
y
and
n
x
are the misalignment angles from the sensor to the body frame around
the y and x axis and a
n
are the acceleration errors, all given in the navigation frame. As
the system should not be translating, any velocity is an error, so the v
n
term obtained
from the navigation equation (9.1) is equal to a velocity error v
n
.
To excite all the sensor errors, the system is rotated through 3 sets of 3 rotations each.
The rotations needed are shown in Fig. 15.1.
1 (180
) 1 (180
)
z
b
y
b
x
b
y
b
Set 2
2 (180
)
3 (180
)
x
b
z
b
2 (180
)
3 (180
)
x
b
Set 3
1 (180
)
2 (180
)
3 (180
)
z
b
Set 1
y
b
Figure 15.1: Rotation sequences used for calibration
To be able to analytically identify the errors v
n
is logged just before and after the
rotations. Thus, the equations that need to be solved are of the following type (after
adding time dependency on (15.1)):
v
n
(T) v
n
(0) =
_
_
a
n
(T) +
_
_
g
n
y
(T)
g
n
x
(T)
0
_
_
_
_
_
_
a
n
(0) +
_
_
g
n
y
(0)
g
n
x
(0)
0
_
_
_
_
(15.2)
= a
n
+
_
_
g
n
y
g
n
x
0
_
_
(15.3)
Where T are the time after a rotation, and 0 is the time before a rotation. The prex
denotes that the following term is the dierence of the term from 0 to T.
To obtain a
n
and
n
, the sensor error model from chapter Ch. 14 is used. The
accelerometer error model is
a
n
(t) = C
n
b
(t)a
b
(t) = C
n
b
(t)
_
_
_
_
a
Bx
a
By
a
Bz
_
_
+
_
_
a
SFx
a
MAxy
a
MAxz
a
MAyx
a
SFy
a
MAyz
a
MAzx
a
MAzy
a
SFz
_
_
a
b
(t)
_
_
(15.4)
Where a
b
are the errors in the measured accelerations, the a
B
terms are a constant
bias, a
SF
are scale factors, a
MA
are misalignments and a
b
is the acceleration due to
gravity. From this, a
n
is obtained as
a
n
= a
n
(T) a
n
(0) (15.5)
15.2. CALIBRATION EQUATIONS 108
Identically, the gyro error model is
n
(t) = C
n
b
(t)
b
(t) = C
n
b
(t)
_
_
_
_
g
Bx
g
By
g
Bz
_
_
+
_
_
g
SFx
g
MAxy
g
MAxz
g
MAyx
g
SFy
g
MAyz
g
MAzx
g
MAzy
g
SFz
_
_
b
(t)
_
_
(15.6)
To obtain
n
,
n
is integrated
n
=
_
T
0
C
n
b
(t)
_
_
g
SFx
g
MAxy
g
MAxz
g
MAyx
g
SFy
g
MAyz
g
MAzx
g
MAzy
g
SFz
_
_
b
(t)dt (15.7)
Where the gyro bias errors are disregarded as they are expected to contribute only a
fraction of an arc second to the error during the short interval from 0 to T.
The rotation matrix from body to navigation corresponding to rotation set 1 is
Rotation 1 and 2
C
n
b
(t) =
_
_
cos((t)) 0 sin((t))
0 1 0
sin((t)) 0 cos((t))
_
_
(15.8)
Rotation 3
C
n
b
(t) =
_
_
cos((t)) sin((t)) 0
sin((t)) cos((t)) 0
0 0 1
_
_
(15.9)
Where (t) and (t) are the rotation angles at time t.
With this, solving (15.2) for rotation 1 gives
C
n
b
(0) =
_
_
1 0 0
0 1 0
0 0 1
_
_
a
b
(0) =
_
_
0
0
g
_
_
(15.10)
C
n
b
(T) =
_
_
1 0 0
0 1 0
0 0 1
_
_
a
b
(T) =
_
_
0
0
g
_
_
(15.11)
a
n
=
_
_
a
MAxz
g
a
MAyz
g
a
SFz
_
_
_
_
a
MAxz
g
a
MAyz
g
a
SFz
_
_
+
_
_
2a
Bx
0
2a
Bz
_
_
=
_
_
2a
Bx
2a
MAyz
g
2a
Bz
_
_
(15.12)
n
=
_

0
_
_
(g
MAxy
cos() + g
MAzy
sin())
g
SFy
(g
MAxy
cos() + g
MAzy
sin())
_
_
d =
_
_
2g
MAzy
g
SFy
2g
MAxy
_
_
(15.13)
v
n
(T) v
n
(0) =
_
_
2a
Bx
2a
MAyz
g
2a
Bz
_
_
+
_
_
g
SFy
g
2g
MAzy
g
0
_
_
(15.14)
Solving for the remaining rotations, one obtains the measurements given in table 15.1,
where for brevity, the following denitions are made
Set j, Rotation k:
j
k
= v
n
x
(T) v
n
x
(0),
j
k
= v
n
xy
(T) v
n
y
(0) (15.15)
Set 1 Set 2 Set 3
1
1
=2a
Bx
+g
SFy
g
2
1
=2a
Bz
+g
SFx
g
3
1
=2a
By
+g
SFz
g
1
1
=2(a
MAyz
+g
MAzy
)g
2
1
=2(a
MAxy
+g
MAyx
)g
3
1
=2(a
MAzx
+g
MAxz
)g
1
2
=2a
Bx
+g
SFy
g
2
2
=2a
Bz
+g
SFx
g
3
2
=2a
By
+g
SFz
g
1
2
=2(a
MAyz
+g
MAzy
)g
2
2
=2(a
MAxy
+g
MAyx
)g
3
2
=2(a
MAzx
+g
MAxz
)g
1
3
=2a
Bx
2(a
MAxz
g
MAxz
)g
2
3
=2a
Bz
2(a
MAzy
g
MAzy
)g
3
3
=2a
By
2(a
MAyx
g
MAyx
)g
1
3
=2a
By
2(a
MAyz
g
MAyz
)g
2
3
=2a
Bx
2(a
MAxy
g
MAxy
)g
3
3
=2a
Bz
2(a
MAzx
g
MAzx
)g
Table 15.1: Table of observation equations for calibration coecients
From these equations, expressions for the gyro scale factors, accelerometer biases and
misalignments are obtained. The expressions are seen in Tab. 15.2. The missing ac-
celerometer misalignments are set to 0, which is equal to dening the sensor frame so
that the xy plane is spanned by the x and y accelerometer, with the x accelerometer
corresponding to the x-axis. The accelerometer scale factors can be determined by get-
ting the v
n
z
component at the start of the rst rotation and at the end of the second
rotation in each set after subtracting the corresponding bias term. For the a
SFz
term:
For set 1 rotation 1 :
1
1
= v
n
z
(0) (15.16)
For set 1 rotation 2 :
1
2
= v
n
z
(T) (15.17)
a
SFz
=

1
1
+
1
2
2a
Bz
2g
(15.18)
Remaining is to nd the rotation matrix from the sensor frame to the body frame and
the gyro bias. This is done by assuming the following approximations:
v
n
x
= a
n
x
g

n
y
g

n
y
(15.19)
v
n
y
= a
n
y
+ g

n
x
g

n
x
(15.20)
These approximations are valid as the accelerometer errors will be very small after just
a few iterations of the compensation procedure.
In this way, the x and y gyro bias is found from set 1 rotation 3:
g
Bx
=
v
n
y
(T) v
n
y
(0)
2g
(15.21)
g
By
=
v
n
x
(T) v
n
x
(0)
2g
(15.22)
And the z gyro bias is found from set 2 rotation 3:
g
Bz
=
v
n
y
(T) v
n
y
(0)
2g
(15.23)
The rotation matrix from sensor to body frame is found by calculating
n
x
and
n
y
as
well as the heading angle
n
z
. All are calculated from set 1 rotation 3:
15.3. SMOOTHING FILTER 110
n
x
=
v
n
y
(T) + v
n
y
(0)
2g
(15.24)
n
y
=
v
n
x
(T) + v
n
x
(0)
2g
(15.25)
n
z
=
v
n
y
(T) + v
n
y
(0)
2g
n
y
(15.26)
Where
n
y
is the earth rate y component in the navigation frame.
g
SFy
=

1
2
+
1
1
2g
g
SFx
=

2
2
+
2
1
2g
g
SFz
=

3
2
+
3
1
2g
a
Bx
=

1
2
1
1
4
a
Bz
=

2
2
2
1
4
a
By
=

3
2
3
1
4
g
MAxz
=

1
3
+2a
Bx
2g
g
MAxy
=

2
3
+2a
Bx
2g
g
MAzy
=

1
1
2g
g
MAyz
=

1
3
+2a
By
2g
g
MAyx
=

2
1
2g
g
MAzx
=

3
3
+2a
Bz
2g
+ a
MAzx
a
MAzx
=
3
1
2g
g
MAxz
a
MAzy
=
32+2a
Bz
2g
+ g
MAzy
a
MAyx
=

3
3
+2a
By
2g
+ g
MAyx
Table 15.2: Solutions to the observation equations
15.3 Smoothing Filter
To remove noise from the measurements, and to obtain the rate of change of the hor-
izontal acceleration errors, a smoothing lter is applied to the measurements with the
dynamic equation
_
_
v
v
v
_
_
=
_
_
0 1 0
0 0 1
0 0 0
_
_
_
_
v
v
v
_
_
+
_
_
0
0
w
k
_
_
(15.27)
and measurement matrix
y = h
_
_
v
v
v
_
_
+ w
r
(15.28)
where
h =
_
0 1 0
w
r
= Measurement noise
The estimated error terms that are obtained in one calibration run is used by the
navigation algorithms in the following run, so the remaining errors in coecients tend
to converge to zero.
15.4 Calibration Example
This section shows how the remaining sensor errors converge to zero as the calibration
is run. The simulation model from Ch. 8 is run, and the sensor inputs are given bias,
misalignments, scale factors and random noise to simulate uncompensated sensors. Also,
the navigation algorithm includes sensor compensation using the calibration coecients
obtained during calibration.
Table 15.3 shows the actual errors, and those estimated by the calbration procedure
after 5 iterations. Figure 15.2 shows a monte-carlo simulation of how the calibration
coecients converge towards the true errors after a few iterations.
Error True Estimated
g
Bx
0.1E-4 0.098728E-04
g
By
0.2E-4 0.20059E-04
g
Bz
0.3E-4 0.29865E-04
g
SFx
1E-4 1.0072E-04
g
SFy
2E-4 2.0099E-04
g
SFz
3E-4 3.0153E-04
g
MAxy
0.1E-4 1.8477E-05
g
MAxz
0.2E-4 2.0168E-05
g
MAyx
0.3E-4 3.4114E-05
g
MAyz
0.4E-4 3.0360E-05
g
MAzx
0.5E-4 6.1056E-05
g
MAzy
0.6E-4 5.7603E-05
a
Bx
0.01 0.01
a
By
0.02 0.02
a
Bz
0.03 0.03
a
SFx
0.001 0.001
a
SFy
0.002 0.002
a
SFz
0.003 0.003
a
MAyx
0.3E-4 3.3082E-05
a
MAzx
0.5E-4 5.3109E-05
a
MAzy
0.6E-4 5.6882E-05
Table 15.3: True and estimated sensor errors
The sensor calibration algorithms have not been tested with real-life measurements.
This is mainly because it is dicult to obtain the sensor errors in a particular IMU, and
thus, it is not possible to quantify the dierence between the estimated errors and the
actual errors.
15.4. CALIBRATION EXAMPLE 112
1 2 3 4 5 6 7 8
7
6
5
4
3
2
1
0
x 10
5
Gyro X bias estimate
iteration
e
s
t
i
m
a
t
e
Figure 15.2: Calibration estimation of gyro X bias error.
Chapter 16
Summary
Throughout this part has all the theory been derived which constitute the mathematical
foundation of an inertial navigation system.
The geodetic constants and equations was presented in Ch. 6 along with a simplied
gravity model with an accuracy suitable for INS.
Equations for the earth simulation was derived which was used in later chapters to
yield predictable and controlled inputs to the navigation equations for analysis of the
navigation and alignment algorithms.
As the Kalman lter used in this thesis is an indirect variant of the normal direct Kalman
lter Ch. 10 shows the dierences between the two lters an derives the equations used
in later chapters to implement this lter. The indirect Kalman lter is an advantageous
approach when the system model has high dynamics and/or is non-linear. The indirect
Kalman lter is implemented using an error model of the non-linear system which can
be approximated linear. Furthermore, the dynamics of the error model is usually much
lower than that of the real system. An indirect Kalman lter requires an error model of
the INS, which is derived in Ch. 11. Two dierent error models are derived. Although
these error models model the same system they have dierent properties as they describe
the errors with dierent parameters. The -error model is preferable when in alignment
mode due to the simplicity and the fact that it is indeed the parameter which is
sought estimated. The -error model is preferable in the navigation mode as this model
becomes simpler than the -error model when moving.
Using the -error model the alignment algorithm has been developed in Ch. 12. Align-
ment is a matter of estimating the attitude and heading of the INS. This part is very
critical as even small errors in initial attitude leads to heavily decreased navigation preci-
sion. Alignment uses the knowledge of the gravity vector and earth rotation in a known
frame and estimates attitude and heading using an indirect Kalman lter technique.
As the INS inevitably suer from unbounded errors due to the double integration of
noisy measurements, these errors need to be bounded if the INS is to be used over
longer periods. In order to bound the errors the INS is aided with external reference
measurements. Two approaches has been presented in this thesis in Ch. 13. Aiding with
a GPS and with a speedometer. Aiding with a GPS gives an observations set which
include the position which bound the position error. Aiding with a speedometer only
gives an observation set which include the velocity. With only the reference velocity the
113
114
position error can only be decreased but is still unbounded.
The last two chapters in this part describe the modelling of the sensors used by the
INS and the estimation of the errors of these sensors. The knowledge of these sensor
errors facilitate a more precise alignment and navigation as the sensor measurements
can be corrected for these sensor errors. Furthermore, some of the sensor errors can be
estimated under navigation in order to correct the sensor measurements on-line.
Part III
Thesis Summary
Chapter 17
Conclusion
The main purpose of this thesis were to develop algotihms for an inertial navigation
system. This is accomplished by implementing algorithms for the main parts of an INS:
Navigation, alignment and aiding.
The basis when developing an inertial navigation systems consists of the equation of
motion. The equation of motion translates measured accelerations into a velocity with
respect to the surface of the earth, the ground speed. This equation have been derived
and form the foundation when developing the earth simulation model and the navigation
equations.
The earth simulation model is not a part of an INS, but was developed in order to make
reproducible test data simulating various sensor errors. This model was used to testing
the precision of the INS when disturbed by dierent noise sources.
The development of the navigation equations is based on the equation of motion, and
these navigation equations are the most essential parts of an INS. The equations devel-
oped were veried against measurements obtained from a medium accuracy INS system,
a Kearfott T16. Tests showed that the output from the navigation equations developed
in this thesis followed the output from the Kearfott T16 suciently accurate to validate
a verication of the system. Verication of the developed navigation equations also
veried the earth simulation model, as the equations in the simulation model showed to
be the inverse of the navigation equations. Further tests with dierent errors sources
revealed that gyro bias was the predominant cause to unbounded position errors. These
tests concluded that a precise gyroscope is essential if high performance is required. The
tests also showed that an INS experience oscillatory errors with dominant frequencies
at 84 [min] and 24 [hour] which is known as the Schuler and Earth oscillation.
The alignment algorithm is based on an indirect Kalman lter, which uses an error
model of the system. Two dierent error models of the INS have been developed. A
-error model suitable for alignment and a -error model suitable when in navigation
mode.
An alignment algorithm has been developed, which were veried using both simulated
test data and real-world data from the Kearfott T16 system. The alignment algorithm
was able to estimate C
n
b
precisely using the simulated test data and showed to estimate
C
n
b
close to the one from the Kearfott T16. This showed that the developed alignment
algorithm worked.
118
119 CHAPTER 17. CONCLUSION
In order to aid the navigation equations, to address the unbounded position error charac-
terising of an unaided INS, two dierent aiding approaches have been proposed: Aiding
with a speedometer and aiding with a GPS. While aiding with a speedometer only
decreases the position, error aiding with a GPS bounds the position error. Aiding al-
gorithms for loosely coupled INS/GPS integration were developed which enabled data
from the navigation equations to be fused with data from the aiding device using an
indirect Kalman lter. The aiding algorithm was shown to be able to bound the errors
in the INS system.
In order to gain as high precision as possible the sensors needs to be calibrated. A sensor
model together with a calibration procedure is used in order to identify and estimate
scale factors, misalignment and sensor biases.
As all the equations needed for an inertial navigation system with aiding have been
developed and have been veried to work, the purpose of this thesis have been met.
Chapter 18
Improvements and Discussion
Before the algorithms developed in this thesis can be implemented and used in the real
world, several aspect are to be considered.
Before implementation on a microprocessor the equations need to be discretized. Al-
though the equations have been discretized and implemented as a matlab les in this
thesis, the errors involved with the discretization procedure have not been analyzed.
The choice of discretization method aect the accuracy of the discrete model, and in
order to increase the accuracy a more advanced discretization method. A simple dis-
cretization method based on small time steps has been chosen in this thesis. A more
precise discretization method could be used but in either case should the errors intro-
duced by discretization be analyzed. This problem is documented rigorously in the
literature, and several solutions have been presented. It has also been argued that the
two-speed approach of the integration algorithms are no longer needed, as modern com-
puters are fast enough to execute the entire algorithms at full speed. However, the
two-speed approach still oers lower requirements to the processing power of the main
computer, and thus allows a lower power consumption. As power consumption are of
great concern in underwater vehicles, INS will probably use a two-speed approach in
systems used for underwater applications.
A choice of execution rate of the navigation equations must also be made. This choice
depends heavily on the vibrational prole of the sensor array [Savage, 2000]. This has
also not been addressed in this thesis.
The navigation algorithms need to be validated more rigorously than done in this re-
port. The earth simulation model can be used to test some performance aspects of the
navigation algorithms by supplying it with various trajectory proles, but tests in the
real world are important to validate the navigation equations completely.
The indirect Kalman approach has been chosen in this thesis to avoid running a non-
linear Kalman ler with a Kalman cycle greater than 2kHz. It has not been investigated
which performance improvements might be gained from running a direct non-linear
Kalman lter at this frequency.
When correcting the rotation matrices a rst order approximation has been used through-
out this thesis. This introduces errors which could be avoided with a more advanced
correction.
120
121 CHAPTER 18. IMPROVEMENTS AND DISCUSSION
When aiding with GPS only the loosely coupled integration approach have been imple-
mented. The tightly coupled INS/GPS integration could prove more precise and more
work should be done to implement that approach.
The validation of the calibration equations were made using simulated sensor outputs.
This was done as it is dicult to obtain the values for the errors in a particular IMU.
A validation of the calibration equations could be made by navigating using an un-
calibrated IMU, and thereafter navigating using a calibrated IMU. If the navigation
performance have improved, the calibration equations work. This has not been done,
however, as the hardware needed for the calibration has not been available.
Bibliography
[Britting, 1971] Britting, K. R. (1971). Inertial Navigation Systems Analysis. John
Wiley & Sons.
[Brown and Hwang, 1997] Brown, R. G. and Hwang, P. Y. C. (1997). Introduction to
Random Signals and Applied Kalman Filtering. John Wiley and Sons., 3rd edition.
[Diesel, 1987] Diesel, J. W. (1987). Calibration of a ring laser gyro inertial navigation
system. In Biennial Guidance Test Symposium, number 13th in Biennial Guidance
Test Symposium.
[Rogers, 2007] Rogers, R. M. (2007). Applied Mathematics in Integrated Navigation
Systems. American Institute of Aeronautics and Astronautics, 3rd edition edition.
[Savage, 1997] Savage, P. G. (1997). Strapdown inertial navigation - lecture notes.
Technical report, Strapdown Associates.
[Savage, 2000] Savage, P. G. (2000). Strapdown Analytics. Strapdown Associates.
[Titterton and Weston, 1997] Titterton, D. H. and Weston, J. L. (1997). Strapdown
inertial navigation technology. Peter Perengrinus Ltd., 1st edition.
[Titterton and Weston, 2005] Titterton, D. H. and Weston, J. L. (2005). Strapdown
inertial navigation technology. Peter Perengrinus Ltd., 2nd edition.
122

Inertial Navigation System: Department of Control Engineering 10th Semester Aalborg University 2008

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: Department of Control Engineering 10th Semester Aalborg University 2008

Uploaded by

Copyright:

Available Formats

Inertial Navigation System

, but longitude is not uniquely de-

, which will also cause a sudden jump.

, but it does not

/hr]. It can be seen, that

is a vertical unit vector and

operator brings the vector enclosed in the brackets to skew symmetric

with respect to the earth the gimbal will need to

to be level. The gimbal is simply rotated/tilted until the accelerometer

] in [roll, pitch, yaw].

in roll and pitch error. As can be seen in Fig. 12.3

but with a small component on the x axis. So

, which is the latitude at the test site at Aberdeen, a rotation of

Table 12.1: Comparison of a 15min ne alignment

after 15 minutes. The sensor outputs

/s. The horizontal

change almost linear whilst a sine at 0

/hour] which is equal to 8 10

rotations[Diesel, 1987]. With no translatory motion, only

You might also like