INS

Inertial Navigation System
iy , ey
ωie N, gy U, gz
E, gx
iz
λ l
Equator
L
ix ex
ez
Department of Control Engineering

10th Semester
Aalborg University 2008
Master project
Section of Automation and Control
AALBORG UNIVERSITY 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/
Synopsis:
This thesis documents the development of algorithms

Title: for an inertial navigation system (INS). In general,
Inertial Navigation System inertial navigation consists of three parts: Alignment,
navigation and aiding. Algorithms for these parts are
Theme: derived in this thesis.
Estimation and nonlinear control In order to create reproducable test data for verifi-
cation and analysis of the algorithms, an earth simu-
lation model is developed. The algorithms were also
Period: tested against a reference INS, a Kearfott T16 INS,
Fall-Spring Semester 2007-2008 to verify that the algorithms also work on real-world
data with real noise sources.
The navigation equations needed for an INS for ter-
Project group: restrial navigation are developed and tested against
08gr1030a the reference INS. Tests showed that the developed
navigation equations were able to follow the reference
trajectory to a satisfactory degree, thus validating the
Group members: navigation equations.
Rolf Christensen The alignment algorithms make use of an indirect
Nikolaj Fogh Kalman filter, 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
Supervisor: and heading of the system. The alignment algorithm
was verified against data from the reference system,
Anders la Cour-Harbo
and was shown to be able to estimate tilt and heading
Morten Bisgaard
to a satisfactory degree.
Algorithms for aiding the INS navigation equations
Copies: 5 were developed for a speedometer and a GPS. These
algorithms also use the indirect Kalman filter and the
Pages: 122
error model. Using data from the reference system
Attachments: None together with GPS measurements, it was shown that
the developed algorithms were able to aid the naviga-
Finished: 4th of June 2008 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.
8. Semester
Sektion for Automation og kontrol
Institut for Elektroniske Systemer
AALBORG UNIVERSITY
Det Ingeniør-, 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/
Synopsis:
Denne afhandling dokumenterer udviklingen af de al-

Titel: goritmer der bruges i inertial navigation systemer
Inertial Navigation System (INS). Generelt består inertial navigation af tre dele:
Alignment, navigering og aiding. Denne afhandling
Tema: indeholder algoritmer til alle disse dele.
Estimation and nonlinear control En simuleringsmodel af jorden er blevet lavet for at
kunne lave reproducerbare test data til verifikation og
analyse of de udviklede algoritmer. For at verificere
Periode: at algoritmerne også fungerer med data fra virkelighe-
Efterår-Forår 2007-2008 den, er algoritmerne også blevet testet med et refer-
encesystem, en Kearfott T16 INS.
De navigeringsligninger som er nødvendige for nav-
Projektgruppe: igering i nærheden af jorden er blevet udviklet og
08gr1030a testet med referencesystemet. Testene viste at de ud-
viklede navigeringsligninger var i stand til at følge
referencesystemet til en tilfredsstillende grad, hvilket
Gruppemedlemmer: gør at navigeringsligningerne er verificerede.
Rolf Christensen En alignment algoritme bruger et indirect Kalman fil-
Nikolaj Fogh 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 hældning og retning
Vejleder: af systemet. Alignment algoritmen er blevet verifi-
Anders la Cour-Harbo ceret op imod data fra referencesystemet, og det er
Morten Bisgaard blevet vist at den var i stand til at estimere hældning
og retning tilfredsstillende.
Aidingalgoritmer til navigeringsligningerne er blevet
Kopier: 5 udviklet for en hastighedsmåler og en GPS. Disse al-
goritmer bruger også et indirect Kalman filter og en
Sider: 122
fejlmodel. Ved brug af data fra referencesystemet
Bilag: Ingen samt simulerede GPS målinger blev det vist at de ud-
viklede algoritmer kunne aide navigeringsligningerne
Færdiggjort: 4. Juni 2008 og give bedre resultater end nogle af systemerne alene.
Det blev også vist at INS systemet var i stand til
at følge 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 3th of September 2007 to the 4th of June 2008 under the theme
Estimation and Nonlinear Control.
For a complete understanding of the report, a technical and scientific level corre-
sponding to that of 9th 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 first
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 Verification 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 Verification 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 find back
home to the camp or a cruise missile which should hit its target with high precision.
Through time several different 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 influences. 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 first 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 refined,
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 1950’s. Until the 1970’s only the
gimbaled systems had been investigated but in the late 1970’s 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 1960’s and the Fibre Optic
Gyro (FOG) in the 1970’s. 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 first 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 1980’s. 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 offers 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 fine 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 identification.
Inserting the parameters in the developed algorithms will yield the final software for
the INS.
6
Our Contributions
The authors have found it difficult to find 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 filter 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 fine 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 defines 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 influences 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 effects of measurement uncertainties affect 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 filter 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 suffer from.
Part III will include a system test, which verifies 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 define 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 bx , by and bz , with x, y and z being the principal axes of the frame. A
vector r referenced in the b frame will be denoted rb . The subscript notion will be used
to extract the x, y and z components of the vector.
T
rb = rxb ryb rzb

(2.1)
It is often needed to represent a vector in different 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 figure, there are
two coordinate frames: The i coordinate frame, spanned by ix , iy and iz , and the j
coordinate frame spanned by jx , jy and jz . The j coordinate frame has been rotated
relative to the i frame through the iz axis by an angle α. The rzi , rzj , iz , jz 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 figure as the bold lines rxi , ryi , rzi , and in the j frame as
the bold lines rxj , rxj , rxj . Figure 2.1 can now be used to find the equations transforming
the vector r from the i frame to the j frame.
iy
jy
ryi
α
ζ α
rxj
ζ jx
r
α
j
ryj rx,1
α ξ
ix
rzi , rzj , iz , jz rxi
Figure 2.1: Rotation around the positive y axis
ζ + ryj = ryi cos(α)

ζ = rxi sin(α)
ryj = ryi cos(α) − rxi sin(α)
j
rx,1 = rxi cos(α)
ξ = ryi sin(α)
j
rxj = rx,1 + ξ = rxi cos(α) + ryi sin(α)
rzj = rzi
It is customary to write these equations in matrix form as

 j    i 
rx cos(φ) sin(φ) 0 rx
 ryj  =  − sin(φ) cos(φ) 0   ryi 
rzj 0 0 1 rzi
rj = Cji ri
Here, Cji is the rotation matrix transforming r from frame i to frame j. The rotation
matrix is also called the Direction Cosine Matrix (DCM).
The DCM’s corresponding to rotations around all principal axis are stated here for
completeness
9 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES
 
1 0 0
Rotation around X axis  0 cos(φ) sin(φ)  (2.2)
0 − sin(φ) cos(φ)
 
cos(θ) 0 − sin(θ)
Rotation around Y axis  0 1 0  (2.3)
sin(θ) 0 cos(θ)
 
cos(ψ) sin(ψ) 0
Rotation around Z axis  − sin(ψ) cos(ψ) 0  (2.4)
0 0 1
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:
cos(ψ) cos(θ) sin(ψ) cos(φ)+cos(θ) sin(θ) sin(φ) sin(ψ) sin(φ)−cos(ψ) sin(θ) cos(φ)
j
Ci = − sin(ψ) cos(θ) cos(ψ) cos(φ)−sin(ψ) sin(θ) sin(φ) cos(ψ) sin(φ) (2.5)
sin(θ) − sin(φ) cos(θ) cos(θ) cos(φ)
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:
Cji = (Cij )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:
A ri = vi ⇓
Cji ACij rj = vj
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
j’th coordinate system this is written as:
h iT
ωijj = j
ωij,x j
ωij,y j
ωij,z
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 differently. A physical vector measured
by a sensor is denoted with a tilde and a computed quantity is denoted by a hat:
ω̃ijj = 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:
T
a = a1 a2 a3
 
0 −a3 a2
A =  a3 0 −a1  (2.7)
−a2 a1 0
Time Derivative of the Direction Cosine Matrix
At time t, the relation of frame i and j is expressed by the DCM Cji (t). During the
time ∆t, the i frame is rotated to a new orientation so the DCM is now Cji (t + ∆t).
By definition, the time derivative of a DCM is:
∆Cji Cji (t + ∆t) − Cji (t)

Ċji = lim = lim (2.8)
∆t→0 ∆t ∆t→0 ∆t
Using a geometrical interpretation the small angle rotation Cji (t + ∆t) is the same as
Cji (I + ∆θ i ) where ∆θ i is the skew symmetric matrix related to the small angle rotation
from of Cji from time t to time t + ∆t. The skew symmetric form is due to the small
angle simplifications applied to the cosine and sine parts of the DCM from (2.5). The
result is:
 
0 ∆θγ −∆θβ
i
∆θ = −∆θγ 0 ∆θα  (2.9)
∆θβ −∆θα 0
where ∆θα , ∆θβ , ∆θγ is the small angles that the i frame has been rotated through.
Substituting (2.9) in (2.8) yields:
∆θ i
Ċji = Cji lim (2.10)
δt→0 ∆t
11 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES
i
Inspecting the part ∆θ
∆t when ∆t → 0 reveals that it is the same as the angular velocity
of the i’th frame with respect to the j’th frame and this is denoted as ωjii . This vector
can be expressed as a skew symmetric matrix:
ωjii → Ωiji (2.11)
where Ωiji is a skew symmetric version of ωjii 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
Ωiij = Cij Ωjij Cji
Using this property, (2.10) can be written as:
Ċji = Cji Ωiji = −Ωjij Cji (2.12)

Chapter 3
Frames of Reference and Position
Determination
Before explaining the principles of INS, it is important to define 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 defined 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 (ix , iy or iz ) is
a matter of preference, and in the literature, different ones are used. In this thesis, the
iy vector is chosen, with the iz vector going through the equator at the longitude where
navigation was started, while the ix 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
CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION
13
iy , ey
ωie N, gy U, gz
E, gx
iz
λ l
Equator
L
ix ex
ez
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-fixed frame (ECEF) e is defined, 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 ey axis, where
λ = ωe t. The ECEF frame is seen in Fig. 3.1. Using (2.3), the rotation matrix from
ECIF to ECEF is
 
cos(λ) 0 − sin(λ)
Cei =  0 1 0  (3.1)
sin(λ) 0 cos(λ)
Local Geodetic/Geographic Frame
The Local Geodetic/Geographic Frame g defined 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 influence 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 ey axis at a longitude angle
L, and then through a latitude angle l through the new ex 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 gx = ge , gy = gn and gz = gu . 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 defined as a frame
which has origin at the position of the sensor frame and has one principal axis pointing
vertically up or down. The difference 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 undefined, and the South
pole, where North has infinitely 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 gz 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 simplifies
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 nz . Using (2.2), (2.3) and (2.4), the
rotation matrix from the ECEF to the NF is
CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION
15
ny gy
nx
gx
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.
cos(L) cos(α)−sin(L) sin(l) sin(α) cos(l) sin(α) − sin(L) cos(α)−cos(L) sin(l) sin(α)

Cne = − cos(L) sin(α)−sin(L) sin(l) cos(α) cos(α) cos(l) sin(L) sin(α)−cos(L) sin(l) cos(α) (3.2)
cos(l) sin(L) sin(l) cos(L) cos(l)
And the rotation matrix from the geodetic frame to the navigation frame is
 
cos(α) sin(α) 0
Cng =  − sin(α) cos(α) 0  (3.3)
0 0 1
Body Frame
The body frame (BF) is fixed 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 defined 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 suffers from the same problems as the NED navigation
frame mechanization. At the poles, the latitude is 90◦ , but longitude is not uniquely de-
fined. 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 DCM’s internally to keep track of po-
sition. Having a singularity also decreases navigation performance close to the poles
due to numerical problems because of finite precision within the navigation computer.
Historically, quaternions have been used because they required less processing power
during navigation as compared to DCM’s. However, they tend to be unintuitive and
significant calculations are needed for outputting the variables of interest (latitude, lon-
gitude, etc.). Today, the choice between DCM’s and quaternions is more a matter of
16
preference[Savage, 1997]. In this thesis, the Cne 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 Cne matrix so Cij corresponds
to the i’th column and the j’th row of the Cne matrix as follows:
 
C11 C21 C31
Cne =  C12 C22 C32  (3.4)
C13 C23 C33
Yields

C13
L = arctan (3.5)
C11 C22 − C21 C12
l = arcsin(C23 ) (3.6)

C21
a = arctan (3.7)
C22
As seen, only the first two columns of Cen 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 by and bz direction.
bz
bx
abz by
ω
aby
Accelerometers Gyro
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 figure 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
iy , ey
bz
nn
nu
by
l
h
θ
R0
iz , ez
Figure 4.2: INS system around the meridian plane. The platform is tilted by an
angle θ with respect to the earth surface.
vnn
l˙ =
R0 + h
ḣ = vun (4.1)
To find these we need to find the velocities of the system in the navigation frame, vnn
and vun . 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
ann = aby cos θ + abz sin(θ)

anu = −aby sin θ + abz 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.
vnn
θ̇ = ω − (4.3)
R0 + h
The vnn /(R0 + 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 influences 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
CHAPTER 4. PRINCIPLE OF A STRAPDOWN INERTIAL NAVIGATION
19 SYSTEM
inertial frame is the transport rate of (4.3), the derivative of the velocity of the system
is:
vnn
v̇nn = ann − vn (4.4)
R0 + h u
vnn
v̇un = anu − g + vn (4.5)
R0 + h n
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 sufficiently 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 identified
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 influence on the INS. The important
aspects of this topic is introduced in the first 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.
Accelerations
Longitude, latitude Angular Rates Longitude, latitude
height and attitude Simulation INS height and attitude
trajectories model trajectories
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 filtering. Indirect Kalman filtering uses an error model of the INS
which is derived in Ch. 11. Using this error model, indirect Kalman filtering equations
and the navigation equations, the alignment algorithm is derived in Ch. 12.
Two different aiding approaches is presented in Ch. 13. GPS aiding and speedometer
aiding.
The last two chapters of this part present a somewhat different 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 identified using a test fixture 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
field 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
defines 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
re = 6378137 [ m ] (6.3)
The polar radius of the earth, also known as the semiminor axis radius
rp = 6399592 [ m ] (6.4)
The eccentricity of the earth is defined as

s
rp2
ǫ = 1 − 2 = 0.0818191908426 (6.5)
re
Two important geometric aspects of geodecy is the meridional and the normal radius
of curvature. These are defined 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
North
rmer
Earth
Surface
Circle of curvature
Figure 6.1: Figure showing the difference between the circle one would follow if one
traveled directly north compared to the earth’s 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
re (1 − ǫ2 )
rmer = (6.6)
(1 − ǫ2 sin2 (l))3/2
Normal (east-west) radius of curvature
re
rnor = q (6.7)
(1 − ǫ2 sin2 (l))
6.2 Gravity Model
From [Britting, 1971], the gravitational magnitude can be calculated approximately

using the following formula.

µ 3
g= 2 1 − J2 (1 − 3 cos(2l)) − rωe2 cos2 (l) (6.8)
r 4
Where J2 , 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
 
0
gn =  0  (6.9)
µ 3 2
2
r2 1 − J
4 2 (1 − 3 cos(2l)) − rω e cos (l)
From [Savage, 1997], r is approximated as:
r = re 1 − ǫ sin2 (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 sufficient 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 differential 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 Cni yields the acceleration in
the navigation frame. The acceleration due to gravity is given in the navigation frame
as gn .
an = Cni r̈i + gn (7.1)
Where an 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 ri seen from the earth frame
transformed into the navigation frame, defined as follows
vn , Cne ṙe (7.2)
The result of this chapter is an expression relating vn to the accelerometer measurements

given in the navigation frame an .
The dependence of ṙe on ri is obtained as follows
29
30
re = Cei ri (7.3)
ṙe = Cei ṙi + Ċei ri
= Cei ṙi + Cei Ωiei ri
= Cei ṙi − Ωiie ri

(7.4)
Notice the sign change due to Ωiei = −Ωiie . Inserting into (7.2) and differentiating vn
with respect to time yields
vn = Cni ṙi − Cni Ωiie ri (7.5)

n
v̇ = Ċni ṙi + Cni r̈i − Ċni Ωiie ri − Cni Ωiie ṙi
Cni Ωiie ṙi + r̈i − Ωini Ωiie ri − Ωiie ṙi

=
Cni Ωini − Ωiie ṙi + r̈i − Ωini Ωiie ri

= (7.6)
By isolating ṙi from (7.4) one obtains
ṙi = Cie ṙe + Ωiie ri (7.7)

Inserting this into (7.6) and rearranging the terms yields
v̇n = Cni Ωini − Ωiie Cie ṙe + Ωiie ri + r̈i − Ωini Ωiie ri

= Cni Ωini Cie ṙe + Ωini Ωiie ri − Ωiie Cie ṙe − Ωiie Ωiie ri + r̈i − Ωini Ωiie ri

= Cni Ωini Cie − Ωiie Cie ṙe − Ωiie Ωiie ri + r̈i

(7.8)
Knowing that Cie = Cin Cne and Ωini = Ωine − Ωiie and inserting (7.2) yields the following
v̇n = Cni r̈i − Ωien + 2Ωiie Cin vn − Ωiie Ωiie ri

(7.9)
i
By isolating the inertial acceleration r̈ and multiplying by Cni one obtains the first term
of (7.1)
Cni r̈i = v̇n + Cni Ωien + 2Ωiie Cin vn + Cni Ωiie Ωiie ri

(7.10)
Inserting into (7.1) and rearranging yields the equation that was sought after
v̇n = an − (Ωnen + 2Ωnie ) vn − Cni Ωiie Ωiie ri − gn (7.11)
It is customary to include the centripetal term Ωiie Ωiie ri in the equation of gravity,
resulting in a so called “apparent” or “plumb bob” gravity term:
gn , gn + Cni Ωiie Ωiie ri (7.12)
Rearranging and inserting the expression for gn 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 = an − (Ωnen + 2Ωnie ) vn − gn (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-defined 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 = an − (Ωnen + 2Ωnie ) vn − gn (8.1)
Rearranging and rotating yields the wanted output
ab = Cbn (v̇n + (Ωnen + 2Ωnie ) vn + gn ) (8.2)
The plumb-bob gravity term gn was calculated in Ch. 6 as
 
0
gn =  0  (8.3)
µ
1 − 43 J2 (1 − 3 cos(2l)) − rωe2 cos2 (l)

r2
The Ωnie 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
Ωnie = Cne Ωeie Cen (8.4)

m
n
T
ωie = cos(l) sin(α)ωe cos(l) cos(α)ωe sin(l)ωe (8.5)
The wander angle (Fig.3.2) is initialized at 0 and follows the following equation[Rogers, 2007].
α̇ = L̇ sin(l) (8.6)
The Cbn matrix is calculated using the body rotations Ωnbn supplied by the user
Ċbn = Cbn Ωnbn (8.7)

At start of simulation, the Cbn matrix is initialized by the user.
Left is to find the vn and Ωnen terms.
First, the horizontal parts of vn is found. This is done by first 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 find 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.
vng
h
g
ωeg,e
rmer
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.
veg = g
ωeg,n (rnor + h) (8.8)
vng = g
−ωeg,e (rmer + h) (8.9)
Where [Savage, 2000]
T
g
−l˙ L̇ cos(l) L̇ sin(l)

ωeg = (8.10)
As the navigation frame is locally level, the vertical term of vg is equal to the derivative
of the height h.
8.2. GYRO OUTPUT 34
vug = ḣ (8.11)
Hence,
T
vg = ˙ mer + h) ḣ

L̇(rnor + h) cos(l) l(r (8.12)
This is then rotated into the navigation frame through the wander angle using (3.3).
 ˙ mer + h) sin(α) 
L̇(rnor + h) cos(l) cos(α) + l(r
˙ mer + h) cos(α) 
vn = Cng vg =  −L̇(rnor + h) cos(l) sin(α) + l(r (8.13)
ḣ
The remaining term is Ωnen . In the wander azimuth configuration, 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 = Cng (ωeg
g g
+ ωgn ) (8.14)
With T
n

ωgn = 0 0 −L̇ sin(l) (8.15)
Yields
−l˙ cos(α) + L̇ cos(l) sin(α)
 
T
n n
−l˙ L̇ cos(l) 0 =  l˙ sin(α) + L̇ cos(l) cos(α) 

ωen = Cg (8.16)
0
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 Cbn is calculated using (8.7).
8.2 Gyro Output
The outputs from the gyros are the components of the Ωbib matrix which is given as
Ωbib = Cbn (Ωnie + Ωnen + Ωnnb ) (8.17)

Which is calculated by inserting (8.5), (8.16) and the body rotations Ωnnb given by the
user.
To use this earth simulation model L, l and h need to be differentiable. 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 defined 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 defined 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 DCM’s
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 first 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 sufficient 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 = Cnb ab − (Ωnen + 2Ωnie ) vn − gn (9.1)
At start of navigation, the IMU is not moving relative to the earth surface, so vn is
initialized at 0.
The measured acceleration ab , gravity gn (8.3) and earth rotation Ωnie (8.5) are all
known. The Cnb matrix is calculated from gyro inputs as follows:
Ċnb = Cnb Ωbnb = Cnb (Ωbib − Cbn (Ωnen + Ωnie )Cnb ) (9.2)
Where Ωbib 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 Ωnen .
Apart from being used to calculate the derivative of the ground speed, the transport
rate is also used to calculate the Cne 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 Cne matrix is given initial values by the initial alignment algorithm. From there on
it follows the following equation
Ċne = −Ωnen Cne (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 Ωnen is obtained from vn by rotating vn 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
vg = Cgn vn (9.4)
By rearranging (8.8) and (8.9), and inserting it into the two first positions of (8.10), one
obtains
g
iT
veg
h
g −vn
ωeg = (rmer +h) (rnor +h) L̇ sin(l) (9.5)
n
This is used to calculate ωen using
n g
= Cng ωeg g

ωen + ωgn (9.6)
With T
g

ωgn = 0 0 −L̇ sin(l) (9.7)
Substituting (9.5) and (9.7) into (9.6) and isolating vn yields the relationship between
ground speed and transport rate:
 „ « 
sin2 (α) cos2 (α)
sin(α) cos(α)( rnor1 +h − rmer1
+h ) rnor +h − rmer +h 0
n  n
ωen = v (9.8)
 „ «
cos2 (α) sin2 (α)
− sin(α) cos(α)( rnor1 +h − rmer
1
) 0
 − rnor +h − rmer +h +h
0 0 0
= Fvn (9.9)
n
Note that the z part of ω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 defined at
the poles. The rest of this section will rewrite F to use the entries in the Cne matrix
instead, overcoming this limitation. This is done as follows:
Using the trigonometric identity sin2 (α) + cos2 (α) = 1 gives
" 2
#
sin(α) cos(α)( rnor1 +h − rmer
1
+h ) − rnor1 +h +( rnor1 +h − rmer
1
+h ) cos (α) 0
F= 1
rnor +h − ( rnor1 +h − rmer1 +h ) sin2 (α) − sin(α) cos(α)( rnor1 +h − rmer
1
+h ) 0 (9.10)
0 0 0
37 CHAPTER 9. NAVIGATION EQUATIONS
Using the entries in the Cne matrix as done in Ch. 3, the sin(α) and cos(α) terms can
be denoted
C21 C21 C22 C22

sin(α) = = p cos(α) = =p (9.11)
cos(l) 1 − (C23 )2 cos(l) 1 − (C23 )2
Yielding the final result, which can be used in navigation without the wander angle
constraint
)2
 
C21 C22 (C
1−(C23 )2
( rnor1 +h − rmer1 +h ) − rnor1 +h +( rnor1 +h − rmer
1
+h ) 1−(C
22
23 )
2 0
F= (C21 ) 2 C C22
 (9.12)
1
rnor +h − ( rnor1 +h − rmer1 +h ) 1−(C 23 )
2
21
− 1−(C
23 )
2 ( rnor1 +h − rmer1 +h ) 0
0 0 0
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 definition always pointing downwards, hence:
ḣ = −vzn (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.
vzn
n
v̇z,ins + + v̇zn R + R
h
- - -
c3 c2 c1
R δh + -
haid
Figure 9.1: Vertical channel control loop
ḣ = vzn − c1 (h − haid ) (9.14)

Z
n n n n n n n
v̇z = ȧz − ((Ωen + 2Ωie ) v )z − gz − c2 (h − haid ) − c3 (h − haid )dt (9.15)
9.2. VERTICAL CHANNEL 38
The PID gains c1 , c2 and c3 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̇ = δvzn − c1 (δh − δhaid ) (9.16)
Assuming that the error of the aiding device is constant [Rogers, 2007], the second
derivative is:
˙
δ ḧ + c1 (δ ḧ) = δv̈zn (9.17)
The δv̈zn term is obtained from (9.15) as follows: Assuming that earth rotation and
transport rate are negligibly small compared to the other terms, it simplifies to
Z
δ v̇zn = δ ȧnz + δgzn − c2 (δh − δhaid ) − c3 (δh − δhaid )dt (9.18)
The gravity model is approximated by an inverse square law as
g0 r02

gzn (r) ≈− (9.19)
r2
where g0 is the gravity at the surface of the earth, r0 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
dgzn (r) 2g
δgzn = − δr = δh (9.20)
dr r
where g is the gravity magnitude at the equator.

Assuming δanz to be constant, differentiating (9.18) yields
2g
δv̈zn ≈ δ ḣ − c2 (δ ḣ − δhaid ) − c3 (δh − δhaid ) (9.21)
r
which can be inserted into (9.17)

˙ 2g
δ ḧ + c1 δ ḧ + c2 − δ ḣ + c3 δh = c3 δhaid (9.22)
r
Taking the Laplace transform

2g
s3 + c1 s2 + c2 − s + c3 = 0 (9.23)
r
With no control (c1 = c2 = c3 = 0),
2g
s3 − =0 (9.24)
r
p
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 c1 , c2 and c3 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
s3 + 3λs2 + 4λ2 s + 2λ3 = 0 (9.25)
Such that
2g
c1 = 3λ c2 = 4λ2 + c3 = 2λ3 (9.26)
r
Where λ is usually chosen to be 0.01 [Rogers, 2007].
9.3 Navigation Output
The latitude, longitude and height can be extracted from the Cne matrix as described
in Ch. 3:

C13
L = arctan (9.27)
C11 C22 − C21 C12
l = arcsin(C23 ) (9.28)

C21
a = arctan (9.29)
C22
The velocities in north and east can be expressed as
vN = vyn cos(α) + vxn sin(α) (9.30)

vE = vxn cos(α) − vyn 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 first 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
algorithm is 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 Cnb 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 first 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 verified. 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
Our system vs. Kearfott T16

57.192
Our system
Kearfott T16
57.191
57.19
57.189
57.188
Latitude
57.187
57.186
57.185
57.184
57.183
57.182
−2.086 −2.085 −2.084 −2.083 −2.082 −2.081 −2.08 −2.079 −2.078 −2.077
Longitude
Figure 9.2: Comparison between our INS and the Kearfott INS.
further North than the start point. This difference is due to the drift of the INS when
no aiding device is present.
It is more difficult 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 influences 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
−3 Roll Velocity
x 10
15 0.1
East
Error [mrad]
10 0.05
Error [m/s]
North
5 0
0 −0.05
−5 −0.1
0 10 20 30 40 0 10 20 30 40
−3 Pitch Latitude
x 10
5 100
Error [mrad]
0
Error [m]
50
−5
0
−10
−15 −50
0 10 20 30 40 0 10 20 30 40
−3 Yaw Longitude
x 10
5 200
Error [mrad]
0
Error [m]
100
−5
0
−10
−15 −100
0 10 20 30 40 0 10 20 30 40
Time [h] 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
Roll Velocity
0.1 1
East
Error [mrad]
0.05
Error [m/s]
0.5 North
0
0
−0.05
−0.1 −0.5
0 10 20 30 40 0 10 20 30 40
Pitch Latitude
0.1 5000
Error [mrad]
0.05
Error [m]
0
0
−5000
−0.05
−0.1 −10000
0 10 20 30 40 0 10 20 30 40
Yaw 4 Longitude
x 10
10 10
Error [mrad]
Error [m]
5 5
0 0
0 10 20 30 40 0 10 20 30 40
Time [h] 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
−3 Roll Velocity
x 10
2 0.04
East
Error [mrad]
1 0.02
Error [m/s]
North
0 0
−1 −0.02
−2 −0.04
0 10 20 30 40 0 10 20 30 40
−3 Pitch Latitude
x 10
2 200
Error [mrad]
1 100
Error [m]
0 0
−1 −100
−2 −200
0 10 20 30 40 0 10 20 30 40
Yaw Longitude
−0.02 500
Error [mrad]
−0.03
Error [m]
0
−0.04
−500
−0.05
−0.06 −1000
0 10 20 30 40 0 10 20 30 40
Time [h] Time [h]
Figure 9.5: Error propagation with an initial heading error of 25E-6 [rad]. All
errors are bounded.
Initial position error
−5 Roll −4 Velocity
x 10 x 10
1 2
East
Error [mrad]
0.5 1
Error [m/s]
North
0 0
−0.5 −1
−1 −2
0 10 20 30 40 0 10 20 30 40
−5 Pitch Latitude
x 10
1 2
Error [mrad]
0.5 1
Error [m]
0 0
−0.5 −1
−1 −2
0 10 20 30 40 0 10 20 30 40
−4 Yaw Longitude
x 10
1 4
Error [mrad]
0.5 2
Error [m]
0 0
−0.5 −2
−1 −4
0 10 20 30 40 0 10 20 30 40
Time [h] Time [h]
Figure 9.6: Error propagation with an initial position error of 1 [m]. All errors are
bounded.
Initial pitch error
Roll Velocity
0.05 0.5
East
Error [mrad]
Error [m/s]
North
0 0
−0.05 −0.5
0 10 20 30 40 0 10 20 30 40
Pitch Latitude
0.15 1000
Error [mrad]
0.1 500
Error [m]
0.05 0
0 −500
−0.05 −1000
0 10 20 30 40 0 10 20 30 40
Yaw Longitude
0.06 2000
Error [mrad]
0.04 1000
Error [m]
0.02 0
0 −1000
−0.02 −2000
0 10 20 30 40 0 10 20 30 40
Time [h] 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
Roll Velocity
0.02 0.1
East
Error [mrad]
0.01 0.05
Error [m/s]
North
0 0
−0.01 −0.05
−0.02 −0.1
0 10 20 30 40 0 10 20 30 40
Pitch Latitude
0.02 100
Error [mrad]
0.01 50
Error [m]
0 0
−0.01 −50
−0.02 −100
0 10 20 30 40 0 10 20 30 40
Yaw Longitude
0.02 200
Error [mrad]
0.01 100
Error [m]
0 0
−0.01 −100
−0.02 −200
0 10 20 30 40 0 10 20 30 40
Time [h] 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 figures are oscillations with a period of 84.4 [min].
The Schuler oscillations apparent in the figures 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 Cne 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 significant
as gyro errors. Position errors are not very significant, whereas initial velocity errors
should be kept to a minimum.
Assumed velocity
Assumed acceleration Assumed acceleration
Torquing due to transport rate Torquing due to transport rate
θ θ
gun gun
n n
g g
gen gen
(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
filter 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 filter but an indirect
Kalman filter which is used in this thesis. Where the normal direct Kalman filter seeks
to estimate the states directly the indirect Kalman filter estimates the error in these
states and corrects the real states. For this reason is the model used by the Kalman
filter 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
filtering. The differences 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 difference between a normal Kalman filter and the indirect Kalman filter
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 filtering 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 filter. The states in an indirect Kalman filter is hence called error states.
With this in mind the system model is described as follows:
ẋ = A(t)x(t) + Gp (t)np (t) (10.1)
where
x = Error state vector

A = Error state dynamic matrix
np = Process noise vector
Gp = Process noise dynamic matrix
49
10.1. KALMAN FILTERING 50
with the measurement equation as follows:
z(t) = H(t)x(t) + Gm (t)nm (t) (10.2)

where
z = Measurement vector
H = Measurement matrix
nm = Measurement noise vector
Gm = Measurement noise dynamic matrix
These equations describe the system used by the Kalman filter to estimate the error
states x. As the filter estimates error states, the input to the filter 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 zobs which describes the actual, non-linear observation which is defined as
follows:
zobs = 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:
zobs = rins − rgps (10.4)

With error free measurement this observation equation is indeed zero.
An example of an indirect Kalman filter 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 filter. The Kalman
filter outputs the optimal estimate of the errors in the INS states.
Looking at the figure, it can be seen that the indirect Kalman filter is allowed to adjust
the states of the system through a process called “control reset”. As the estimate from
the Kalman filter 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:
xn (+c ) = x̂n (+e ) + unc (10.5)

where
uc = 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.
CHAPTER 10. ERROR ESTIMATION USING INDIRECT KALMAN
51 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 un to the INS parameters in the following way:
ξinsn (+c ) = gins (ξinsn (−), ucn ) (10.6)
where
gins () = Functional operator to apply uc to the INS parameters
The gins () operation differs depending on which type of state that should corrected. In
this report two different 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 filter input in the correct form. With the INS error model the filter input should
also be an error. This be either position error, velocity error, etc. The Kalman filter
estimates the error states and these are used to correct the INS states and thus indirectly
resetting the error states.
Aiding Device
Foreground Equations
+ Filter Error State
Measurements - Input Estimate
INS System Kalman Filter
Control Reset of INS States/Indirect Reset of Error State
Figure 10.1: Indirect Kalman filter with control reset
The ideal control vector uc 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 filter cycle, as xn (+e ) is
the best estimate of the error.
In order to point out the differences between a ”normal” direct Kalman filter and an
indirect Kalman filter the Kalman equations for both filters are listed. First of all the
normal direct Kalman equations are listed:
Measurement update
Kn = Pn (−)HT T
n (Hn Pn (−)Hn + Rn)
-1
x̂n (+) = x̂n (−) + Kn (z̃n − ẑn )

Pn (+) = (I − Kn Hn)Pn (−)
Time update
x̂n (−) = Φn x̂n-1 (+)
ẑn = Hn x̂n (−)
Secondly the indirect Kalman filter equations are listed. The parts which are special for
the indirect Kalman filter is indented in order to compare it with the ’normal’ Kalman
filter.
10.2. MOTIVATING EXAMPLE 52
zobsn = f(ξinsn , ξaidn )

Measurement update
Kn = Pn (−)HT T
n (Hn Pn (−)Hn + Rn)
-1
x̂n (+e ) = x̂n (−) + Kn (zobsn − ẑn )

Pn (+) = (I − Kn Hn)Pn (−)
ucn = −x̂n (+e)
xn (+c ) = x̂n (+e ) + unc
ξinsn(+c ) = gins (ξinsn (−), ucn )
Time update
x̂n (−) = Φn x̂n-1 (+c )
ẑn = Hn x̂n (−) (10.7)
where
(+e ) = Designation for parameter after estimation control but before control reset
(+c ) = Designation for parameter after control reset
Kn = 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 filter is to estimate the errors in the velocity in order to
get a better estimate of the velocity.
The indirect Kalman filter is presented in Fig. 10.2.
Velocity measured by
aiding device
vref
Observation set for Kalman filter
- the Kalman filter estimate
Measured a Ts v + zobs = v - vref Kalman v
acceleration z-1 filter
Control reset
Figure 10.2: One dimensional indirect Kalman filter
As seen in the figure, 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 filter.
The Kalman filter 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.
CHAPTER 10. ERROR ESTIMATION USING INDIRECT KALMAN
53 FILTERING
Control reset
v
Measured Add_2
acceleration Add_1
a + Error corrected
Tn + v velocity
+
+
1
z
Discrete time
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 filtering it is normally quite simple to choose between e.g. a normal
linear, extended or unscented Kalman filter. 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 filter. 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 filter 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 filter 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 filtering 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 filter 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 filter approach requires the use of an extended Kalman filter
which should be run at 2kHz. This might pose a problem when implementing on a
microprocessor. Using the indirect Kalman filter, 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 filter is chosen.
10.3. SUMMARY 54
10.3 Summary
During this chapter, the indirect Kalman filter technique has been described and the
differences and advantages with respect to the direct Kalman filter have been outlined.
As described, the indirect Kalman filter is advantageous if the system is characterized
by high dynamics and/or non-linearity. In order to utilize the indirect Kalman filter
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 filter.
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 defined as the angle between the actual DCMs and
the DCMs computed in the navigation computer and the velocity difference between
the actual velocity of the INS and the velocity used by the navigation computer. The
errors to be defined 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 simplified for use in the error equations. The earth is
considered a sphere instead of an ellipsoid which yields the following equations:
n
Ċb = Cnb Ωbib − (Ωbie + Ωben )Cnb
ωien
= Cne ωie
e
n 1
ωen = (unzn × vn ) + ρR unzn
R
v̇n = Cnb an + gn − (ωen
n
+ 2ωie n
) × vn
e
Ċn = Cen Ωnen
ḣ = vn · unzn (11.1)
Where unzn = 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 sufficiently 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 finite
precision and integration errors. In this chapter, only the errors from the sensors is
considered as the computer errors can be minimized by choosing sufficient 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:
˙ n n
Ĉnb = Ĉb Ω̃bib − (Ω̂bie + Ω̂ben )Ĉb
n n e
ω̂ie = Ĉe ω̂ie
1
ω̂nen = (unzn × v̂n ) + ρ̂R unzn
R̂
n
v̂˙ n = Ĉb ãn + ĝn − (ω̂en
n n
+ 2ω̂ie ) × v̂n
˙ e
Ĉen = Ĉn Ω̂nen
˙
ĥ = v̂n · unzn (11.2)
In order to develope the error mode an error is defined as follows:
δa = â − a (11.3)
Where â is the computed state and a is the actual state.

Using this error definition 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 defined 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 Cnb matrix.
In navigation mode it is mainly the position error which is of interest. This means that
the error in Cne 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:
δCnb = Ĉnb − Cnb

δvn = v̂n − vn
δCen = Ĉen − Cen
δ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̇nb = δCnb Ω̃bib + Cnb δΩbib − (δΩnie + δΩnen )Cnb − (Ωnie + Ωnen )δCnb
δ v̇n = δCnb ãb + Cnb δab − (2δωie
n n
+ δωen ) × vn − (2ωie
n n
+ ωen ) × δvn + δgn
δ Ċen = δCen Ωnen + Cen δΩnen
n
δωie = δCne ωie
e
n 1 δR
δωen = (unzn × δvn ) − 2 (unzn × vn ) + δρR unzn
R R
δ ḣ = δvn · unzn (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 defined
as follows:
Ĉnb = (I − Γn )Cnb ⇒ δCnb = −Γn Cnb

Ĉne = (I − En )Cne ⇒ δCne = −En Cne
Ĉen = Ĉne T = Cen (I + En ) ⇒ δCen = Cen En (11.6)
where:
Γ, E = Skew symmetric form of γ n and ǫn .

γ n = The rotation vector associated with δCnb
ǫn = The rotation vector associated with δCen (11.7)
With this small angle approximation the error equations are rewritten. Using (11.6)
n
δωie is rewritten:
n
δωie = −En Cne ωie
e
= −ǫn × ωie
n
(11.8)
The gravity error model in the δvn term from (11.5) is the variation from the true
gravity to the one estimated by the computer. This gravity error is influenced 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:
R02 n
gn = −g0 u + ∆gn (11.9)
R2 zn
where
g0 = Gravity magnitude on the earth surface

R0 = Earth’s radius
R = Distance from the earth center to the INS
∆gn = Correction to inverse square model to take into account
the centripetal acceleration and earth mass distribution.
The gravity error δgn is found using a special approach. As gravity is dependent on the
parameter R, variation of gravity is defined to be gn differentiated with respect to R.
This definition is used to define 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:
dgn
gnvar =
dR
n dgn
δg = δR (11.10)
dR
Using this approach and the gravity model from (11.9) δgn is as follows:
R02 2g
δgn = 2g0 δR unzn + δ∆gn ≈ 0 δh unzn + δgnM (11.11)
R3 R
where
δgnM = Unmodeled gravity error
Using (11.6), (11.8) and (11.11) δ v̇n from (11.2) is rewritten:
δ v̇n = −γ n × fn + Cnb δfb − (2δωie
n n
+ δωen ) × vn −
n 2g
(ωen − 2ǫn × ωie n
) × δvn + δhunzn + δgnM (11.12)
R
In order to rewrite δ Ċen (11.6) is substituted into δ Ċen from (11.5) which yields:
Ċen En + Cen Ėn = Cen En Ωnen + Cen δΩnen (11.13)
n
Cen Ė = Cen En Ωnen − Cen Ωnen En + Cen δΩnen ⇓
n
Ė = (En Ωnen − Ωnen En ) + δΩnen (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:
v1 × (v2 × v3 ) = (v1 · v3 )v2 − (v1 · v2 )v3 (11.15)
Multiplying the term in brackets from (11.14) with an arbitrary vector (v) and switching
to vector form:
(En Ωnen − Ωnen En )v = ǫn × (ωen
n n
) × v) − ωen × (ǫn ) × v) (11.16)
Applying the triple vector product identity yields:
(En Ωnen − Ωnen En )v = (ǫn · v)ωen
n
− (ǫn · ωen
n n
)v − (ωen · v)ǫn + (ωen
n
· ǫn )v
= (ǫn · v)ωen
n n
− (ωen · v)ǫn (11.17)
Using the triple vector product to collect term again:
(En Ωnen − Ωnen En )v = (ǫn × ωen
n
)×v (11.18)
Converting this into matrix form where the term in the bracket should be represented
by the skew symmetric counterpart:
(En Ωnen − Ωnen En )v = (ǫn × ωen
n ∗
) 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:
(En Ωnen − Ωnen En ) = (ǫn × ωen
n ∗
) ⇓
n
Ė = (ǫn × ωen
n ∗
) + δΩnen (11.20)
which can be represented in vector form:
ǫ̇n = ǫn × ωen
n n
+ δωen (11.21)
In the same manner as with Ċen , Ċnb is rewritten by using the expression from (11.6)
and then replacing with the expression from (11.5):
δ Ċnb = −Γ̇n Cnb − Γn Ċnb

= −Γn Cnb Ω̃bib + Cnb δΩbib − (δΩnie + δΩnen )Cnb + (Ωnie + Ωnen )Γn Cnb (11.22)
In order to get rid of the derivative of Ċnb this is substituted with the expression from
(11.1) yielding:
h i
Γ̇n Cnb = −Γn Cnb Ω̃bib − (Ωnie + Ωnen )Cnb
+ Γn Cnb Ω̃bib − Cnb δΩbib + (δΩnie δΩnen )Cnb − (Ωnie + Ωnen )Γn Cnb
= Γn (Ωnie + Ωnen )Cnb − (Ωnie + Ωnen )Γn Cnb
− Cnb δΩbib + (δΩnie + δΩnen )Cnb (11.23)
Which can be simplified further by right side multiplying with (Cnb )−1
Γ̇n = Γn (Ωnie + Ωnen ) − (Ωnie + Ωnen )Γn − Cnb δΩbib Cbn + δΩnie + δΩnen
= [γ n × (ωie
n n
)] − Cnb δΩbib Cbn + δΩnie + δΩnen
∗
+ ωen (11.24)
Using the similarity transform simplifies the expression to:
Γ̇n = [γ n × (ωie
n n
)] − (Cnb δωib
b ∗
) + δΩnie + δΩnen
∗
+ ωen (11.25)
which can be written in vector form:
γ̇ n = γ n × (ωie
n n
+ ωen ) − Cnb δωib
b n
+ δωie n
+ δωen (11.26)
n
Equation (11.12), (11.21), (11.26) along with δωen , ḣ from (11.5) and the approximation
δR = δh constitute the error model for (11.1) in terms of the sensor errors δab and δωib
b
:
n n
b n
+ δωie n
+ δωen
2g
δ v̇n = −γ n × ãn + Cnb δab − (2δωie
n n
+ δωen ) × vn − (ωen
n
− 2ǫn × ωie
n
) × δvn + δh unzn + δgnM
R
ǫ̇n = ǫn × ωen n n
+ δωen
n 1 δh
R R
δ ḣ = δCne ωie
e
(11.27)
However, this error model, described by the equations (11.27), suffer 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 filter 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 defined now in the earth frame:
Ĉeb = (I − Ψe )Ceb ⇒ δCeb = −Ψe Ceb

δve = v̂e − ve
δre = rˆe − re (11.28)
where
Ψ = The skew symmetric counterpart of ψ

which is equal to the angular error in Ceb
r = The position vector from the earth’s 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:
δCeb = δCen Cnb + Cen δCnb (11.29)
which makes it possible to describe the Ψ part of (11.28) as:
Ψe = −(δCen Cnb + Cen δCnb )Ceb T

= −(Cen Cnb En Cnb − Cen Γn Cnb )(Cnb )T (Cen )T
= Cen (Γn − En )Cne (11.30)
This can be rearranged into:
(Cen )T Ψe Cen = Γn − En (11.31)
Using the similarity transformation yields the following:
ψ n = γ n − ǫn (11.32)
In order to relate δve with δvn it is first recognized that:
ve = Cen vn
v̂e = Ĉen v̂n
Ĉen = Cen + δCen (11.33)
Substituting (11.33) into δve from (11.28), using the definition of δvn from (11.4) and
neglecting any second order errors as they are assumed insignificant yields:
δve = Ĉen v̂n − Cen vn = (Cen + δCen )v̂n − Cen vn

= Cen (v̂n − vn ) + δCen vn = Cen δvn + δCen vn (11.34)
Using small angle approximation yields:
δve = Cen (δvn + En vn ) = Cen (δvn + ǫn × vn ) (11.35)
As the error model should be expressed in navigation frame the velocity error in navi-
gation frame is defined:
δυ n , Ĉne δve (11.36)

With definition (11.35) it becomes:
δυ n = δvn + ǫn × vn (11.37)
A relation from δre with ǫn and h is obtained by first defining re :
re = Rueze (11.38)
Where R is the length of re , and ueze is the unit vector pointing from the center of the
earth to the INS in earth frame. The error is defined in the standard way and neglecting
second order errors yield:
δre = R̂ûeze − Rueze

= (R + δR)(Cen + δCen )unzn − RCen ueze
≈ RδCen unzn + δRCen unzn (11.39)
Transforming this into the navigation frame yields:
δrn = REn unzn + δRunzn (11.40)
which can be represented in vector form:
δrn = R(ǫn × unzn ) + δRunzn (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 unzn 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:
δrnH = R(ǫn × unzn ) (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
δυ n = δvn + ǫn × vn
δrn = R(ǫn × unzn ) + δRunzn
δrnH = R(ǫn × unzn )
δR = δh (11.43)
In order to obtain the differential equation for the new error model the equivalent
parameters from (11.27) is substituted and differentiated.
The Ψ̇n equation is obtained by subtracting ǫ̇n from γ̇ n and substituting the parts with
their equivalent from (11.43):
Ψ̇n = Ψn × (ωie
n n
b
(11.44)
The δ υ̇ n differential equation is derived by differentiating the δυ n expression from

(11.43):
δ υ̇ n = δ v̇n + ǫ̇n × vn + ǫn × v̇n (11.45)

Substituting δ v̇n , v̇n and ǫn with expressions from (11.1) and (11.27) and substituting
δvn with an rearranged δ υ̇ n expression from (11.43) yields:
δ υ̇ n = Cnb δab − γ n × ãn − (2ωien n
+ ωen ) × (δυ n − ǫn × vn )
n 2g
− (δωen − 2ǫn × ωien
) × vn + δhunzn + δgnM + (ǫn × ωenn n
+ δωen ) × vn
h R i
+ ǫn × Cnb ãb − (2ωie
n n
+ ωen ) × vn + gn
2g
= Cnb δab − (γ n − ǫn ) × ãn + ǫn × gn − (2ωie
n n
+ ωen ) × δυ n + δhunzn
R
+ δgnM + (2ωie
n n
+ ωen ) × (ǫn × vn ) + [ǫn × (2ωie
n n
+ ωen )] × vn
n n
− ǫ × [(2ωie + ωen ) × vn ] (11.46)
The three last parts can be shown to sum to zero by using the triple vector product
from (11.15). Defining Ω = 2Ωnie + Ωnen the triple vector product is used on the three
last parts and summarized with the following result:
Ω × (ǫ × v) + (ǫ × Ω) × v − ǫ × (Ω × v) m
ǫ(Ω · v) − v(Ω · ǫ) − ǫ(Ω · v) + Ω(ǫ · v) − Ω(ǫ · v) + v(ǫ · Ω) m
v(ǫ · Ω − Ω · ǫ) = 0 (11.47)
Furthermore (γ n − ǫn ) can be substituted with Ψn . Lastly the ∆gn part from gn from
(11.9) can be neglected as it is very small compared to gn . As gn point the same way
as unzn the following expression can be substituted using δrnH from (11.43):
ǫn × gn = −g(ǫn × unzn )
g
= − δrnH (11.48)
R
All of these rearrangements result in the final expression for δ υ̇ n :
2g g
δ υ̇ n = Cnb δab − Ψn × ãn − (2ωie
n n
+ ωen δRunzn − δrnH + δgnM (11.49)
) × δυ n +
R R
What remains is to relate the new position vector rn to the old states ǫn and h. By
definition:
δ ṙe = δve (11.50)
and furthermore:
δrn = Cne δre (11.51)
Differentiating (11.51):
δ ṙn = Ċne δre + Cne δ ṙe (11.52)
Substituting (11.50) and (11.51) into (11.52) with the expression of Ċne from (11.1)
yields:
δ ṙn = −Ωnen Cne δre + Cne δve
= δυ n − ωen
n
× δrn (11.53)
The new error model with the new error states Ψ̇n , υ̇ n and ṙn is summarized below:
Ψ̇n = Ψn × (ωie
n n
b
2g g
δ υ̇ n = Cnb δab − Ψn × ãn − (2ωie
n n
+ ωen ) × δυ n + δRunzn − δrnH + δgnM
R R
δ ṙn = δυ n − ωen
n
× δrn (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:
 ψ˙   0 Ωc −Ωb −Ωβ 0 0 0 0 0 0  ψx

x
0 Ωa +Ωα 0 0 0 0 0 0
 ψy   Ω +Ωc   ψy 
−Ω
 ψz   b β −Ωa −Ωα 0 0 0 0 0 0 0   ψz 
 δvx   0 −an U an
N 0 2Ωc −2Ωb +Ωβ −g/r 0 0   δvx 
 y=
 δv   an 0 −an −2Ωc 0 2Ωa +Ωα 0 −g/r 0
  δv 
U E  y
 δvz   −anN anE 0 2Ωb +Ωβ −2Ωa −Ωα 0 0 0 2g/r   δvz 
 δrx   0 0 0 1 0 0 0 0 −Ωβ
  δrx 
    
δry 0 0 0 0 1 0 0 0 Ωα δry
δrz 0 0 0 0 0 1 Ωβ −Ωα 0 δrz
(11.55)
where
Ωa = cos(l) sin(α)ωe
Ωb = cos(l) cos(α)ωe
Ωc = sin(l)ωe
Ω = −l˙ cos(α) + L̇ cos(l) sin(α)
α
Ωβ = l˙ sin(α) + L̇ cos(l) cos(α)

l̇ = −vgN /(r + h)
L̇ = vgE /(r + h) cos(l)
11.2 Summary
During this chapter has the error model of the INS been developed. Two different
approaches has been used which leads to two different error models. These two error
models describe the error propagation of the INS but has different characteristics as
they describe different 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 different situations. The first
error model, (γ) error model, describe the errors in Cnb 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 filter and thus making it
preferable.
With the error model developed and the indirect Kalman filter 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 fine alignment. Coarse alignment is an analytical alignment
which suffers 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
fine alignment to use an indirect Kalman filter 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 simplifies the coarse alignment as the
earth rotation is only present on two axes.
The first section will describe the coarse alignment. The second section describes the
fine alignment procedure which uses the indirect Kalman filtering technique and the
error model developed in the preceding chapter. The last section is a verification 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 Cnb 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 Cnb will be called leveling in
order to gain greater understanding.
12.2 Coarse Alignment
The gravity vector along with the Earth’s 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 Cbn 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 gb and ωie b
which are the measurements from the
accelerometer and gyroscope. These quantities are related in the following way:
gb = Cbn gn
b
ωie = Cbn ωie
n
T T
where gn = n

0 0 g and ωie = 0 ωie cos(l) ωie sin(l) . Knowing the quan-
b
b b b
T b
b b b
T
tities g = gx gy gz and ωie = ωx ωy ωz from the measurements of
the accelerometer and gyroscope, this system is easily solved.
gx ωx gx
C31 = C21 = − tan(l) (12.1)
g ωie cos(l) g
gy ωy gy
C32 = C22 = − tan(l) (12.2)
g ωie cos(l) g
gz ωz gz
C33 = C13 = − tan(l) (12.3)
g ωie cos(l) g
where C21 , C22 , ... C33 are the elements of the direction cosine matrix Cbn . The last
three elements are easily determined with the use of the orthogonality property of the
direction cosine matrix, (Cbn )−1 = (Cbn )T .
12.3. FINE ALIGNMENT 66
C21 = −C12 C33 + C13 C32 (12.4)

C22 = C11 C33 − C31 C13 (12.5)
C23 = −C11 C32 + C31 C12 (12.6)
As seen from these equations this DCM is only uniquely defined 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 filter with the result from the coarse alignment
as the initial Cnb . At fine 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 suffers from
measurement noise which prevents it from calculating the true value of Cnb . However,
the errors in the Cnb 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 Cnb 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 fine alignment, the estimates of roll and pitch errors are used as control resets
to correct the Cnb matrix, so the platform is leveled. The yaw error can be calculated
n
from the horizontal parts of the estimated earth rotation ω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
effects 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 different from 0 will be an error. This
error is used as the driving parameter for the indirect Kalman filter. The indirect
Kalman filter then estimates the errors in the rotation matrix from body frame to
navigation frame Cnb , error in earth rotation ωien
, error in velocity vn and error in the
difference in position from the initial position ∆rn .
The navigation equations from Sec. 11.1 is restated below.
n
Ċb = Cnb Ωbib − (Ωnie + Ωnen )Cnb
ωien
= Cne ωie
e
n 1
ωen = (unzn × vn ) + ρR unzn
R
v̇n = Cnb an + gn − (ωen
n
+ 2ωie n
) × vn
e
Ċn = Cen Ωnen
ḣ = vn · unzn (12.7)
As fine alignment is performed when the INS is in a quasi-stationary situation the

n
navigation equations can be simplified as both ωen and vn are zero. Applying these
simplifications gives the following simplified navigation equations:
n
Ċb = Cnb Ωbib − Ωnie Cnb
n
ωie = Cne ωie
e
v̇nH = (Cnb an )H
∆ṙnH = vnH (12.8)
where H indicate the horizontal parts and ∆rnH
is the position divergence defined as the
difference in position from the initial position.
Equation (12.8) is called the foreground equation as it is not within the Kalman filter.
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 filter.
In order to level the body frame analytically Cnb is torqued so that no components are
present in v̇nH , which correspond to horizontal tilt of Cnb . 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 Cnb to steady state, the estimated earth rotation ωie n
b b
needs to correspond to the measurements from the gyroscope Ωib = Ωie .
As mentioned before, the driving parameter for the Kalman filter is the position diver-
gence vector ∆rnH . This vector is a measure of the attitude error Ψn and estimation
n
error of ω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 ∆rnH . Furthermore, an estimation error of
n
ωie will tilt the INS from the correct leveled attitude and thus again lead to a non-zero
∆rnH .
Using the approach described in Ch.10, the Kalman filter 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 Cnb in steady state. First of all, Cnb needs to
be leveled in order to cancel out the gravitation force in the horizontal plane. This
n
is performed by introducing a quantity called ωtilt used to correct Cnb . 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.
n n
Both ωie and ωtilt are directly connected to ∆rnH as any error in ∆rnH is either due to an
n
attitude error, estimation error of ωie or both. Last, the Kalman filter is allowed to reset
the errors in the last two INS states, vn and ∆rnH . In order to interlink ∆rnH with the four
parameters which are to be corrected, the Kalman gains are introduced. The Kalman
gains describe how ∆rnH is correlated with the system states. Four Kalman gains are
needed to correct the four parameters. Kalman gains k1 and k2 are used to estimate
n n
ωie and ωtilt and k3 and k4 are used to estimate velocity vn and position divergence
n
∆rH . The navigation equations (12.8) are now expanded into the form utilized during
fine alignment:
n
Ċb = Cnb Ωbib − (ωie
n n
+ ωtilt )Cnb
n
ωtilt = k2 unzn × ∆rnH
n e n
ωie = ωie H + uzn ωie sin l
n n
ω̇ie H = k1 uzn × ∆rnH
v̇nH = (Cnb an )H − k3 ∆rnH
∆ṙnH = vnH − k4 ∆rnH (12.9)
12.3. FINE ALIGNMENT 68
where
unzn = Unit vector along the n-frame vertical axis (z)

k1 , k2 , k3 , k4 = Kalman gains
n
ωtilt = The angular rate feedback used to correct the attitude error associated with Cnb
The cross product with unzn 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 figure Fig. 12.1(a)
the body has not been rotated and in Fig. 12.1(b) the body has been rotated positive
around x.
g = 9.82
z z
y
x x
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.
unzn = [0 0 1]T
unzn × [x y z]T = [-y x 0]T (12.10)
A block diagram of the fine alignment procedure can be seen in Fig. 12.2.
Figure 12.2 shows the fine 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 filter input. This filter
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 fine alignment a further description of
the derivation of the Kalman gains is done.
Vertical part
INS System of ie
n
Kalman
Filter
b n
ib n n b n n n ieH
C
b C b ib ( tilt ie )C
b K1
n
1 tilt
K2
s
n
Cb
ab n b n
vHn (Cb)Ha uzn
n
1 vH
K3
s n
r H
vH
n K4
n
1 rH Filter Input
s
Figure 12.2: Illustration of the fine 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 fine alignment within the Kalman filter framework as described in
Ch. 10 the observation set needs to be defined. As there is no aiding apart from the
knowledge of the quasi-stationarity, the observation set is:
zobs = ∆rnH − ∆rnref H (12.11)
where ∆rnref H is the reference position for the position divergence which is approximated
to zero due to quasi-stationarity.
As mentioned earlier, fine alignment uses an indirect Kalman filter to estimate the error
states which means that the model used by the Kalman filter should be the error model
which is derived in Ch. 11. The error model used when deriving the Kalman gains for
fine alignment is the model from (11.27). This error model is restated below:
n n
b n
+ δωie n
+ δωen
δ v̇n = −γ n × an + Cnb δab − (2δωie
n n
+ δωen ) × vn − (ωen
n
− 2ǫn × ωie
n
) × δvn +
2g
δhunzn + δgnM
R
ǫ̇n = ǫn × ωen n n
+ δωen
n 1 δh
R R
δ ḣ = δCne ωie
e
(12.12)
This error model is simplified as certain parameters are not relevant when performing
alignment. Using the fact that the platform is quasi-stationary zeroes the velocity vn
n
and the transport rate ωen giving the following expression:
γ̇ n = γ n × ωie
n n
− Cnb δ ω̃bib + δωie
2g
δ v̇n = −γ n × an + Cnb δab − 2δωie
n
× δvn + δhunR + δgnM (12.13)
R
These equations should be expanded to include the position divergence as this is the
driving parameter of the Kalman filter. This is easily included by recognizing that the
error in the position divergence vector is:
δ∆ṙn = δvn
(12.14)
As can be seen from (11.8) when ǫn is constant, the rate of the earth rotation error is
zero:
n d n e
δ ω̇ie =− (ǫ × ωie )=0 (12.15)
dt
Remembering that it is only the horizontal parameters which is of interest, the error
model for the Kalman filter is simplified 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 × ωie
n
− Cnb δ ω̃bib + δωie
n
2g
δ v̇n = −γ n × fn + Cnb δfb − 2δωie
n
× δvn + δh unR + δgnM (12.16)
R
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 first equation
is assumed negligibly small[Savage, 2000]. Secondly, the vertical part of γ n is defined
to be zero at the beginning of fine alignment. This can be achieved as the local level
navigation frame n has an arbitrarily chosen heading which can be chosen to coincide
with Cnb heading at fine alignment initialization. As the fine 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
n
assumptions and with the augmentation with the model of δωie from Sec. 12.4, the error
model is presented in matrix form:
 
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
 
 n 0 0 0 0 0 0 0 0 0

  n
δ ω̇ie 1
 0 0 0 0 0 0 0 0 δωnie
 γ̇ n  = 0 1 0 0 0 0 0 0 0 (12.17)
 γn
 
δ v̇n

0 0 1 0 0 0 0 0 0 δv
−fnU fnN
 
0 0 0 0 0 0 0
0 fnU −fnE
 
0 0 0 0 0 0
0 0 0 −fnN fnE 0 0 0 0
 
0 0 0 0 0 0
 0 0 0 0 0 0 
 
 0 0 0 0 0 0 
 
−c11 −c12 −c13 0 0 0  b
  δ ω̃ib
−c21
+ −c22 −c23 0 0 0  δfb (12.18)
−c31 −c32 −c33 0 0 0  ib
 
 0 0 0 c11 c12 c13 
 
 0 0 0 c21 c22 c23 
0 0 0 c31 c32 c33
This model is reduced to the horizontal version simply by removing the third parameter
of each variable. The horizontal version is presented below:
 
0 0 0 0 0 0
 n 0 0 0 0 0 0 δω n
 
δ ω̇ie 
ie

 γ̇ n  = 1 0 0 0 0 0  n 

0 1 0 γ (12.19)
n 0 0 0 δvn
δ v̇ H  0 0 0 −fnU 0 0 H
0 0 fnU 0 0 0
 
0 0 0 0
 0 0 0 0 
  b
−c11 −c12 0 0  δ ω̃ib
+−c21 −c22 0  δfb
 (12.20)
 0  ib H
 0 0 c11 c12 
0 0 c21 c22
ALIGNMENT 72
This yields the following model:
n n n n
x = [δωie H , γH , δvH , δ∆rH ]
b
np = [δωib , δfb , 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 1 0 0 0 0 0 0
A =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
 
0 0 0 0 0 0 0 0
 0 0 0 0 0 0 0 0
 
−C11 −C12 0 0 0 0 0 0
 
−C21 −C22 0 0 0 0 0 0
Gp = 
 
 0 0 C11 C12 0 0 0 0

 0 0 C21 C22 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

np = Process noise vector
A = Error states dynamic matrix
Gp = 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 filter:
z = δzobs = δ∆rnH − δ∆rnref H (12.22)
where
δ∆rnH = The position divergence error vector

δ∆rnref H = Is the error due to the assumption of ∆rnref H equal to zero
If δ∆rnref H is defined to be the difference between the true value and the value used by
the INS:
δ∆rnref H = ∆rnINS H − ∆rntrue H (12.23)
where
∆rnINS H = 0
The true value can be recognized as the vibrating part of the position. This means that
δ∆rnref H can be written as:
δ∆rnref H = −∆rnvib H (12.24)

where
∆rnvib H = The horizontal position error due to the quasi-stationary situation of the INS
With these definitions, the measurement equation is written as:
z = δ∆rnH + ∆rnvib H (12.25)
With both the state and measurement equation defined the first thing to notice is that
the error state dynamic matrix is constant which enables an offline calculation. The
discrete version is calculated in matlab and shown here:
1 0 0 0 0 0 0 0
 
 0 1 0 0 0 0 0 0
 
 Tn 0 1 0 0 0 0 0
 
 0 Tn 0 1 0 0 0 0
Φn =  g 2  (12.26)
 g 0 2 − 2 Tn 0 −gTn 1 0 0 0 
 Tn 0 gTn 0 0 1 0 0
2 g 3 g 2

 0 − 6 Tn 0 − 2 Tn Tn 0 1 0
g 3
6 Tn 0 gT2n Tn 0 0 0 1
where:
Φn = Discrete version of the error state dynamic matrix
Tn = Sampling time
The next thing to calculate is the integrated process noise matrix Qn which is used
in the covariance equations. From [Savage, 2000] a second order approximation matrix
can be found to be:
1 1
Q1n = Gp QpDens GTp Tn Qn ≈ Q1n + Φn Q1n ΦT n (12.27)
2 2
where:
Q1n = A first order approximation of the integrated process noise matrix
QpDens = The process noise density associated with the process noise vector np
It is assumed that the densities for the sensor errors in np are equal and the process
noise density is defined as:
 
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
QpDens = 
0
 (12.28)
 0 0 0 qf 0 0 0

0 0 0 0 0 qf 0 0
 
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
First of all Q1n is calculated:

 
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 
Q1n =
0
 Tn (12.29)
 0 0 0 qf 0 0 0 

0 0 0 0 0 qf 0 0 
 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0
ALIGNMENT 74
Q
The reason for Q1n being equal to pDensTn is due to the orthogonality principle of Cnb
n nT
where Cb Cb = I. Due to this property the integrated process noise is also constant
and can therefore be calculated offline.
The final task is to construct the output equations. Using the definition 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:
Hn = [0 0 0 0 0 0 I2x2 ]
Gmn = I2x2
nmn = ∆rnvib H
If we define the elements of nmn to have equal and uncorrelated variance the measure-
ment noise covariance matrix can be written as follows [Savage, 2000]:
Rn = E(nmn nT
mn ) = prvib H I2x2 (12.30)
where
prvib H = Horizontal quasi-stationary position random motion variance

E = Is the expected value operator (12.31)
Again it should be noted that both Hn , Gmn and Rn are independent of time, attitude
and position.
With both the state and measurement equations established, the Kalman filter gain and
error covariance matrices can be calculated according to the following general Kalman
filter equations:
Pn = Φn Pn-1 (+)ΦTn + Qn
−1
Kn = Pn (−)HT n Hn Pn (−)H T
n + Gm n R n GT
mn (12.32)
Pn (+) = (I − Kn Hn )Pn (−)
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:
unc = −x̂n (−e) (12.33)
Using this reset the discrete Kalman equations from (10.7) is as follows:
xn (+c) = x̂n (+e) + un = 0

x̂n (−) = Φn x̂n-1 (+c) = 0
ẑn = Hn x̂n (−) = 0
x̂n (+e) = x̂n (−) + Kn (zobsn − ẑn ) = Kn zobsn (12.34)
From (12.11) the observation is:
zobsn = ∆rnH (12.35)
This leads to the calculation of the control vector:
unc = −Kn ∆rnH (12.36)

Equation (12.36) using Kn from (12.32) constitute the complete Kalman filter 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 defining the individual parts of the control vector:
n n n n T
uc = [δωie Hc , γHc , δvHc , δ∆rHc ] (12.37)
where
c = Designation for the uc 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 Cnb is performed as follows. As defined in (11.6) errors in the rotation matrix Cnb
is:
δCnb = −Γn Cnb (12.39)
This error can also be interpreted as the correction to Cnb (−) why the corrected Cnb (+c )
is:
Cnb (+c ) = Cnb (−) + δCnb m

Cnb (+c ) = (I − ΓnH )Cnb (−) (12.40)
With this correction to the rotation matrix the four INS states are corrected in the
following way:
n n n
ωie Hn (+c) = ωie Hn (−) + δωie Hcn
n n n
Cb n (+c) = [I2x2 − ΓHcn ]Cb n (−)
vnHn (+c) = vnHn (−) + δvnHnc
∆rnHn (+c) = ∆rnHn (−) + δ∆rnHnc (12.41)
To sum up, the fine 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 Verification of Alignment
As the coarse and fine alignment has been developed, they are to be verified. Verification
of alignment is divided into two steps which are described below.
Step One:
In step one, the alignment schemes are verified by manually inputting acceleration and
gyroscopic measurements which would be measured by gyroscopes and accelerometers
at a specific and known orientation in space. This way the coarse alignment should make
a perfect one shot alignment. Secondly to verify the fine alignment the same inputs are
used while selecting the initial rotation matrix Cnb with a small error. This resembles a
real situation where the coarse alignment estimates Cnb with errors. The fine alignment
12.5. VERIFICATION OF ALIGNMENT 76
filter should correct Cnb and at fine 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 fine 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 Cnb and by inputting accelerometer
and gyroscope measurement which resembles a leveled INS pointing north, Cnb 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. Cnb 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 Cnb , fine alignment should decrease this error and
estimate the Cnb with greater accuracy. The error in the initial Cnb 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 Cnb is driven by the
error in the horizontal position. This error should be driven to zero as Cnb and earth
n
rotation ω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 Cnb , 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 Cnb and ωie n
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 Cnb 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 fine alignment only
torque horizontally to level the platform. Simultaneously it estimates the earth rotation
which is used to estimate the yaw angle.
0.1
East Acceleration
North Acceleration
Acceleration [m/s2] 0.05
−0.05
−0.1
−0.15
0 0.2 0.4 0.6 0.8 1
Time [min]
Figure 12.3: Acceleration due to misalignment
−4
x 10
4
East Position
3 North Position
2
0
Position [m]
−1
−2
−3
−4
−5
−6
0 0.2 0.4 0.6 0.8 1
Time [min]
Figure 12.4: Position divergence due to misalignment

1
Roll Error
Error [°] 0.5
0
−0.5
0 0.2 0.4 0.6 0.8 1
1
Pitch Error
Error [°]
0.5
0
−0.5
0 0.2 0.4 0.6 0.8 1
1
Yaw Error
Error [°]
0.5
0
−0.5
0 0.2 0.4 0.6 0.8 1
Time [min]
Figure 12.5: Euler rotation associated with Cnb
The Euler rotation associated with Cnb 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 Cnb . The
fine 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.95e−5 is expected on the y axis and much smaller on the x axis. As
n
can be seen in Fig. 12.6 ω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 Cnb .
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 fine alignment. This
was compared to a 15min fine 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 fine alignment used in this thesis estimate Cnb to
somewhat the same values. Although the differences 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 fine alignment developed does function with real-world measurements. To test if the
−3
x 10
1
Earth Rotation Rate Around X
0.5 Earth Rotation Rate Around Y
0
Angular Rate [rad/s]
−0.5
−1
−1.5
−2
−2.5
−3
0 0.2 0.4 0.6 0.8 1
Time [min]
Figure 12.6: Estimate of the earth rotation rate
−5
x 10
10
Earth Rotation Rate Around X
Earth Rotation Rate Around Y
8
Angular Rate [rad/s]
0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1

Time [min]
Figure 12.7: Estimate of the earth rotation rate

100
Yaw Angle [ °] 50
−50
−100
0 2 4 6 8 10
0
Yaw Angle [ °]
−0.5
−1
2 3 4 5 6 7 8 9 10
Time [min]
Figure 12.8: Wander angle at fine 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 fine alignment
estimated Cnb is a good estimate the accelerometer data is rotated through Cnb in order
to verify that Cnb 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 fine alignment needs some time
to estimate Cnb . The results are seen in Fig. 12.2. The gravity in Aberdeen, where the
fnx -2.55347332e-05
fny 6.11895069e-04
fnz 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 Cnb does actually
level the INS.
0.2
anx
0
−0.2
0 2 4 6 8 10
0.2
any
−0.2
0 2 4 6 8 10
10.5
10
anz
9.5
9
0 2 4 6 8 10
Time [min]
Figure 12.9: Accelerations rotated into n-frame
12.6 Discussion
As has been shown in the verification 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 infinite convergence rate as the Kalman gains can be infinitely 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 difference on the theoretical gravity in Aberdeen and the one measured is
approximately 0.1mg. This difference stems from different 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 fine alignment precision.
As with the navigation equations, it is interesting to investigate what might influence

alignment. Two different 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 effects arise from non-quasi-stationarity. In practice, two
movements differs 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 effect is present which is due to the moving navigation frame.
This effect 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.
−6 East Axis −9 East Axis

x 10 x 10
−6.413 −8.2
Acceleration [m/s2]
Roration Rate [/s]
−6.4135 −8.3
−6.414 −8.4
−6.4145 −8.5
0 5 10 15 0 5 10 15
−6 North Axis −9 North Axis
x 10 x 10
3.475 3.4
Acceleration [m/s2]
Roration Rate [/s]
3.4749 3.2
3.4748 3
3.4747 2.8
0 5 10 15 0 5 10 15
−6 Up Axis −10 Up Axis
x 10 x 10
−1.8 10
Acceleration [m/s ]
2
Roration Rate [/s]
9
−2
8
−2.2
7
−2.4 6
0 5 10 15 0 5 10 15
Time [min] Time [min]
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 effect 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:

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/s2 ] and 10−9 [rad/s] for nonzero
velocity. High-grade inertial sensors today experience bias stabilities of 10−3 [m/s2 ] and
10−8 [rad/s]. Hence, it can be concluded, that sensor errors have a larger influence on
12.6. DISCUSSION 84
−6 East Axis −6 East Axis

x 10 x 10
1 4
Acceleration [m/s2]
Roration Rate [/s]

0.5 3
0 2
−0.5 1
−1 0
0 5 10 15 0 5 10 15
−6 North Axis −7 North Axis
x 10 x 10
1 0
Acceleration [m/s2]
Roration Rate [/s]
0.5
−0.5
0
−1
−0.5
−1 −1.5
0 5 10 15 0 5 10 15
−6 Up Axis −5 Up Axis
x 10 x 10
1 8.7267
Acceleration [m/s2]
Roration Rate [/s]
0.5
8.7267
0
8.7267
−0.5
−1 8.7267
0 5 10 15 0 5 10 15
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:

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 influence 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 Cnb , the fine alignment procedure is performed afterwards.
Fine alignment uses an indirect Kalman filter in order to estimate attitude and heading
of the INS. In order to perform a precise fine 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 filter
to estimate attitude and heading of the INS.
The fine alignment procedure has been tested with both simulated inputs and with real-
world sensor measurements. Both tests showed that the alignment procedure worked
and fine alignment was able to decrease the error in the one shot coarse alignment
estimate.
Chapter 13
Inertial Navigation with Aiding
Unaided INS navigation inevitably suffer 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 different
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 filter is able to estimate with greater accuracy. This report will describe the
use of two different 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
different 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 filtering.
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 different quantities and
suffer from different 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 different error characteristics makes for a beneficial 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 offset 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 specific time.
As the clock in the receiver is not synchronized with the satellite clock this clock offset
also needs to be determined. For this reason, four satellites are needed to determine the
position.
A general GPS scheme is illustrated below.
Code tracking
Loop Pseudo Range
Antenna and Code

RF Hardware Correlation
Carrier
Delta Range
Tracking Loop
Figure 13.1: Illustration of a simplified GPS receiver scheme

[Brown and Hwang, 1997]
The first 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 different
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
effect.
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]:
q
σ1 = (xsat1 − xgps )2 + (ysat1 − ygps )2 + (zsat1 − zgps )2 + c∆t
q
q
q
σ4 = (xsat4 − xgps )2 + (ysat4 − ygps )2 + (zsat4 − zgps )2 + c∆t (13.1)
where
σi = Pseudorange from the i’th satellite to the GPS receiver antenna.
xsati , ysati , zsati = Position of the i’th satellite
xgps , ygps , zgps = Position of the GPS receiver antenna
c = Speed of light
∆t = Offset 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 i’th satellite
where the radius is corrected with the clock offset. 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 different approaches. Uncoupled, loosely
coupled and tightly coupled aiding. A short introduction will describe the advantages
and drawbacks of the different 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 simplified 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 filter. The main advantages of this
GPS receiver
GPS position and

Antenna and RF Correlation and velocity estimates
hardware tracking loops GPS Kalman filter
INS/GPS Kalman Error estimations

Position filter integration
INS
Inertial sensors INS computation INS position and

velocity estimates
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 filters. As the output from the GPS Kalman
filter is used as a measurement this is assumed to have white noise by the integration
Kalman filter. This is not necessary true as the output from the GPS may be time
correlated. This can happen if the integration filter 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 reflections. 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 filter requires
knowledge of the covariance matrix of the GPS filter output. This covariance matrix
differ 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.

GPS receiver
GPS pseudo-range and

Antenna and RF Correlation and delta-range estimates
hardware tracking loops
INS/GPS Kalman Error estimations

Position filter integration
INS
Inertial sensors INS computation INS pseudo-range and

delta-range estimates
Figure 13.3: Tightly coupled INS/GPS approach
As can be seen in the figure, it is now the pseudo-range and delta-range which are
used in the integration filter. 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 filters 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 i’th 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:
zobsi = 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 i’th signal reception of the GPS antenna
t̃GPSi = The time of signal transmission from the i’th satellite
c = The speed of light
The measurement equation for the Kalman filter needs to be in a form suitable for
the filter which is in error states. For this reason the measurement equation is the
differenced version of (13.2), z = ẑobs − zobs which is:
zi = δpi − c(δtreci − δtGPSi ) (13.4)
This is the measurement equation for the Kalman filter so an expression needs to be
derived for the three quantities δpi , δtreci , δtGPSi
The quantity δpi is the error in the range from the INS to the i’th satellite. This error
originate from two different 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 fixed coordinate system(E-frame) the range is also defined in the E-frame:
p̂ei = r̂esi − r̂e − l̂e (13.5)
where
r̂esi = Position vector from the center of the earth to the i’th 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 find the range error the magnitude of (13.5) is defined as:
p̂2i = (p̂ei )T p̂ei (13.6)
In order to find the range error δpi the differential of (13.6) is found:
2p̂i δp̂i = (δpei )T p̂ei + (p̂ei )T δpei = 2(p̂ei )T δpei (13.7)
which can be rearranged into:

(p̂ei )T δpei
δpi = (13.8)
p̂i
δpei can be substituted with the differential of (13.5) to give:
(p̂ei )T (δresi − δre − δle )

δpi = (13.9)
p̂i
As it can be seen from (13.9), δpi is calculated from a vector dot product. As a dot
product is invariant p̂i can be evaluated in the navigation frame[Savage, 2000]:
(Cne p̂ei )T (Cne δresi − δrn − δln )

δpi = (13.10)
p̂i
The lever arm error can be expressed as follows:
δln = δ(Cnb lb ) = δCnb lb + Cnb δlb (13.11)
Using (11.6) on (13.11) reformulates it to:
δln = −Γg Cnb lb + Cnb δlb = (Cnb lb ) × γ + Cnb δlb (13.12)
With these rearrangement δ p̂i from (13.4) is:
(Cne p̂ei )T (Cne δresi − δrn − (Cnb lb ) × γ + Cnb δlb )

δpi = (13.13)
p̂i
13.2. GPS ERROR MODEL ANALYSIS 92
The last thing to do before implementation with the Kalman filter is to derive expres-
sions for the lever arm error δlb , position error of the i’th satellite δr̂esi and the clock
error δtreci ,δtreci .
The true lever arm is modelled as a constant and a flexing term:
lb = lbconst + lbflex l̇bconst = 0 (13.14)
As the lever arm is assume constant the lever used by the computer is a constant:
l̂b = lbconst l̇bconst = 0 (13.15)
The lever arm error is the estimated lever arm subtracted from the true lever arm:
δlb = δlbconst − lbflex δ l̇bconst = 0 (13.16)
For INS/GPS integration, it is customary to assume that δr̂esi and δtreci 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 suffer 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 defined
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:
1
t̂recn = t̂recn-1 + tosc0 = t̂recn-1 + (13.17)
fosc0
where
t̂rec = Clock time

tosc0 = Nominal time between clock ticks
fosc0 = 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 1 1 1
= = (13.19)
∆t fosc0 ∆t fosc0 ∆t
Recognizing that the left-hand term is the derivative of t̂recn yields:
f̃osc
t̂˙ rec = (13.20)
fosc0
where
f̃osc = 1/∆t = The actual clock frequency
As it is the error in the clock time δrreci from (13.4) which is needed this is derived as
the differential of (13.20) plus a random term [Savage, 2000]:
δfosc
δ ṫf = δtreci = δtf + nt (13.21)
fosc0
where
δtf = Is the receiver clock error without the random part due to jitter and quantisation
nt = Is the random part of receiver clock error due to jitter and quantisation
δfosc = Is the oscillatory part of the receiver clock error
The random part nt 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 first order Markov process[Savage, 2000]:
δfosc = δfosc,rand + δfosc,Mark

δ ḟosc,rand = nosc,rand
δ ḟosc,Mark = −Cosc,Mark δfosc,Mark + nosc,Mark (13.22)
where
δfosc,rand , δfosc,Mark = Is the random and first order Markov process error contribution
Cosc,Mark , nosc,Mark = Is the correlation frequency and white noise error contribution
for the first order Markov process
nosc,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 specific 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 differs 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:
vnINS,aid = Cnb vbref + Cne l̇e (13.24)
The observation equation for the Kalman filter is the INS estimated velocity (v̂n ) minus
the measured velocity (vnINS,aid ):
˙
zobs = v̂n − vnINS,aid = v̂n − Ĉnb v̂bref − Ĉne 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:
zobs = v̂n − Ĉnb v̂bref − Ĉnb (ω̃ib

b
× l̂b ) (13.26)
The associated measurement equation is the differential of (13.26):
z = δvn − δCnb v̂bref − Ĉnb δvbref − Ĉnb (ω̃ib

b
× δlb ) (13.27)
13.3. IMPLEMENTATION OF NAVIGATION WITH AIDING 94
where δCnb (ω̃ib

b
× lb ) and Cnb (δ ω̃ib
b
× lb ) 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 =δvn + (γ n × Cnb )v̂bref − Ĉnb δvbref − Ĉnb (ω̃ib

b
× (δlbconst − lbflex ))
δvn − v̂nref × γ n − Ĉnb δvbref − Ĉnb (ω̃ib
b
× δlbconst ) + Ĉnb (ω̃ib
b
× lbflex) (13.28)
An approximation for v̂bref derived from (13.24) gives the following:

b
v̂nref = v̂n − Ĉnb (ω̃ib
b
× l̂ ) (13.29)
Using this expression in (13.28) gives:

b
z = δvn − (v̂n − Ĉnb (ω̃ib
b
× l̂ )) × γ n − Ĉnb δvbref − Ĉnb (ω̃ib
b
b
× lbflex)
(13.30)
b b
As the parameter l̂ can be set freely as long as δlbconst is taken account, l̂ is chosen
to be zero. Furthermore, the measurement equation is modified 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
δυ n = δvn + ǫn × vn (13.31)
Using these properties gives the following:
z = δυ n − v̂n × ψ n − Ĉnb δvbref − Ĉnb (ω̃ib

b
b
× lbflex ) (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
δvbref . As this expression differs 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 specific GPS which is used
n
n n
rINSvINS
INS rn
vn
n +
rINS
rn v
n
Kalman
Filter
-
GPS rn n
v
GPS GPS
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 figure 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 filter 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 filter is developed using the same approach as in Sec. 12.4.
The observation equation is:
rn − rnGPS

zobs = nINS (13.33)
vINS − vnGPS
The measurement equation is:
z = ẑnobs − znobs m
n
+ δrnINS − rntrue − δrnGPS
n
δrINS − δrnGPS

r
z = ntrue = (13.34)
vtrue + δvnINS − vntrue − δvnGPS δvnINS − δvnGPS
where
δrnINS , δvnINS = Is the error in the INS estimation of position and velocity
δrnGPS , δvnGPS = Is the error in the GPS estimation of position and velocity
δrntrue , δvntrue = Is the true position and velocity
With the measurement equation derived, the model for the Kalman filter is:
ẋ = Ax + Gp np
z = Hx + Gm nm (13.35)
where
T
x = ψ n δvn δrn

(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:
 
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
H=
0
 (13.37)
 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
The measurement noise is derived in the same way as in Sec. 12.4 and is as follows:

I 0
Gmn = 3x3
0 I3x3
n
δvGPS
nmn = (13.38)
δrnGPS
As in Sec. 12.4 nm is assumed to have equal and uncorrelated variance which leads to
the following measurement covariance matrix:

I δGPSvelocity 0
Rn = E(nmn nT
mn) = 3x3 (13.39)
0 I3x3 δGPSposition
where
δGPSvelocity , δGPSposition = GPS velocity and position variance

E = Is the expected value operator
The last thing before implementation is to find 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 −TaU TaN 1 T(2Ωc ) T(−2Ωb −Ωβ ) −T gr 0 0 
 

Φ=
 TaU 0 −TaE T(−2Ωc ) 1 T(2Ωa +Ωα ) 0 −T gr 0  
 −TaN TaE 0 T(2Ωb +Ωβ ) T(−2Ωa −Ωα ) 1 0 0 2T gr 
 
 0 −0.5T2 aU 0.5T2 aN T 0 0 1 0 −Ωβ 
0.5T2 aU −0.5T2 aE
 
0 0 T 0 0 1 Ωα
−0.5T2 aN 0.5T2 aE 0 0 0 T Ωβ −Ωα 1
(13.40)
The integrated process noise matrix is found using the same approach as in Sec. 12.4
where:
1 1
Q1n = Gp QpDens GT
p Tn Qn ≈ Q + Φn Q1n ΦT (13.41)
2 1n 2 n
So the first order process noise matrix is:

 
qw 0 0 0 0 0 0 0 0
0 qw 0 0 0 0 0 0 0
 
0 0 qw 0 0 0 0 0 0
 
0 0 0 qf 0 0 0 0 0
 
Q1n 0
= 0 0 0 qf 0 0 0 0
 Tn (13.42)
0 0 0 0 0 qf 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
The last thing to define the control reset vector and how it is used correct the INS
states.
The control reset vector is defined as in Sec. 12.4 which gives:
uc = [ψ n , δvv , δrv ]T (13.43)
The states which are corrected are the body to navigation rotation matrix Cnb with γ n ,
navigation to Earth rotation matrix Cen with ǫn and velocity vn with δvn . If one look
at uc the only state which can be directly corrected is the velocity. In order to correct
Cnb and Cen some transformations are necessary. As ψ contain both the error in Cnb and
Cen , δr is needed as it describe the errors in Cen . It has been defined 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 δrn relation from (11.43) ǫn is calculated from δrn in the following way:
δrn = R(ǫn × unzn ) + δRunzn m

   
δrx /R ǫy
δry /R = −ǫx + δRu (13.44)
δrz /R 0
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):
Cnb (+c) = [I3x3 − Γn ]Cnb

Cen (+c) = [I3x3 − En ]Cen
vn (+c) = v(−c) + δvn (13.46)
With these equations derived, the Kalman filter is ready for implementation and verifi-
cation.
13.4 Verification 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 first 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.
57.194
Aided position
57.193 Real position
57.192
57.191
Latitude [°]
57.19
57.189
57.188
57.187
57.186
−2.086 −2.084 −2.082 −2.08 −2.078
Longitude [°]
Figure 13.5: This figure 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
57.194
Aided position
57.193 GPS position
Real position
57.192
57.191
Latitude [°]
57.19
57.189
57.188
57.187
57.186
−2.086 −2.084 −2.082 −2.08 −2.078
Longitude [°]
Figure 13.6: This figure 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 first 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 first minute of
navigation. What can be seen is that the fused position (dotted) does not suffer from
the same sudden jumps to the same degree as the GPS (x).
13.5 Summary
As unaided INS inevitably suffers from unbounded position error, some kind of aiding
is preferable if INS is to be used over longer time periods. During this chapter, two
different 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 different approaches are described in this thesis where the loosely coupled
13.5. SUMMARY 100
57.192 57.1912
57.191
57.1911
57.19
Latitude [°]
57.189 57.191
57.188
57.1909
57.187
57.186 57.1908
0 1 2 3 4 5 0 0.2 0.4 0.6 0.8 1
−2.078 −2.0818
−2.08 −2.0819
Longitude [°]
−2.082 −2.082
−2.084 −2.0821
−2.086 −2.0822
0 1 2 3 4 5 0 0.2 0.4 0.6 0.8 1
Figure 13.7: This figure 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 different physical phenomena
to measure specific 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 different lengths if the path is rotated. This is illustrated
in Fig. 14.1
Light path
CW beam
Light
ω
source r
Beam splitter/
detector
CCW beam
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 difference 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 difference 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 fibre 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 difference can be measured as a change of intensity by photodetectors. This
makes the IFOG an analog rate sensor. As the IFOG uses fibre 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 insignificant 1◦ /hour/g
g2 -dependent bias insignificant 0.1◦ /hour/g2
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 Newton’s Second Law of motion to measure specific force.
F = ma (14.1)
A figure 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-off.
14.3. SENSOR ERROR MODELS 104
Acceleration
Case
Spring
Proof mass Displacement pick-off
Spring
Figure 14.2: Conceptual drawing of a accelerometer
It is important to notice that gravity influences 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 suffers 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 difference
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 influence of acceleration on the output of the sensor.
Temperature sensitivity The influence of temperature on the output of the sensor.
Quantization noise The noise introduced by converting an analog signal into a finite
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:
   
Bx (T ) SFx (T ) MAxy (T ) MAxz (T )
δo(t) =  By (T )  +  MAyx (T ) SFy (T ) MAyz (T )  i(t) (14.2)
Bz (T ) MAzx (T ) MAzy (T ) SFz (T )
Where i are the measured input, o are the output, the B’s are the biases, the SF’s are
the scale factors, the MA are the misalignments (with the subscripts defining 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 influencing the signal itself.
The repeatable errors, however, can be identified 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 final 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 influences the measurements. A rotation and a
measurement can be done in 5 to 20 seconds depending on environmental effects.
15.2 Calibration Equations
In order to find some of the sensor misalignment errors, it is necessary to define 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 simplified 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 simplified error model of the system is:
106
107 CHAPTER 15. CALIBRATION OF SENSORS
θzn −θyn −gθyn

    
0 0
δ v̇n = δan + θzn 0 θxn  0 = δan +  gθxn  (15.1)
θyn θxn 0 g 0
Where θyn and θxn are the misalignment angles from the sensor to the body frame around
the y and x axis and δan 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.
Set 1 Set 2 Set 3

zb yb xb
3 (180◦ ) 3 (180◦ ) 3 (180◦ )
2 (180◦ ) 2 (180◦ ) 2 (180◦ )
xb zb yb zb
1 (180◦ ) yb 1 (180◦ ) xb 1 (180◦ )
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)):
−gθyn (T ) −gθyn (0)
     
δ v̇n (T ) − δ v̇n (0) = δan (T ) +  gθxn (T )  − δan (0) +  gθxn (0) (15.2)
0 0
n
 
−g∆θy
= ∆δan +  g∆θxn  (15.3)
0
Where T are the time after a rotation, and 0 is the time before a rotation. The ∆ prefix
denotes that the following term is the difference of the term from 0 to T .
To obtain ∆δan and ∆θ n , the sensor error model from chapter Ch. 14 is used. The
accelerometer error model is
    
aBx aSFx aMAxy aMAxz
δan (t) = Cnb (t)δab (t) = Cnb (t)  aBy  +  aMAyx aSFy aMAyz  ab (t)
aBz aMAzx aMAzy aSFz
(15.4)
Where δab are the errors in the measured accelerations, the aB terms are a constant
bias, aSF are scale factors, aMA are misalignments and ab is the acceleration due to
gravity. From this, ∆δan is obtained as
∆δan = δan (T ) − δan (0) (15.5)

15.2. CALIBRATION EQUATIONS 108
Identically, the gyro error model is
   
gBx gSFx gMAxy gMAxz
δω n (t) = Cnb (t)δω b (t) = Cnb (t)  gBy  +  gMAyx gSFy gMAyz  ω b (t)
gBz gMAzx gMAzy gSFz
(15.6)
To obtain ∆θ n , δω n is integrated
 
Z T gSFx gMAxy gMAxz
∆θ n = Cnb (t)  gMAyx gSFy gMAyz  ω b (t)dt (15.7)
0 gMAzx gMAzy gSFz
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
 
cos(φ(t)) 0 − sin(φ(t))
Cnb (t) =  0 1 0  (15.8)
sin(φ(t)) 0 cos(φ(t))
Rotation 3
 
cos(ψ(t)) − sin(ψ(t)) 0
Cnb (t) =  sin(ψ(t)) cos(ψ(t)) 0  (15.9)
0 0 1
Where φ(t) and ψ(t) are the rotation angles at time t.

With this, solving (15.2) for rotation 1 gives
   
1 0 0 0
Cnb (0) =  0 1 0  ab (0) =  0  (15.10)
0 0 1 g
   
−1 0 0 0
Cnb (T ) =  0 1 0  ab (T ) =  0  (15.11)
0 0 −1 −g
       
aMAxz g aMAxz g −2aBx −2aBx
∆δan =  −aMAyzg  −  aMAyzg  +  0  =  −2aMAyzg  (15.12)
aSFz aSFz −2aBz −2aBz
   
Z π (gMAxy cos(φ) + gMAzy sin(φ)) −2gMAzy
∆θ n = −  gSFy  dφ =  −πgSFy  (15.13)
0 (gMAxy cos(φ) + gMAzy sin(φ)) 2gMAxy
   
−2aBx gSFy πg
δ v̇n (T ) − δ v̇n (0) =  −2aMAyzg  +  −2gMAzyg  (15.14)
−2aBz 0
Solving for the remaining rotations, one obtains the measurements given in table 15.1,
where for brevity, the following definitions are made
Set j, Rotation k: αjk = δ v̇xn (T ) − δ v̇xn (0), βkj = δ v̇xy

n
(T ) − δ v̇yn (0) (15.15)
Set 1 Set 2 Set 3

α11 =−2aBx +gSFy πg α21 =−2aBz +gSFx πg α31 =−2aBy +gSFz πg
1
β1 =−2(aMAyz +gMAzy )g β12 =−2(aMAxy +gMAyx )g β13 =−2(aMAzx +gMAxz )g
α12 =2aBx +gSFy πg α22 =2aBz +gSFx πg α32 =2aBy +gSFz πg
1
β2 =2(aMAyz +gMAzy )g β22 =2(aMAxy +gMAyx )g 3
β2 =2(aMAzx +gMAxz )g
α13 =−2aBx −2(aMAxz −gMAxz )g α23 =−2aBz −2(aMAzy −gMAzy )g α33 =−2aBy −2(aMAyx −gMAyx )g
β31 =−2aBy −2(aMAyz −gMAyz )g β32 =−2aBx −2(aMAxy −gMAxy )g β33 =−2aBz −2(aMAzx −gMAzx )g
Table 15.1: Table of observation equations for calibration coefficients
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 defining 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̇zn component at the start of the first rotation and at the end of the second
rotation in each set after subtracting the corresponding bias term. For the aSFz term:
For set 1 rotation 1 : γ11 = δ v̇zn (0) (15.16)

For set 1 rotation 2 : γ21 = δ v̇zn (T ) (15.17)
γ 1 + γ21 − 2aBz
aSFz = 1 (15.18)
2g
Remaining is to find the rotation matrix from the sensor frame to the body frame and
the gyro bias. This is done by assuming the following approximations:
δv̈xn = δ ȧnx − g θ̇yn ≈ −g θ̇yn (15.19)

δv̈yn = δ ȧny + g θ̇xn ≈ g θ̇xn (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:
δv̈yn (T ) − δv̈yn (0)

gBx = (15.21)
2g
δv̈xn (T ) − δv̈xn (0)
gBy = (15.22)
−2g
And the z gyro bias is found from set 2 rotation 3:
δv̈yn (T ) − δv̈yn (0)

gBz = (15.23)
2g
The rotation matrix from sensor to body frame is found by calculating θxn and θyn as
well as the heading angle θzn . All are calculated from set 1 rotation 3:
15.3. SMOOTHING FILTER 110
δ v̇yn (T ) + δ v̇yn (0)

θxn = (15.24)
2g
n
δ v̇ (T ) + δ v̇xn (0)
θyn = x (15.25)
−2g
n
δv̈ y (T ) + δv̈yn (0)
θzn = (15.26)
−2gΩny
Where Ωny is the earth rate y component in the navigation frame.
α12 +α11 α22 +α21 α32 +α31

gSFy = 2πg gSFx =
2πg gSFz = 2πg
α1 −α1 α2 −α2 α3 −α3
aBx = 2 4 1 aBz = 2 4 1 aBy = 2 4 1
α1 +2a α2 +2a −β 1
gMAxz = 3 2g Bx gMAxy = 3 2g Bx gMAzy = 2g1
β 1 +2a β2 β 3 +2a
gMAyz = 3 2g By gMAyx = 2g1 gMAzx = 3 2g Bz + aMAzx
α3 α3 +2a
aMAzx = − 2g1 − gMAxz aMAzy = α32+2a
2g
Bz
+ gMAzy aMAyx = 3 2g By + gMAyx
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 filter is applied to the measurements with the
dynamic equation
 ˙      
δv 0 1 0 δv 0
 δ v̇  =  0 0 1   δ v̇  +  0  (15.27)
δv̈ 0 0 0 δv̈ wk
and measurement matrix
 
δv
y = h δ v̇  + wr (15.28)
δv̈
where

h= 0 1 0
wr = 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 coefficients 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 coefficients
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
coefficients converge towards the true errors after a few iterations.
Error True Estimated

gBx −0.1E-4 −0.098728E-04
gBy 0.2E-4 0.20059E-04
gBz −0.3E-4 −0.29865E-04
gSFx 1E-4 1.0072E-04
gSFy −2E-4 −2.0099E-04
gSFz 3E-4 3.0153E-04
gMAxy −0.1E-4 −1.8477E-05
gMAxz 0.2E-4 2.0168E-05
gMAyx −0.3E-4 −3.4114E-05
gMAyz 0.4E-4 3.0360E-05
gMAzx −0.5E-4 −6.1056E-05
gMAzy 0.6E-4 5.7603E-05
aBx 0.01 0.01
aBy −0.02 −0.02
aBz 0.03 0.03
aSFx 0.001 0.001
aSFy −0.002 −0.002
aSFz 0.003 0.003
aMAyx −0.3E-4 −3.3082E-05
aMAzx −0.5E-4 −5.3109E-05
aMAzy 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 difficult to obtain the sensor errors in a particular IMU, and
thus, it is not possible to quantify the difference between the estimated errors and the
actual errors.
15.4. CALIBRATION EXAMPLE 112
−5 Gyro X bias estimate

x 10
0
−1
−2
−3
estimate
−4
−5
−6
−7
1 2 3 4 5 6 7 8
iteration
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 simplified
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 filter used in this thesis is an indirect variant of the normal direct Kalman
filter Ch. 10 shows the differences between the two filters an derives the equations used
in later chapters to implement this filter. The indirect Kalman filter is an advantageous
approach when the system model has high dynamics and/or is non-linear. The indirect
Kalman filter 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 filter requires an error model of
the INS, which is derived in Ch. 11. Two different error models are derived. Although
these error models model the same system they have different properties as they describe
the errors with different 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 filter technique.
As the INS inevitably suffer 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 different 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 verified 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 sufficiently accurate to validate
a verification of the system. Verification of the developed navigation equations also
verified the earth simulation model, as the equations in the simulation model showed to
be the inverse of the navigation equations. Further tests with different 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 filter, which uses an error
model of the system. Two different 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 verified using both simulated
test data and real-world data from the Kearfott T16 system. The alignment algorithm
was able to estimate Cnb precisely using the simulated test data and showed to estimate
Cnb 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 different 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 filter. 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 verified 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 files in this
thesis, the errors involved with the discretization procedure have not been analyzed.
The choice of discretization method affect 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 offers 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 profile 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 profiles, 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 filer 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 filter at this frequency.
When correcting the rotation matrices a first 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 difficult 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

INS

Uploaded by

Copyright:

Available Formats

INS

Uploaded by

Document Information

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

INS

Uploaded by

Copyright:

Available Formats

Inertial Navigation System

Department of Control Engineering

This thesis documents the development of algorithms

Denne afhandling dokumenterer udviklingen af de al-

Rolf Christensen Nikolaj Fogh

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

8 Earth Simulation Model 32

10 Error Estimation Using Indirect Kalman Filtering 49

13 Inertial Navigation with Aiding 86

14 Inertial Sensors 102

15 Calibration of Sensors 106

III Thesis Summary 116

18 Improvements and Discussion 120

Historical Development of INS

Scope of This Project

The thesis structure is as follows:

Coordinate Frames and Transformations

It is often needed to represent a vector in different coordinate frames. One coordinate

Figure 2.1: Rotation around the positive y axis

ζ + ryj = ryi cos(α)

It is customary to write these equations in matrix form as

If an equation is wished to be expressed in another coordinate system this can be

Matrix A is said to be transformed under a similarity transformation.

Relative Angular Velocity of Frames

Computed and Measured Quantities

ω̃ijj = measured angular velocity

Skew Symmetrix Matrices

Throughout the thesis, it is sometimes convenient to change between a vector rep-

Time Derivative of the Direction Cosine Matrix

∆Cji Cji (t + ∆t) − Cji (t)

ωjii → Ωiji (2.11)

Ωiij = Cij Ωjij Cji

Using this property, (2.10) can be written as:

Ċji = Cji Ωiji = −Ωjij Cji (2.12)

Earth-Centered Inertial Frame

In INS applications, the inertial frame i is chosen as an earth-centered inertial frame

In terrestrial navigation, one is primarily interested in knowing longitude, latitude,

Earth-Centered, Earth-Fixed Frame

Local Geodetic/Geographic Frame

Figure 4.1: Inertial Measurement Unit measuring in two dimensions.

ann = aby cos θ + abz sin(θ)

Inertial Navigation Systems

Figure 5.1: Simulation and INS

6.1 Geodetic Variables and Constants

The eccentricity of the earth is defined as

6.2 Gravity Model

From [Britting, 1971], the gravitational magnitude can be calculated approximately

Where J2 , and µ are empirical constants calculated by geodetic measurements and r is

From [Savage, 1997], r is approximated as:

As shown in the example in Ch. 4, an INS is based on integrating the acceleration

Derivation of Equations of Motion

an = Cni r̈i + gn (7.1)

Where an is the acceleration in the navigation frame.

vn , Cne ṙe (7.2)

The result of this chapter is an expression relating vn to the accelerometer measurements

vn = Cni ṙi − Cni Ωiie ri (7.5)

By isolating ṙi from (7.4) one obtains