Are you sure?
This action might not be possible to undo. Are you sure you want to continue?
ω
ie
i
y
, e
y
N, g
y
E, g
x
U, g
z
Equator
l
L
λ
e
z
i
z
i
x
e
x
Department of Control Engineering
10th Semester
Aalborg University 2008
A A L B O R G U N I V E R S I T Y
Master project
Section of Automation and Control
Department of Electronic Systems
Faculty of Engineering, Science and Medicine
Fredrik Bajers Vej 7 C3
DK9220 Aalborg Øst
Denmark
Telephone +45 96 35 87 02
Fax +45 98 15 17 39
http://www.control.aau.dk/
Title:
Inertial Navigation System
Theme:
Estimation and nonlinear control
Period:
FallSpring Semester 20072008
Project group:
08gr1030a
Group members:
Rolf Christensen
Nikolaj Fogh
Supervisor:
Anders la CourHarbo
Morten Bisgaard
Copies: 5
Pages: 122
Attachments: None
Finished: 4
th
of June 2008
Synopsis:
This thesis documents the development of algorithms
for an inertial navigation system (INS). In general,
inertial navigation consists of three parts: Alignment,
navigation and aiding. Algorithms for these parts are
derived in this thesis.
In order to create reproducable test data for veriﬁ
cation and analysis of the algorithms, an earth simu
lation model is developed. The algorithms were also
tested against a reference INS, a Kearfott T16 INS,
to verify that the algorithms also work on realworld
data with real noise sources.
The navigation equations needed for an INS for ter
restrial navigation are developed and tested against
the reference INS. Tests showed that the developed
navigation equations were able to follow the reference
trajectory to a satisfactory degree, thus validating the
navigation equations.
The alignment algorithms make use of an indirect
Kalman ﬁlter, which estimates the errors in the INS
states. For this, an error model is developed, which
describes the propagation of errors within the INS.
An alignment algorithm is developed to estimate tilt
and heading of the system. The alignment algorithm
was veriﬁed against data from the reference system,
and was shown to be able to estimate tilt and heading
to a satisfactory degree.
Algorithms for aiding the INS navigation equations
were developed for a speedometer and a GPS. These
algorithms also use the indirect Kalman ﬁlter and the
error model. Using data from the reference system
together with GPS measurements, it was shown that
the developed algorithms were able to aid the naviga
tion equations and produce better results than either
of the systems in a standalone setup. It was also
shown that the INS was able to track position during
GPS outages.
A A L B O R G U N I V E R S I T Y
8. Semester
Sektion for Automation og kontrol
Institut for Elektroniske Systemer
Det Ingeniør, Natur, og Sundheds
videnskabelige Fakultet
Fredrik Bajers Vej 7 C3
DK9220 Aalborg Øst
Danmark
Telefon +45 96 35 87 02
Fax +45 98 15 17 39
http://www.control.aau.dk/
Titel:
Inertial Navigation System
Tema:
Estimation and nonlinear control
Periode:
EfterårForår 20072008
Projektgruppe:
08gr1030a
Gruppemedlemmer:
Rolf Christensen
Nikolaj Fogh
Vejleder:
Anders la CourHarbo
Morten Bisgaard
Kopier: 5
Sider: 122
Bilag: Ingen
Færdiggjort: 4. Juni 2008
Synopsis:
Denne afhandling dokumenterer udviklingen af de al
goritmer der bruges i inertial navigation systemer
(INS). Generelt består inertial navigation af tre dele:
Alignment, navigering og aiding. Denne afhandling
indeholder algoritmer til alle disse dele.
En simuleringsmodel af jorden er blevet lavet for at
kunne lave reproducerbare test data til veriﬁkation og
analyse of de udviklede algoritmer. For at veriﬁcere
at algoritmerne også fungerer med data fra virkelighe
den, er algoritmerne også blevet testet med et refer
encesystem, en Kearfott T16 INS.
De navigeringsligninger som er nødvendige for nav
igering i nærheden af jorden er blevet udviklet og
testet med referencesystemet. Testene viste at de ud
viklede navigeringsligninger var i stand til at følge
referencesystemet til en tilfredsstillende grad, hvilket
gør at navigeringsligningerne er veriﬁcerede.
En alignment algoritme bruger et indirect Kalman ﬁl
ter som estimerer fejlene i INS tilstandene. Derfor er
der blevet udviklet en fejlmodel, som beskriver udbre
delsen af fejl i INS systemet. En alignment algoritme
er blevet udviklet som estimerer hældning og retning
af systemet. Alignment algoritmen er blevet veriﬁ
ceret op imod data fra referencesystemet, og det er
blevet vist at den var i stand til at estimere hældning
og retning tilfredsstillende.
Aidingalgoritmer til navigeringsligningerne er blevet
udviklet for en hastighedsmåler og en GPS. Disse al
goritmer bruger også et indirect Kalman ﬁlter og en
fejlmodel. Ved brug af data fra referencesystemet
samt simulerede GPS målinger blev det vist at de ud
viklede algoritmer kunne aide navigeringsligningerne
og give bedre resultater end nogle af systemerne alene.
Det blev også vist at INS systemet var i stand til
at 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 3
th
of September 2007 to the 4
th
of June 2008 under the theme
Estimation and Nonlinear Control.
For a complete understanding of the report, a technical and scientiﬁc level corre
sponding to that of 9
th
semester students at the Section of Automation and Control is
required.
Literature references are written according to the Harvardmethod: [Last name of
author, year of publication]. If a literary work has more than two authors, the ﬁrst
authors last name is written followed by “et al.”. A complete bibliography is found at
page 121.
Throughout the project Matlab has been used for data processing and algorithm
implementation. The version of Matlab used is 7.4.0.287 (R2007a).
The report is copyrighted by the members of the group and Aalborg University.
Aalborg University, 31st of May, 2007
Rolf Christensen Nikolaj Fogh
Contents
I Introduction 2
1 Introduction 4
2 Nomenclature and Mathematical Techniques 7
3 Frames of Reference and Position Determination 12
4 Principle of a Strapdown Inertial Navigation System 17
II Inertial Navigation Systems 22
5 Introduction 24
6 Geodesy 26
6.1 Geodetic Variables and Constants . . . . . . . . . . . . . . . . . . . . 26
6.2 Gravity Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
7 Equations of Motion 29
8 Earth Simulation Model 32
8.1 Accelerometer Output . . . . . . . . . . . . . . . . . . . . . . . . . . 32
8.2 Gyro Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
9 Navigation Equations 35
9.1 General Navigation Equations . . . . . . . . . . . . . . . . . . . . . . 35
9.2 Vertical Channel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
9.3 Navigation Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
9.4 Discrete Integration Algorithms . . . . . . . . . . . . . . . . . . . . . 39
9.5 Validation of Navigation Algorithm . . . . . . . . . . . . . . . . . . . 40
9.6 Analysis of Long Term Error Propagation . . . . . . . . . . . . . . . 41
10 Error Estimation Using Indirect Kalman Filtering 49
10.1 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
10.2 Motivating Example . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
10.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
11 Error Equations 55
11.1 Derivation of Error Equations . . . . . . . . . . . . . . . . . . . . . . 55
11.2 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
12 Alignment 64
12.1 Comparison between Gimballed and Strapdown Alignment . . . . . . 65
12.2 Coarse Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
12.3 Fine Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
12.4 Indirect Kalman Filter Applied to QuasiStationary Fine Alignment 70
12.5 Veriﬁcation 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 Veriﬁcation of Navigation with Aiding . . . . . . . . . . . . . . . . . 97
13.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
14 Inertial Sensors 102
14.1 Optical Angular Rate Sensors . . . . . . . . . . . . . . . . . . . . . . 102
14.2 Accelerometers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
14.3 Sensor Error Models . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
15 Calibration of Sensors 106
15.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
15.2 Calibration Equations . . . . . . . . . . . . . . . . . . . . . . . . . . 106
15.3 Smoothing Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
15.4 Calibration Example . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
16 Summary 113
III Thesis Summary 116
17 Conclusion 118
18 Improvements and Discussion 120
Bibliography 122
Part I
Introduction
Chapter 1
Introduction
Why INS
No matter how far we go back in time it has always been critical to know where one
is positioned. It being a neanderthal going hunting whom should be able to ﬁnd back
home to the camp or a cruise missile which should hit its target with high precision.
Through time several diﬀerent 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 inﬂuences. This
makes it suitable for navigation where GPS signal is not present or if the signals are
being jammed. When relying only on inertial measurements, however, the accuracy
of the INS degrades with time due to measurement inaccuracies. But used in a short
time span, INS gives a much higher precision than GPS. This makes INS very suitable
for missiles where precision is crucial, but which are only airborne for a few seconds.
High precision INS are also suitable for submarines where high precision is needed and
external positioning aids are not readily available.
4
5 CHAPTER 1. INTRODUCTION
Historical Development of INS
The ﬁrst applications using inertial measurements for navigation purposes appeared at
the end of the 19th century. They were simple gyro compasses were able to determine the
direction of true north. Under WW2 the development of the gimballed INS was reﬁned,
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 lowend gimballed systems but with a lower price tag.
This made INS solutions applicable to military aircraft and the ﬁrst commercial aircraft
Boeing 757. The advantages of a nonmechanic 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 lowercost
applications.
Strapdown INS
The strapdown implementation oﬀers 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 errorprone 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 highend gimballed systems.
The problem with strapdown inertial navigation systems is, that the sensor assembly
is not mechanically isolated from the rotations of the body, and thus, the gyros must
be able to capture these rotations. This requires the strapdown gyros to have a much
higher dynamic range than the gyros employed in gimballed systems. The RLG and
the FOG both have both ﬁne accuracy and a large dynamic range, making them ideally
suitable for strapdown INS.
Scope of This Project
This project seeks to implement algorithms for a lowcost 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 identiﬁcation.
Inserting the parameters in the developed algorithms will yield the ﬁnal software for
the INS.
6
Our Contributions
The authors have found it diﬃcult to ﬁnd literature about INS which are both com
prehensive and easily understandable. This thesis seeks to provide algorithms for all
the steps necessary to implement an INS, while still keeping the derivations relatively
simple. In particular, the application of the indirect Kalman ﬁlter to the alignment and
aiding problem will be explained, a topic which is missing in most literature.
To the authors knowledge, there have been conducted no analysis of the errors arising
from assuming the system to be quasistationary during the ﬁne alignment process.
This thesis includes results from a simulation, where the quasistationary 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 deﬁnes 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 inﬂuences 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 eﬀects of measurement uncertainties aﬀect the system, and an error
model is derived. This model is used during the initial alignment phase, which
will estimate the initial attitude, azimuth and position of the system. In the next
chapter inertial navigation with aiding is described. The error equations are used
to “fuse” the INS with an external aiding device. An indirect Kalman ﬁlter is
used both in alignment mode and to “fuse” the INS with an aiding device in an
optimal way. The last two chapters models the sensors used in this project and
the determines the errors they suﬀer from.
Part III will include a system test, which veriﬁes 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 deﬁne 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 nonbold letters, vectors as lowercase bold letters and matrices
as capital bold letters
v = scalar
r = vector
Ω = matrix
Coordinate Frames and Transformations
To describe the kinematics of the IMU a set of frames and notations must be agreed
upon. All frames used in this report will span a three dimensional Euclidean space,
consisting of three orthogonal basis vectors. The basis vectors of the b frame will be
denoted by b
x
, b
y
and b
z
, with x, y and z being the principal axes of the frame. A
vector r referenced in the b frame will be denoted r
b
. The subscript notion will be used
to extract the x, y and z components of the vector.
r
b
=
r
b
x
r
b
y
r
b
z
T
(2.1)
It is often needed to represent a vector in diﬀerent coordinate frames. One coordinate
frame is created from another by a series of rotations around the principal axes of the
frame. Such a rotation around the y axis is shown in Fig. 2.1. In the ﬁgure, there are
two coordinate frames: The i coordinate frame, spanned by i
x
, i
y
and i
z
, and the j
coordinate frame spanned by j
x
, j
y
and j
z
. The j coordinate frame has been rotated
relative to the i frame through the i
z
axis by an angle α. The r
i
z
, r
j
z
, i
z
, j
z
vectors are
coincident and invisible, as they protrude out through the page. Using the nomenclature
7
8
described above one can derive the components of the r vector in the j frame as the
components of the r vector in the i frame, and the angle α. The components of a vector
r in the i frame is found in the ﬁgure as the bold lines r
i
x
, r
i
y
, r
i
z
, and in the j frame as
the bold lines r
j
x
, r
j
x
, r
j
x
. Figure 2.1 can now be used to ﬁnd the equations transforming
the vector r from the i frame to the j frame.
i
y
α
ζ
ξ
α
ζ
α
α
r
j
x
r
i
y
r
i
x
r
j
x
r
j
y
r
j
x,1
r
i
z
, r
j
z
, i
z
, j
z
i
x
j
y
Figure 2.1: Rotation around the positive y axis
ζ + r
j
y
= r
i
y
cos(α)
ζ = r
i
x
sin(α)
r
j
y
= r
i
y
cos(α) −r
i
x
sin(α)
r
j
x,1
= r
i
x
cos(α)
ξ = r
i
y
sin(α)
r
j
x
= r
j
x,1
+ ξ = r
i
x
cos(α) + r
i
y
sin(α)
r
j
z
= r
i
z
It is customary to write these equations in matrix form as
r
j
x
r
j
y
r
j
z
¸
¸
=
cos(φ) sin(φ) 0
−sin(φ) cos(φ) 0
0 0 1
¸
¸
r
i
x
r
i
y
r
i
z
¸
¸
r
j
= C
j
i
r
i
Here, C
j
i
is the rotation matrix transforming r from frame i to frame j. The rotation
matrix is also called the Direction Cosine Matrix (DCM).
The DCM’s corresponding to rotations around all principal axis are stated here for
completeness
9 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES
Rotation around X axis
1 0 0
0 cos(φ) sin(φ)
0 −sin(φ) cos(φ)
¸
¸
(2.2)
Rotation around Y axis
cos(θ) 0 −sin(θ)
0 1 0
sin(θ) 0 cos(θ)
¸
¸
(2.3)
Rotation around Z axis
cos(ψ) sin(ψ) 0
−sin(ψ) cos(ψ) 0
0 0 1
¸
¸
(2.4)
The rotation matrix from arbitrary frame “i” to arbitrary frame “j” is constructed by
multiplying the rotation around “z” with rotation around “y” and with rotation around
“x” and results in the following rotation matrix:
C
j
i
=
¸
cos(ψ) cos(θ) sin(ψ) cos(φ)+cos(θ) sin(θ) sin(φ) sin(ψ) sin(φ)−cos(ψ) sin(θ) cos(φ)
−sin(ψ) cos(θ) cos(ψ) cos(φ)−sin(ψ) sin(θ) sin(φ) cos(ψ) sin(φ)
sin(θ) −sin(φ) cos(θ) cos(θ) cos(φ)
(2.5)
It is important to note, that the order in which the rotations around the principal are
applied are not arbitrary. An XYZ rotation is not in general the same as an YXZ
rotation.
As a general rule when a DCM transforms from one coordinate system which is or
thogonal to another coordinate system which is also orthogonal the following property
exist:
C
j
i
= (C
i
j
)
T
Similarity Transformations
If an equation is wished to be expressed in another coordinate system this can be
achieved using the DCM. If the following equation which is expressed in frame i is
wished expressed in frame j this can be achieved as follows:
Ar
i
= v
i
⇓
C
j
i
AC
i
j
r
j
= v
j
Matrix A is said to be transformed under a similarity transformation.
Relative Angular Velocity of Frames
When describing the relative angular velocity of two frames, it is necessary to describe
both which frames are involved and in which coordinate system the angular velocity is
expressed. If e.g. the angular velocity of frame j relative to frame i is expressed in the
j’th coordinate system this is written as:
ω
j
ij
=
ω
j
ij,x
ω
j
ij,y
ω
j
ij,z
T
10
Computed and Measured Quantities
In order to distinguish between actual measurements from sensors and computed quan
tities used in the computer, they are denoted diﬀerently. A physical vector measured
by a sensor is denoted with a tilde and a computed quantity is denoted by a hat:
˜ ω
j
ij
= measured angular velocity
ˆ ω
k
ik
= computed angular velocity
Skew Symmetrix Matrices
Throughout the thesis, it is sometimes convenient to change between a vector rep
resentation and a matrix representation. In doing so, the skew symmetric matrix is
introduced. Using the skew symmetric matrix, the cross product of two vectors reduces
to doing a matrix multiplication:
a ×b = Ab (2.6)
where A is the skew symmetric matrix form of a. The elements in the skew symmetric
matrix is as follows:
a =
a
1
a
2
a
3
T
A =
0 −a
3
a
2
a
3
0 −a
1
−a
2
a
1
0
¸
¸
(2.7)
Time Derivative of the Direction Cosine Matrix
At time t, the relation of frame i and j is expressed by the DCM C
j
i
(t). During the
time ∆t, the i frame is rotated to a new orientation so the DCM is now C
j
i
(t + ∆t).
By deﬁnition, the time derivative of a DCM is:
˙
C
j
i
= lim
∆t→0
∆C
j
i
∆t
= lim
∆t→0
C
j
i
(t + ∆t) − C
j
i
(t)
∆t
(2.8)
Using a geometrical interpretation the small angle rotation C
j
i
(t + ∆t) is the same as
C
j
i
(I+∆θ
i
) where ∆θ
i
is the skew symmetric matrix related to the small angle rotation
from of C
j
i
from time t to time t + ∆t. The skew symmetric form is due to the small
angle simpliﬁcations applied to the cosine and sine parts of the DCM from (2.5). The
result is:
∆θ
i
=
0 ∆θ
γ
−∆θ
β
−∆θ
γ
0 ∆θ
α
∆θ
β
−∆θ
α
0
¸
¸
(2.9)
where ∆θ
α
, ∆θ
β
, ∆θ
γ
is the small angles that the i frame has been rotated through.
Substituting (2.9) in (2.8) yields:
˙
C
j
i
= C
j
i
lim
δt→0
∆θ
i
∆t
(2.10)
11 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES
Inspecting the part
∆θ
i
∆t
when ∆t →0 reveals that it is the same as the angular velocity
of the i’th frame with respect to the j’th frame and this is denoted as ω
i
ji
. This vector
can be expressed as a skew symmetric matrix:
ω
i
ji
→Ω
i
ji
(2.11)
where Ω
i
ji
is a skew symmetric version of ω
i
ji
of the form from (2.7).
These two rotational representations will be used interchangably throughout the report.
The frame of reference can be changed by transforming the matrix under the similarity
transform
Ω
i
ij
= C
i
j
Ω
j
ij
C
j
i
Using this property, (2.10) can be written as:
˙
C
j
i
= C
j
i
Ω
i
ji
= −Ω
j
ij
C
j
i
(2.12)
Chapter 3
Frames of Reference and Position
Determination
Before explaining the principles of INS, it is important to deﬁne 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 deﬁned 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.
EarthCentered Inertial Frame
In INS applications, the inertial frame i is chosen as an earthcentered inertial frame
(ECIF) which has origin at the center of the earth, and one axis parallel to the direction
of earth rotation. The choice of elementary vector representing this axis (i
x
, i
y
or i
z
) is
a matter of preference, and in the literature, diﬀerent ones are used. In this thesis, the
i
y
vector is chosen, with the i
z
vector going through the equator at the longitude where
navigation was started, while the i
x
completes the righthand coordinate system. The
frame is seen in Fig. 3.1.
In terrestrial navigation, one is primarily interested in knowing longitude, latitude,
height, north velocity, east velocity, azimuth and orientation. These could be obtained
using only the inertial frame. However, most inertial navigation systems today use an
additional set of frames.
12
13 CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION
ω
ie
i
y
, e
y
N, g
y
E, g
x
U, g
z
Equator
l
L
λ
e
z
i
z
i
x
e
x
Figure 3.1: The inertial frame is spanned by ix, iy and iz. The earth frame is the
inertial frame rotated around the y axis through the angle λ. The geodetic frame
is the earth frame rotated around the y axis through the longitude angle L, and
thereafter rotated negatively around the new x axis through the latitude angle l.
14
EarthCentered, EarthFixed Frame
The earthcentered, earthﬁxed frame (ECEF) e is deﬁned, to be rotating with the earth
at the earth spin rate (ω
e
). At time t = 0 the ECEF and ECIF are coincident, but as
time progresses, the ECEF frame is rotated at an angle λ through the e
y
axis, where
λ = ω
e
t. The ECEF frame is seen in Fig. 3.1. Using (2.3), the rotation matrix from
ECIF to ECEF is
C
e
i
=
cos(λ) 0 −sin(λ)
0 1 0
sin(λ) 0 cos(λ)
¸
¸
(3.1)
Local Geodetic/Geographic Frame
The Local Geodetic/Geographic Frame g deﬁned 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 righthand 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 inﬂuence navigation performance and can be freely
chosen as long as the constraints just mentioned are met. A common geodetic frame,
which is also the geodetic frame in this thesis, is the East, North, Up frame (ENU),
which is produced by rotating the ECEF frame around the e
y
axis at a longitude angle
L, and then through a latitude angle l through the new e
x
axis. This frame is seen in
Fig. 3.1. With the geographic frame, the x, y and z axes are also called e, n and u axes
in the case of an ENU frame, so g
x
= g
e
, g
y
= g
n
and g
z
= g
u
. One other popular
formulation is the NorthEastDown (NED) frame.
Navigation Frame
As with the local geodetic frame, the navigation frame (NF) n is deﬁned as a frame
which has origin at the position of the sensor frame and has one principal axis pointing
vertically up or down. The diﬀerence 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 undeﬁned, and the South
pole, where North has inﬁnitely many solutions. To overcome this limitation, current
INS typically uses a mechanization known as wander angle (WA) [Savage, 2000]. This
mechanization is constructed from the geodetic frame by rotating the geodetic frame
through the g
z
axis through an angle α known as the wander angle. This rotation is
seen in Fig. 3.2.
As will be evident later in this thesis, the wander angle mechanization also simpliﬁes
the navigation equations which reduces the computational demand of the system. As
with the ENU frame, one can still integrate the accelerations to obtain velocities, but
they must be rotated through α to obtain it in North and East components. Height
is still a double integration of acceleration in the n
z
. Using (2.2), (2.3) and (2.4), the
rotation matrix from the ECEF to the NF is
15 CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION
n
y
α
g
x
n
x
g
y
Figure 3.2: The wander angle α is the angle which the navigation frame is rotated
around the z axis with respect to the geodetic frame.
C
n
e
=
¸
cos(L) cos(α)−sin(L) sin(l) sin(α) cos(l) sin(α) −sin(L) cos(α)−cos(L) sin(l) sin(α)
−cos(L) sin(α)−sin(L) sin(l) cos(α) cos(α) cos(l) sin(L) sin(α)−cos(L) sin(l) cos(α)
cos(l) sin(L) sin(l) cos(L) cos(l)
(3.2)
And the rotation matrix from the geodetic frame to the navigation frame is
C
n
g
=
cos(α) sin(α) 0
−sin(α) cos(α) 0
0 0 1
¸
¸
(3.3)
Body Frame
The body frame (BF) is ﬁxed to the casing of the IMU, and rotated with respect to the
navigation frame by a roll, pitch and yaw angle.
Sensor frame
The sensors are not always perfectly aligned mechanically with respect to the body
frame. The sensor frame is deﬁned so that the xy plane is spanned by the x and y
accelerometer, with the x accelerometer corresponding to the xaxis. 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 suﬀers from the same problems as the NED navigation
frame mechanization. At the poles, the latitude is 90
◦
, but longitude is not uniquely de
ﬁned. Therefore, it is not possible to restore longitude information after a pole traversal,
and the navigation information is destroyed. Although one might argue that naviga
tion involving pole traversal are not common, and thus the singularity is not an issue,
INS today uses either quaternions parameters or 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 ﬁnite 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
signiﬁcant 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 C
n
e
DCM is used to keep track of the po
sition on earth. From (3.2), longitude, latitude and wander angle can be extracted by
isolating the L, l and a terms. Assigning the entries of the C
n
e
matrix so C
ij
corresponds
to the i’th column and the j’th row of the C
n
e
matrix as follows:
C
n
e
=
C
11
C
21
C
31
C
12
C
22
C
32
C
13
C
23
C
33
¸
¸
(3.4)
Yields
L = arctan
C
13
C
11
C
22
−C
21
C
12
(3.5)
l = arcsin(C
23
) (3.6)
a = arctan
C
21
C
22
(3.7)
As seen, only the ﬁrst two columns of C
e
n
are needed to obtain the wanted information.
Omitting the calculation of the third column reduces the processing power needed for
the navigation equation.
Chapter 4
Principle of a Strapdown Inertial
Navigation System
In order to get an idea of the concepts and principles of inertial navigation systems, a
simple example is given in two dimensions. Many of the concepts in the two dimensional
case can be directly transferred into the three dimensional case.
When navigating on the earth, we would like to know our current position relative to
some other point on the earth surface. In many applications, one uses the notion of
longitude, latitude and height above the earth surface to indicate where one is positioned
relative to the earth.
First, consider a 2 dimensional YZ IMU as depicted in Fig. 4.1. Here, it is assumed
that the sensor frame and the body frame are coincident. This system is allowed to tilt
by an angle θ at a rate ω, as well as accelerate in the b
y
and b
z
direction.
a
b
z
ω
Accelerometers Gyro
a
b
y
b
z
b
x
b
y
Figure 4.1: Inertial Measurement Unit measuring in two dimensions.
Being a 2D INS, our system is only able to navigate around the meridional (northsouth)
plane. Also, assume that the earth is not rotating, so the inertial frame is equal to the
earth frame. This two dimensional system is depicted in Fig. 4.2. For simplicity, the
navigation frame is chosen as a ENU geodetic frame.
In this system, the variables of interest is the latitude l, the height h, and the tilt θ of
the system. By introducing the navigation frame, it can be seen from the ﬁgure that
the derivative of l (in radians) is equal to the velocity of the body to the north divided
by the radius of the earth plus the height of the system. The derivative of h is equal to
the velocity of the body in the vertical direction.
17
18
i
y
, e
y
θ
l
h
R
0
n
n
b
y
n
u
b
z
i
z
, e
z
Figure 4.2: INS system around the meridian plane. The platform is tilted by an
angle θ with respect to the earth surface.
˙
l =
v
n
n
R
0
+ h
˙
h = v
n
u
(4.1)
To ﬁnd these we need to ﬁnd the velocities of the system in the navigation frame, v
n
n
and v
n
u
. These are obtained using the measured accelerations. As these measurements
are given in the body frame, these needs to be rotated into the navigation frame as
follows
a
n
n
= a
b
y
cos θ + a
b
z
sin(θ)
a
n
u
= −a
b
y
sinθ + a
b
z
sin(θ) (4.2)
Where the tilt of the platform θ needs to be calculated. The derivative of θ is equal to
the angular rotation of the body with respect to the inertial frame ω minus the rotation
of the navigation frame with respect to the earth frame.
˙
θ = ω −
v
n
n
R
0
+ h
(4.3)
The v
n
n
/(R
0
+ h) term is also called the “transport rate”. It is the rate at which one
needs to tilt the platform in order to keep it level on the ground. At start of navigation,
the attitude θ is initialized at some initial value (0 if the system is level on the ground
at the start of navigation), and the above equation can then be used to calculate it at
any point from there on.
Having obtained the accelerations in the navigation frame, one can obtain the velocity
as an integration of the acceleration. The acceleration measured by the accelerometers
is comprised of both gravity and mass movement. Because the navigation frame is level
on earth, gravity only inﬂuences the u axis
As the navigation is rotating with respect to the inertial frame, one must take into
account the accelerations arising from this rotation. This is also known as the coriolis
force. Recognizing that the rotation rate of the navigation frame with respect to the
19
CHAPTER 4. PRINCIPLE OF A STRAPDOWN INERTIAL NAVIGATION
SYSTEM
inertial frame is the transport rate of (4.3), the derivative of the velocity of the system
is:
˙ v
n
n
= a
n
n
−
v
n
n
R
0
+ h
v
n
u
(4.4)
˙ v
n
u
= a
n
u
−g +
v
n
n
R
0
+ h
v
n
n
(4.5)
These are integrated to yield the velocity of the system. At start of navigation, the
system must be stationary so the initial velocities are 0.
The job of the navigation computer is to integrate (4.1), (4.3), (4.4) and (4.5) to yield
the latitude, height and tilt of the system.
The principles explained in this chapter are used in the next part to derive the navigation
equations for a full three dimensional case.
Part II
Inertial Navigation Systems
Chapter 5
Introduction
Inertial navigation consists of three major parts: Alignment, navigation and aiding.
During the alignment phase, the alignment algorithm estimates the initial value of the
body frame with respect to the navigation frame, that is, the roll, pitch and yaw of the
system. This is used by the navigation algorithm to transform the measurements from
the body frame to the navigation frame. By requiring the IMU to be stationary during
alignment, this can be done by measuring gravity and earth rotation.
When the alignment phase has obtained a suﬃciently 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 identiﬁed
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 inﬂuence on the INS. The important
aspects of this topic is introduced in the ﬁrst chapter (Ch. 6).
The next chapter (Ch. 7) derives the equation of motion of an INS which are used by
24
25 CHAPTER 5. INTRODUCTION
both an earth simulation model and the navigation equations. The earth simulation
model translates longitude, latitude, height and attitude inputs to accelerations and
angular rates that the accelerometers and gyros would experience during navigation.
The navigation equations reverses the process as seen in Fig. 5.1.
model
INS
Longitude, latitude
height and attitude
trajectories
Longitude, latitude
height and attitude
trajectories
Accelerations
Angular Rates
Simulation
Figure 5.1: Simulation and INS
The earth simulation model and navigation equations are developed in the two following
chapters, Ch. 8 and Ch. 9. The earth simulation model is derived in order to test the
developed algorithms under controlled conditions.
Before developing the alignment and aiding algorithms, chapter 10 gives an introduction
to indirect Kalman ﬁltering. Indirect Kalman ﬁltering uses an error model of the INS
which is derived in Ch. 11. Using this error model, indirect Kalman ﬁltering equations
and the navigation equations, the alignment algorithm is derived in Ch. 12.
Two diﬀerent aiding approaches is presented in Ch. 13. GPS aiding and speedometer
aiding.
The last two chapters of this part present a somewhat diﬀerent 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 identiﬁed using a test ﬁxture to
rotate the IMU through a series of rotations.
Chapter 6
Geodesy
In order to develop the equations of motion of a vehicle in the vicinity of the earth,
knowledge is needed about the geometric aspects of the earth as well as the gravitational
ﬁeld surrounding the earth. These values are known as geodetic variables and constants.
The most widely used model of the earth is the World Geodetic System (WGS84), which
deﬁnes many of the constants that are used in INS, as well as a gravity model.
6.1 Geodetic Variables and Constants
The rotational rate of the earth have been well known for many years, and can be
expected to be constant
ω
e
= 7292115.0 · 10
−11
[ rad/s ] (6.1)
Expressed in earth coordinates
ω
e
ie
=
0 ω
e
0
(6.2)
The equatorial radius of the earth, also known as the semimajor axis radius
r
e
= 6378137 [ m] (6.3)
The polar radius of the earth, also known as the semiminor axis radius
r
p
= 6399592 [ m] (6.4)
The eccentricity of the earth is deﬁned as
ǫ =
1 −
r
2
p
r
2
e
= 0.0818191908426 (6.5)
Two important geometric aspects of geodecy is the meridional and the normal radius
of curvature. These are deﬁned as the radius of the circle one would travel in, if one
continued ones current curve.
The meridional radius of curvature is the radius of the circle one would follow if one
travelled directly north or south. This is depicted in Fig. 6.1. The normal radius of
26
27 CHAPTER 6. GEODESY
Surface
r
mer
North
Circle of curvature
Earth
Figure 6.1: Figure showing the diﬀerence 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 (northsouth) radius of curvature
r
mer
=
r
e
(1 −ǫ
2
)
(1 −ǫ
2
sin
2
(l))
3/2
(6.6)
Normal (eastwest) radius of curvature
r
nor
=
r
e
(1 −ǫ
2
sin
2
(l))
(6.7)
6.2 Gravity Model
From [Britting, 1971], the gravitational magnitude can be calculated approximately
using the following formula.
g =
µ
r
2
1 −
3
4
J
2
(1 −3 cos(2l))
−rω
2
e
cos
2
(l) (6.8)
Where J
2
, and µ are empirical constants calculated by geodetic measurements and r is
the radius from earth surface to the point of navigation. As apparent gravity is always
pointing vertically, the gravity in the navigation frame is
g
n
=
0
0
µ
r
2
1 −
3
4
J
2
(1 −3 cos(2l))
−rω
2
e
cos
2
(l)
¸
¸
(6.9)
From [Savage, 1997], r is approximated as:
r = r
e
1 −ǫ sin
2
(l)
+ h (6.10)
The geodetic values and gravity model described in this chapter is used throughout this
thesis. It is used both in the earth simulation model and in the navigation algorithm
as suﬃcient 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 diﬀerential equation of the speed of a vehicle
relative to earth. In later chapters, this equation is integrated to yield velocity and
subsequently position of the INS.
Derivation of Equations of Motion
Acceleration is comprised of two terms: The acceleration due to mass movement and
the acceleration due to gravity. Due to the principle of equivalence, these cannot be
distinguished and are both measured by the accelerometers [Britting, 1971]. Accelera
tion due to mass movement is the double derivative of the position vector in the inertial
frame. Rotating this into the navigation frame through C
n
i
yields the acceleration in
the navigation frame. The acceleration due to gravity is given in the navigation frame
as g
n
.
a
n
= C
n
i
¨r
i
+g
n
(7.1)
Where a
n
is the acceleration in the navigation frame.
As the output from an INS is velocity and position relative to earth, it is convenient
to derive this equation in terms of the geographically referred earth referenced velocity,
also known as the “ground speed”. This is the velocity of r
i
seen from the earth frame
transformed into the navigation frame, deﬁned as follows
v
n
C
n
e
˙ r
e
(7.2)
The result of this chapter is an expression relating v
n
to the accelerometer measurements
given in the navigation frame a
n
.
The dependence of ˙ r
e
on r
i
is obtained as follows
29
30
r
e
= C
e
i
r
i
(7.3)
˙ r
e
= C
e
i
˙ r
i
+
˙
C
e
i
r
i
= C
e
i
˙ r
i
+C
e
i
Ω
i
ei
r
i
= C
e
i
˙ r
i
−Ω
i
ie
r
i
(7.4)
Notice the sign change due to Ω
i
ei
= −Ω
i
ie
. Inserting into (7.2) and diﬀerentiating v
n
with respect to time yields
v
n
= C
n
i
˙ r
i
− C
n
i
Ω
i
ie
r
i
(7.5)
˙ v
n
=
˙
C
n
i
˙ r
i
+ C
n
i
¨r
i
−
˙
C
n
i
Ω
i
ie
r
i
−C
n
i
Ω
i
ie
˙ r
i
= C
n
i
Ω
i
ie
˙ r
i
+ ¨r
i
−Ω
i
ni
Ω
i
ie
r
i
−Ω
i
ie
˙ r
i
= C
n
i
Ω
i
ni
−Ω
i
ie
˙ r
i
+ ¨r
i
−Ω
i
ni
Ω
i
ie
r
i
(7.6)
By isolating ˙ r
i
from (7.4) one obtains
˙ r
i
= C
i
e
˙ r
e
+Ω
i
ie
r
i
(7.7)
Inserting this into (7.6) and rearranging the terms yields
˙ v
n
= C
n
i
Ω
i
ni
−Ω
i
ie
C
i
e
˙ r
e
+Ω
i
ie
r
i
+ ¨r
i
−Ω
i
ni
Ω
i
ie
r
i
= C
n
i
Ω
i
ni
C
i
e
˙ r
e
+Ω
i
ni
Ω
i
ie
r
i
−Ω
i
ie
C
i
e
˙ r
e
−Ω
i
ie
Ω
i
ie
r
i
+ ¨r
i
−Ω
i
ni
Ω
i
ie
r
i
= C
n
i
Ω
i
ni
C
i
e
−Ω
i
ie
C
i
e
˙ r
e
−Ω
i
ie
Ω
i
ie
r
i
+ ¨r
i
(7.8)
Knowing that C
i
e
= C
i
n
C
n
e
and Ω
i
ni
= Ω
i
ne
−Ω
i
ie
and inserting (7.2) yields the following
˙ v
n
= C
n
i
¨r
i
−
Ω
i
en
+ 2Ω
i
ie
C
i
n
v
n
−Ω
i
ie
Ω
i
ie
r
i
(7.9)
By isolating the inertial acceleration ¨r
i
and multiplying by C
n
i
one obtains the ﬁrst term
of (7.1)
C
n
i
¨r
i
= ˙ v
n
+C
n
i
Ω
i
en
+ 2Ω
i
ie
C
i
n
v
n
+C
n
i
Ω
i
ie
Ω
i
ie
r
i
(7.10)
Inserting into (7.1) and rearranging yields the equation that was sought after
˙ v
n
= a
n
−(Ω
n
en
+ 2Ω
n
ie
) v
n
−C
n
i
Ω
i
ie
Ω
i
ie
r
i
−g
n
(7.11)
It is customary to include the centripetal term Ω
i
ie
Ω
i
ie
r
i
in the equation of gravity,
resulting in a so called “apparent” or “plumb bob” gravity term:
g
n
g
n
+C
n
i
Ω
i
ie
Ω
i
ie
r
i
(7.12)
Rearranging and inserting the expression for g
n
into (7.11) yields the ground speed of
a vehicle as a function of accelerometer measurements, the rotation rate of earth, the
rotation of the navigation frame relative to earth and the apparent gravity:
31 CHAPTER 7. EQUATIONS OF MOTION
˙ v
n
= a
n
−(Ω
n
en
+ 2Ω
n
ie
) v
n
−g
n
(7.13)
In the following chapters, this equation is used both to develop a simulation model, and
the INS navigation equations.
Chapter 8
Earth Simulation Model
In this chapter, an earth simulation model is derived using the equations of motion
derived in Ch. 7. This model enables one to create repeatable test data as well as
test the system under suboptimal, but welldeﬁned conditions such as erroneous sensor
outputs.
This chapter is divided into two sections. As two sensors are used, accelerometer and
gyroscope, the earth simulation model should output both the acceleration and rate
sensed by the sensors when the vehicle is moving.
8.1 Accelerometer Output
In this section the accelerometer output from the earth simulation model is derived.
Restating the equation of motion (7.13):
˙ v
n
= a
n
−(Ω
n
en
+ 2Ω
n
ie
) v
n
−g
n
(8.1)
Rearranging and rotating yields the wanted output
a
b
= C
b
n
( ˙ v
n
+ (Ω
n
en
+ 2Ω
n
ie
) v
n
+g
n
) (8.2)
The plumbbob gravity term g
n
was calculated in Ch. 6 as
g
n
=
0
0
µ
r
2
1 −
3
4
J
2
(1 −3 cos(2l))
−rω
2
e
cos
2
(l)
¸
¸
(8.3)
The Ω
n
ie
term is obtained by rotating the rotation rate matrix in earth coordinates into
the navigation frame using the similarity transform
32
33 CHAPTER 8. EARTH SIMULATION MODEL
Ω
n
ie
= C
n
e
Ω
e
ie
C
e
n
(8.4)
ω
n
ie
=
cos(l) sin(α)ω
e
cos(l) cos(α)ω
e
sin(l)ω
e
T
(8.5)
The wander angle (Fig.3.2) is initialized at 0 and follows the following equation[Rogers, 2007].
˙ α =
˙
Lsin(l) (8.6)
The C
b
n
matrix is calculated using the body rotations Ω
n
bn
supplied by the user
˙
C
b
n
= C
b
n
Ω
n
bn
(8.7)
At start of simulation, the C
b
n
matrix is initialized by the user.
Left is to ﬁnd the v
n
and Ω
n
en
terms.
First, the horizontal parts of v
n
is found. This is done by ﬁrst observing the horizontal
parts of the ground speed in the geodetic frame as a function of the horizontal angular
rates of the geodetic frame. By doing this, one can ﬁnd a relationship between the
horizontal angular rates and the horizontal velocity of the geographic frame. Figure 8.1
shows the relationship for the north velocity.
ω
g
eg,e
h
v
g
n
r
mer
Figure 8.1: Relationship between north velocity and east angular rate of the geo
graphic frame. The bold ellipse is the earth surface, the dashed line is the meridional
radius of curvature.
v
g
e
= ω
g
eg,n
(r
nor
+ h) (8.8)
v
g
n
= −ω
g
eg,e
(r
mer
+ h) (8.9)
Where [Savage, 2000]
ω
g
eg
=
−
˙
l
˙
Lcos(l)
˙
Lsin(l)
T
(8.10)
As the navigation frame is locally level, the vertical term of v
g
is equal to the derivative
of the height h.
8.2. GYRO OUTPUT 34
v
g
u
=
˙
h (8.11)
Hence,
v
g
=
˙
L(r
nor
+ h) cos(l)
˙
l(r
mer
+ h)
˙
h
T
(8.12)
This is then rotated into the navigation frame through the wander angle using (3.3).
v
n
= C
n
g
v
g
=
˙
L(r
nor
+ h) cos(l) cos(α) +
˙
l(r
mer
+ h) sin(α)
−
˙
L(r
nor
+ h) cos(l) sin(α) +
˙
l(r
mer
+ h) cos(α)
˙
h
¸
¸
(8.13)
The remaining term is Ω
n
en
. In the wander azimuth conﬁguration, the vertical part is
set to 0, and the horizontal terms are obtained by rotating the horizontal parts of the
geographic rotation rate through the wander angle.
ω
n
en
= C
n
g
(ω
g
eg
+ω
g
gn
) (8.14)
With
ω
n
gn
=
0 0 −
˙
Lsin(l)
T
(8.15)
Yields
ω
n
en
= C
n
g
−
˙
l
˙
Lcos(l) 0
T
=
−
˙
l cos(α) +
˙
Lcos(l) sin(α)
˙
l sin(α) +
˙
Lcos(l) cos(α)
0
¸
¸
(8.16)
To get the outputs from the accelerometers, (8.3), (8.5), (8.13) and (8.16) is inserted
into (8.2), and the rotation matrix C
b
n
is calculated using (8.7).
8.2 Gyro Output
The outputs from the gyros are the components of the Ω
b
ib
matrix which is given as
Ω
b
ib
= C
b
n
(Ω
n
ie
+Ω
n
en
+Ω
n
nb
) (8.17)
Which is calculated by inserting (8.5), (8.16) and the body rotations Ω
n
nb
given by the
user.
To use this earth simulation model L, l and h need to be diﬀerentiable. 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 deﬁned 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 deﬁned 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 ﬁrst section derives the general navigation equations. These equations describe
how the velocity of the vehicle changes which can be integrated to give the position of
the INS. The next section describes the problem with the unstable vertical channel of
the INS and proposes a solution to stabilize this channel. The last section describes the
idea of dividing the integration of the attitude matrix into a fast and slow part. This
is used if the processor power is not suﬃcient to integrate the whole part with the high
sample rate.
9.1 General Navigation Equations
In order to determine the velocity and position of the INS, this section seeks to derive
expressions for the terms in the equation of motion (9.1).
˙ v
n
= C
n
b
a
b
−(Ω
n
en
+ 2Ω
n
ie
) v
n
−g
n
(9.1)
At start of navigation, the IMU is not moving relative to the earth surface, so v
n
is
initialized at 0.
The measured acceleration a
b
, gravity g
n
(8.3) and earth rotation Ω
n
ie
(8.5) are all
known. The C
n
b
matrix is calculated from gyro inputs as follows:
˙
C
n
b
= C
n
b
Ω
b
nb
= C
n
b
(Ω
b
ib
−C
b
n
(Ω
n
en
+Ω
n
ie
)C
n
b
) (9.2)
Where Ω
b
ib
is the input from the gyros, and earth rotation is known. The remaining
term in both (9.1) and (9.2) is an expression for the transport rate Ω
n
en
.
Apart from being used to calculate the derivative of the ground speed, the transport
rate is also used to calculate the C
n
e
matrix. As explained in Ch. 3, this matrix is used to
35
9.1. GENERAL NAVIGATION EQUATIONS 36
calculate latitude, longitude and wander angle of the system. At the start of navigation
the C
n
e
matrix is given initial values by the initial alignment algorithm. From there on
it follows the following equation
˙
C
n
e
= −Ω
n
en
C
n
e
(9.3)
which is continuously integrated by the INS.
The remainder of this section deals with the derivation of the transport rate.
The transport rate matrix Ω
n
en
is obtained from v
n
by rotating v
n
into the geographic
frame and using the relationship between ground speed and rotation rate in the geo
graphic frame, and then rotating back to the navigation frame, so
v
g
= C
g
n
v
n
(9.4)
By rearranging (8.8) and (8.9), and inserting it into the two ﬁrst positions of (8.10), one
obtains
ω
g
eg
=
−v
g
n
(rmer+h)
v
g
e
(rnor+h)
˙
Lsin(l)
T
(9.5)
This is used to calculate ω
n
en
using
ω
n
en
= C
n
g
ω
g
eg
+ω
g
gn
(9.6)
With
ω
g
gn
=
0 0 −
˙
Lsin(l)
T
(9.7)
Substituting (9.5) and (9.7) into (9.6) and isolating v
n
yields the relationship between
ground speed and transport rate:
ω
n
en
=
sin(α) cos(α)(
1
rnor+h
−
1
rmer+h
)
„
sin
2
(α)
rnor+h
−
cos
2
(α)
rmer+h
«
0
−
„
cos
2
(α)
rnor+h
−
sin
2
(α)
rmer+h
«
−sin(α) cos(α)(
1
rnor+h
−
1
rmer+h
) 0
0 0 0
¸
¸
¸v
n
(9.8)
= Fv
n
(9.9)
Note that the z part of ω
n
en
is always zero because of the wander angle mechanization.
This reduces the computational demands of the system.
As can be seen, this expression contains the wander angle α, which is not deﬁned at
the poles. The rest of this section will rewrite F to use the entries in the C
n
e
matrix
instead, overcoming this limitation. This is done as follows:
Using the trigonometric identity sin
2
(α) + cos
2
(α) = 1 gives
F =
¸
sin(α) cos(α)(
1
rnor+h
−
1
rmer+h
) −
1
rnor+h
+(
1
rnor+h
−
1
rmer+h
) cos
2
(α) 0
1
rnor+h
−(
1
rnor+h
−
1
rmer+h
) sin
2
(α) −sin(α) cos(α)(
1
rnor+h
−
1
rmer+h
) 0
0 0 0
¸
(9.10)
37 CHAPTER 9. NAVIGATION EQUATIONS
Using the entries in the C
n
e
matrix as done in Ch. 3, the sin(α) and cos(α) terms can
be denoted
sin(α) =
C
21
cos(l)
=
C
21
1 −(C
23
)
2
cos(α) =
C
22
cos(l)
=
C
22
1 −(C
23
)
2
(9.11)
Yielding the ﬁnal result, which can be used in navigation without the wander angle
constraint
F =
C
21
C
22
1−(C
23
)
2
(
1
rnor+h
−
1
rmer+h
) −
1
rnor+h
+(
1
rnor+h
−
1
rmer+h
)
(C
22
)
2
1−(C
23
)
2
0
1
rnor+h
−(
1
rnor+h
−
1
rmer+h
)
(C
21
)
2
1−(C
23
)
2
−
C
21
C
22
1−(C
23
)
2
(
1
rnor+h
−
1
rmer+h
) 0
0 0 0
¸
¸
(9.12)
9.2 Vertical Channel
The altitude (h) of the system can be obtained simply by noting that the Z axis of the
locally level navigation frame is by deﬁnition always pointing downwards, hence:
˙
h = −v
n
z
(9.13)
However, relying only on inertial measurements for calculation of the vertical channel
will render it exponentially unstable[Rogers, 2007]. This is because of the dependence
of g on h. As h increases, g decreases, yielding an increased upward acceleration. As a
general rule when navigation time exceed 10 minutes, external measurements must be
used to keep the vertical channel within acceptable values. [Savage, 2000]. A common
way of aiding the vertical channel is by implemented the following PID servo control
loop, which is depicted in Fig. 9.1. The purpose of the loop is to drive the error δh
towards 0.
˙ v
n
z,ins + + +
+
  

h
aid
h
δh
v
n
z
c
1
c
3
c
2
˙ v
n
z
Figure 9.1: Vertical channel control loop
˙
h = v
n
z
−c
1
(h −h
aid
) (9.14)
˙ v
n
z
= ˙ a
n
z
−((Ω
n
en
+ 2Ω
n
ie
) v
n
)
n
z
−g
n
z
−c
2
(h −h
aid
) −c
3
(h −h
aid
)dt (9.15)
9.2. VERTICAL CHANNEL 38
The PID gains c
1
, c
2
and c
3
are selected using classical servo control theory. To ana
lyze the vertical channel control loop, the Laplace transform of the error forms of the
equations are derived as follows:
δ
˙
h = δv
n
z
−c
1
(δh −δh
aid
) (9.16)
Assuming that the error of the aiding device is constant [Rogers, 2007], the second
derivative is:
δ
˙
¨
h + c
1
(δ
¨
h) = δ¨ v
n
z
(9.17)
The δ¨ v
n
z
term is obtained from (9.15) as follows: Assuming that earth rotation and
transport rate are negligibly small compared to the other terms, it simpliﬁes to
δ ˙ v
n
z
= δ ˙ a
n
z
+ δg
n
z
−c
2
(δh −δh
aid
) −c
3
(δh −δh
aid
)dt (9.18)
The gravity model is approximated by an inverse square law as
g
n
z
(r) ≈ −
g
0
r
2
0
r
2
(9.19)
where g
0
is the gravity at the surface of the earth, r
0
is the radius of the earth at the
point directly under the INS and r is the length from the center of the earth to the INS.
The error form is derived as
δg
n
z
= −
dg
n
z
(r)
dr
δr =
2g
r
δh (9.20)
where g is the gravity magnitude at the equator.
Assuming δa
n
z
to be constant, diﬀerentiating (9.18) yields
δ¨ v
n
z
≈
2g
r
δ
˙
h −c
2
(δ
˙
h −δh
aid
) −c
3
(δh −δh
aid
) (9.21)
which can be inserted into (9.17)
δ
˙
¨
h + c
1
δ
¨
h +
c
2
−
2g
r
δ
˙
h + c
3
δh = c
3
δh
aid
(9.22)
Taking the Laplace transform
s
3
+ c
1
s
2
+
c
2
−
2g
r
s + c
3
= 0 (9.23)
With no control (c
1
= c
2
= c
3
= 0),
s
3
−
2g
r
= 0 (9.24)
39 CHAPTER 9. NAVIGATION EQUATIONS
which has the solutions 0 and ±
2g/r, yielding an unstable system because of the
positive root.
The aiding device is unable to capture fast changes in height, but have stable character
istics, whereas the height measured by the INS are unstable, but includes fast changes.
The c
1
, c
2
and c
3
PID gains should be chosen so that the resulting h contains both the
stable characteristics of the aiding device, and the high dynamics of the height captured
by the INS. In order to obtain this, the PID gains are usually chosen as follows
s
3
+ 3λs
2
+ 4λ
2
s + 2λ
3
= 0 (9.25)
Such that
c
1
= 3λ c
2
= 4λ
2
+
2g
r
c
3
= 2λ
3
(9.26)
Where λ is usually chosen to be 0.01 [Rogers, 2007].
9.3 Navigation Output
The latitude, longitude and height can be extracted from the C
n
e
matrix as described
in Ch. 3:
L = arctan
C
13
C
11
C
22
−C
21
C
12
(9.27)
l = arcsin(C
23
) (9.28)
a = arctan
C
21
C
22
(9.29)
The velocities in north and east can be expressed as
v
N
= v
n
y
cos(α) + v
n
x
sin(α) (9.30)
v
E
= v
n
x
cos(α) −v
n
y
sin(α) (9.31)
9.4 Discrete Integration Algorithms
The navigation equations are integrated by discrete digital algorithms in the navigation
computer. However, the relatively high dynamics experienced during vehicle motion
causes some problems regarding the bandwidth of the system. The integration algo
rithms must be executed at a rate fast enough to include all major dynamics of the
system. Conversely, the processing power needed to execute the relatively complex al
gorithms must not exceed the available processing power. Earlier, systems either used
a ﬁrst order algorithm at a very high update rate (10  20 KHz), or a higher order algo
rithm at lower update rates (50  100 Hz). In many systems today, digital integration is
accomplished by dividing the integration algorithm into two parts [Savage, 2000], one
9.5. VALIDATION OF NAVIGATION ALGORITHM 40
for the parts with high dynamics, which needs to be run at a high iteration rate and
one for the parts with low dynamics, which can be run at a lower rate. This approach
is known as a twospeed approach. The parts running with a high iteration rate (1 
2 KHz) are lower order integration algorithms, which are run by specialized hardware
or highly optimized software algorithms. The parts running with a lower iteration rate
(50  100 Hz) are executed by the navigation computer software.
However, as the processing power available in modern microcomputers has increased
dramatically during the last decades, the integration problem have decreased, as it can
be solved by integrating all the terms in the navigation equations in the navigation
computer.
9.5 Validation of Navigation Algorithm
In order to validate the navigation algorithm derived in this chapter, the navigation
algorithmis compared to an already existing INS unit. This makes it possible to compare
the performance with respect to a commercial product. As a reference INS the Kearfott
T16 medium precision INS has been chosen.
The Kearfott unit makes it possible to log both longitude/latitude determined by the
Kearfott and the raw sensor data. It does not say at which sample rate the sensor data
is available to the INS algorithms running within the Kearfott, but the sensor data is
available to the user at 50Hz.
The test was performed by strapping the Kearfott unit to the backseat of a car and
driving a route of approximately 1.5km. The speed did not exceed 60km/h and modest
acceleration was utilized. Furthermore any sudden turns were avoided. The drive time
was approximately 5.6 minutes.
The same longitude/latitude and velocity was used for both systems. The initial rotation
matrix C
n
b
is the one determined by the Kearfott used by the navigation equations
derived in this chapter.
The test started with a 15 min alignment and immediately after alignment completion
followed by driving in order to minimize the drift of the Kearfott unit.
By applying the sensor outputs obtained from the Kearfott unit to the navigation algo
rithms derived in this chapter, the latitude and longitude results was compared to those
of the Kearfott. The results are shown in Fig. 9.2.
It can be seen that the navigation developed in this chapter followed the reference
system quite accurately during the ﬁrst minutes of the navigation. After a while, the
navigation developed in this thesis drifts away from the reference system and at the end
the navigation developed in this chapter drifts South. This is quite possibly because of
the lower update rate available (50 Hz) compared to the operating speed of the reference
system (normally >2kHz), although this cannot be veriﬁed. 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 hooklike 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
41 CHAPTER 9. NAVIGATION EQUATIONS
−2.086 −2.085 −2.084 −2.083 −2.082 −2.081 −2.08 −2.079 −2.078 −2.077
57.182
57.183
57.184
57.185
57.186
57.187
57.188
57.189
57.19
57.191
57.192
Longitude
L
a
t
i
t
u
d
e
Our system vs. Kearfott T16
Our system
Kearfott T16
Figure 9.2: Comparison between our INS and the Kearfott INS.
further North than the start point. This diﬀerence is due to the drift of the INS when
no aiding device is present.
It is more diﬃcult 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 inﬂuences the long
term (48 [hour]) performance of the system. Gyro and accelerometer bias errors are
examined, as well as initial tilt, heading, velocity and position errors.
In the simulations, the gyro and accelerometer output is calculated by the earth simu
lation model, where the INS was set to being stationary at 57.191 [deg] latitude 0 [deg]
longitude. The sensor frame was assumed equal to the navigation frame. Sensor errors
were introduced by adding a bias between the simulation model and the navigation
equations.
9.6. ANALYSIS OF LONG TERM ERROR PROPAGATION 42
Accelerometer bias error
0 10 20 30 40
−5
0
5
10
15
x 10
−3
Roll
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−0.1
−0.05
0
0.05
0.1
E
r
r
o
r
[
m
/
s
]
Velocity
East
North
0 10 20 30 40
−15
−10
−5
0
5
x 10
−3
Pitch
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−50
0
50
100
Latitude
E
r
r
o
r
[
m
]
0 10 20 30 40
−15
−10
−5
0
5
x 10
−3
Yaw
E
r
r
o
r
[
m
r
a
d
]
Time [h]
0 10 20 30 40
−100
0
100
200
Longitude
E
r
r
o
r
[
m
]
Time [h]
Figure 9.3: Error propagation with an accelerometer bias of 50 [µg]. It can be seen
that apart from the heading, all errors are bounded. The heading drift is so small it
can be assumed negligible.
43 CHAPTER 9. NAVIGATION EQUATIONS
Gyro bias error
0 10 20 30 40
−0.1
−0.05
0
0.05
0.1
Roll
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−0.5
0
0.5
1
E
r
r
o
r
[
m
/
s
]
Velocity
East
North
0 10 20 30 40
−0.1
−0.05
0
0.05
0.1
Pitch
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−10000
−5000
0
5000
Latitude
E
r
r
o
r
[
m
]
0 10 20 30 40
0
5
10
Yaw
E
r
r
o
r
[
m
r
a
d
]
Time [h]
0 10 20 30 40
0
5
10
x 10
4
Longitude
E
r
r
o
r
[
m
]
Time [h]
Figure 9.4: Error propagation with an gyro bias of 0.01 [
◦
/hr]. It can be seen, that
longitude and heading error grow unbounded.
9.6. ANALYSIS OF LONG TERM ERROR PROPAGATION 44
Initial heading error
0 10 20 30 40
−2
−1
0
1
2
x 10
−3
Roll
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−0.04
−0.02
0
0.02
0.04
E
r
r
o
r
[
m
/
s
]
Velocity
East
North
0 10 20 30 40
−2
−1
0
1
2
x 10
−3
Pitch
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−200
−100
0
100
200
Latitude
E
r
r
o
r
[
m
]
0 10 20 30 40
−0.06
−0.05
−0.04
−0.03
−0.02
Yaw
E
r
r
o
r
[
m
r
a
d
]
Time [h]
0 10 20 30 40
−1000
−500
0
500
Longitude
E
r
r
o
r
[
m
]
Time [h]
Figure 9.5: Error propagation with an initial heading error of 25E6 [rad]. All
errors are bounded.
45 CHAPTER 9. NAVIGATION EQUATIONS
Initial position error
0 10 20 30 40
−1
−0.5
0
0.5
1
x 10
−5
Roll
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−2
−1
0
1
2
x 10
−4
E
r
r
o
r
[
m
/
s
]
Velocity
East
North
0 10 20 30 40
−1
−0.5
0
0.5
1
x 10
−5
Pitch
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−2
−1
0
1
2
Latitude
E
r
r
o
r
[
m
]
0 10 20 30 40
−1
−0.5
0
0.5
1
x 10
−4
Yaw
E
r
r
o
r
[
m
r
a
d
]
Time [h]
0 10 20 30 40
−4
−2
0
2
4
Longitude
E
r
r
o
r
[
m
]
Time [h]
Figure 9.6: Error propagation with an initial position error of 1 [m]. All errors are
bounded.
9.6. ANALYSIS OF LONG TERM ERROR PROPAGATION 46
Initial pitch error
0 10 20 30 40
−0.05
0
0.05
Roll
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−0.5
0
0.5
E
r
r
o
r
[
m
/
s
]
Velocity
East
North
0 10 20 30 40
−0.05
0
0.05
0.1
0.15
Pitch
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−1000
−500
0
500
1000
Latitude
E
r
r
o
r
[
m
]
0 10 20 30 40
−0.02
0
0.02
0.04
0.06
Yaw
E
r
r
o
r
[
m
r
a
d
]
Time [h]
0 10 20 30 40
−2000
−1000
0
1000
2000
Longitude
E
r
r
o
r
[
m
]
Time [h]
Figure 9.7: Error propagation with an initial pitch and roll error of 25E6 [rad].
All errors are bounded.
47 CHAPTER 9. NAVIGATION EQUATIONS
Initial velocity error
0 10 20 30 40
−0.02
−0.01
0
0.01
0.02
Roll
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−0.1
−0.05
0
0.05
0.1
E
r
r
o
r
[
m
/
s
]
Velocity
East
North
0 10 20 30 40
−0.02
−0.01
0
0.01
0.02
Pitch
E
r
r
o
r
[
m
r
a
d
]
0 10 20 30 40
−100
−50
0
50
100
Latitude
E
r
r
o
r
[
m
]
0 10 20 30 40
−0.02
−0.01
0
0.01
0.02
Yaw
E
r
r
o
r
[
m
r
a
d
]
Time [h]
0 10 20 30 40
−200
−100
0
100
200
Longitude
E
r
r
o
r
[
m
]
Time [h]
Figure 9.8: Error propagation with an initial velocity error of 0.1 [m/s]. The
heading drift is so small it can be assumed negligible.
9.6. ANALYSIS OF LONG TERM ERROR PROPAGATION 48
One general characteristic on all the ﬁgures are oscillations with a period of 84.4 [min].
The Schuler oscillations apparent in the ﬁgures arise from the torquing of the platform
in order to keep it level. Figure 9.9 shows an INS with an exaggerated initial tilt error.
Figure 9.9(a) is the initial time where navigation is started, and Fig. 9.9(b) is at a later
time. In (a), is the system initiated with an tilt error of θ. This will result in gravity
being resolved wrongly, and will give an acceleration in the east direction. As the INS
assumes that the system is moving east, it will torque the C
n
e
matrix (the platform)
around the north axis using the transport rate, although the system is actually not
moving. At a time between (a) and (b), the platform will be level relative to earth, but
the INS will assume it still has an east velocity component, so it will continue to torque
the platform. In (b), it is seen that the INS will measure an acceleration to the west,
but will still have a velocity to the east. Because of the acceleration, the velocity will
decrease, and the result is an oscillation with the Schuler period of 84.4 [min]. This can
be recognized as being the frequency of a pendulum with the same length as the earth
radius.
Interesting is it to see, that initial tilt errors and gyro bias are by far the biggest
contributor to the errors in the system. From Fig. 9.4 and Fig. 9.7 is it seen that even
small gyro bias and tilt error give rise to position errors in kilometers. All the other
errors give rise to a position error with a maximum of 500m and most of them below.
What is also very important to notice is that the unbounded yaw error is also mainly
due to gyro bias. Although both accelerometer bias and initial velocity error contribute
to an unbounded yaw error, their contributions are very small compared to that of
the gyro bias. This shows the importance of good alignment of an INS. It is also the
main cause of the requirement of 1 [nm/h] INS to have a gyro bias of 0.01 [
◦
/h] or
less[Savage, 1997]. Accelerometer bias does give rise to errors, but not as signiﬁcant
as gyro errors. Position errors are not very signiﬁcant, whereas initial velocity errors
should be kept to a minimum.
Assumed velocity
θ
g
n
g
n
u
g
n
e
θ
g
n
g
n
e
g
n
u
Torquing due to transport rate
Assumed acceleration
Torquing due to transport rate
Assumed acceleration
(a) (b)
Figure 9.9: Schuler oscilation. The platform is tilted with respect to the earth.
This gives rise to an oscilation because of gravity being resolved incorrectly.
An INS continuously integrates the equations derived in this chapter, to obtain attitude,
velocity and position of an INS. However, before navigation, one needs to know the initial
attitude and heading of the system. This is done in a process called alignment, which
will be explained in the following chapters.
Chapter 10
Error Estimation Using Indirect
Kalman Filtering
The process of alignment is to determine attitude and heading of the INS. As the
attitude and heading is not directly measurable these are to be estimated. The Kalman
ﬁlter technique proposes an optimal way of estimating states why this approach is also
used in this thesis. However it is not a standard direct Kalman ﬁlter but an indirect
Kalman ﬁlter which is used in this thesis. Where the normal direct Kalman ﬁlter seeks
to estimate the states directly the indirect Kalman ﬁlter estimates the error in these
states and corrects the real states. For this reason is the model used by the Kalman
ﬁlter an error model, describing the error propagation of the system. This chapter
introduces the reader to the principles of and the notation used in indirect Kalman
ﬁltering. The diﬀerences 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 diﬀerence between a normal Kalman ﬁlter and the indirect Kalman ﬁlter
approach is how the system is modeled. Normally the system model describes how the
states, e.g. position, velocity, etc., develops over time. In indirect Kalman ﬁltering the
system model describes how the errors in the states develop over time. This means that
instead of estimating the states directly, only the errors in the states are estimated by
the Kalman ﬁlter. The states in an indirect Kalman ﬁlter is hence called error states.
With this in mind the system model is described as follows:
˙ x = A(t)x(t) +G
p
(t)n
p
(t) (10.1)
where
x = Error state vector
A = Error state dynamic matrix
n
p
= Process noise vector
G
p
= Process noise dynamic matrix
49
10.1. KALMAN FILTERING 50
with the measurement equation as follows:
z(t) = H(t)x(t) +G
m
(t)n
m
(t) (10.2)
where
z = Measurement vector
H = Measurement matrix
n
m
= Measurement noise vector
G
m
= Measurement noise dynamic matrix
These equations describe the system used by the Kalman ﬁlter to estimate the error
states x. As the ﬁlter estimates error states, the input to the ﬁlter must also be errors.
Hence, the system must be aided by some external state measurements which can be
compared to the states of the system to generate measurements of the errors in the
system.
The measurement equation is a linearized version of the general nonlinear observation
equation z
obs
which describes the actual, nonlinear observation which is deﬁned as
follows:
z
obs
= f(ξ
ins
, ξ
aid
) (10.3)
where
ξ
ins
= INS navigation states (position, velocity, etc.)
ξ
aid
= Aiding device navigation states (position, velocity, etc.)
f() = Function which compares the parameters from the INS and the aiding device.
It is constructed such that with an error free INS and an error free aiding device
this function is equal to zero
An example of the observation equation could be the position estimated by the INS
minus the position estimated by a GPS. With these estimates the observation equation
is:
z
obs
= r
ins
−r
gps
(10.4)
With error free measurement this observation equation is indeed zero.
An example of an indirect Kalman ﬁlter is depicted in Fig. 10.1. The INS system
calculates the INS states (position, velocity, attitude etc.), where some are compared to
the aiding device states to generate the observations for the Kalman ﬁlter. The Kalman
ﬁlter outputs the optimal estimate of the errors in the INS states.
Looking at the ﬁgure, it can be seen that the indirect Kalman ﬁlter is allowed to adjust
the states of the system through a process called “control reset”. As the estimate from
the Kalman ﬁlter is in fact the best estimate of the errors in the states, it seems obvious
to use this information to adjust the states and thus also the error states (as they depend
directly on the states). The error states are reset as follows:
x
n
(+
c
) = ˆ x
n
(+
e
) +u
nc
(10.5)
where
u
c
= Control vector used to control the error build up in the states
(+
c
) = Designation of the corrected error state just after the states have been reset
(+
e
) = Designation of the estimated error state just before the control reset vector is applied.
51
CHAPTER 10. ERROR ESTIMATION USING INDIRECT KALMAN
FILTERING
The control reset operation will prevent errors from being build up at the integrators,
as the errors are set to 0 at each time step. The implementation of correcting the INS
parameters involves applying u
n
to the INS parameters in the following way:
ξ
insn
(+
c
) = g
ins
(ξ
insn
(−), u
cn
) (10.6)
where
g
ins
() = Functional operator to apply u
c
to the INS parameters
The g
ins
() operation diﬀers depending on which type of state that should corrected. In
this report two diﬀerent types of states are to be corrected. One type of correction is
to simply subtract the two states from each other. This correction is applicable when
correcting states like position and velocity. But when correcting the rotation matrices
subtraction is not applicable and they are corrected as seen in (12.41).
The idea of resetting the error states by control resetting the actual states is illustrated
in Fig. 10.1. The measurements are parsed through the INS algorithms in order to get
the ﬁlter input in the correct form. With the INS error model the ﬁlter input should
also be an error. This be either position error, velocity error, etc. The Kalman ﬁlter
estimates the error states and these are used to correct the INS states and thus indirectly
resetting the error states.
INSSystem
Filter
Input
ErrorState
Estimate
KalmanFilter
ControlResetofINSStates/IndirectResetofErrorState
ForegroundEquations
+

AidingDevice
Measurements
Figure 10.1: Indirect Kalman ﬁlter with control reset
The ideal control vector u
c
is of course ˜ x
n
(+
e
) as it can be seen from (10.5). This
control vector would reset the errors to zero at every kalman ﬁlter cycle, as x
n
(+
e
) is
the best estimate of the error.
In order to point out the diﬀerences between a ”normal” direct Kalman ﬁlter and an
indirect Kalman ﬁlter the Kalman equations for both ﬁlters are listed. First of all the
normal direct Kalman equations are listed:
Measurement update
K
n
= P
n
(−)H
T
n
(H
n
P
n
(−)H
T
n
+R
n
)
1
ˆ x
n
(+) = ˆ x
n
(−) +K
n
(˜z
n
−ˆz
n
)
P
n
(+) = (I −K
n
H
n
)P
n
(−)
Time update
ˆ x
n
(−) = Φ
n
ˆ x
n1
(+)
ˆ z
n
= H
n
ˆ x
n
(−)
Secondly the indirect Kalman ﬁlter equations are listed. The parts which are special for
the indirect Kalman ﬁlter is indented in order to compare it with the ’normal’ Kalman
ﬁlter.
10.2. MOTIVATING EXAMPLE 52
z
obsn
= f(ξ
insn
, ξ
aidn
)
Measurement update
K
n
= P
n
(−)H
T
n
(H
n
P
n
(−)H
T
n
+R
n
)
1
ˆ x
n
(+
e
) = ˆ x
n
(−) +K
n
(z
obsn
− ˆz
n
)
P
n
(+) = (I −K
n
H
n
)P
n
(−)
u
cn
= −ˆ x
n
(+
e
)
x
n
(+
c
) = ˆ x
n
(+
e
) +u
nc
ξ
insn
(+
c
) = g
ins
(ξ
insn
(−), u
cn
)
Time update
ˆ x
n
(−) = Φ
n
ˆ x
n1
(+
c
)
ˆz
n
= H
n
ˆ x
n
(−) (10.7)
where
(+
e
) = Designation for parameter after estimation control but before control reset
(+
c
) = Designation for parameter after control reset
K
n
= Kalman gain
10.2 Motivating Example
For this example a one dimensional accelerometer is used. The accelerometer delivers
noisy acceleration measurements and the system consist of an integrator in order to
determine the velocity. Furthermore another speedometer is used which delivers velocity
measurements. The Kalman ﬁlter is to estimate the errors in the velocity in order to
get a better estimate of the velocity.
The indirect Kalman ﬁlter is presented in Fig. 10.2.
z1
Kalman
filter
Measured
acceleration
a
v
Velocitymeasuredby
aidingdevice
v
ref
Observationsetfor
theKalmanfilter
z =v
obs
v
ref
Kalmanfilter
estimate
v
Controlreset
+

Ts
Figure 10.2: One dimensional indirect Kalman ﬁlter
As seen in the ﬁgure, the measured acceleration is integrated to give the velocity. The
reference velocity measured by the aiding speedometer is subtracted from the systems
estimated velocity to give the observation set. The reason for this is to get the observa
tion set to contain velocity errors which is the driving parameter for the Kalman ﬁlter.
The Kalman ﬁlter then estimates errors as the system model is an error model. This
error is of course fed back to the integrator to correct the velocity. The correction inside
the integrator is shown below.
53
CHAPTER 10. ERROR ESTIMATION USING INDIRECT KALMAN
FILTERING
1
z
+
+
T
n
+
+
Measured
acceleration
a
Add_1
Add_2
Controlreset
v
v
Errorcorrected
velocity
Discretetime
delay
Figure 10.3: Illustration of correction of states inside a discrete integrator
As seen in Fig. 10.3 the integrator is a discrete integrator with an addition block inside
the integration loop. This method ensures that the correct value is store, thus avoiding
residual error buildup in the integrator.
10.2.1 Choice of Kalman Filter
When using Kalman ﬁltering it is normally quite simple to choose between e.g. a normal
linear, extended or unscented Kalman ﬁlter. It might not be quite as intuitive when
it could be advantageous to do the extra work to derive an error model in order to
implement the indirect Kalman ﬁlter. Two guidelines can be used to when the IKF
should be considered.
If the system has high dynamics and one is trying to estimate the states, then the
Kalman ﬁlter cycle has to be very fast. This sets demands to the processor in order to
do the Kalman calculations before the next Kalman cycle. This might pose a problem
when a system should be implemented on a microprocessor. It might not be possible
to do all the calculations within each Kalman cycle. Secondly, if the system is also
nonlinear then a nonlinear Kalman ﬁlter should be chosen. This leads to even more
complicated calculation which sets even higher demands to the processor chosen.
As mentioned earlier the use of indirect Kalman ﬁltering requires the system model to
be an error model. This error model can be approximated to be linear and in INS,
normally has much lower dynamics than the real system. For these reasons a normal
linear Kalman ﬁlter can be used with a much lower Kalman cycle setting much lower
demands to the processor.
The reasons for choosing the IKF for this project is that the model of the INS has
very high dynamics. In order to describe the motion with high precision, the model
should be run at more than 2kHz and furthermore the is model nonlinear. Choosing
a standard direct Kalman ﬁlter approach requires the use of an extended Kalman ﬁlter
which should be run at 2kHz. This might pose a problem when implementing on a
microprocessor. Using the indirect Kalman ﬁlter, it is only necessary to run when new
data is available from the aiding device (1 Hz for typital GPS receivers). For this reason,
the indirect Kalman ﬁlter is chosen.
10.3. SUMMARY 54
10.3 Summary
During this chapter, the indirect Kalman ﬁlter technique has been described and the
diﬀerences and advantages with respect to the direct Kalman ﬁlter have been outlined.
As described, the indirect Kalman ﬁlter is advantageous if the system is characterized
by high dynamics and/or nonlinearity. In order to utilize the indirect Kalman ﬁlter
an error model of the system needs to be developed and this error model of the INS is
developed in the next chapter.
Chapter 11
Error Equations
During this chapter is the error model of the INS developed which describes the prop
agation of errors within the INS. These errors are position, velocity, rotation, attitude
and heading error of the INS. This error model is developed as it is required as model
for the indirect Kalman ﬁlter.
11.1 Derivation of Error Equations
The purpose of the error model is to describe the propagation of the errors in the
navigation equations. The errors are deﬁned as the angle between the actual DCMs and
the DCMs computed in the navigation computer and the velocity diﬀerence between
the actual velocity of the INS and the velocity used by the navigation computer. The
errors to be deﬁned 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 simpliﬁed for use in the error equations. The earth is
considered a sphere instead of an ellipsoid which yields the following equations:
˙
C
n
b
= C
n
b
Ω
b
ib
−(Ω
b
ie
+Ω
b
en
)C
n
b
ω
n
ie
= C
n
e
ω
e
ie
ω
n
en
=
1
R
(u
n
zn
×v
n
) + ρ
R
u
n
zn
˙ v
n
= C
n
b
a
n
+g
n
−(ω
n
en
+ 2ω
n
ie
) ×v
n
˙
C
e
n
= C
e
n
Ω
n
en
˙
h = v
n
· u
n
zn
(11.1)
Where u
n
zn
=
0 0 1
is a vertical unit vector and ρ
R
is the magnitude of the
transport rate around the vertical.
Certain approximations and assumptions has been made deriving the navigation equa
55
11.1. DERIVATION OF ERROR EQUATIONS 56
tions above which lead to one type of errors. These errors can be minimized by making
a suﬃciently accurate model.
Secondly, another type of errors is introduced. These errors arise when the INS computer
evaluates the navigation equations. The equations used in the computer are not exactly
equivalent to the equations in (11.1) due to several sources of error. These errors arise
because of measurement inaccuracies from the sensors and computer errors due to ﬁnite
precision and integration errors. In this chapter, only the errors from the sensors is
considered as the computer errors can be minimized by choosing suﬃcient length of
word.
The navigation equations in the navigation computer are equal to (11.1), but with a
hat to indicate that they include the second type of errors:
˙
ˆ
C
n
b
=
ˆ
C
n
b
˜
Ω
b
ib
−(
ˆ
Ω
b
ie
+
ˆ
Ω
b
en
)
ˆ
C
n
b
ˆ ω
n
ie
=
ˆ
C
n
e
ˆ ω
e
ie
ˆ ω
n
en
=
1
ˆ
R
(u
n
zn
× ˆ v
n
) + ˆ ρ
R
u
n
zn
˙
ˆ v
n
=
ˆ
C
n
b
˜ a
n
+ ˆ g
n
−( ˆ ω
n
en
+ 2ˆ ω
n
ie
) × ˆ v
n
˙
ˆ
C
e
n
=
ˆ
C
e
n
ˆ
Ω
n
en
˙
ˆ
h = ˆ v
n
· u
n
zn
(11.2)
In order to develope the error mode an error is deﬁned as follows:
δa = ˆ a −a (11.3)
Where ˆ a is the computed state and a is the actual state.
Using this error deﬁnition 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 deﬁned by (11.3).
Before deriving the error model the errors of interest are stated. As the INS is to be
used in two modes, alignment mode and navigation mode, two sets of errors are of
interest. During alignment, the primary error of interest is the error in the C
n
b
matrix.
In navigation mode it is mainly the position error which is of interest. This means that
the error in C
n
e
matrix and the error in height is important. Furthermore, the errors
in the velocity of the INS is necessary in order to estimate the position of the vehicle
when navigating. This leads to the following errors which are of interest:
δC
n
b
=
ˆ
C
n
b
−C
n
b
δv
n
= ˆ v
n
−v
n
δC
e
n
=
ˆ
C
e
n
−C
e
n
δh =
ˆ
h −h (11.4)
The error equations can be obtained by subtracting (11.1) from (11.2) and neglecting
the parts which are a multiplication of two errors as they are assumed to be very small.
This yields the following expression for the errors of interest:
57 CHAPTER 11. ERROR EQUATIONS
δ
˙
C
n
b
= δC
n
b
˜
Ω
b
ib
+C
n
b
δΩ
b
ib
−(δΩ
n
ie
+ δΩ
n
en
)C
n
b
−(Ω
n
ie
+Ω
n
en
)δC
n
b
δ ˙ v
n
= δC
n
b
˜ a
b
+C
n
b
δa
b
−(2δω
n
ie
+ δω
n
en
) ×v
n
−(2ω
n
ie
+ω
n
en
) ×δv
n
+ δg
n
δ
˙
C
e
n
= δC
e
n
Ω
n
en
+C
e
n
δΩ
n
en
δω
n
ie
= δC
n
e
ω
e
ie
δω
n
en
=
1
R
(u
n
zn
×δv
n
) −
δR
R
2
(u
n
zn
×v
n
) + δρ
R
u
n
zn
δ
˙
h = δv
n
· u
n
zn
(11.5)
Although these equations describe the error propagation it does not directly describe
the errors of interest. These equations show the derivative of the error DCM and not the
derivative of the error angle of the DCM. By introducing small angle approximations to
(11.5) these equations can be transformed into a suitable form. These approximations
are, of course, only applicable when the error angles are small, so the error equations
derived here can only be used in such cases. The small angle approximation is deﬁned
as follows:
ˆ
C
n
b
= (I −Γ
n
)C
n
b
⇒δC
n
b
= −Γ
n
C
n
b
ˆ
C
n
e
= (I −E
n
)C
n
e
⇒δC
n
e
= −E
n
C
n
e
ˆ
C
e
n
=
ˆ
C
n
e
T
= C
e
n
(I +E
n
) ⇒δC
e
n
= C
e
n
E
n
(11.6)
where:
Γ, E = Skew symmetric form of γ
n
and ǫ
n
.
γ
n
= The rotation vector associated with δC
n
b
ǫ
n
= The rotation vector associated with δC
e
n
(11.7)
With this small angle approximation the error equations are rewritten. Using (11.6)
δω
n
ie
is rewritten:
δω
n
ie
= −E
n
C
n
e
ω
e
ie
= −ǫ
n
×ω
n
ie
(11.8)
The gravity error model in the δv
n
term from (11.5) is the variation from the true
gravity to the one estimated by the computer. This gravity error is inﬂuenced both by
errors on the altitude estimation and errors in the actual gravity to the gravity model
in the computer. Gravity can be expressed as a simple inverse square law model plus a
term taking into account the deviations from the simple model:
g
n
= −g
0
R
2
0
R
2
u
n
zn
+ ∆g
n
(11.9)
where
g
0
= Gravity magnitude on the earth surface
R
0
= Earth’s radius
R = Distance from the earth center to the INS
∆g
n
= Correction to inverse square model to take into account
the centripetal acceleration and earth mass distribution.
The gravity error δg
n
is found using a special approach. As gravity is dependent on the
parameter R, variation of gravity is deﬁned to be g
n
diﬀerentiated with respect to R.
11.1. DERIVATION OF ERROR EQUATIONS 58
This deﬁnition is used to deﬁne the error in gravity. The error in the gravity is then
simply the variation of gravity multiplied with the error in the position as follows:
g
n
var
=
dg
n
dR
δg
n
=
dg
n
dR
δR (11.10)
Using this approach and the gravity model from (11.9) δg
n
is as follows:
δg
n
= 2g
0
R
2
0
R
3
δRu
n
zn
+ δ∆g
n
≈
2g
0
R
δhu
n
zn
+ δg
n
M
(11.11)
where
δg
n
M
= Unmodeled gravity error
Using (11.6), (11.8) and (11.11) δ ˙ v
n
from (11.2) is rewritten:
δ ˙ v
n
= −γ
n
×f
n
+C
n
b
δf
b
−(2δω
n
ie
+ δω
n
en
) ×v
n
−
(ω
n
en
−2ǫ
n
×ω
n
ie
) ×δv
n
+
2g
R
δhu
n
zn
+ δg
n
M
(11.12)
In order to rewrite δ
˙
C
e
n
(11.6) is substituted into δ
˙
C
e
n
from (11.5) which yields:
˙
C
e
n
E
n
+C
e
n
˙
E
n
= C
e
n
E
n
Ω
n
en
+C
e
n
δΩ
n
en
(11.13)
C
e
n
˙
E
n
= C
e
n
E
n
Ω
n
en
−C
e
n
Ω
n
en
E
n
+C
e
n
δΩ
n
en
⇓
˙
E
n
= (E
n
Ω
n
en
−Ω
n
en
E
n
) + δΩ
n
en
(11.14)
In order to describe this equation in vector form and in a somewhat simpler form the
terms in the brackets can be reduced using the triple vector product identity, here shown
for three arbitrary vectors:
v
1
×(v
2
×v
3
) = (v
1
· v
3
)v
2
−(v
1
· v
2
)v
3
(11.15)
Multiplying the term in brackets from (11.14) with an arbitrary vector (v) and switching
to vector form:
(E
n
Ω
n
en
−Ω
n
en
E
n
)v = ǫ
n
×(ω
n
en
) ×v) −ω
n
en
×(ǫ
n
) ×v) (11.16)
Applying the triple vector product identity yields:
(E
n
Ω
n
en
−Ω
n
en
E
n
)v = (ǫ
n
· v)ω
n
en
−(ǫ
n
· ω
n
en
)v −(ω
n
en
· v)ǫ
n
+ (ω
n
en
· ǫ
n
)v
= (ǫ
n
· v)ω
n
en
−(ω
n
en
· v)ǫ
n
(11.17)
Using the triple vector product to collect term again:
(E
n
Ω
n
en
−Ω
n
en
E
n
)v = (ǫ
n
×ω
n
en
) ×v (11.18)
Converting this into matrix form where the term in the bracket should be represented
by the skew symmetric counterpart:
(E
n
Ω
n
en
−Ω
n
en
E
n
)v = (ǫ
n
×ω
n
en
)
∗
v (11.19)
Where the ()
∗
operator brings the vector enclosed in the brackets to skew symmetric
form. As the vector v is an arbitrary vector this will be chosen as identity:
(E
n
Ω
n
en
−Ω
n
en
E
n
) = (ǫ
n
×ω
n
en
)
∗
⇓
˙
E
n
= (ǫ
n
×ω
n
en
)
∗
+ δΩ
n
en
(11.20)
59 CHAPTER 11. ERROR EQUATIONS
which can be represented in vector form:
˙ ǫ
n
= ǫ
n
×ω
n
en
+ δω
n
en
(11.21)
In the same manner as with
˙
C
e
n
,
˙
C
n
b
is rewritten by using the expression from (11.6)
and then replacing with the expression from (11.5):
δ
˙
C
n
b
= −
˙
Γ
n
C
n
b
−Γ
n
˙
C
n
b
= −Γ
n
C
n
b
˜
Ω
b
ib
+C
n
b
δΩ
b
ib
−(δΩ
n
ie
+ δΩ
n
en
)C
n
b
+ (Ω
n
ie
+Ω
n
en
)Γ
n
C
n
b
(11.22)
In order to get rid of the derivative of
˙
C
n
b
this is substituted with the expression from
(11.1) yielding:
˙
Γ
n
C
n
b
= −Γ
n
C
n
b
˜
Ω
b
ib
−(Ω
n
ie
+Ω
n
en
)C
n
b
+Γ
n
C
n
b
˜
Ω
b
ib
−C
n
b
δΩ
b
ib
+ (δΩ
n
ie
δΩ
n
en
)C
n
b
−(Ω
n
ie
+Ω
n
en
)Γ
n
C
n
b
= Γ
n
(Ω
n
ie
+Ω
n
en
)C
n
b
−(Ω
n
ie
+Ω
n
en
)Γ
n
C
n
b
−C
n
b
δΩ
b
ib
+ (δΩ
n
ie
+ δΩ
n
en
)C
n
b
(11.23)
Which can be simpliﬁed further by right side multiplying with (C
n
b
)
−1
˙
Γ
n
= Γ
n
(Ω
n
ie
+Ω
n
en
) −(Ω
n
ie
+Ω
n
en
)Γ
n
−C
n
b
δΩ
b
ib
C
b
n
+ δΩ
n
ie
+ δΩ
n
en
= [γ
n
×(ω
n
ie
+ω
n
en
)]
∗
−C
n
b
δΩ
b
ib
C
b
n
+ δΩ
n
ie
+ δΩ
n
en
(11.24)
Using the similarity transform simpliﬁes the expression to:
˙
Γ
n
= [γ
n
×(ω
n
ie
+ω
n
en
)]
∗
−(C
n
b
δω
b
ib
)
∗
+ δΩ
n
ie
+ δΩ
n
en
(11.25)
which can be written in vector form:
˙ γ
n
= γ
n
×(ω
n
ie
+ω
n
en
) −C
n
b
δω
b
ib
+ δω
n
ie
+ δω
n
en
(11.26)
Equation (11.12), (11.21), (11.26) along with δω
n
en
,
˙
h from (11.5) and the approximation
δR = δh constitute the error model for (11.1) in terms of the sensor errors δa
b
and δω
b
ib
:
˙ γ
n
= γ
n
×(ω
n
ie
+ω
n
en
) −C
n
b
δω
b
ib
+ δω
n
ie
+ δω
n
en
δ ˙ v
n
= −γ
n
×˜a
n
+C
n
b
δa
b
−(2δω
n
ie
+ δω
n
en
) ×v
n
−(ω
n
en
−2ǫ
n
×ω
n
ie
) ×δv
n
+
2g
R
δhu
n
zn
+ δg
n
M
˙ ǫ
n
= ǫ
n
×ω
n
en
+ δω
n
en
δω
n
en
=
1
R
(u
n
zn
×δv
n
) −
δh
R
2
(u
n
zn
×v
n
) + δρ
R
u
n
zn
δ
˙
h = δC
n
e
ω
e
ie
(11.27)
However, this error model, described by the equations (11.27), suﬀer from one disad
vantage. The position error is described in form of four parts: ǫ
n
and h. This means
that there is some redundant information as the position error can be described by three
parts. The redundant information is in ǫ
n
where the vertical part equals the azimuth
error of the navigation frame. During the navigation mode where it is the position error
vector which is of interest, this model is inconvenient as it does not described this po
sition error directly. This disadvantage disappears if the error model is to be used in a
situation where the position error is not important and can therefore be neglected. This
situation is e.g. when aligning with the use of a Kalman ﬁlter as the position is assumed
to be known precisely. But when using the error model in a navigation situation it is
convenient to have the position error described only by three parts thus removing the
redundant information.
11.1. DERIVATION OF ERROR EQUATIONS 60
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 deﬁned now in the earth frame:
ˆ
C
e
b
= (I −Ψ
e
)C
e
b
⇒δC
e
b
= −Ψ
e
C
e
b
δv
e
= ˆ v
e
−v
e
δr
e
=
ˆ
r
e
−r
e
(11.28)
where
Ψ = The skew symmetric counterpart of ψ
which is equal to the angular error in C
e
b
r = The position vector from the earth’s center to the INS
The task is now to relate these new earthframe error equations to the navigationframe
error equation already developed.
First of all it should be noted that:
δC
e
b
= δC
e
n
C
n
b
+C
e
n
δC
n
b
(11.29)
which makes it possible to describe the Ψ part of (11.28) as:
Ψ
e
= −(δC
e
n
C
n
b
+ C
e
n
δC
n
b
)C
e
b
T
= −(C
e
n
C
n
b
E
n
C
n
b
−C
e
n
Γ
n
C
n
b
)(C
n
b
)
T
(C
e
n
)
T
= C
e
n
(Γ
n
−E
n
)C
n
e
(11.30)
This can be rearranged into:
(C
e
n
)
T
Ψ
e
C
e
n
= Γ
n
−E
n
(11.31)
Using the similarity transformation yields the following:
ψ
n
= γ
n
−ǫ
n
(11.32)
In order to relate δv
e
with δv
n
it is ﬁrst recognized that:
v
e
= C
e
n
v
n
ˆ v
e
=
ˆ
C
e
n
ˆ v
n
ˆ
C
e
n
= C
e
n
+ δC
e
n
(11.33)
Substituting (11.33) into δv
e
from (11.28), using the deﬁnition of δv
n
from (11.4) and
neglecting any second order errors as they are assumed insigniﬁcant yields:
δv
e
=
ˆ
C
e
n
ˆ v
n
−C
e
n
v
n
= (C
e
n
+ δC
e
n
)ˆ v
n
−C
e
n
v
n
= C
e
n
(ˆ v
n
−v
n
) + δC
e
n
v
n
= C
e
n
δv
n
+ δC
e
n
v
n
(11.34)
Using small angle approximation yields:
δv
e
= C
e
n
(δv
n
+E
n
v
n
) = C
e
n
(δv
n
+ǫ
n
×v
n
) (11.35)
As the error model should be expressed in navigation frame the velocity error in navi
gation frame is deﬁned:
δυ
n
ˆ
C
n
e
δv
e
(11.36)
61 CHAPTER 11. ERROR EQUATIONS
With deﬁnition (11.35) it becomes:
δυ
n
= δv
n
+ǫ
n
×v
n
(11.37)
A relation from δr
e
with ǫ
n
and h is obtained by ﬁrst deﬁning r
e
:
r
e
= Ru
e
ze
(11.38)
Where R is the length of r
e
, and u
e
ze
is the unit vector pointing from the center of the
earth to the INS in earth frame. The error is deﬁned in the standard way and neglecting
second order errors yield:
δr
e
=
ˆ
Rˆ u
e
ze
−Ru
e
ze
= (R + δR)(C
e
n
+ δC
e
n
)u
n
zn
−RC
e
n
u
e
ze
≈ RδC
e
n
u
n
zn
+ δRC
e
n
u
n
zn
(11.39)
Transforming this into the navigation frame yields:
δr
n
= RE
n
u
n
zn
+ δRu
n
zn
(11.40)
which can be represented in vector form:
δr
n
= R(ǫ
n
×u
n
zn
) + δRu
n
zn
(11.41)
From this position error vector the horizontal and vertical parts are easily found. The
vertical parts is the dot product of (11.41) with u
n
zn
and as expected it is equal to δR.
The vertical part is simply found by subtracting the vertical part from (11.41) and is
equal to:
δr
n
H
= R(ǫ
n
×u
n
zn
) (11.42)
To this point the old error model has been related to the new error states and is sum
marized here:
ψ
n
= γ
n
−ǫ
n
δυ
n
= δv
n
+ǫ
n
×v
n
δr
n
= R(ǫ
n
×u
n
zn
) + δRu
n
zn
δr
n
H
= R(ǫ
n
×u
n
zn
)
δR = δh (11.43)
In order to obtain the diﬀerential equation for the new error model the equivalent
parameters from (11.27) is substituted and diﬀerentiated.
The
˙
Ψ
n
equation is obtained by subtracting ˙ ǫ
n
from ˙ γ
n
and substituting the parts with
their equivalent from (11.43):
˙
Ψ
n
= Ψ
n
×(ω
n
ie
+ω
n
en
) −C
n
b
δω
b
ib
(11.44)
The δ ˙ υ
n
diﬀerential equation is derived by diﬀerentiating the δυ
n
expression from
(11.43):
δ ˙ υ
n
= δ ˙ v
n
+ ˙ ǫ
n
×v
n
+ǫ
n
× ˙ v
n
(11.45)
11.1. DERIVATION OF ERROR EQUATIONS 62
Substituting δ ˙ v
n
, ˙ v
n
and ǫ
n
with expressions from (11.1) and (11.27) and substituting
δv
n
with an rearranged δ ˙ υ
n
expression from (11.43) yields:
δ ˙ υ
n
= C
n
b
δa
b
−γ
n
× ˜ a
n
−(2ω
n
ie
+ω
n
en
) ×(δυ
n
−ǫ
n
×v
n
)
−(δω
n
en
−2ǫ
n
×ω
n
ie
) ×v
n
+
2g
R
δhu
n
zn
+ δg
n
M
+ (ǫ
n
×ω
n
en
+ δω
n
en
) ×v
n
+ǫ
n
×
C
n
b
˜ a
b
−(2ω
n
ie
+ω
n
en
) ×v
n
+g
n
= C
n
b
δa
b
−(γ
n
−ǫ
n
) × ˜ a
n
+ǫ
n
×g
n
−(2ω
n
ie
+ω
n
en
) ×δυ
n
+
2g
R
δhu
n
zn
+ δg
n
M
+ (2ω
n
ie
+ω
n
en
) ×(ǫ
n
×v
n
) + [ǫ
n
× (2ω
n
ie
+ω
n
en
)] ×v
n
−ǫ ×[(2ω
n
ie
+ ω
n
en
) ×v
n
] (11.46)
The three last parts can be shown to sum to zero by using the triple vector product
from (11.15). Deﬁning Ω = 2Ω
n
ie
+ Ω
n
en
the triple vector product is used on the three
last parts and summarized with the following result:
Ω ×(ǫ ×v) + (ǫ ×Ω) ×v −ǫ ×(Ω ×v)
ǫ(Ω · v) −v(Ω · ǫ) −ǫ(Ω · v) +Ω(ǫ · v) −Ω(ǫ · v) +v(ǫ · Ω)
v(ǫ · Ω −Ω · ǫ) = 0 (11.47)
Furthermore (γ
n
−ǫ
n
) can be substituted with Ψ
n
. Lastly the ∆g
n
part from g
n
from
(11.9) can be neglected as it is very small compared to g
n
. As g
n
point the same way
as u
n
zn
the following expression can be substituted using δr
n
H
from (11.43):
ǫ
n
×g
n
= −g(ǫ
n
×u
n
zn
)
= −
g
R
δr
n
H
(11.48)
All of these rearrangements result in the ﬁnal expression for δ ˙ υ
n
:
δ ˙ υ
n
= C
n
b
δa
b
−Ψ
n
× ˜ a
n
−(2ω
n
ie
+ω
n
en
) ×δυ
n
+
2g
R
δRu
n
zn
−
g
R
δr
n
H
+ δg
n
M
(11.49)
What remains is to relate the new position vector r
n
to the old states ǫ
n
and h. By
deﬁnition:
δ ˙ r
e
= δv
e
(11.50)
and furthermore:
δr
n
= C
n
e
δr
e
(11.51)
Diﬀerentiating (11.51):
δ ˙ r
n
=
˙
C
n
e
δr
e
+C
n
e
δ ˙ r
e
(11.52)
Substituting (11.50) and (11.51) into (11.52) with the expression of
˙
C
n
e
from (11.1)
yields:
δ ˙ r
n
= −Ω
n
en
C
n
e
δr
e
+C
n
e
δv
e
= δυ
n
−ω
n
en
×δr
n
(11.53)
The new error model with the new error states
˙
Ψ
n
, ˙ υ
n
and ˙ r
n
is summarized below:
˙
Ψ
n
= Ψ
n
×(ω
n
ie
+ω
n
en
) −C
n
b
δω
b
ib
δ ˙ υ
n
= C
n
b
δa
b
−Ψ
n
× ˜ a
n
−(2ω
n
ie
+ω
n
en
) ×δυ
n
+
2g
R
δRu
n
zn
−
g
R
δr
n
H
+ δg
n
M
δ ˙ r
n
= δυ
n
−ω
n
en
×δr
n
(11.54)
63 CHAPTER 11. ERROR EQUATIONS
This new error model is of course equivalent to the one from (11.27) with the relation
found in (11.43). The ψerror model is shown below in matrix form:
˙
ψx
ψy
ψz
δvx
δvy
δvz
δrx
δry
δrz
¸
¸
¸
¸
¸
¸
¸
¸
¸
=
0 Ω
c
−Ω
b
−Ω
β
0 0 0 0 0 0
−Ω
c
0 Ω
a
+Ω
α
0 0 0 0 0 0
Ω
b
+Ω
β
−Ω
a
−Ω
α
0 0 0 0 0 0 0
0 −a
n
U
a
n
N
0 2Ω
c
−2Ω
b
+Ω
β
−g/r 0 0
a
n
U
0 −a
n
E
−2Ω
c
0 2Ω
a
+Ω
α
0 −g/r 0
−a
n
N
a
n
E
0 2Ω
b
+Ω
β
−2Ω
a
−Ω
α
0 0 0 2g/r
0 0 0 1 0 0 0 0 −Ω
β
0 0 0 0 1 0 0 0 Ω
α
0 0 0 0 0 1 Ω
β
−Ω
α
0
¸
¸
¸
¸
¸
¸
¸
¸
¸
ψx
ψy
ψz
δvx
δvy
δvz
δrx
δry
δrz
¸
¸
¸
¸
¸
¸
¸
¸
¸
(11.55)
where
Ω
a
= cos(l) sin(α)ω
e
Ω
b
= cos(l) cos(α)ω
e
Ω
c
= sin(l)ω
e
Ω
α
= −
˙
l cos(α) +
˙
Lcos(l) sin(α)
Ω
β
=
˙
l sin(α) +
˙
Lcos(l) cos(α)
˙
l = −v
g
N
/(r + h)
˙
L = v
g
E
/(r + h) cos(l)
11.2 Summary
During this chapter has the error model of the INS been developed. Two diﬀerent
approaches has been used which leads to two diﬀerent error models. These two error
models describe the error propagation of the INS but has diﬀerent characteristics as
they describe diﬀerent 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 diﬀerent situations. The ﬁrst
error model, (γ) error model, describe the errors in C
n
b
along with velocity and position
errors which makes this error model preferable when doing alignment as alignment
tries to estimate these states. When in navigation mode the second error model, (ψ)
error model, is simpler to calculate enabling a faster Kalman ﬁlter and thus making it
preferable.
With the error model developed and the indirect Kalman ﬁlter technique described the
alignment algorithm is ready to be developed.
Chapter 12
Alignment
As mentioned earlier an INS simply integrates measured acceleration and rotation rate
to determine the position of the body frame. But the position is always relative to
the starting position and for this reason the INS needs to know both the position,
attitude and heading before navigation can begin. The position is assumed known but
the attitude and heading needs to be determined. This is the process of alignment.
The idea behind alignment is to use the measurements from the accelerometers and
gyroscopes to determine the orientation of the body frame with respect to a reference
frame, in this thesis the navigation frame. The Alignment process is divided into two
parts: Coarse alignment and ﬁne alignment. Coarse alignment is an analytical alignment
which suﬀers from a lack of precision as measurement errors and other noise sources are
not considered. It does, however, estimate the attitude and heading accurately enough
to justify the small angle approximations made in the error model, and hence allow
ﬁne alignment to use an indirect Kalman ﬁlter using the error model to obtain a more
precise alignment. This chapter will explain the alignment procedure.
Alignment is achieved using sensor measurements when the platform is in a quasi
stationary situation. Quasistationarity 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 simpliﬁes the coarse alignment as the
earth rotation is only present on two axes.
The ﬁrst section will describe the coarse alignment. The second section describes the
ﬁne alignment procedure which uses the indirect Kalman ﬁltering technique and the
error model developed in the preceding chapter. The last section is a veriﬁcation of the
alignment where it is tested against both simulated and realworld measurements.
64
65 CHAPTER 12. ALIGNMENT
12.1 Comparison between Gimballed and Strapdown
Alignment
Understanding the concept of alignment when using a strapdown INS is easier when
visualizing the gimballed situation. When aligning a gimballed INS the gimbal is phys
ically rotated until two of the the sensor axes are level with respect to the earth. This
means that if a plane is tilted 45
◦
with respect to the earth the gimbal will need to
be rotated 45
◦
to be level. The gimbal is simply rotated/tilted until the accelerometer
measures [0 0 g]
T
which means that the INS is now level with the earth. When using
a strapdown INS the sensors are mounted directly to the body and can not be rotated
so this is achieved analytically. By adjusting C
n
b
until the accelerometer measurements
equals [0 0 g]
T
in the navigation frame, results in the same as physically rotating the
gimbal. So throughout this thesis the concept of adjusting C
n
b
will be called leveling in
order to gain greater understanding.
12.2 Coarse Alignment
The gravity vector along with the 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 C
b
n
relating the
navigation frame with the body frame. In the nframe gravity has only one component
which is the vertical, and earth rotation is also known due to the knowledge of the
latitude. The measured quantities are g
b
and ω
b
ie
which are the measurements from the
accelerometer and gyroscope. These quantities are related in the following way:
g
b
= C
b
n
g
n
ω
b
ie
= C
b
n
ω
n
ie
where g
n
=
0 0 g
T
and ω
n
ie
=
0 ω
ie
cos(l) ω
ie
sin(l)
T
. Knowing the quan
tities g
b
=
g
b
x
g
b
y
g
b
z
T
and ω
b
ie
=
ω
b
x
ω
b
y
ω
b
z
T
from the measurements of
the accelerometer and gyroscope, this system is easily solved.
C
31
=
g
x
g
C
21
=
ω
x
ω
ie
cos(l)
−
g
x
g
tan(l) (12.1)
C
32
=
g
y
g
C
22
=
ω
y
ω
ie
cos(l)
−
g
y
g
tan(l) (12.2)
C
33
=
g
z
g
C
13
=
ω
z
ω
ie
cos(l)
−
g
z
g
tan(l) (12.3)
where C
21
, C
22
, ... C
33
are the elements of the direction cosine matrix C
b
n
. The last
three elements are easily determined with the use of the orthogonality property of the
direction cosine matrix, (C
b
n
)
−1
= (C
b
n
)
T
.
12.3. FINE ALIGNMENT 66
C
21
= −C
12
C
33
+ C
13
C
32
(12.4)
C
22
= C
11
C
33
−C
31
C
13
(12.5)
C
23
= −C
11
C
32
+ C
31
C
12
(12.6)
As seen from these equations this DCM is only uniquely deﬁned if the latitude is not
equal to ±90
◦
. This means that this alignment procedure is not possible if the INS is
positioned at the North or South Pole.
12.3 Fine Alignment
Fine alignment uses an indirect Kalman ﬁlter with the result from the coarse alignment
as the initial C
n
b
. At ﬁne alignment completion, the attitude and azimuth heading is
estimated with greater accuracy than possible with coarse alignment. The reason for
this is that the coarse alignment is a one shot alignment procedure which suﬀers from
measurement noise which prevents it from calculating the true value of C
n
b
. However,
the errors in the C
n
b
matrix can be assumed to be small, which enables the use of the
previously derived error equations as this error model uses small angle approximations.
After coarse alignment, the remaining errors in the C
n
b
matrix consists of small roll, pitch
and yaw angles. Roll and pitch errors are also denoted tilt errors, as they correspond
to the platform being not truly level. The yaw error is the deviation of the y axis of the
navigation frame from pointing true North.
During ﬁne alignment, the estimates of roll and pitch errors are used as control resets
to correct the C
n
b
matrix, so the platform is leveled. The yaw error can be calculated
from the horizontal parts of the estimated earth rotation ω
n
ie
. The vertical part of
earth rotation is known analytically as a function of latitude, and does not need to be
estimated. A precise estimate of earth rotation is also needed in order to cancel the
eﬀects 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 quasistationary, any position diﬀerent from 0 will be an error. This
error is used as the driving parameter for the indirect Kalman ﬁlter. The indirect
Kalman ﬁlter then estimates the errors in the rotation matrix from body frame to
navigation frame C
n
b
, error in earth rotation ω
n
ie
, error in velocity v
n
and error in the
diﬀerence in position from the initial position ∆r
n
.
The navigation equations from Sec. 11.1 is restated below.
˙
C
n
b
= C
n
b
Ω
b
ib
−(Ω
n
ie
+Ω
n
en
)C
n
b
ω
n
ie
= C
n
e
ω
e
ie
ω
n
en
=
1
R
(u
n
zn
×v
n
) + ρ
R
u
n
zn
˙ v
n
= C
n
b
a
n
+g
n
−(ω
n
en
+ 2ω
n
ie
) ×v
n
˙
C
e
n
= C
e
n
Ω
n
en
˙
h = v
n
· u
n
zn
(12.7)
As ﬁne alignment is performed when the INS is in a quasistationary situation the
67 CHAPTER 12. ALIGNMENT
navigation equations can be simpliﬁed as both ω
n
en
and v
n
are zero. Applying these
simpliﬁcations gives the following simpliﬁed navigation equations:
˙
C
n
b
= C
n
b
Ω
b
ib
−Ω
n
ie
C
n
b
ω
n
ie
= C
n
e
ω
e
ie
˙ v
n
H
= (C
n
b
a
n
)
H
∆˙ r
n
H
= v
n
H
(12.8)
where
H
indicate the horizontal parts and ∆r
n
H
is the position divergence deﬁned as the
diﬀerence in position from the initial position.
Equation (12.8) is called the foreground equation as it is not within the Kalman ﬁlter.
This equation is the mapping from inertial measurements to velocities and positions
which is called ”INS System” in Fig. 10.1. The quantities calculated in this foreground
equation is not to be mistaken for the error states estimated within the Kalman ﬁlter.
In order to level the body frame analytically C
n
b
is torqued so that no components are
present in ˙ v
n
H
, which correspond to horizontal tilt of C
n
b
. This means that the INS
is torqued only in the horizontal plane but with no rotation around the vertical axis.
Furthermore, in order to bring C
n
b
to steady state, the estimated earth rotation ω
n
ie
needs to correspond to the measurements from the gyroscope Ω
b
ib
= Ω
b
ie
.
As mentioned before, the driving parameter for the Kalman ﬁlter is the position diver
gence vector ∆r
n
H
. This vector is a measure of the attitude error Ψ
n
and estimation
error of ω
n
ie
and can thus be used to estimate these two parameters. As any tilt from
horizontal will give rise to a component of the earth gravitation in the horizontal plane,
any tilt error will thus lead to a nonzero ∆r
n
H
. Furthermore, an estimation error of
ω
n
ie
will tilt the INS from the correct leveled attitude and thus again lead to a nonzero
∆r
n
H
.
Using the approach described in Ch.10, the Kalman ﬁlter is used not only to estimate the
error states, but also to correct errors in the INS states. As mentioned two parameters
needs to be corrected in order to bring C
n
b
in steady state. First of all, C
n
b
needs to
be leveled in order to cancel out the gravitation force in the horizontal plane. This
is performed by introducing a quantity called ω
n
tilt
used to correct C
n
b
. Secondly, the
earth rotation rate needs to be estimated. The earth rate is divided into horizontal and
vertical parts as the vertical part is known due the knowledge of the latitude of the INS.
Both ω
n
ie
and ω
n
tilt
are directly connected to ∆r
n
H
as any error in ∆r
n
H
is either due to an
attitude error, estimation error of ω
n
ie
or both. Last, the Kalman ﬁlter is allowed to reset
the errors in the last two INS states, v
n
and ∆r
n
H
. In order to interlink ∆r
n
H
with the four
parameters which are to be corrected, the Kalman gains are introduced. The Kalman
gains describe how ∆r
n
H
is correlated with the system states. Four Kalman gains are
needed to correct the four parameters. Kalman gains k
1
and k
2
are used to estimate
ω
n
ie
and ω
n
tilt
and k
3
and k
4
are used to estimate velocity v
n
and position divergence
∆r
n
H
. The navigation equations (12.8) are now expanded into the form utilized during
ﬁne alignment:
˙
C
n
b
= C
n
b
Ω
b
ib
−(ω
n
ie
+ω
n
tilt
)C
n
b
ω
n
tilt
= k
2
u
n
zn
×∆r
n
H
ω
n
ie
= ω
e
ieH
+u
n
zn
ω
ie
sin l
˙ ω
n
ie
H
= k
1
u
n
zn
×∆r
n
H
˙ v
n
H
= (C
n
b
a
n
)
H
−k
3
∆r
n
H
∆˙ r
n
H
= v
n
H
−k
4
∆r
n
H
(12.9)
12.3. FINE ALIGNMENT 68
where
u
n
zn
= Unit vector along the nframe vertical axis (z)
k
1
, k
2
, k
3
, k
4
= Kalman gains
ω
n
tilt
= The angular rate feedback used to correct the attitude error associated with C
n
b
The cross product with u
n
zn
has been introduced in order to assign the correct sign and
gain to the correction. It can be realized by inspecting Fig. 12.1. In ﬁgure Fig. 12.1(a)
the body has not been rotated and in Fig. 12.1(b) the body has been rotated positive
around x.
z
y
x
z
x
y
g=9.82
a b
Figure 12.1: Illustration of the accelerometer measurements due to an rotation of
the body.
As can be seen in Fig. 12.1 if the frame is rotated positive about the xaxis then the
accelerometer with sensitivity axis y will measure a positive acceleration. This means
that an positive acceleration on the yaxis should result in an negative rotation about
the xaxis. Looking at the vector product this property will be revealed.
u
n
zn
= [0 0 1]
T
u
n
zn
× [x y z]
T
= [y x 0]
T
(12.10)
A block diagram of the ﬁne alignment procedure can be seen in Fig. 12.2.
Figure 12.2 shows the ﬁne alignment scheme. As can be seen the input to the INS
system is the measured accelerations and rotation rates. The accelerations are double
integrated in order to get the divergence position which is the ﬁlter input. This ﬁlter
input is multiplied with the Kalman gains in order to get estimates of the errors states.
These error states are then used to correct the states in the INS system.
Having established the initial understanding of ﬁne alignment a further description of
the derivation of the Kalman gains is done.
69 CHAPTER 12. ALIGNMENT
b
n ib
Cb
b
ib
n nn
n
C
b
n
n
C
b
n n n
C
b
n
a
b
v
n
H
n
(C)
b H
a
b
b
v
n
H
FilterInput
1
s
1
s
1
s
r
n
H
K
1
K
3
K
4
u
zn
n
n
tilt
ie
K
2
Kalman
Filter
Verticalpart
of
n
ie
n
ie
H
INSSystem
v
n
H
n
H
r
n
tilt
) (
Figure 12.2: Illustration of the ﬁne alignment procedure.
12.4. INDIRECT KALMAN FILTER APPLIED TO QUASISTATIONARY FINE
ALIGNMENT 70
12.4 Indirect Kalman Filter Applied to QuasiStationary
Fine Alignment
In order to describe ﬁne alignment within the Kalman ﬁlter framework as described in
Ch. 10 the observation set needs to be deﬁned. As there is no aiding apart from the
knowledge of the quasistationarity, the observation set is:
z
obs
= ∆r
n
H
−∆r
n
refH
(12.11)
where ∆r
n
ref
H
is the reference position for the position divergence which is approximated
to zero due to quasistationarity.
As mentioned earlier, ﬁne alignment uses an indirect Kalman ﬁlter to estimate the error
states which means that the model used by the Kalman ﬁlter should be the error model
which is derived in Ch. 11. The error model used when deriving the Kalman gains for
ﬁne alignment is the model from (11.27). This error model is restated below:
˙ γ
n
= γ
n
×(ω
n
ie
+ω
n
en
) −C
n
b
δω
b
ib
+ δω
n
ie
+ δω
n
en
δ ˙ v
n
= −γ
n
×a
n
+C
n
b
δa
b
−(2δω
n
ie
+ δω
n
en
) ×v
n
−(ω
n
en
−2ǫ
n
×ω
n
ie
) ×δv
n
+
2g
R
δhu
n
zn
+ δg
n
M
˙ ǫ
n
= ǫ
n
×ω
n
en
+ δω
n
en
δω
n
en
=
1
R
(u
n
zn
×δv
n
) −
δh
R
2
(u
n
zn
×v
n
) + δρ
R
u
n
zn
δ
˙
h = δC
n
e
ω
e
ie
(12.12)
This error model is simpliﬁed as certain parameters are not relevant when performing
alignment. Using the fact that the platform is quasistationary zeroes the velocity v
n
and the transport rate ω
n
en
giving the following expression:
˙ γ
n
= γ
n
×ω
n
ie
−C
n
b
δ˜ ω
ib
b
+ δω
n
ie
δ ˙ v
n
= −γ
n
×a
n
+C
n
b
δa
b
−2δω
n
ie
×δv
n
+
2g
R
δhu
n
R
+ δg
n
M
(12.13)
These equations should be expanded to include the position divergence as this is the
driving parameter of the Kalman ﬁlter. This is easily included by recognizing that the
error in the position divergence vector is:
δ∆˙ r
n
= δv
n
(12.14)
As can be seen from (11.8) when ǫ
n
is constant, the rate of the earth rotation error is
zero:
δ ˙ ω
n
ie
= −
d
dt
(ǫ
n
×ω
e
ie
) = 0 (12.15)
Remembering that it is only the horizontal parameters which is of interest, the error
model for the Kalman ﬁlter is simpliﬁed to only contain the horizontal parts. This is
done as follows:
First of all the quasistationary error model from Sec. 12.4 is repeated below:
71 CHAPTER 12. ALIGNMENT
˙ γ
n
= γ
n
×ω
n
ie
−C
n
b
δ˜ ω
ib
b
+ δω
n
ie
δ ˙ v
n
= −γ
n
× f
n
+C
n
b
δf
b
−2δω
n
ie
×δv
n
+
2g
R
δhu
n
R
+ δg
n
M
(12.16)
Certain assumptions are made when reducing the error model to only containing hori
zontal parts. First of all, the horizontal tilt/earth rate product from the ﬁrst equation
is assumed negligibly small[Savage, 2000]. Secondly, the vertical part of γ
n
is deﬁned
to be zero at the beginning of ﬁne alignment. This can be achieved as the local level
navigation frame n has an arbitrarily chosen heading which can be chosen to coincide
with C
n
b
heading at ﬁne alignment initialization. As the ﬁne alignment is performed
within a short time period, the vertical error in γ
n
does not grow large and thus can be
neglected. Lastly, the earth rate error/velocity error product is also assumed negligibly
small[Savage, 2000]. Furthermore, the last two terms of the second equation can be
removed as they are of no interest when looking at the horizontal parts. With these
assumptions and with the augmentation with the model of δω
n
ie
from Sec. 12.4, the error
model is presented in matrix form:
δ ˙ ω
n
ie
˙ γ
n
δ ˙ v
n
¸
¸
=
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0
0 0 0 0 −f
n
U
f
n
N
0 0 0
0 0 0 f
n
U
0 −f
n
E
0 0 0
0 0 0 −f
n
N
f
n
E
0 0 0 0
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
δω
n
ie
γ
n
δv
n
¸
¸
(12.17)
+
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
−c
11
−c
12
−c
13
0 0 0
−c
21
−c
22
−c
23
0 0 0
−c
31
−c
32
−c
33
0 0 0
0 0 0 c
11
c
12
c
13
0 0 0 c
21
c
22
c
23
0 0 0 c
31
c
32
c
33
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
δ ˜ ω
b
ib
δf
b
ib
(12.18)
This model is reduced to the horizontal version simply by removing the third parameter
of each variable. The horizontal version is presented below:
δ ˙ ω
n
ie
˙ γ
n
δ ˙ v
n
¸
¸
H
=
0 0 0 0 0 0
0 0 0 0 0 0
1 0 0 0 0 0
0 1 0 0 0 0
0 0 0 −f
n
U
0 0
0 0 f
n
U
0 0 0
¸
¸
¸
¸
¸
¸
¸
¸
δω
n
ie
γ
n
δv
n
¸
¸
H
(12.19)
+
0 0 0 0
0 0 0 0
−c
11
−c
12
0 0
−c
21
−c
22
0 0
0 0 c
11
c
12
0 0 c
21
c
22
¸
¸
¸
¸
¸
¸
¸
¸
¸
δ ˜ ω
b
ib
δf
b
ib
H
(12.20)
12.4. INDIRECT KALMAN FILTER APPLIED TO QUASISTATIONARY FINE
ALIGNMENT 72
This yields the following model:
x = [δω
n
ieH
, γ
n
H
, δv
n
H
, δ∆r
n
H
]
n
p
= [δω
b
ib
, δf
b
, 0, 0, 0, 0]
A =
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 0 −g 0 0 0 0
0 0 g 0 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
G
p
=
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
−C
11
−C
12
0 0 0 0 0 0
−C
21
−C
22
0 0 0 0 0 0
0 0 C
11
C
12
0 0 0 0
0 0 C
21
C
22
0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
(12.21)
where:
x = Error states of the system
n
p
= Process noise vector
A = Error states dynamic matrix
G
p
= Process noise dynamic matrix
The measurement equation is the error form of (12.11) in order to bring it to an error
form suited for the indirect Kalman ﬁlter:
z = δz
obs
= δ∆r
n
H
−δ∆r
n
ref
H
(12.22)
where
δ∆r
n
H
= The position divergence error vector
δ∆r
n
refH
= Is the error due to the assumption of ∆r
n
refH
equal to zero
If δ∆r
n
ref
H
is deﬁned to be the diﬀerence between the true value and the value used by
the INS:
δ∆r
n
ref
H
= ∆r
n
INS
H
−∆r
n
true
H
(12.23)
where
∆r
n
INSH
= 0
The true value can be recognized as the vibrating part of the position. This means that
δ∆r
n
ref
H
can be written as:
δ∆r
n
ref
H
= −∆r
n
vib
H
(12.24)
73 CHAPTER 12. ALIGNMENT
where
∆r
n
vib
H
= The horizontal position error due to the quasistationary situation of the INS
With these deﬁnitions, the measurement equation is written as:
z = δ∆r
n
H
+ ∆r
n
vib
H
(12.25)
With both the state and measurement equation deﬁned the ﬁrst thing to notice is that
the error state dynamic matrix is constant which enables an oﬄine calculation. The
discrete version is calculated in matlab and shown here:
Φ
n
=
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
T
n
0 1 0 0 0 0 0
0 T
n
0 1 0 0 0 0
0 −
g
2
T
2
n
0 −gT
n
1 0 0 0
g
2
T
2
n
0 gT
n
0 0 1 0 0
0 −
g
6
T
3
n
0 −
g
2
T
2
n
T
n
0 1 0
g
6
T
3
n
0 gT
2
n
T
n
0 0 0 1
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
(12.26)
where:
Φ
n
= Discrete version of the error state dynamic matrix
T
n
= Sampling time
The next thing to calculate is the integrated process noise matrix Q
n
which is used
in the covariance equations. From [Savage, 2000] a second order approximation matrix
can be found to be:
Q
1n
= G
p
Q
pDens
G
T
p
T
n
Q
n
≈
1
2
Q
1n
+
1
2
Φ
n
Q
1n
Φ
T
n
(12.27)
where:
Q
1n
= A ﬁrst order approximation of the integrated process noise matrix
Q
pDens
= The process noise density associated with the process noise vector n
p
It is assumed that the densities for the sensor errors in n
p
are equal and the process
noise density is deﬁned as:
Q
pDens
=
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 q
ω
0 0 0 0 0
0 0 0 q
ω
0 0 0 0
0 0 0 0 q
f
0 0 0
0 0 0 0 0 q
f
0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
(12.28)
First of all Q
1n
is calculated:
Q
1n
=
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 q
ω
0 0 0 0 0
0 0 0 q
ω
0 0 0 0
0 0 0 0 q
f
0 0 0
0 0 0 0 0 q
f
0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
T
n
(12.29)
12.4. INDIRECT KALMAN FILTER APPLIED TO QUASISTATIONARY FINE
ALIGNMENT 74
The reason for Q
1n
being equal to
Q
pDens
Tn
is due to the orthogonality principle of C
n
b
where C
n
b
C
n
b
T
= I. Due to this property the integrated process noise is also constant
and can therefore be calculated oﬄine.
The ﬁnal task is to construct the output equations. Using the deﬁnition of the measure
ment equation in Sec. 10.1, (12.25) and (12.21) the measurement matrix, measurement
noise vector and measurement dynamic noise matrix is as follows:
H
n
= [0 0 0 0 0 0 I
2x2
]
G
mn
= I
2x2
n
mn
= ∆r
n
vib
H
If we deﬁne the elements of n
mn
to have equal and uncorrelated variance the measure
ment noise covariance matrix can be written as follows [Savage, 2000]:
R
n
= E(n
mn
n
T
mn
) = p
r
vib
H
I
2x2
(12.30)
where
p
r
vib
H
= Horizontal quasistationary position random motion variance
E = Is the expected value operator (12.31)
Again it should be noted that both H
n
, G
mn
and R
n
are independent of time, attitude
and position.
With both the state and measurement equations established, the Kalman ﬁlter gain and
error covariance matrices can be calculated according to the following general Kalman
ﬁlter equations:
P
n
= Φ
n
P
n1
(+)Φ
T
n
+Q
n
K
n
= P
n
(−)H
T
n
H
n
P
n
(−)H
T
n
+G
mn
R
n
G
T
mn
−1
(12.32)
P
n
(+) = (I −K
n
H
n
)P
n
(−)
The Kalman gains are repetitively calculated allowing an estimation of the states and a
control reset in order to bound the error build up. As mentioned in Sec. 10.1 the ideal
reset is:
u
nc
= −ˆ x
n
(−
e
) (12.33)
Using this reset the discrete Kalman equations from (10.7) is as follows:
x
n
(+
c
) = ˆ x
n
(+
e
) + u
n
= 0
ˆ x
n
(−) = Φ
n
ˆ x
n1
(+
c
) = 0
ˆz
n
= H
n
ˆ x
n
(−) = 0
ˆ x
n
(+
e
) = ˆ x
n
(−) +K
n
(z
obsn
− ˆ z
n
) = K
n
z
obsn
(12.34)
From (12.11) the observation is:
z
obsn
= ∆r
n
H
(12.35)
This leads to the calculation of the control vector:
u
nc
= −K
n
∆r
n
H
(12.36)
75 CHAPTER 12. ALIGNMENT
Equation (12.36) using K
n
from (12.32) constitute the complete Kalman ﬁlter and
the last thing to develop is the method to apply the control vector to the foreground
equations from (12.9). This is done by deﬁning the individual parts of the control vector:
u
c
= [δω
n
ie
Hc
, γ
n
Hc
, δv
n
Hc
, δ∆r
n
Hc
]
T
(12.37)
where
c
= Designation for the u
c
component which are to be applied
to the foreground equations (12.38)
The last thing to determine is how to correct the INS states with the control vector
from (12.37).
As the control vector contains the errors of the INS states it is obvious to simply subtract
the control vector from the INS state to get the correct state. Subtracting the control
vector from the earth rotation, velocity and position is straightforward, as these error
states are directly accessible from the control vector. Applying horizontal attitude error
γ
n
H
to C
n
b
is performed as follows. As deﬁned in (11.6) errors in the rotation matrix C
n
b
is:
δC
n
b
= −Γ
n
C
n
b
(12.39)
This error can also be interpreted as the correction to C
n
b
(−) why the corrected C
n
b
(+
c
)
is:
C
n
b
(+
c
) = C
n
b
(−) + δC
n
b
C
n
b
(+
c
) = (I −Γ
n
H
)C
n
b
(−) (12.40)
With this correction to the rotation matrix the four INS states are corrected in the
following way:
ω
n
ieHn
(+
c
) = ω
n
ieHn
(−) + δω
n
ieHcn
C
n
b
n
(+
c
) = [I
2x2
−Γ
n
Hcn
]C
n
b
n
(−)
v
n
Hn
(+
c
) = v
n
Hn
(−) + δv
n
Hnc
∆r
n
Hn
(+
c
) = ∆r
n
Hn
(−) + δ∆r
n
Hnc
(12.41)
To sum up, the ﬁne alignment process consist of integrating the foreground equations,
(12.8), while applying the control vector, (12.37), which has been calculated by the use
of the Kalman gain (12.32).
12.5 Veriﬁcation of Alignment
As the coarse and ﬁne alignment has been developed, they are to be veriﬁed. Veriﬁcation
of alignment is divided into two steps which are described below.
Step One:
In step one, the alignment schemes are veriﬁed by manually inputting acceleration and
gyroscopic measurements which would be measured by gyroscopes and accelerometers
at a speciﬁc and known orientation in space. This way the coarse alignment should make
a perfect one shot alignment. Secondly to verify the ﬁne alignment the same inputs are
used while selecting the initial rotation matrix C
n
b
with a small error. This resembles a
real situation where the coarse alignment estimates C
n
b
with errors. The ﬁne alignment
12.5. VERIFICATION OF ALIGNMENT 76
ﬁlter should correct C
n
b
and at ﬁne alignment completion it should once again resemble
the perfect rotation matrix from coarse alignment.
Step Two:
In step two, the alignment scheme developed in this report is tested against an already
existing INS system. The system tested against is a Kearfott INS system. With this
INS it is possible to get both the roll, pitch and yaw angles after it has performed
coarse and ﬁne alignment while accessing the raw measurements from the accelerometer
and gyroscope. This way it is possible to compare the roll, pitch and yaw angles with
and already existing system. Although it is not possible to distinguish which of the two
systems is the most correct, it is possible to compare the results and indicate the system
developed in this thesis does work correctly.
12.5.1 Step One
Coarse Alignment:
The coarse alignment calculates the rotation matrix C
n
b
and by inputting accelerometer
and gyroscope measurement which resembles a leveled INS pointing north, C
n
b
should
be unity. This is of course a special case with lots of zeros in the solution so several
other rotations of the body has also been tested. The special case is with accelerometer
input [0 0 − g]
T
and gyroscopic input [0 ω
ie
cos(l) −ω
ie
sin(l)] where the platform is
positioned level on the earth at latitude l. C
n
b
should be unity as the body frame is
coincident with the nframe and a test showed that the coarse alignment worked. Testing
with other rotations of the body frame showed that the coarse alignment worked.
Fine Alignment:
With the same acceleration and gyroscopic inputs as with the special case under coarse
alignment, but with an initial error in C
n
b
, ﬁne alignment should decrease this error and
estimate the C
n
b
with greater accuracy. The error in the initial C
n
b
has been chosen to
be [0.5
◦
, 0.7
◦
, −0.4
◦
] in [roll, pitch, yaw].
As mentioned in Sec. 12.4 the resets and torquing signals to correct C
n
b
is driven by the
error in the horizontal position. This error should be driven to zero as C
n
b
and earth
rotation ω
n
ie
is estimated more and more precisely. This means that the acceleration
and position should be driven to zero. Due to the errors in the initial C
n
b
, horizontal
accelerations will be present due to the gravitationally force. It will be expected to
give a positive acceleration to the north and a negative acceleration to the east due to
the misalignment in 0.5
◦
, and 0.7
◦
in roll and pitch error. As can be seen in Fig. 12.3
the initial north acceleration due to the gravitation projected onto the x axis can be
calculated as: sin(0.5
◦
) · g = .086 which coincides with the graph. The same goes for
the east acceleration. It can also be seen that it converges to zero an expected.
The same characteristics can be seen by investigating Fig. 12.4. The position starts out
being zero, as it is the best guess to where the platform is positioned. By due to the
misalignment, the position changes until a correct estimate of C
n
b
and ω
n
ie
is given, at
which point the position is zero. Due to the fact that the closer the position gets to
zero the smaller the torquing signal, it will converge towards zero slower and slower. It
is again seen that the north position starts out going positive north and negative east
due to the components of gravity.
The next thing to verify is that the rotation matrix C
n
b
converges to the expected value.
With the same accelerometer and gyroscopic measurement, the expected rotation would
be unity with a yaw angle error. This originate from the fact that ﬁne alignment only
torque horizontally to level the platform. Simultaneously it estimates the earth rotation
which is used to estimate the yaw angle.
77 CHAPTER 12. ALIGNMENT
0 0.2 0.4 0.6 0.8 1
−0.15
−0.1
−0.05
0
0.05
0.1
Time [min]
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
East Acceleration
North Acceleration
Figure 12.3: Acceleration due to misalignment
0 0.2 0.4 0.6 0.8 1
−6
−5
−4
−3
−2
−1
0
1
2
3
4
x 10
−4
Time [min]
P
o
s
i
t
i
o
n
[
m
]
East Position
North Position
Figure 12.4: Position divergence due to misalignment
12.5. VERIFICATION OF ALIGNMENT 78
0 0.2 0.4 0.6 0.8 1
−0.5
0
0.5
1
E
r
r
o
r
[
°
]
Roll Error
0 0.2 0.4 0.6 0.8 1
−0.5
0
0.5
1
E
r
r
o
r
[
°
]
Pitch Error
0 0.2 0.4 0.6 0.8 1
−0.5
0
0.5
1
Time [min]
E
r
r
o
r
[
°
]
Yaw Error
Figure 12.5: Euler rotation associated with C
n
b
The Euler rotation associated with C
n
b
is shown in Fig.12.5. As can be seen, the rotation
around x (roll), y (pitch) and z (yaw) starts out being the initial errors in the C
n
b
. The
ﬁne alignment then levels the platform by torquing the horizontal parts to zero. As can
be seen in Fig. 12.5 only the roll and pitch rotation is converging to zero whilst the yaw
error is not torqued.
The expected value of the horizontal earth rotation rate is to be mainly on the y axis
due to the small yaw angle of −0.4
◦
but with a small component on the x axis. So
at latitude 57.19
◦
, which is the latitude at the test site at Aberdeen, a rotation of
cos(57.19) · ω
ie
= 3.95e−5 is expected on the y axis and much smaller on the x axis. As
can be seen in Fig. 12.6 ω
n
ie
starts out being initialized as zero. But as soon as there is
an error in the position divergence vector the horizontal earth rate is changed.
A closer inspection, which can be seen in Fig. 12.7, reveals that the rotation around y
converges to around 3.9e5 and small rotation approximately zero around x.
In order to take the misalignment around z into account the yaw angle needs to be
calculated. It is expected that the yaw angle converges towards the yaw error in C
n
b
.
As can be seen in Fig. 12.8 the yaw angle is stationary at −0.4
◦
.
Step Two:
Step two shows the result from the test against an already existing INS system. The
Kearfott system was positioned on a table and completed a 15min ﬁne alignment. This
was compared to a 15min ﬁne alignment with the equations developed in this thesis.
The results are compared directly in Tab. 12.1:
As can be seen the Kearfott and the ﬁne alignment used in this thesis estimate C
n
b
to
somewhat the same values. Although the diﬀerences are in tenth of a degree, which is
unacceptable for an actual usage, they cannot be compared directly as there is some
uncertainty to how the Kearfott data should be interpreted. But the results imply that
the ﬁne alignment developed does function with realworld measurements. To test if the
79 CHAPTER 12. ALIGNMENT
0 0.2 0.4 0.6 0.8 1
−3
−2.5
−2
−1.5
−1
−0.5
0
0.5
1
x 10
−3
Time [min]
A
n
g
u
l
a
r
R
a
t
e
[
r
a
d
/
s
]
Earth Rotation Rate Around X
Earth Rotation Rate Around Y
Figure 12.6: Estimate of the earth rotation rate
0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1
0
2
4
6
8
10
x 10
−5
Time [min]
A
n
g
u
l
a
r
R
a
t
e
[
r
a
d
/
s
]
Earth Rotation Rate Around X
Earth Rotation Rate Around Y
Figure 12.7: Estimate of the earth rotation rate
12.5. VERIFICATION OF ALIGNMENT 80
0 2 4 6 8 10
−100
−50
0
50
100
Y
a
w
A
n
g
l
e
[
°
]
2 3 4 5 6 7 8 9 10
−1
−0.5
0
Y
a
w
A
n
g
l
e
[
°
]
Time [min]
Figure 12.8: Wander angle at ﬁne alignment completion
Kearfott This report
Role 81.2219
◦
81.3166
◦
Pitch 12.04109
◦
12.2752
◦
Yaw 8.0859
◦
8.0990
◦
Table 12.1: Comparison of a 15min ﬁne alignment
estimated C
n
b
is a good estimate the accelerometer data is rotated through C
n
b
in order
to verify that C
n
b
actually levels the INS analytically. If the INS is leveled, then the
rotated accelerations measured in body frame should be [0 0 9.8171]
T
as the nframe
is leveled. The result are seen in Fig. 12.9. As can be seen from Fig. 12.9 the rotated
acceleration with x and y sensitive axes is located around zero with the z sensitive axis
around 9.8. A mean of the values from time 5min to 15min is presented below. The
reason for not including the time before 5min is because ﬁne alignment needs some time
to estimate C
n
b
. The results are seen in Fig. 12.2. The gravity in Aberdeen, where the
f
n
x
2.55347332e05
f
n
y
6.11895069e04
f
n
z
9.81699624
Table 12.2: Measured acceleration rotated into the nframe
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 zdirection.
And this component is very close to the theoretical. This means that C
n
b
does actually
level the INS.
81 CHAPTER 12. ALIGNMENT
0 2 4 6 8 10
−0.2
0
0.2
a
x n
0 2 4 6 8 10
−0.2
0
0.2
a
y n
0 2 4 6 8 10
9
9.5
10
10.5
a
z n
Time [min]
Figure 12.9: Accelerations rotated into nframe
12.6 Discussion
As has been shown in the veriﬁcation 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 inﬁnite convergence rate as the Kalman gains can be inﬁnitely 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 realworld 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 diﬀerence on the theoretical gravity in Aberdeen and the one measured is
approximately 0.1mg. This diﬀerence stems from diﬀerent sources. The alignment is of
course nor perfect but also the theoretical gravity, calculated from (6.9), does not take
the local gravity anomalies into account. Looking at magnitude of the error reveals that
the error in the vertical gravity has the same magnitude as the errors in the horizontal
plane. This indicate that it stem from noise and ﬁne alignment precision.
As with the navigation equations, it is interesting to investigate what might inﬂuence
alignment. Two diﬀerent error sources are dominant in the alignment phase: Sensor
errors and errors due to a nonquasistationary state when assuming quasistationary.
First, it is investigated what eﬀects arise from nonquasistationarity. In practice, two
movements diﬀers from the quasistationary 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 eﬀect is present which is due to the moving navigation frame.
This eﬀect causes accelerations on the horizontal axis.
The earth simulation model from Ch. 8 has been used to simulate the accelerations and
rotation rates sensed by the sensors when moving with a horizontal speed of 10m/s both
North and East. The sensor outputs are showed in Fig. 12.10.
0 5 10 15
−6.4145
−6.414
−6.4135
−6.413
x 10
−6
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
East Axis
0 5 10 15
3.4747
3.4748
3.4749
3.475
x 10
−6
North Axis
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
0 5 10 15
−2.4
−2.2
−2
−1.8
x 10
−6
Up Axis
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
Time [min]
0 5 10 15
−8.5
−8.4
−8.3
−8.2
x 10
−9
R
o
r
a
t
i
o
n
R
a
t
e
[
/
s
]
East Axis
0 5 10 15
2.8
3
3.2
3.4
x 10
−9
North Axis
R
o
r
a
t
i
o
n
R
a
t
e
[
/
s
]
0 5 10 15
6
7
8
9
10
x 10
−10
Time [min]
R
o
r
a
t
i
o
n
R
a
t
e
[
/
s
]
Up Axis
Figure 12.10: Errors in accelerations and rotation rates due to horizontal velocity
when assuming the INS to be stationary. All sensors outputs are in body frame.
Figure 12.10 shows the errors in both acceleration and rotation rate. As expected
there are accelerations errors on both horizontal and vertical axes. The errors in the
horizontal axes are due to the Coriolis eﬀect 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 NorthEast.
In order to examine the alignment errors due to this horizontal velocity alignment has
83 CHAPTER 12. ALIGNMENT
been performed with both quasistationary and nonquasistationary 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:
Alignment Error Magnitude [
◦
]
Heading 46 · 10
−6
Pitch 5 · 10
−6
Roll 6 · 10
−6
Table 12.4: Comparison of alignment error when aligning with a yaw rotation.
As can be seen from Tab.12.4, the error in both heading and attitude are in millidegrees.
What is interesting to see, is that the accelerations and angular rates arising from the
nonquasistationarity are in the order of 10
−6
[m/s
2
] and 10
−9
[rad/s] for nonzero
velocity. Highgrade inertial sensors today experience bias stabilities of 10
−3
[m/s
2
] and
10
−8
[rad/s]. Hence, it can be concluded, that sensor errors have a larger inﬂuence on
12.6. DISCUSSION 84
0 5 10 15
−1
−0.5
0
0.5
1
x 10
−6
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
East Axis
0 5 10 15
−1
−0.5
0
0.5
1
x 10
−6
North Axis
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
0 5 10 15
−1
−0.5
0
0.5
1
x 10
−6
Up Axis
A
c
c
e
l
e
r
a
t
i
o
n
[
m
/
s
2
]
Time [min]
0 5 10 15
0
1
2
3
4
x 10
−6
R
o
r
a
t
i
o
n
R
a
t
e
[
/
s
]
East Axis
0 5 10 15
−1.5
−1
−0.5
0
x 10
−7
North Axis
R
o
r
a
t
i
o
n
R
a
t
e
[
/
s
]
0 5 10 15
8.7267
8.7267
8.7267
8.7267
x 10
−5
Time [min]
R
o
r
a
t
i
o
n
R
a
t
e
[
/
s
]
Up Axis
Figure 12.11: Errors in accelerations and rotation rate due to rotation around yaw
when assuming the INS to be stationary. All sensors outputs are in body frame.
85 CHAPTER 12. ALIGNMENT
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:
Alignment Error Magnitude [
◦
]
Heading 71 · 10
−3
Pitch 5.1 · 10
−6
Roll 7.9 · 10
−6
Table 12.5: Comparison of alignment error when aligning with a gyro bias of 0.01
[
◦
/hour].
What can be derived from these test is that gyro bias causes larger alignment errors
than horizontal velocity. And vertical rotation does only inﬂuence alignment marginal.
12.7 Summary
During this chapter, has the alignment procedure been developed and been tested with
simulated and realworld measurements.
The alignment procedure is divided into two phases. First, the coarse alignment is
performed which is a one shot approach where the gravity and earth rotation of a
known frame is compared to the acceleration and gyroscope measurement. In order
to get a better estimate of C
n
b
, the ﬁne alignment procedure is performed afterwards.
Fine alignment uses an indirect Kalman ﬁlter in order to estimate attitude and heading
of the INS. In order to perform a precise ﬁne alignment, the INS is assumed to be in
quasistationary state. Quasistationary means that over time the position is zero. This
knowledge along with the knowledge of the sensor errors is used with the Kalman ﬁlter
to estimate attitude and heading of the INS.
The ﬁne alignment procedure has been tested with both simulated inputs and with real
world sensor measurements. Both tests showed that the alignment procedure worked
and ﬁne alignment was able to decrease the error in the one shot coarse alignment
estimate.
Chapter 13
Inertial Navigation with Aiding
Unaided INS navigation inevitably suﬀer 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 diﬀerent
forms of aiding can be used giving various results. Some of the more popular aiding
devices are Doppler Velocity Logs (DVL), Global Positioning System (GPS) and baro
metric measurements. The main idea is that with independent information at hand the
Kalman ﬁlter is able to estimate with greater accuracy. This report will describe the
use of two diﬀerent 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
diﬀerent ways to implement GPS with an INS system. An error model of the GPS will
be derived leading to an error model of the INS/GPS system ready for Kalman ﬁltering.
Secondly, the same procedure will be followed describing how to aid with a speedometer.
This chapter will end with a description of the approach used in this report to aid the
INS with a GPS.
13.1 Aiding with GPS
The idea behind aiding with GPS originate from the fact that although the two systems
produce the same outputs, position and velocity, they measure diﬀerent quantities and
suﬀer from diﬀerent 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 longtime 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 diﬀerent error characteristics makes for a beneﬁcial 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 oﬀset 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 speciﬁc time.
As the clock in the receiver is not synchronized with the satellite clock this clock oﬀset
also needs to be determined. For this reason, four satellites are needed to determine the
position.
A general GPS scheme is illustrated below.
Antennaand
RFHardware
Code
Correlation
Codetracking
Loop
Carrier
TrackingLoop
DeltaRange
PseudoRange
Figure 13.1: Illustration of a simpliﬁed GPS receiver scheme
[Brown and Hwang, 1997]
The ﬁrst block of Fig. 13.1 is the antenna and RF hardware which is used to receive the
GPS signals. As all the satellites transmits on the same frequency the received signals
are a mix up of all the visible satellites signals. The code correlation algorithm extracts
the signal from the correct GPS which is then sent to the code and carrier tracking loop.
The code tracking loop is the algorithm which actually estimates the pseudo range. The
pseudo range is the designation for the time the signal has traveled from the diﬀerent
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
eﬀect.
As mentioned earlier, in order to calculate the position of the receiver at least four
satellites are needed. With these four satellites, the position is estimated by solving the
following system[Brown and Hwang, 1997]:
σ
1
=
(x
sat1
−x
gps
)
2
+ (y
sat1
−y
gps
)
2
+ (z
sat1
−z
gps
)
2
+ c∆t
σ
2
=
(x
sat2
−x
gps
)
2
+ (y
sat2
−y
gps
)
2
+ (z
sat2
−z
gps
)
2
+ c∆t
σ
3
=
(x
sat3
−x
gps
)
2
+ (y
sat3
−y
gps
)
2
+ (z
sat3
−z
gps
)
2
+ c∆t
σ
4
=
(x
sat4
−x
gps
)
2
+ (y
sat4
−y
gps
)
2
+ (z
sat4
−z
gps
)
2
+ c∆t (13.1)
where
σ
i
= Pseudorange from the i’th satellite to the GPS receiver antenna.
x
sati
, y
sati
, z
sati
= Position of the i’th satellite
x
gps
, y
gps
, z
gps
= Position of the GPS receiver antenna
c = Speed of light
∆t = Oﬀset 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 oﬀset. 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 diﬀerent approaches. Uncoupled, loosely
coupled and tightly coupled aiding. A short introduction will describe the advantages
and drawbacks of the diﬀerent 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 simpliﬁed representation of this integration is seen below:
As can be seen in Fig. 13.2, the output from the GPS algorithms and INS measurements
are used as the input to the Kalman integration ﬁlter. The main advantages of this
89 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
AntennaandRF
hardware
Correlationand
trackingloops
GPSKalmanfilter
Inertialsensors
INScomputation
INS/GPSKalman
filterintegration
GPSreceiver
INS
GPSpositionand
velocityestimates
INSpositionand
velocityestimates
Errorestimations
Position
Figure 13.2: Lossely coupled INS/GPS approach
approach is that it can be implemented with any existing INS system and with any
GPS receiver. Furthermore, both systems will function separately although one of the
systems fails although this requires some kind of error detection. As can also be seen
the GPS receiver is also aided with INS states. This is due to the fact that the position
is used in the code tracking loop so the corrected position can be used to estimate the
receiver clock error within the GPS receiver.
The main disadvantage with the loosely coupled integration is the fact that this in
tegration is a cascade of two Kalman ﬁlters. As the output from the GPS Kalman
ﬁlter is used as a measurement this is assumed to have white noise by the integration
Kalman ﬁlter. This is not necessary true as the output from the GPS may be time
correlated. This can happen if the integration ﬁlter samples data faster than the GPS
can supply uncorrelated data. It can also happen due to multipath interference which
happens when the same satellite signal is received several times due to reﬂections. Yet
another problem is the fact that at least four satellites are needed to get position and
velocity output from the GPS. In situations where less than four satellites are available,
aiding is not possible. Furthermore is it a problem that the integration ﬁlter requires
knowledge of the covariance matrix of the GPS ﬁlter output. This covariance matrix
diﬀer with satellite geometry and most GPS receivers does not give this information.
[Titterton and Weston, 2005].
13.1.4 Tightly Coupled
The tightly coupled aiding approach is shown in Fig. 13.3.
AntennaandRF
hardware
Correlationand
trackingloops
Inertialsensors
INScomputation
INS/GPSKalman
filterintegration
GPSreceiver
INS
INSpseudorangeand
deltarangeestimates
Errorestimations
Position
GPSpseudorangeand
deltarangeestimates
Figure 13.3: Tightly coupled INS/GPS approach
As can be seen in the ﬁgure, it is now the pseudorange and deltarange which are
used in the integration ﬁlter. These quantities are the outputs from the GPS tracking
loops and can be used to model errors in the INS system. As with the loosely coupled
13.2. GPS ERROR MODEL ANALYSIS 90
integration, the GPS tracking loops are aided with the INS states in the same manner
as with the loosely coupled approach. The advantages of this approach is as follows:
• The statistical problem with the output from the loosely coupled approach is
eliminated by integrating the two Kalman ﬁlters in one.
• This approach will function although four satellites are not visible to the GPS
receiver though accuracy will decrease drastically.
As can be seen from Fig. 13.3 the GPS position and velocity is not present. These
estimates can be produced in a separate loop and error detection may be performed on
these estimates.
This approach is preferable to the loosely coupled approach but the implementation is
more complicated and the pseudorange and deltarange 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:
z
obsi
= ˆ p
i
− ˜ p
GPSi
(13.2)
The GPS range is calculated using the measured time interval between the time the
satellite sent the signal to the time the receiver received the signal. This can be expressed
as follows:
˜ p
GPSi
= c(
˜
t
reci
−
˜
t
GPSi
) (13.3)
where
˜
t
reci
= The time of the 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
91 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
The measurement equation for the Kalman ﬁlter needs to be in a form suitable for
the ﬁlter which is in error states. For this reason the measurement equation is the
diﬀerenced version of (13.2), z = ˆ z
obs
−z
obs
which is:
z
i
= δp
i
−c(δt
reci
−δt
GPSi
) (13.4)
This is the measurement equation for the Kalman ﬁlter so an expression needs to be
derived for the three quantities δp
i
, δt
reci
, δt
GPSi
The quantity δp
i
is the error in the range from the INS to the i’th satellite. This error
originate from two diﬀerent error sources. First of all, the position estimated by the
INS is errorprone and secondly, the position of the satellite, which is estimated from
the ephemeris data, is errorprone. As the ephemeris data gives the satellite position in
earth ﬁxed coordinate system(Eframe) the range is also deﬁned in the Eframe:
ˆ p
e
i
= ˆr
e
si
−ˆr
e
−
ˆ
l
e
(13.5)
where
ˆr
e
si
= Position vector from the center of the earth to the 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 ﬁnd the range error the magnitude of (13.5) is deﬁned as:
ˆ p
2
i
= (ˆ p
e
i
)
T
ˆ p
e
i
(13.6)
In order to ﬁnd the range error δp
i
the diﬀerential of (13.6) is found:
2ˆ p
i
δˆ p
i
= (δp
e
i
)
T
ˆ p
e
i
+ (ˆ p
e
i
)
T
δp
e
i
= 2(ˆ p
e
i
)
T
δp
e
i
(13.7)
which can be rearranged into:
δp
i
=
(ˆ p
e
i
)
T
δp
e
i
ˆ p
i
(13.8)
δp
e
i
can be substituted with the diﬀerential of (13.5) to give:
δp
i
=
(ˆ p
e
i
)
T
(δr
e
si
−δr
e
−δl
e
)
ˆ p
i
(13.9)
As it can be seen from (13.9), δp
i
is calculated from a vector dot product. As a dot
product is invariant ˆ p
i
can be evaluated in the navigation frame[Savage, 2000]:
δp
i
=
(C
n
e
ˆ p
e
i
)
T
(C
n
e
δr
e
si
−δr
n
−δl
n
)
ˆ p
i
(13.10)
The lever arm error can be expressed as follows:
δl
n
= δ(C
n
b
l
b
) = δC
n
b
l
b
+C
n
b
δl
b
(13.11)
Using (11.6) on (13.11) reformulates it to:
δl
n
= −Γ
g
C
n
b
l
b
+C
n
b
δl
b
= (C
n
b
l
b
) ×γ +C
n
b
δl
b
(13.12)
With these rearrangement δ ˆ p
i
from (13.4) is:
δp
i
=
(C
n
e
ˆ p
e
i
)
T
(C
n
e
δr
e
si
−δr
n
−(C
n
b
l
b
) ×γ +C
n
b
δl
b
)
ˆ p
i
(13.13)
13.2. GPS ERROR MODEL ANALYSIS 92
The last thing to do before implementation with the Kalman ﬁlter is to derive expres
sions for the lever arm error δl
b
, position error of the i’th satellite δˆr
e
si
and the clock
error δt
reci
,δt
reci
.
The true lever arm is modelled as a constant and a ﬂexing term:
l
b
= l
b
const
+l
b
ﬂex
˙
l
b
const
= 0 (13.14)
As the lever arm is assume constant the lever used by the computer is a constant:
ˆ
l
b
= l
b
const
˙
l
b
const
= 0 (13.15)
The lever arm error is the estimated lever arm subtracted from the true lever arm:
δl
b
= δl
b
const
−l
b
ﬂex
δ
˙
l
b
const
= 0 (13.16)
For INS/GPS integration, it is customary to assume that δˆr
e
si
and δt
reci
are zero as
ephemeris data are very accurate and the atomic clock in the satellite is very precise.
The main source of error originate from the receiver clock error. This receiver clock
error is modelled as follows.
A clock suﬀer 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 deﬁned
how a clock is normally constructed. Normally a clock is simply a matter of counting
ticks from a oscillatory source, such as a crystal. So the receiver clock time is given by:
ˆ
t
recn
=
ˆ
t
recn1
+ t
osc0
=
ˆ
t
recn1
+
1
f
osc0
(13.17)
where
ˆ
t
rec
= Clock time
t
osc0
= Nominal time between clock ticks
f
osc0
= Nominal clock frequency (13.18)
Rearranging this clock time equation and dividing with the actual time between clock
ticks ∆t gives:
ˆ
t
recn
−
ˆ
t
recn1
∆t
=
1
f
osc0
∆t
=
1
f
osc0
1
∆t
(13.19)
Recognizing that the lefthand term is the derivative of
ˆ
t
recn
yields:
˙
ˆ
t
rec
=
˜
f
osc
f
osc0
(13.20)
where
˜
f
osc
= 1/∆t = The actual clock frequency
As it is the error in the clock time δr
reci
from (13.4) which is needed this is derived as
the diﬀerential of (13.20) plus a random term [Savage, 2000]:
δ
˙
t
f
=
δf
osc
f
osc0
δt
reci
= δt
f
+ n
t
(13.21)
93 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
where
δt
f
= Is the receiver clock error without the random part due to jitter and quantisation
n
t
= Is the random part of receiver clock error due to jitter and quantisation
δf
osc
= Is the oscillatory part of the receiver clock error
The random part n
t
can be modelled as white noise as it is not correlated from satellite
tosatellite or from cycletocycle. The oscillatory part can be modelled as a random
varying parameter plus a ﬁrst order Markov process[Savage, 2000]:
δf
osc
= δf
osc,rand
+ δf
osc,Mark
δ
˙
f
osc,rand
= n
osc,rand
δ
˙
f
osc,Mark
= −C
osc,Mark
δf
osc,Mark
+ n
osc,Mark
(13.22)
where
δf
osc,rand
, δf
osc,Mark
= Is the random and ﬁrst order Markov process error contribution
C
osc,Mark
, n
osc,Mark
= Is the correlation frequency and white noise error contribution
for the ﬁrst order Markov process
n
osc,rand
= Is the white noise error source for the oscillatory part of the receiver clock error
(13.23)
This section described the derivation of the GPS error model needed when using tightly
coupled integration. The last thing before implementation is to determine the numeric
values of the clock error model. As this involves modeling and testing the crystal of
the speciﬁc 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 diﬀers somewhat from aiding with a position. The lever arm
between the INS and aiding device needs to be introduced as the INS and aiding device
normally does not output velocity in the exact same position. This means that if the
INS rotate the aiding device would output an erroneous velocity if not correct for the
lever arm. So the velocity of the INS measured by the aiding device is the measured
velocity corrected for lever arm velocity in the following way:
v
n
INS,aid
= C
n
b
v
b
ref
+C
n
e
˙
l
e
(13.24)
The observation equation for the Kalman ﬁlter is the INS estimated velocity (ˆ v
n
) minus
the measured velocity (v
n
INS,aid
):
z
obs
= ˆ v
n
−v
n
INS,aid
= ˆ v
n
−
ˆ
C
n
b
ˆ v
b
ref
−
ˆ
C
n
e
˙
ˆ
l
e
(13.25)
The velocity of the lever arm can be expressed as the rotation of the INS multiplied
with the lever arm by which the observation equation is expressed as:
z
obs
= ˆ v
n
−
ˆ
C
n
b
ˆ v
b
ref
−
ˆ
C
n
b
( ˜ ω
b
ib
×
ˆ
l
b
) (13.26)
The associated measurement equation is the diﬀerential of (13.26):
z = δv
n
−δC
n
b
ˆ v
b
ref
−
ˆ
C
n
b
δv
b
ref
−
ˆ
C
n
b
( ˜ ω
b
ib
×δl
b
) (13.27)
13.3. IMPLEMENTATION OF NAVIGATION WITH AIDING 94
where δC
n
b
( ˜ ω
b
ib
× l
b
) and C
n
b
(δ ˜ ω
b
ib
× l
b
) have been neglected as they are assumed very
small [Savage, 2000]. Using (11.6) and the lever arm error model from (13.16) on (13.27)
reveals:
z =δv
n
+ (γ
n
×C
n
b
)ˆ v
b
ref
−
ˆ
C
n
b
δv
b
ref
−
ˆ
C
n
b
( ˜ ω
b
ib
×(δl
b
const
−l
b
ﬂex
))
δv
n
− ˆ v
n
ref
×γ
n
−
ˆ
C
n
b
δv
b
ref
−
ˆ
C
n
b
( ˜ ω
b
ib
×δl
b
const
) +
ˆ
C
n
b
( ˜ ω
b
ib
×l
b
ﬂex
) (13.28)
An approximation for ˆ v
b
ref
derived from (13.24) gives the following:
ˆ v
n
ref
= ˆ v
n
−
ˆ
C
n
b
( ˜ ω
b
ib
×
ˆ
l
b
) (13.29)
Using this expression in (13.28) gives:
z = δv
n
−(ˆ v
n
−
ˆ
C
n
b
( ˜ ω
b
ib
×
ˆ
l
b
)) ×γ
n
−
ˆ
C
n
b
δv
b
ref
−
ˆ
C
n
b
( ˜ ω
b
ib
×δl
b
const
) +
ˆ
C
n
b
( ˜ ω
b
ib
×l
b
ﬂex
)
(13.30)
As the parameter
ˆ
l
b
can be set freely as long as δl
b
const
is taken account,
ˆ
l
b
is chosen
to be zero. Furthermore, the measurement equation is modiﬁed 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
= δv
n
+ǫ
n
×v
n
(13.31)
Using these properties gives the following:
z = δυ
n
− ˆ v
n
× ψ
n
−
ˆ
C
n
b
δv
b
ref
−
ˆ
C
n
b
( ˜ ω
b
ib
×δl
b
const
) +
ˆ
C
n
b
( ˜ ω
b
ib
×l
b
ﬂex
) (13.32)
At this point, the integration of a speedometer is almost ready to be implemented. The
only thing missing is an expression for the velocity error experienced by the speedometer
δv
b
ref
. As this expression diﬀers 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 speciﬁc GPS which is used
95 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
+

INS
GPS
GPS
INS
INS
r v
n n
r v
n n
GPS
INS
Kalman
Filter
v
n
n
r
n
r
v
n
n
r
n
Figure 13.4: Illustration of the loosely coupled INS/GPS integration
as measurement noise. The precision of the GPS is described in the data sheet and
therefore, this approach can always be implemented in any INS system. A ﬁgure of the
INS/GPS integration is presented below.
As can be seen from Fig. 13.4 the GPS measurements are subtracted from the INS
measurements to form the observation set. The Kalman ﬁlter estimates the errors
which are used to reset the INS. If possible the GPS receiver can be aided with the
position which can be used to aid the code tracking loop in the GPS receiver.
Derivation of Kalman Filter Equations
The Kalman ﬁlter is developed using the same approach as in Sec. 12.4.
The observation equation is:
z
obs
=
¸
r
n
INS
−r
n
GPS
v
n
INS
−v
n
GPS
(13.33)
The measurement equation is:
z = ˆ z
n
obs
− z
n
obs
z =
¸
r
n
true
+ δr
n
INS
−r
n
true
−δr
n
GPS
v
n
true
+ δv
n
INS
−v
n
true
−δv
n
GPS
=
¸
δr
n
INS
−δr
n
GPS
δv
n
INS
−δv
n
GPS
(13.34)
where
δr
n
INS
, δv
n
INS
= Is the error in the INS estimation of position and velocity
δr
n
GPS
, δv
n
GPS
= Is the error in the GPS estimation of position and velocity
δr
n
true
, δv
n
true
= Is the true position and velocity
With the measurement equation derived, the model for the Kalman ﬁlter is:
˙ x = Ax +G
p
n
p
z = Hx +G
m
n
m
(13.35)
where
x =
ψ
n
δv
n
δr
n
T
(13.36)
The error state dynamic matrix (A) is the time varying ψerror model from (11.54) and
13.3. IMPLEMENTATION OF NAVIGATION WITH AIDING 96
the process noise dynamic matrix is derived from (13.34) and is as follows:
H =
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
¸
¸
¸
¸
¸
¸
¸
¸
(13.37)
The measurement noise is derived in the same way as in Sec. 12.4 and is as follows:
G
mn
=
¸
I
3x3
0
0 I
3x3
n
mn
=
¸
δv
n
GPS
δr
n
GPS
(13.38)
As in Sec. 12.4 n
m
is assumed to have equal and uncorrelated variance which leads to
the following measurement covariance matrix:
R
n
= E(n
mn
n
T
mn
) =
¸
I
3x3
δGPS
velocity
0
0 I
3x3
δGPS
position
(13.39)
where
δGPS
velocity
, δGPS
position
= GPS velocity and position variance
E = Is the expected value operator
The last thing before implementation is to ﬁnd the discrete error state dynamic matrix
and the integrated process noise matrix.
The error model which should be used when in navigation mode is the one from (11.54).
The discrete error state dynamic matrix is computed in Matlab shown below:
Φ =
1 T(Ω
c
) T(−Ω
b
−Ω
β
) 0 0 0 0 0 0
T(−Ω
c
) 1 T(Ω
a
+Ω
α
) 0 0 0 0 0 0
T(Ω
b
+Ω
β
) T(−Ω
a
−Ω
α
) 1 0 0 0 0 0 0
0 −Ta
U
Ta
N
1 T(2Ω
c
) T(−2Ω
b
−Ω
β
) −T
g
r
0 0
Ta
U
0 −Ta
E
T(−2Ω
c
) 1 T(2Ω
a
+Ω
α
) 0 −T
g
r
0
−Ta
N
Ta
E
0 T(2Ω
b
+Ω
β
) T(−2Ω
a
−Ω
α
) 1 0 0 2T
g
r
0 −0.5T
2
a
U
0.5T
2
a
N
T 0 0 1 0 −Ω
β
0.5T
2
a
U
0 −0.5T
2
a
E
0 T 0 0 1 Ω
α
−0.5T
2
a
N
0.5T
2
a
E
0 0 0 T Ω
β
−Ω
α
1
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
(13.40)
The integrated process noise matrix is found using the same approach as in Sec. 12.4
where:
Q
1n
= G
p
Q
pDens
G
T
p
T
n
Q
n
≈
1
2
Q
1n
+
1
2
Φ
n
Q
1n
Φ
T
n
(13.41)
So the ﬁrst order process noise matrix is:
97 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
Q
1n
=
q
w
0 0 0 0 0 0 0 0
0 q
w
0 0 0 0 0 0 0
0 0 q
w
0 0 0 0 0 0
0 0 0 q
f
0 0 0 0 0
0 0 0 0 q
f
0 0 0 0
0 0 0 0 0 q
f
0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
T
n
(13.42)
The last thing to deﬁne the control reset vector and how it is used correct the INS
states.
The control reset vector is deﬁned as in Sec. 12.4 which gives:
u
c
= [ψ
n
, δv
v
, δr
v
]
T
(13.43)
The states which are corrected are the body to navigation rotation matrix C
n
b
with γ
n
,
navigation to Earth rotation matrix C
e
n
with ǫ
n
and velocity v
n
with δv
n
. If one look
at u
c
the only state which can be directly corrected is the velocity. In order to correct
C
n
b
and C
e
n
some transformations are necessary. As ψ contain both the error in C
n
b
and
C
e
n
, δr is needed as it describe the errors in C
e
n
. It has been deﬁned earlier that the
navigation frame does not rotate with respect to the geodetic frame. This means that
all the heading error is in γ
n
. But in order to extract γ
n
from ψ, ǫ
n
is needed. Using
the δr
n
relation from (11.43) ǫ
n
is calculated from δr
n
in the following way:
δr
n
= R(ǫ
n
×u
n
zn
) + δRu
n
zn
δr
x
/R
δr
y
/R
δr
z
/R
¸
¸
=
ǫ
y
−ǫx
0
¸
¸
+ δRu (13.44)
Having constructed ǫ
n
, γ
n
can be calculated from the ψ
n
relation from (11.43) in the
following way:
γ
n
= ψ
n
+ǫ
n
(13.45)
At this point, all the necessary parameters are available and the INS states are corrected
as described in (12.41):
C
n
b
(+
c
) = [I
3x3
−Γ
n
]C
n
b
C
e
n
(+
c
) = [I
3x3
−E
n
]C
e
n
v
n
(+
c
) = v(−
c
) + δv
n
(13.46)
With these equations derived, the Kalman ﬁlter is ready for implementation and veriﬁ
cation.
13.4 Veriﬁcation of Navigation with Aiding
In order to test the aiding scheme developed in the preceding section both a GPS and
an INS is needed. It has not been possible to get GPS data while navigating with an
INS and for this reason another approach has been used to verify the aiding scheme.
13.4. VERIFICATION OF NAVIGATION WITH AIDING 98
The test data used to verify navigation with aiding is the same as used in Sec. 9.5. With
a drive time of approximately 5.6 [min], the Kearfott does only experience minor drift
why the Kearfott track can be used as the reference track. In order to emulate the GPS
position a noise signal has been added to the Kearfott navigation data with a standard
deviation of 5 [m] and extracted with a sample time of one second. These data have
been used as the GPS position.
Two test scenarios are presented. One test scenario where the INS starts out with
15 [min] alignment followed by navigation without aiding. The track is the same track
used to verify the navigation equations in Sec. 9.5. The second test scenario is a scenario
where the INS again starts with 15 [min] alignment. The same track is followed as in
the ﬁrst test scenario but navigation is aided with the GPS data. A GPS outage of 40 [s]
duration is simulated after 3.3 [min] driving time. During this time, the INS navigates
without any aiding.
The two test scenarios should show how aiding is able to bound the position error, and
how the INS system keeps the system “on track” during GPS outages.
Figure 13.5 shows navigation without aiding.
−2.086 −2.084 −2.082 −2.08 −2.078
57.186
57.187
57.188
57.189
57.19
57.191
57.192
57.193
57.194
Longitude [°]
L
a
t
i
t
u
d
e
[
°
]
Aided position
Real position
Figure 13.5: This ﬁgure shows navigation on realworld 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
99 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
−2.086 −2.084 −2.082 −2.08 −2.078
57.186
57.187
57.188
57.189
57.19
57.191
57.192
57.193
57.194
Longitude [°]
L
a
t
i
t
u
d
e
[
°
]
Aided position
GPS position
Real position
Figure 13.6: This ﬁgure shows navigation on realworld measurements aiding with
a GPS.
without aiding but the position starts to drift from the reference track. It does, however,
follow the turns made by the vehicle. As can be seen the “x” which indicate the GPS
position is absent. Just before the turn to the South the GPS data is valid again and
thus pulls the navigation towards the reference track.
What can not be seen in Fig. 13.6 is how the INS/GPS position variates around the
reference track. In order to investigate that Fig. 13.7 shows a 2d plot of the latitude
and longitude.
The ﬁrst row of Fig. 13.7 shows the latitude where the second row shows the longitude
of the INS. Although it is hard to see any details on the two plots on the left column
they show that the fused INS/GPS position has not been time shifted. But the plots on
the right column shows a detailed view of the latitude and longitude the ﬁrst minute of
navigation. What can be seen is that the fused position (dotted) does not suﬀer from
the same sudden jumps to the same degree as the GPS (x).
13.5 Summary
As unaided INS inevitably suﬀers from unbounded position error, some kind of aiding
is preferable if INS is to be used over longer time periods. During this chapter, two
diﬀerent 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 diﬀerent approaches are described in this thesis where the loosely coupled
13.5. SUMMARY 100
0 1 2 3 4 5
57.186
57.187
57.188
57.189
57.19
57.191
57.192
L
a
t
i
t
u
d
e
[
°
]
0 0.2 0.4 0.6 0.8 1
57.1908
57.1909
57.191
57.1911
57.1912
0 1 2 3 4 5
−2.086
−2.084
−2.082
−2.08
−2.078
Time [min]
L
o
n
g
i
t
u
d
e
[
°
]
0 0.2 0.4 0.6 0.8 1
−2.0822
−2.0821
−2.082
−2.0819
−2.0818
Time [min]
Figure 13.7: This ﬁgure shows the positions of the fused INS/GPS (dotted), the
GPS positions (x) and the reference track (solid).
101 CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
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 diﬀerent physical phenomena
to measure speciﬁc 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 diﬀerent lengths if the path is rotated. This is illustrated
in Fig. 14.1
ω
r
CCW beam
CW beam
Beam splitter/
Light
detector
source
Light path
Figure 14.1: The fundamental principle of optical angular rate sensors
Light is generated by the light source, and split into two beams travelling clockwise and
counterclockwise around the circular light path with the radius r. With no rotation,
ω is 0, and the transit time t for both beams to go around the light path is the same.
102
103 CHAPTER 14. INERTIAL SENSORS
However, if the light path is rotated, the path length for each of the beams are changed,
and the transit time diﬀerence 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 diﬀerence 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 rateintegrating. 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 broadband light beams are trans
mitted CW and CCW through a ﬁbre optic coil, which is spun around the sensitive
axis. As the IFOG is rotated, the length which the beams have to travel changes, and
the phase diﬀerence can be measured as a change of intensity by photodetectors. This
makes the IFOG an analog rate sensor. As the IFOG uses ﬁbre optic coils instead of
a laser cavity, the need for high quality mirrors, and precision machinery is avoided,
which makes the IFOG cheaper than the RLG. The performance of the IFOG, however,
appeals more to medium accuracy applications. A comparison of typical accuracy of
RLG and IFOG sensors are shown in table Tab. 14.1[Titterton and Weston, 1997].
RLG IFOG
gindependent bias <0.001 −10
◦
/hour <0.5 −50
◦
/hour
gdependent bias insigniﬁcant 1
◦
/hour/g
g
2
dependent bias insigniﬁcant 0.1
◦
/hour/g
2
scale factor errors few parts per million to 0.01% 0.05 −0.5%
bandwidth >200 Hz >100 Hz
maximum input rate >1000
◦
/s >1000
◦
/s
Table 14.1: Comparison of typical gyro accuracy
14.2 Accelerometers
An accelerometer uses Newton’s Second Law of motion to measure speciﬁc force.
F = ma (14.1)
A ﬁgure of an accelerometer is shown in Fig. 14.2. By having a proof mass suspended
by springs, the displacement of the mass due to acceleration can be measured by the
pickoﬀ.
14.3. SENSOR ERROR MODELS 104
Displacement pickoﬀ
Spring
Spring
Proof mass
Acceleration
Case
Figure 14.2: Conceptual drawing of a accelerometer
It is important to notice that gravity inﬂuences 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 openloop accelerometers.
These suﬀers from nonlinearities 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 forcerebalance 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 diﬀerence
from a 1:1 input/output mapping.
Misalignment The inaccuracies in sensor alignment, when 3 1dof sensors are mounted
in a frame.
gsensitivity The inﬂuence of acceleration on the output of the sensor.
Temperature sensitivity The inﬂuence of temperature on the output of the sensor.
Quantization noise The noise introduced by converting an analog signal into a ﬁnite
precision digital signal.
Random noise/random walk Stochastic elements arising from other unmodelled noise
sources.
105 CHAPTER 14. INERTIAL SENSORS
All of these can be nonlinear. Normally, bias, scale factor, misalignments, gsensitivity
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, scalefactor misalignments and temperature depen
dency. Therefore, most gyro and accelerometer errors are modelled by the following
equation:
δo(t) =
B
x
(T)
B
y
(T)
B
z
(T)
¸
¸
+
SF
x
(T) MA
xy
(T) MA
xz
(T)
MA
yx
(T) SF
y
(T) MA
yz
(T)
MA
zx
(T) MA
zy
(T) SF
z
(T)
¸
¸
i(t) (14.2)
Where
i
are the measured input, o are the output, the B’s are the biases, the SF’s are
the scale factors, the MA are the misalignments (with the subscripts deﬁning 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 inﬂuencing the signal itself.
The repeatable errors, however, can be identiﬁed through a process called calibration
and thus be compensated for. The term “sensor error” will throughout this chapter
signify only a repeatable sensor error.
15.1 Introduction
While it is possible to identify all the sensor errors, this can take much time and requires
expensive calibration equipment. Depending on the required accuracy of the ﬁnal INS,
a weighing between sensor compensation (cost and time) and INS accuracy must be
made.
In this project, it is chosen to identify constant but temperature dependent errors.
This is done by identifying the constant errors at various temperatures by rotating the
IMU through a series of 180
◦
rotations[Diesel, 1987]. With no translatory motion, only
gravity and the rotation of the earth inﬂuences the measurements. A rotation and a
measurement can be done in 5 to 20 seconds depending on environmental eﬀects.
15.2 Calibration Equations
In order to ﬁnd some of the sensor misalignment errors, it is necessary to deﬁne 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 simpliﬁed 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 simpliﬁed error model of the system is:
106
107 CHAPTER 15. CALIBRATION OF SENSORS
δ ˙ v
n
= δa
n
+
0 θ
n
z
−θ
n
y
θ
n
z
0 θ
n
x
θ
n
y
θ
n
x
0
¸
¸
0
0
g
¸
¸
= δa
n
+
−gθ
n
y
gθ
n
x
0
¸
¸
(15.1)
Where θ
n
y
and θ
n
x
are the misalignment angles from the sensor to the body frame around
the y and x axis and δa
n
are the acceleration errors, all given in the navigation frame. As
the system should not be translating, any velocity is an error, so the ˙ v
n
term obtained
from the navigation equation (9.1) is equal to a velocity error δ ˙ v
n
.
To excite all the sensor errors, the system is rotated through 3 sets of 3 rotations each.
The rotations needed are shown in Fig. 15.1.
1 (180
◦
) 1 (180
◦
)
z
b
y
b
x
b
y
b
Set 2
2 (180
◦
)
3 (180
◦
)
x
b
z
b
2 (180
◦
)
3 (180
◦
)
x
b
Set 3
1 (180
◦
)
2 (180
◦
)
3 (180
◦
)
z
b
Set 1
y
b
Figure 15.1: Rotation sequences used for calibration
To be able to analytically identify the errors δ ˙ v
n
is logged just before and after the
rotations. Thus, the equations that need to be solved are of the following type (after
adding time dependency on (15.1)):
δ ˙ v
n
(T) −δ ˙ v
n
(0) =
¸
δa
n
(T) +
−gθ
n
y
(T)
gθ
n
x
(T)
0
¸
¸
¸
−
¸
δa
n
(0) +
−gθ
n
y
(0)
gθ
n
x
(0)
0
¸
¸
¸
(15.2)
= ∆δa
n
+
−g∆θ
n
y
g∆θ
n
x
0
¸
¸
(15.3)
Where T are the time after a rotation, and 0 is the time before a rotation. The ∆ preﬁx
denotes that the following term is the diﬀerence of the term from 0 to T.
To obtain ∆δa
n
and ∆θ
n
, the sensor error model from chapter Ch. 14 is used. The
accelerometer error model is
δa
n
(t) = C
n
b
(t)δa
b
(t) = C
n
b
(t)
¸
a
Bx
a
By
a
Bz
¸
¸
+
a
SFx
a
MAxy
a
MAxz
a
MAyx
a
SFy
a
MAyz
a
MAzx
a
MAzy
a
SFz
¸
¸
a
b
(t)
¸
(15.4)
Where δa
b
are the errors in the measured accelerations, the a
B
terms are a constant
bias, a
SF
are scale factors, a
MA
are misalignments and a
b
is the acceleration due to
gravity. From this, ∆δa
n
is obtained as
∆δa
n
= δa
n
(T) −δa
n
(0) (15.5)
15.2. CALIBRATION EQUATIONS 108
Identically, the gyro error model is
δω
n
(t) = C
n
b
(t)δω
b
(t) = C
n
b
(t)
¸
g
Bx
g
By
g
Bz
¸
¸
+
g
SFx
g
MAxy
g
MAxz
g
MAyx
g
SFy
g
MAyz
g
MAzx
g
MAzy
g
SFz
¸
¸
ω
b
(t)
¸
(15.6)
To obtain ∆θ
n
, δω
n
is integrated
∆θ
n
=
T
0
C
n
b
(t)
g
SFx
g
MAxy
g
MAxz
g
MAyx
g
SFy
g
MAyz
g
MAzx
g
MAzy
g
SFz
¸
¸
ω
b
(t)dt (15.7)
Where the gyro bias errors are disregarded as they are expected to contribute only a
fraction of an arc second to the error during the short interval from 0 to T.
The rotation matrix from body to navigation corresponding to rotation set 1 is
Rotation 1 and 2
C
n
b
(t) =
cos(φ(t)) 0 −sin(φ(t))
0 1 0
sin(φ(t)) 0 cos(φ(t))
¸
¸
(15.8)
Rotation 3
C
n
b
(t) =
cos(ψ(t)) −sin(ψ(t)) 0
sin(ψ(t)) cos(ψ(t)) 0
0 0 1
¸
¸
(15.9)
Where φ(t) and ψ(t) are the rotation angles at time t.
With this, solving (15.2) for rotation 1 gives
C
n
b
(0) =
1 0 0
0 1 0
0 0 1
¸
¸
a
b
(0) =
0
0
g
¸
¸
(15.10)
C
n
b
(T) =
−1 0 0
0 1 0
0 0 −1
¸
¸
a
b
(T) =
0
0
−g
¸
¸
(15.11)
∆δa
n
=
a
MAxz
g
−a
MAyz
g
a
SFz
¸
¸
−
a
MAxz
g
a
MAyz
g
a
SFz
¸
¸
+
−2a
Bx
0
−2a
Bz
¸
¸
=
−2a
Bx
−2a
MAyz
g
−2a
Bz
¸
¸
(15.12)
∆θ
n
= −
π
0
(g
MAxy
cos(φ) + g
MAzy
sin(φ))
g
SFy
(g
MAxy
cos(φ) + g
MAzy
sin(φ))
¸
¸
dφ =
−2g
MAzy
−πg
SFy
2g
MAxy
¸
¸
(15.13)
δ ˙ v
n
(T) −δ ˙ v
n
(0) =
−2a
Bx
−2a
MAyz
g
−2a
Bz
¸
¸
+
g
SFy
πg
−2g
MAzy
g
0
¸
¸
(15.14)
Solving for the remaining rotations, one obtains the measurements given in table 15.1,
where for brevity, the following deﬁnitions are made
109 CHAPTER 15. CALIBRATION OF SENSORS
Set j, Rotation k: α
j
k
= δ ˙ v
n
x
(T) −δ ˙ v
n
x
(0), β
j
k
= δ ˙ v
n
xy
(T) −δ ˙ v
n
y
(0) (15.15)
Set 1 Set 2 Set 3
α
1
1
=−2a
Bx
+g
SFy
πg α
2
1
=−2a
Bz
+g
SFx
πg α
3
1
=−2a
By
+g
SFz
πg
β
1
1
=−2(a
MAyz
+g
MAzy
)g β
2
1
=−2(a
MAxy
+g
MAyx
)g β
3
1
=−2(a
MAzx
+g
MAxz
)g
α
1
2
=2a
Bx
+g
SFy
πg α
2
2
=2a
Bz
+g
SFx
πg α
3
2
=2a
By
+g
SFz
πg
β
1
2
=2(a
MAyz
+g
MAzy
)g β
2
2
=2(a
MAxy
+g
MAyx
)g β
3
2
=2(a
MAzx
+g
MAxz
)g
α
1
3
=−2a
Bx
−2(a
MAxz
−g
MAxz
)g α
2
3
=−2a
Bz
−2(a
MAzy
−g
MAzy
)g α
3
3
=−2a
By
−2(a
MAyx
−g
MAyx
)g
β
1
3
=−2a
By
−2(a
MAyz
−g
MAyz
)g β
2
3
=−2a
Bx
−2(a
MAxy
−g
MAxy
)g β
3
3
=−2a
Bz
−2(a
MAzx
−g
MAzx
)g
Table 15.1: Table of observation equations for calibration coeﬃcients
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 deﬁning the sensor frame so
that the xy plane is spanned by the x and y accelerometer, with the x accelerometer
corresponding to the xaxis. The accelerometer scale factors can be determined by get
ting the ˙ v
n
z
component at the start of the ﬁrst rotation and at the end of the second
rotation in each set after subtracting the corresponding bias term. For the a
SFz
term:
For set 1 rotation 1 : γ
1
1
= δ ˙ v
n
z
(0) (15.16)
For set 1 rotation 2 : γ
1
2
= δ ˙ v
n
z
(T) (15.17)
a
SFz
=
γ
1
1
+ γ
1
2
−2a
Bz
2g
(15.18)
Remaining is to ﬁnd the rotation matrix from the sensor frame to the body frame and
the gyro bias. This is done by assuming the following approximations:
δ¨ v
n
x
= δ ˙ a
n
x
−g
˙
θ
n
y
≈ −g
˙
θ
n
y
(15.19)
δ¨ v
n
y
= δ ˙ a
n
y
+ g
˙
θ
n
x
≈ g
˙
θ
n
x
(15.20)
These approximations are valid as the accelerometer errors will be very small after just
a few iterations of the compensation procedure.
In this way, the x and y gyro bias is found from set 1 rotation 3:
g
Bx
=
δ¨ v
n
y
(T) −δ¨ v
n
y
(0)
2g
(15.21)
g
By
=
δ¨ v
n
x
(T) −δ¨ v
n
x
(0)
−2g
(15.22)
And the z gyro bias is found from set 2 rotation 3:
g
Bz
=
δ¨ v
n
y
(T) − δ¨ v
n
y
(0)
2g
(15.23)
The rotation matrix from sensor to body frame is found by calculating θ
n
x
and θ
n
y
as
well as the heading angle θ
n
z
. All are calculated from set 1 rotation 3:
15.3. SMOOTHING FILTER 110
θ
n
x
=
δ ˙ v
n
y
(T) + δ ˙ v
n
y
(0)
2g
(15.24)
θ
n
y
=
δ ˙ v
n
x
(T) + δ ˙ v
n
x
(0)
−2g
(15.25)
θ
n
z
=
δ¨ v
n
y
(T) + δ¨ v
n
y
(0)
−2gΩ
n
y
(15.26)
Where Ω
n
y
is the earth rate y component in the navigation frame.
g
SFy
=
α
1
2
+α
1
1
2πg
g
SFx
=
α
2
2
+α
2
1
2πg
g
SFz
=
α
3
2
+α
3
1
2πg
a
Bx
=
α
1
2
−α
1
1
4
a
Bz
=
α
2
2
−α
2
1
4
a
By
=
α
3
2
−α
3
1
4
g
MAxz
=
α
1
3
+2a
Bx
2g
g
MAxy
=
α
2
3
+2a
Bx
2g
g
MAzy
=
−β
1
1
2g
g
MAyz
=
β
1
3
+2a
By
2g
g
MAyx
=
β
2
1
2g
g
MAzx
=
β
3
3
+2a
Bz
2g
+ a
MAzx
a
MAzx
= −
α
3
1
2g
−g
MAxz
a
MAzy
=
α32+2a
Bz
2g
+ g
MAzy
a
MAyx
=
α
3
3
+2a
By
2g
+ g
MAyx
Table 15.2: Solutions to the observation equations
15.3 Smoothing Filter
To remove noise from the measurements, and to obtain the rate of change of the hor
izontal acceleration errors, a smoothing ﬁlter is applied to the measurements with the
dynamic equation
˙
δv
δ ˙ v
δ¨ v
¸
¸
=
0 1 0
0 0 1
0 0 0
¸
¸
δv
δ ˙ v
δ¨ v
¸
¸
+
0
0
w
k
¸
¸
(15.27)
and measurement matrix
y = h
δv
δ ˙ v
δ¨ v
¸
¸
+ w
r
(15.28)
where
h =
0 1 0
w
r
= Measurement noise
The estimated error terms that are obtained in one calibration run is used by the
navigation algorithms in the following run, so the remaining errors in coeﬃcients 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,
111 CHAPTER 15. CALIBRATION OF SENSORS
misalignments, scale factors and random noise to simulate uncompensated sensors. Also,
the navigation algorithm includes sensor compensation using the calibration coeﬃcients
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 montecarlo simulation of how the calibration
coeﬃcients converge towards the true errors after a few iterations.
Error True Estimated
g
Bx
−0.1E4 −0.098728E04
g
By
0.2E4 0.20059E04
g
Bz
−0.3E4 −0.29865E04
g
SFx
1E4 1.0072E04
g
SFy
−2E4 −2.0099E04
g
SFz
3E4 3.0153E04
g
MAxy
−0.1E4 −1.8477E05
g
MAxz
0.2E4 2.0168E05
g
MAyx
−0.3E4 −3.4114E05
g
MAyz
0.4E4 3.0360E05
g
MAzx
−0.5E4 −6.1056E05
g
MAzy
0.6E4 5.7603E05
a
Bx
0.01 0.01
a
By
−0.02 −0.02
a
Bz
0.03 0.03
a
SFx
0.001 0.001
a
SFy
−0.002 −0.002
a
SFz
0.003 0.003
a
MAyx
−0.3E4 −3.3082E05
a
MAzx
−0.5E4 −5.3109E05
a
MAzy
0.6E4 5.6882E05
Table 15.3: True and estimated sensor errors
The sensor calibration algorithms have not been tested with reallife measurements.
This is mainly because it is diﬃcult to obtain the sensor errors in a particular IMU, and
thus, it is not possible to quantify the diﬀerence between the estimated errors and the
actual errors.
15.4. CALIBRATION EXAMPLE 112
1 2 3 4 5 6 7 8
−7
−6
−5
−4
−3
−2
−1
0
x 10
−5
Gyro X bias estimate
iteration
e
s
t
i
m
a
t
e
Figure 15.2: Calibration estimation of gyro X bias error.
Chapter 16
Summary
Throughout this part has all the theory been derived which constitute the mathematical
foundation of an inertial navigation system.
The geodetic constants and equations was presented in Ch. 6 along with a simpliﬁed
gravity model with an accuracy suitable for INS.
Equations for the earth simulation was derived which was used in later chapters to
yield predictable and controlled inputs to the navigation equations for analysis of the
navigation and alignment algorithms.
As the Kalman ﬁlter used in this thesis is an indirect variant of the normal direct Kalman
ﬁlter Ch. 10 shows the diﬀerences between the two ﬁlters an derives the equations used
in later chapters to implement this ﬁlter. The indirect Kalman ﬁlter is an advantageous
approach when the system model has high dynamics and/or is nonlinear. The indirect
Kalman ﬁlter is implemented using an error model of the nonlinear system which can
be approximated linear. Furthermore, the dynamics of the error model is usually much
lower than that of the real system. An indirect Kalman ﬁlter requires an error model of
the INS, which is derived in Ch. 11. Two diﬀerent error models are derived. Although
these error models model the same system they have diﬀerent properties as they describe
the errors with diﬀerent parameters. The γerror model is preferable when in alignment
mode due to the simplicity and the fact that it is indeed the γ parameter which is
sought estimated. The ψerror model is preferable in the navigation mode as this model
becomes simpler than the γerror model when moving.
Using the γerror model the alignment algorithm has been developed in Ch. 12. Align
ment is a matter of estimating the attitude and heading of the INS. This part is very
critical as even small errors in initial attitude leads to heavily decreased navigation preci
sion. Alignment uses the knowledge of the gravity vector and earth rotation in a known
frame and estimates attitude and heading using an indirect Kalman ﬁlter technique.
As the INS inevitably suﬀer 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 online.
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 diﬀerent 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 veriﬁed 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 suﬃciently accurate to validate
a veriﬁcation of the system. Veriﬁcation of the developed navigation equations also
veriﬁed the earth simulation model, as the equations in the simulation model showed to
be the inverse of the navigation equations. Further tests with diﬀerent errors sources
revealed that gyro bias was the predominant cause to unbounded position errors. These
tests concluded that a precise gyroscope is essential if high performance is required. The
tests also showed that an INS experience oscillatory errors with dominant frequencies
at 84 [min] and 24 [hour] which is known as the Schuler and Earth oscillation.
The alignment algorithm is based on an indirect Kalman ﬁlter, which uses an error
model of the system. Two diﬀerent 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 veriﬁed using both simulated
test data and realworld data from the Kearfott T16 system. The alignment algorithm
was able to estimate C
n
b
precisely using the simulated test data and showed to estimate
C
n
b
close to the one from the Kearfott T16. This showed that the developed alignment
algorithm worked.
118
119 CHAPTER 17. CONCLUSION
In order to aid the navigation equations, to address the unbounded position error charac
terising of an unaided INS, two diﬀerent aiding approaches have been proposed: Aiding
with a speedometer and aiding with a GPS. While aiding with a speedometer only
decreases the position, error aiding with a GPS bounds the position error. Aiding al
gorithms for loosely coupled INS/GPS integration were developed which enabled data
from the navigation equations to be fused with data from the aiding device using an
indirect Kalman ﬁlter. The aiding algorithm was shown to be able to bound the errors
in the INS system.
In order to gain as high precision as possible the sensors needs to be calibrated. A sensor
model together with a calibration procedure is used in order to identify and estimate
scale factors, misalignment and sensor biases.
As all the equations needed for an inertial navigation system with aiding have been
developed and have been veriﬁed to work, the purpose of this thesis have been met.
Chapter 18
Improvements and Discussion
Before the algorithms developed in this thesis can be implemented and used in the real
world, several aspect are to be considered.
Before implementation on a microprocessor the equations need to be discretized. Al
though the equations have been discretized and implemented as a matlab ﬁles in this
thesis, the errors involved with the discretization procedure have not been analyzed.
The choice of discretization method aﬀect 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
twospeed 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
twospeed approach still oﬀers 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 twospeed 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 proﬁle 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 proﬁles, but tests in the
real world are important to validate the navigation equations completely.
The indirect Kalman approach has been chosen in this thesis to avoid running a non
linear Kalman ﬁler with a Kalman cycle greater than 2kHz. It has not been investigated
which performance improvements might be gained from running a direct nonlinear
Kalman ﬁlter at this frequency.
When correcting the rotation matrices a ﬁrst order approximation has been used through
out this thesis. This introduces errors which could be avoided with a more advanced
correction.
120
121 CHAPTER 18. IMPROVEMENTS AND DISCUSSION
When aiding with GPS only the loosely coupled integration approach have been imple
mented. The tightly coupled INS/GPS integration could prove more precise and more
work should be done to implement that approach.
The validation of the calibration equations were made using simulated sensor outputs.
This was done as it is diﬃcult 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
AALBORG
UNIVERSITY
Master project Section of Automation and Control Department of Electronic Systems Faculty of Engineering, Science and Medicine Fredrik Bajers Vej 7 C3 DK9220 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 for an inertial navigation system (INS). In general, inertial navigation consists of three parts: Alignment, navigation and aiding. Algorithms for these parts are derived in this thesis. In order to create reproducable test data for veriﬁcation and analysis of the algorithms, an earth simulation model is developed. The algorithms were also tested against a reference INS, a Kearfott T16 INS, to verify that the algorithms also work on realworld data with real noise sources. The navigation equations needed for an INS for terrestrial navigation are developed and tested against the reference INS. Tests showed that the developed navigation equations were able to follow the reference trajectory to a satisfactory degree, thus validating the navigation equations. The alignment algorithms make use of an indirect Kalman ﬁlter, which estimates the errors in the INS states. For this, an error model is developed, which describes the propagation of errors within the INS. An alignment algorithm is developed to estimate tilt and heading of the system. The alignment algorithm was veriﬁed against data from the reference system, and was shown to be able to estimate tilt and heading to a satisfactory degree. Algorithms for aiding the INS navigation equations were developed for a speedometer and a GPS. These algorithms also use the indirect Kalman ﬁlter and the error model. Using data from the reference system together with GPS measurements, it was shown that the developed algorithms were able to aid the navigation 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.
Title: Inertial Navigation System Theme: Estimation and nonlinear control Period: FallSpring Semester 20072008 Project group: 08gr1030a Group members: Rolf Christensen Nikolaj Fogh
Supervisor: Anders la CourHarbo Morten Bisgaard Copies: 5 Pages: 122 Attachments: None Finished: 4th of June 2008
.
En simuleringsmodel af jorden er blevet lavet for at kunne lave reproducerbare test data til veriﬁkation og analyse of de udviklede algoritmer. navigering og aiding. Det blev også vist at INS systemet var i stand til at følge referencesystemet under manglende GPS signaler. Natur. og det er blevet vist at den var i stand til at estimere hældning og retning tilfredsstillende. Disse algoritmer bruger også et indirect Kalman ﬁlter og en fejlmodel.control. er algoritmerne også blevet testet med et referencesystem. og Sundhedsvidenskabelige Fakultet Fredrik Bajers Vej 7 C3 DK9220 Aalborg Øst Danmark Telefon +45 96 35 87 02 Fax +45 98 15 17 39 http://www. Ved brug af data fra referencesystemet samt simulerede GPS målinger blev det vist at de udviklede algoritmer kunne aide navigeringsligningerne og give bedre resultater end nogle af systemerne alene. En alignment algoritme er blevet udviklet som estimerer hældning og retning af systemet. Juni 2008 . Aidingalgoritmer til navigeringsligningerne er blevet udviklet for en hastighedsmåler og en GPS. Testene viste at de udviklede navigeringsligninger var i stand til at følge referencesystemet til en tilfredsstillende grad. De navigeringsligninger som er nødvendige for navigering i nærheden af jorden er blevet udviklet og testet med referencesystemet. Titel: Inertial Navigation System Tema: Estimation and nonlinear control Periode: EfterårForår 20072008 Projektgruppe: 08gr1030a Gruppemedlemmer: Rolf Christensen Nikolaj Fogh Vejleder: Anders la CourHarbo Morten Bisgaard Kopier: 5 Sider: 122 Bilag: Ingen Færdiggjort: 4. Generelt består inertial navigation af tre dele: Alignment. Alignment algoritmen er blevet veriﬁceret op imod data fra referencesystemet. For at veriﬁcere at algoritmerne også fungerer med data fra virkeligheden. en Kearfott T16 INS. Semester Sektion for Automation og kontrol Institut for Elektroniske Systemer Det Ingeniør.AALBORG UNIVERSITY 8.aau. En alignment algoritme bruger et indirect Kalman ﬁlter som estimerer fejlene i INS tilstandene. Derfor er der blevet udviklet en fejlmodel.dk/ Synopsis: Denne afhandling dokumenterer udviklingen af de algoritmer der bruges i inertial navigation systemer (INS). hvilket gør at navigeringsligningerne er veriﬁcerede. Denne afhandling indeholder algoritmer til alle disse dele. som beskriver udbredelsen af fejl i INS systemet.
2007 Rolf Christensen Nikolaj Fogh .Preface This thesis is written by group 08gr1030a and documents a project concerning the implementation of an Inertial Navigation System. year of publication]. a technical and scientiﬁc level corresponding to that of 9th semester students at the Section of Automation and Control is required. The report is copyrighted by the members of the group and Aalborg University. The project was proposed by the group themselves. If a literary work has more than two authors. For a complete understanding of the report. the ﬁrst authors last name is written followed by “et al.”. A complete bibliography is found at page 121. Literature references are written according to the Harvardmethod: [Last name of author. Aalborg University. Throughout the project Matlab has been used for data processing and algorithm implementation.4.0. 31st of May. 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. The version of Matlab used is 7.287 (R2007a).
.
. . . . . . . . . . . . . . . . . . 9. . . . . . . .2 Gyro Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8. . . . . . .1 Geodetic Variables and Constants . . . . . . . . . . . . . 9 Navigation Equations 9. . . . . .2 Coarse Alignment . . . . . . . . . . . . . . . . .Contents I Introduction 1 Introduction 2 Nomenclature and Mathematical Techniques 3 Frames of Reference and Position Determination 4 Principle of a Strapdown Inertial Navigation System 2 4 7 12 17 II Inertial Navigation Systems 5 Introduction 6 Geodesy 6. . . 12 Alignment 12. . . . . . . . . .5 Veriﬁcation of Alignment . . . .4 Indirect Kalman Filter Applied to QuasiStationary Fine Alignment 12. . . . 22 24 26 26 27 29 32 32 34 35 35 37 39 39 40 41 49 49 52 54 55 55 63 64 65 65 66 70 75 81 . 12. . . . . . . . . . . . 11. . . . . . . . . .5 Validation of Navigation Algorithm . . . . . . . . . .1 Derivation of Error Equations . . . . . . . . . . . . . . . . . . . . . . .2 Vertical Channel . 10. . . . . . . . . . . . . . . . . . . . . . . . . . 9. . . . 7 Equations of Motion 8 Earth Simulation Model 8. . .3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 Comparison between Gimballed and Strapdown Alignment . .6 Analysis of Long Term Error Propagation 10 Error Estimation Using Indirect 10.1 Kalman Filtering . . . . . . . . . . . . . . .4 Discrete Integration Algorithms . . . .3 Navigation Output . . . . . . . . . . . . . .3 Fine Alignment . 12. . . . . . . . . . . . . . . 9. 8 . . . . . . . .2 Summary . . . . . . . . . . . . . . . . . . . . . . . 9. . . . . . . . . . . . . . . . . . . . 11 Error Equations 11. . . . . . . .1 Accelerometer Output . . . . 12. . 12. . . . . . . . . . . . . . . . . 9. . . . . . . . . . . . . . . . . . . . . . . . Kalman Filtering . . . . . . 6.1 General Navigation Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .6 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . 10. . . . . . . . .2 Gravity Model . . . . . . . . . . . . . . . . .2 Motivating Example . . . . . . . . .
. . . . . . . . . . . . . 15. . . . . . . . . . 13 Inertial Navigation with Aiding 13. . . . . . . . . . .1 Aiding with GPS . . . . . . . . . . . . . . . . . .4 Calibration Example .1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Calibration Equations 15.2 GPS Error Model Analysis . . . . . . . . . . . . . 15. . . . . . . .5 Summary . . . .4 Veriﬁcation of Navigation with Aiding . . . . . . . . . . . . . . .3 Sensor Error Models . . . . . . . . . . . . . . . . . III Thesis Summary 116 118 120 122 17 Conclusion 18 Improvements and Discussion Bibliography . . . . . . . . . . . . . . . . . 14 Inertial Sensors 102 14.1 Optical Angular Rate Sensors . . . . . 13. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .7 Summary . . . 13. . . . . . . .2 Accelerometers . . . . . . . . . . . . . . 102 14. . . . . . . . . . . 85 86 86 90 94 97 99 . . . . . 104 15 Calibration of Sensors 15. . . .3 Smoothing Filter . . .12. . . . . . . . . . . . . . . . . . . . . . . . 16 Summary 106 106 106 110 110 113 . . . . . . . . . . . . . . . . . . . . . . . . . 13. . . . . . . . . .3 Implementation of Navigation with Aiding 13. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 14. . . . . . . . . . . . . . . . .
.
Part I Introduction .
.
Using the measurements from the IMU. however.g. 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. 4 . Furthermore. Using the stars. like GPS. This makes it suitable for navigation where GPS signal is not present or if the signals are being jammed. This sensor array is called an Inertial Measurement Unit (IMU). and not suitable for precision navigation needed by e. This makes INS very suitable for missiles where precision is crucial. An INS consists of accelerometers which measure the translatory acceleration and gyroscopes which measure the angular rotation of the system. INS gives a much higher precision than GPS. It being a neanderthal going hunting whom should be able to ﬁnd back home to the camp or a cruise missile which should hit its target with high precision. e. Through time several diﬀerent approaches has been used to locate and navigate to a destination. a missile or a war ship. Lastly the precision using GPS may be very poor. the INS can calculate the current attitude. velocity and position of the system starting from some known initial point. the accuracy of the INS degrades with time due to measurement inaccuracies. and thus will always work regardless of external inﬂuences.g. but which are only airborne for a few seconds. navigation using GPS is dependent on the satellites governed by the USA which might not always be accessible during a war situation. in mountainous surroundings. This means that INS does not depend on any third party applications. In certain areas.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. High precision INS are also suitable for submarines where high precision is needed and external positioning aids are not readily available. Nowadays GPS is the most widespread navigation system but in certain applications GPS is not a viable solution. An INS is a navigation system which depends entirely on inertial measurements for navigation. When relying only on inertial measurements. But used in a short time span. the sun or special marks in the landscape.
the reliability of the system is increased and the cost and size is reduced. The advantages of a nonmechanic system with low price and low weight was the source of this development. Further development of the gyros lead to even more precise INS during the 1950’s. With the invention and maturing of the laser and optical gyro. In a SINS. The lack of computer processing power postponed the introduction of SINS system until the 1980’s. SINS have even obtained an accuracy which at a much lower price tag rivals highend gimballed systems. INTRODUCTION Historical Development of INS The ﬁrst applications using inertial measurements for navigation purposes appeared at the end of the 19th century. Under WW2 the development of the gimballed INS was reﬁned. Scope of This Project This project seeks to implement algorithms for a lowcost FOG based INS. velocity and position. Strapdown INS The strapdown implementation oﬀers numerous advantages over its gimballed counterpart. In gimballed INS. They were simple gyro compasses were able to determine the direction of true north. and the properties of the selected sensors will be determined using system identiﬁcation. expensive and errorprone moving mechanical parts. This made INS solutions applicable to military aircraft and the ﬁrst commercial aircraft Boeing 757. the gyros must be able to capture these rotations. Inserting the parameters in the developed algorithms will yield the ﬁnal software for the INS. The problem with strapdown inertial navigation systems is. The gimbaled system still achieved better precision but the SINS had an precision which made it applicable in lowercost applications.5 CHAPTER 1. and the Germans used a gimballed INS to navigate their V2 rockets. This requires the strapdown gyros to have a much higher dynamic range than the gyros employed in gimballed systems. 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. the IMU is isolated from vehicle rotations by the use of a gimbal. By disposing of bulky. that the sensor assembly is not mechanically isolated from the rotations of the body. 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. hence the name “strapdown”. These gyros eventually enabled strapdown INS to obtain a degree of accuracy comparable to lowend gimballed systems but with a lower price tag. the sensors are rigidly mounted to the body of the vehicle. and has made it the preferred type of inertial navigation systems today. The main part of this task is to derive the navigation algorithms which translate the IMU measurements into attitude. The principal advantage is apparent from the name “strapdown”. . and thus. making them ideally suitable for strapdown INS. A general sensor model will be derived. The RLG and the FOG both have both ﬁne accuracy and a large dynamic range.
In particular. there have been conducted no analysis of the errors arising from assuming the system to be quasistationary during the ﬁne alignment process. Part III will include a system test. which describes the speed of a vehicle moving relative to earth. To the authors knowledge. First. a topic which is missing in most literature. are used by the navigation equations to compute the position and velocity of a vehicle using measured angular velocity and translatory acceleration. Additionally. and an error model is derived. These equations. as parameters such as the oblateness of the earth and gravity has major inﬂuences on INS. which veriﬁes the system against a reference system. Thereafter. This thesis includes results from a simulation. while still keeping the derivations relatively simple. the subject of geodesy is introduced. Thesis Structure The thesis structure is as follows: Part I gives an introduction to the subject and deﬁnes the goals and scope of the project. Geodesy deals with the parameters concerning the earth. The error equations are used to “fuse” the INS with an external aiding device. Lastly. An indirect Kalman ﬁlter is used both in alignment mode and to “fuse” the INS with an aiding device in an optimal way. which will estimate the initial attitude. and an analysis of the errors that arise from these violations. along with the geodetic variables. The last two chapters models the sensors used in this project and the determines the errors they suﬀer from. a conclusion and discussion will review the results obtained in the system test. some of the mathematical techniques used throughout the report will be introduced. . Part II derives the equations needed for the inertial navigation algorithms. the equations of motion are derived. The following chapters shows how the eﬀects of measurement uncertainties aﬀect the system. azimuth and position of the system. the application of the indirect Kalman ﬁlter to the alignment and aiding problem will be explained. This thesis seeks to provide algorithms for all the steps necessary to implement an INS. and is a key aspect of inertial navigation.6 Our Contributions The authors have found it diﬃcult to ﬁnd literature about INS which are both comprehensive and easily understandable. This model is used during the initial alignment phase. In the next chapter inertial navigation with aiding is described. where the quasistationary requirement are violated.
A vector r referenced in the b frame will be denoted rb .1. as they protrude out through the page. vectors as lowercase 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. y and z components of the vector. Such a rotation around the y axis is shown in Fig. rz . iy and iz . by and bz . The basis vectors of the b frame will be denoted by bx . consisting of three orthogonal basis vectors. jz vectors are coincident and invisible. The j coordinate frame has been rotated i j relative to the i frame through the iz axis by an angle α. One coordinate frame is created from another by a series of rotations around the principal axes of the frame. with x. The subscript notion will be used to extract the x.Chapter 2 Nomenclature and Mathematical Techniques The purpose of this chapter is to deﬁne the notations used throughout this report and some of the fundamental mathematical techniques which is essential in the derivation of some of the equations. The rz . there are two coordinate frames: The i coordinate frame. spanned by ix . y and z being the principal axes of the frame. Using the nomenclature 7 . In the ﬁgure. Scalars are noted with nonbold letters. rb = b rx b ry b rz T (2. and the j coordinate frame spanned by jx .1) It is often needed to represent a vector in diﬀerent coordinate frames. All frames used in this report will span a three dimensional Euclidean space. iz . jy and jz . 2.
1: Rotation around the positive y axis j ζ + ry = = = i ry cos(α) i rx sin(α) i i ry cos(α) − rx sin(α) ζ j ry j rx. iy jy i ry α ζ ζ r j ry α j rx jx α α i j rz . The rotation i matrix is also called the Direction Cosine Matrix (DCM). The components of a vector i i i r in the i frame is found in the ﬁgure as the bold lines rx .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. Figure 2. jz i rx j rx.1 can now be used to ﬁnd the equations transforming the vector r from the i frame to the j frame. Cj is the rotation matrix transforming r from frame i to frame j. ry . rz . iz .1 + ξ = rx cos(α) + ry sin(α) ξ j rx j i rz = rz It is customary to write these equations in matrix form as j i rx rx cos(φ) sin(φ) 0 j i ry = − sin(φ) cos(φ) 0 ry j i 0 0 1 rz rz rj = Cj ri i Here.1 i = rx cos(α) i = ry sin(α) j i i = rx. rx . The DCM’s corresponding to rotations around all principal axis are stated here for completeness . rz . and in the j frame as j j j the bold lines rx . and the angle α. rx .1 ξ ix Figure 2.
y j ωij.3) (2. that the order in which the rotations around the principal are applied are not arbitrary.9 CHAPTER 2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES 1 0 0 Rotation around X axis 0 cos(φ) sin(φ) 0 − sin(φ) cos(φ) cos(θ) 0 − sin(θ) 0 1 0 Rotation around Y axis sin(θ) 0 cos(θ) cos(ψ) sin(ψ) 0 Rotation around Z axis − sin(ψ) cos(ψ) 0 0 0 1 (2. it is necessary to describe both which frames are involved and in which coordinate system the angular velocity is expressed.2) (2.x j ωij.5) It is important to note. An XYZ rotation is not in general the same as an YXZ rotation.4) The rotation matrix from arbitrary frame “i” to arbitrary frame “j” is constructed by multiplying the rotation around “z” with rotation around “y” and with rotation around “x” and results in the following rotation matrix: Cj = i cos(ψ) cos(θ) sin(ψ) cos(φ)+cos(θ) sin(θ) sin(φ) sin(ψ) sin(φ)−cos(ψ) sin(θ) cos(φ) − sin(ψ) cos(θ) cos(ψ) cos(φ)−sin(ψ) sin(θ) sin(φ) cos(ψ) sin(φ) sin(θ) − sin(φ) cos(θ) cos(θ) cos(φ) (2. the angular velocity of frame j relative to frame i is expressed in the j’th coordinate system this is written as: j ωij = j ωij. As a general rule when a DCM transforms from one coordinate system which is orthogonal to another coordinate system which is also orthogonal the following property exist: j Ci = (Ci )T j Similarity Transformations If an equation is wished to be expressed in another coordinate system this can be achieved using the DCM. Relative Angular Velocity of Frames When describing the relative angular velocity of two frames. If e.g.z T . 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 ⇓ Cj ACi rj = vj j i Matrix A is said to be transformed under a similarity transformation.
9) in (2. they are denoted diﬀerently. The skew symmetric form is due to the small angle simpliﬁcations applied to the cosine and sine parts of the DCM from (2.9) ∆θβ −∆θα 0 where ∆θα . A physical vector measured by a sensor is denoted with a tilde and a computed quantity is denoted by a hat: ˜j ωij = measured angular velocity ˆk ωik = computed angular velocity Skew Symmetrix Matrices Throughout the thesis. i By deﬁnition. the time derivative of a DCM is: j Ci (t + ∆t) − Cj (t) ∆Cj i i ˙ = lim Cj = lim i ∆t→0 ∆t→0 ∆t ∆t (2.7) Time Derivative of the Direction Cosine Matrix At time t.8) Using a geometrical interpretation the small angle rotation Cj (t + ∆t) is the same as i Cj (I + ∆θ i ) where ∆θ i is the skew symmetric matrix related to the small angle rotation i j from of Ci from time t to time t + ∆t. In doing so. Using the skew symmetric matrix. the i frame is rotated to a new orientation so the DCM is now Cj (t + ∆t). Substituting (2. the cross product of two vectors reduces to doing a matrix multiplication: a × b = Ab (2. it is sometimes convenient to change between a vector representation and a matrix representation.6) where A is the skew symmetric matrix form of a.8) yields: ∆θ i ˙ Cj = Cj lim i i δt→0 ∆t (2.10 Computed and Measured Quantities In order to distinguish between actual measurements from sensors and computed quantities used in the computer. the skew symmetric matrix is introduced.5). The elements in the skew symmetric matrix is as follows: a= a1 a2 a3 T 0 A = a3 −a2 −a3 0 a1 a2 −a1 0 (2. ∆θβ . The result is: 0 ∆θγ −∆θβ i 0 ∆θα ∆θ = −∆θγ (2.10) . During the i time ∆t. ∆θγ is the small angles that the i frame has been rotated through. the relation of frame i and j is expressed by the DCM Cj (t).
i (2.10) can be written as: ˙ Cj = Cj Ωi = −Ωj Cj i i ji ij i (2.11 CHAPTER 2.12) . This vector can be expressed as a skew symmetric matrix: i ωji → Ωi ji i i where Ωji is a skew symmetric version of ωji of the form from (2. NOMENCLATURE AND MATHEMATICAL TECHNIQUES Inspecting the part ∆θ when ∆t → 0 reveals that it is the same as the angular velocity ∆t i of the i’th frame with respect to the j’th frame and this is denoted as ωji . The frame of reference can be changed by transforming the matrix under the similarity transform j Ωi = Ci Ω j C i j ij ij Using this property.11) These two rotational representations will be used interchangably throughout the report.7). (2.
as they are an order of 10−7 [g] or less. However. i. These could be obtained using only the inertial frame. as this would require knowledge about the entire universe. but in practice this is impossible. the inertial frame i is chosen as an earthcentered inertial frame (ECIF) which has origin at the center of the earth. EarthCentered Inertial Frame In INS applications. the inertial frame should include all forces exerted on the IMU. In terrestrial navigation. In this thesis. it is important to deﬁne the coordinate frames used throughout this report as they are essential to the derivation of the INS equations.Chapter 3 Frames of Reference and Position Determination Before explaining the principles of INS. In order to model the output of the sensors. most inertial navigation systems today use an additional set of frames. 1971]. height. one is primarily interested in knowing longitude. An INS uses accelerometers and gyros that register known physical phenomena caused by the acceleration or rotation of an Inertial Measurement Unit (IMU). The choice of elementary vector representing this axis (ix .1. iy or iz ) is a matter of preference. latitude. with the iz vector going through the equator at the longitude where navigation was started. 3. north velocity.e. An IMU in this thesis deﬁned as a collection of three gyroscopes and three accelerometers arranged as an orthogonal triad. Also the gravitational pull of other celestial bodies can be disregarded. the iy vector is chosen. east velocity. 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. Ideally. a frame in which there are no forces acting. an inertial frame is needed as a reference frame. and one axis parallel to the direction of earth rotation. azimuth and orientation. 12 . that as the resolution of most inertial sensors today are limited. The frame is seen in Fig. it can be shown. and in the literature. Fortunately. diﬀerent ones are used. while the ix completes the righthand coordinate system.
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. .1: The inertial frame is spanned by ix . iy and iz . ey ωie N. gx iz Equator ix ez λ L l ex Figure 3. FRAMES OF REFERENCE AND POSITION DETERMINATION 13 iy . and thereafter rotated negatively around the new x axis through the latitude angle l. gy U. gz E.CHAPTER 3.
the rotation matrix from the ECEF to the NF is . The choice of geodetic frame does not inﬂuence navigation performance and can be freely chosen as long as the constraints just mentioned are met. Up frame (ENU). 3.1. 3. where North is undeﬁned. 3. Using (2. Using this mechanization. 1971].1) Local Geodetic/Geographic Frame The Local Geodetic/Geographic Frame g deﬁned as a frame which has origin at the position of the IMU. one can simply integrate the acceleration resolved in the North and East axis to obtain velocity in north. one pointing vertically down or up. The choice of navigation frame is also known as a mechanization. Using (2. This frame is seen in Fig. where North has inﬁnitely many solutions. EarthFixed Frame The earthcentered. Height is still a double integration of acceleration in the nz . The ECEF frame is seen in Fig. one can still integrate the accelerations to obtain velocities. gy = gn and gz = gu . y and z axes are also called e. and east respectively. The diﬀerence from the geodetic frame is that the navigation frame can be rotated about the vertical axis. the ECEF frame is rotated at an angle λ through the ey axis.2. North. to be rotating with the earth at the earth spin rate (ωe ). With the geographic frame. but as time progresses. but they must be rotated through α to obtain it in North and East components. where λ = ωe t. with the last completing the righthand coordinate frame (pointing east or west). Height is obtained by integrating acceleration in the down axis. the navigation frame (NF) n is deﬁned as a frame which has origin at the position of the sensor frame and has one principal axis pointing vertically up or down. earthﬁxed frame (ECEF) e is deﬁned. n and u axes in the case of an ENU frame. an NED frame is chosen as a navigation frame. A common geodetic frame. (2. using the geodetic frame as a navigation frame introduces a singularity at the North pole. One other popular formulation is the NorthEastDown (NED) frame. so gx = ge . the local geodetic frame is always locally level. As will be evident later in this thesis. Navigation Frame As with the local geodetic frame.3) and (2. the rotation matrix from ECIF to ECEF is cos(λ) 0 Ce = i sin(λ) 0 1 0 − sin(λ) 0 cos(λ) (3. and the South pole.3). with one axis pointing north. In [Britting. 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. However.2).1. As with the ENU frame. 2000].4). As two of the axes are horizontal.14 EarthCentered. is the East. which is also the geodetic frame in this thesis. the wander angle mechanization also simpliﬁes the navigation equations which reduces the computational demand of the system. At time t = 0 the ECEF and ECIF are coincident. the x. which is produced by rotating the ECEF frame around the ey axis at a longitude angle L. To overcome this limitation. This rotation is seen in Fig. current INS typically uses a mechanization known as wander angle (WA) [Savage. and then through a latitude angle l through the new ex axis.
Although one might argue that navigation involving pole traversal are not common. this suﬀers from the same problems as the NED navigation frame mechanization.2: The wander angle α is the angle which the navigation frame is rotated around the z axis with respect to the geodetic frame. Cn = e cos(L) cos(α)−sin(L) sin(l) sin(α) cos(l) sin(α) − sin(L) cos(α)−cos(L) sin(l) sin(α) − cos(L) sin(α)−sin(L) sin(l) cos(α) cos(α) cos(l) sin(L) sin(α)−cos(L) sin(l) cos(α) cos(l) sin(L) sin(l) cos(L) cos(l) (3. the latitude is 90◦ . but longitude is not uniquely deﬁned. the choice between DCM’s and quaternions is more a matter of . Historically. etc. and the navigation information is destroyed.3) Body Frame The body frame (BF) is ﬁxed to the casing of the IMU. Today. longitude. they tend to be unintuitive and signiﬁcant calculations are needed for outputting the variables of interest (latitude. The rotation matrix from the sensor frame to the body frame is found by the sensor calibration algorithm. latitude and height of the system. FRAMES OF REFERENCE AND POSITION DETERMINATION 15 ny α gy nx gx Figure 3. The sensor frame is deﬁned so that the xy plane is spanned by the x and y accelerometer. Having a singularity also decreases navigation performance close to the poles due to numerical problems because of ﬁnite precision within the navigation computer.). pitch and yaw angle. Therefore. with the x accelerometer corresponding to the xaxis. However. quaternions have been used because they required less processing power during navigation as compared to DCM’s. it is not possible to restore longitude information after a pole traversal. INS today uses either quaternions parameters or DCM’s internally to keep track of position. and thus the singularity is not an issue. At the poles. However. Position Tracking An INS could keep track on position by keeping track of longitude. and rotated with respect to the navigation frame by a roll.CHAPTER 3. Sensor frame The sensors are not always perfectly aligned mechanically with respect to the body frame. The sensor inputs are rotated from the sensor frame to the body frame prior to being used by the navigation equations.2) And the rotation matrix from the geodetic frame to the navigation frame is cos(α) Cn = − sin(α) g 0 sin(α) cos(α) 0 0 0 1 (3.
Assigning the entries of the Ce matrix so Cij corresponds n to the i’th column and the j’th row of the Ce matrix as follows: C11 C21 C31 n (3. In this thesis. longitude. .6) (3.2). l and a terms. 1997]. only the ﬁrst two columns of Ce are needed to obtain the wanted information. From (3. n Omitting the calculation of the third column reduces the processing power needed for the navigation equation. the Cn DCM is used to keep track of the poe sition on earth.5) (3.7) As seen. latitude and wander angle can be extracted by n isolating the L.4) Ce = C12 C22 C32 C13 C23 C33 Yields L = l a = = C13 C11 C22 − C21 C12 arcsin(C23 ) C21 arctan C22 arctan (3.16 preference[Savage.
Also. 4. By introducing the navigation frame. 17 . assume that the earth is not rotating. bz bx ab z by ω b ay Accelerometers Gyro Figure 4. as well as accelerate in the by and bz direction. it is assumed that the sensor frame and the body frame are coincident. the variables of interest is the latitude l. First. the navigation frame is chosen as a ENU geodetic frame. This system is allowed to tilt by an angle θ at a rate ω.1. our system is only able to navigate around the meridional (northsouth) plane. and the tilt θ of the system. This two dimensional system is depicted in Fig. When navigating on the earth. the height h. In this system. so the inertial frame is equal to the earth frame. it can be seen from the ﬁgure that the derivative of l (in radians) is equal to the velocity of the body to the north divided by the radius of the earth plus the height of the system. In many applications. consider a 2 dimensional YZ IMU as depicted in Fig. Many of the concepts in the two dimensional case can be directly transferred into the three dimensional case. Being a 2D INS. The derivative of h is equal to the velocity of the body in the vertical direction.Chapter 4 Principle of a Strapdown Inertial Navigation System In order to get an idea of the concepts and principles of inertial navigation systems. Here. one uses the notion of longitude. 4. latitude and height above the earth surface to indicate where one is positioned relative to the earth. a simple example is given in two dimensions. we would like to know our current position relative to some other point on the earth surface.2. For simplicity.1: Inertial Measurement Unit measuring in two dimensions.
n vn R0 + h n ˙ h = vu l˙ = (4. and the above equation can then be used to calculate it at any point from there on. ey nn by l h bz nu θ R0 iz . Recognizing that the rotation rate of the navigation frame with respect to the . These are obtained using the measured accelerations. vn n and vu . Having obtained the accelerations in the navigation frame.18 iy . these needs to be rotated into the navigation frame as follows an = ab cos θ + ab sin(θ) n y z an = −ab sin θ + ab sin(θ) y z u (4.2) Where the tilt of the platform θ needs to be calculated. As these measurements are given in the body frame. one must take into account the accelerations arising from this rotation. 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).1) n To ﬁnd these we need to ﬁnd the velocities of the system in the navigation frame. ez Figure 4. Because the navigation frame is level on earth. 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.3) n The vn /(R0 + h) term is also called the “transport rate”. one can obtain the velocity as an integration of the acceleration. gravity only inﬂuences the u axis As the navigation is rotating with respect to the inertial frame. ˙ θ=ω− n vn R0 + h (4. The platform is tilted by an angle θ with respect to the earth surface.2: INS system around the meridian plane. This is also known as the coriolis force. It is the rate at which one needs to tilt the platform in order to keep it level on the ground. The acceleration measured by the accelerometers is comprised of both gravity and mass movement.
4) and (4.5) These are integrated to yield the velocity of the system.5) to yield the latitude. At start of navigation. PRINCIPLE OF A STRAPDOWN INERTIAL NAVIGATION SYSTEM inertial frame is the transport rate of (4. (4. the system must be stationary so the initial velocities are 0.3).4) (4.19 CHAPTER 4.3).1). The job of the navigation computer is to integrate (4. height and tilt of the system. the derivative of the velocity of the system is: vn = an − ˙n n n vn vn R0 + h u n vn vn vu = an − g + ˙n u R0 + h n (4. The principles explained in this chapter are used in the next part to derive the navigation equations for a full three dimensional case. (4. .
.
.
Part II Inertial Navigation Systems .
.
pitch and yaw. The next chapter (Ch. longitude. Before inertial sensors are incorporated into an INS. velocity and attitude outputs. some of which are reproducible and hence correctable. Ultimately. the navigation phase can begin. In this part. This is used by the navigation algorithm to transform the measurements from the body frame to the navigation frame. 7) derives the equation of motion of an INS which are used by 24 . the alignment algorithm estimates the initial value of the body frame with respect to the navigation frame. A good example is GPS aiding. mathematical formulae needed by the above algorithms are derived. these errors are identiﬁed by a calibration algorithm. inertial navigation systems tend to drift exponentially from the true values. During navigation. this can be done by measuring gravity and earth rotation. pitch and yaw of the system. The important aspects of this topic is introduced in the ﬁrst chapter (Ch. the geometric aspects of the earth. that is. navigation and aiding. However. pitch and yaw from inputs from the gyroscopes. height. This shortcoming can be overcome by aiding the INS with various external sensors which can bound the errors propagating in the INS algorithms. which are the primary variables of interest in inertial navigation systems. 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. The sensors used in INS all have errors. and the magnitude of gravity have a large inﬂuence on the INS. this results in the latitude. because of sensor errors and the double integration from acceleration to position. The structure of the part is as follows: As terrestrial navigation occurs in the vicinity of the earth. the roll.Chapter 5 Introduction Inertial navigation consists of three major parts: Alignment. a navigation algorithm continuously calculate the roll. which can bound the position and velocity of the “fused” INS/GPS system whilst keeping the precision of the INS. 6). During the alignment phase. so acceleration measurements can be resolved in the navigation frame and integrated to yield velocity and subsequently positions with respect to the earth. When the alignment phase has obtained a suﬃciently accurate estimate of the roll. By requiring the IMU to be stationary during alignment.
the alignment algorithm is derived in Ch. indirect Kalman ﬁltering equations and the navigation equations. INTRODUCTION both an earth simulation model and the navigation equations. The earth simulation model translates longitude. Indirect Kalman ﬁltering uses an error model of the INS which is derived in Ch. Longitude. 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.25 CHAPTER 5. 12.1: Simulation and INS The earth simulation model and navigation equations are developed in the two following chapters. So the last two chapters describes how to calibrate the sensors. Ch. latitude. Before developing the alignment and aiding algorithms. 11. GPS aiding and speedometer aiding.1. Using this error model. The last two chapters of this part present a somewhat diﬀerent part of this project than the preceding chapters. chapter 10 gives an introduction to indirect Kalman ﬁltering. The earth simulation model is derived in order to test the developed algorithms under controlled conditions. Two diﬀerent aiding approaches is presented in Ch. 9. Chapter 15 shows how the sensor errors are identiﬁed using a test ﬁxture to rotate the IMU through a series of rotations. latitude height and attitude trajectories INS Figure 5. 8 and Ch. 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. 13. 14. . 5. The sensor calibration equations are based on sensor error models which are presented in Ch. latitude height and attitude trajectories Accelerations Angular Rates Simulation model Longitude.
knowledge is needed about the geometric aspects of the earth as well as the gravitational ﬁeld surrounding the earth. These are deﬁned as the radius of the circle one would travel in. These values are known as geodetic variables and constants.4) (6. which deﬁnes many of the constants that are used in INS.1 Geodetic Variables and Constants The rotational rate of the earth have been well known for many years.2) The equatorial radius of the earth.1) ωe = 7292115.5) Two important geometric aspects of geodecy is the meridional and the normal radius of curvature. The meridional radius of curvature is the radius of the circle one would follow if one travelled directly north or south. 6. also known as the semiminor axis radius rp = 6399592 The eccentricity of the earth is deﬁned as ǫ= 1− 2 rp = 0. as well as a gravity model.Chapter 6 Geodesy In order to develop the equations of motion of a vehicle in the vicinity of the earth.1. This is depicted in Fig.0818191908426 2 re [ m ] (6. 6. if one continued ones current curve. The most widely used model of the earth is the World Geodetic System (WGS84).0 · 10−11 Expressed in earth coordinates e ωie = 0 ωe 0 (6. also known as the semimajor axis radius re = 6378137 The polar radius of the earth.3) [ m ] (6. and can be expected to be constant [ rad/s ] (6. The normal radius of 26 .
the gravitational magnitude can be calculated approximately using the following formula. curvature is the radius of the circle if one travelled directly east or west.2 Gravity Model From [Britting.27 CHAPTER 6. 1971].1: Figure showing the diﬀerence between the circle one would follow if one traveled directly north compared to the earth’s curvature. µ r2 3 2 1 − J2 (1 − 3 cos(2l)) − rωe cos2 (l) 4 g= (6.9) µ 3 2 1 − 4 J2 (1 − 3 cos(2l)) − rωe cos2 (l) r2 From [Savage. As apparent gravity is always pointing vertically. The equations giving the radii is a function of geographic latitude. It is used both in the earth simulation model and in the navigation algorithm as suﬃcient precision is not possible assuming a spherical earth and constant gravity. Meridional (northsouth) radius of curvature re (1 − ǫ2 ) (1 − ǫ2 sin2 (l))3/2 re (1 − ǫ2 sin2 (l)) rmer = (6. GEODESY North rmer Earth Surface Circle of curvature Figure 6.6) Normal (eastwest) radius of curvature rnor = (6.10) The geodetic values and gravity model described in this chapter is used throughout this thesis. and µ are empirical constants calculated by geodetic measurements and r is the radius from earth surface to the point of navigation.8) Where J2 .7) 6. r is approximated as: r = re 1 − ǫ sin2 (l) + h (6. the gravity in the navigation frame is 0 0 gn = (6. 1997]. .
GRAVITY MODEL 28 Before developing the earth simulation model and navigation algorithms the equations of motion need to be derived. relative to the earth. of a vehicle in the vicinity of the earth.6. .2. These equations are derived in the following chapter and describe the motion.
Due to the principle of equivalence. 4. As the output from an INS is velocity and position relative to earth. Rotating this into the navigation frame through Cn yields the acceleration in i the navigation frame. 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 later chapters. The acceleration due to gravity is given in the navigation frame as gn . an = Cn¨i + gn ir Where an is the acceleration in the navigation frame.2) The result of this chapter is an expression relating vn to the accelerometer measurements given in the navigation frame an . In this chapter. also known as the “ground speed”. deﬁned as follows vn n ˙ Ce re (7. it is convenient to derive this equation in terms of the geographically referred earth referenced velocity. 1971]. this equation is integrated to yield velocity and subsequently position of the INS.1) (7. Acceleration due to mass movement is the double derivative of the position vector in the inertial frame. these cannot be distinguished and are both measured by the accelerometers [Britting. Derivation of Equations of Motion Acceleration is comprised of two terms: The acceleration due to mass movement and the acceleration due to gravity. this is done by deriving a diﬀerential equation of the speed of a vehicle relative to earth.Chapter 7 Equations of Motion As shown in the example in Ch. This is the velocity of ri seen from the earth frame transformed into the navigation frame. ˙ The dependence of re on ri is obtained as follows 29 .
11) yields the ground speed of a vehicle as a function of accelerometer measurements.6) and rearranging the terms yields n ˙ v n = Ci i Ωi − Ωie ni (7. Inserting into (7.9) By isolating the inertial acceleration r and multiplying by of (7.12) Rearranging and inserting the expression for gn into (7.2) yields the following ne n e e ie ni i ˙ vn = Cn ¨i − Ωi + 2Ωi Cn vn − Ωi Ωi ri i r en ie ie ie (7.1) and rearranging yields the equation that was sought after ˙ vn = an − (Ωn + 2Ωn ) vn − Cn Ωi Ωi ri − gn en ie i ie ie (7.10) Inserting into (7. ie ie resulting in a so called “apparent” or “plumb bob” gravity term: gn gn + Cn Ωi Ωi ri i ie ie (7.1) ¨i Cn i one obtains the ﬁrst term i ˙ Cn¨i = vn + Cn Ωi + 2Ωi Cn vn + Cn Ωi Ωi ri en ir i ie i ie ie (7.11) It is customary to include the centripetal term Ωi Ωi ri in the equation of gravity.7) r Ci re + Ωi ri + ¨i − Ωi Ωi ri ni ie e˙ ie (7.4) one obtains ˙ ri = Ci re + Ωi ri e˙ ie Inserting this into (7.2) and diﬀerentiating vn ei ie with respect to time yields vn = Cn ri − Cn Ωi ri i ˙ i ie n ˙ n ri + Cn¨i − Cn Ωi ri − Cn Ωi ri ˙ ˙ v = Ci ˙ ir i ie i ie ˙ i ˙ = Cn Ωi ri + ¨i − Ωi Ωi ri − Ωie ri r i ie ˙ ni ie (7. the rotation rate of earth.3) (7.5) = Cn i r Ωi − Ωi ri + ¨i − Ωi Ωi ri ni ie ni ie ˙ (7. the rotation of the navigation frame relative to earth and the apparent gravity: .30 re = Ce ri i ˙ ˙ re = Ce ri + Ce ri i˙ i = Ce ri + Ce Ωi ri i˙ i ei = Ce ri − Ωi ri ie i ˙ (7.8) i = Cn Ωni Ci re + Ωi Ωi ri − Ωi Ci re − Ωi Ωi ri + ¨i − Ωi Ωi ri r i e˙ ni ie ie e ˙ ie ie ni ie n = Ci i r Ωi Ci − Ωi Ci re − Ωi Ωie ri + ¨i ie ni e ie e ˙ Knowing that Ci = Ci Cn and Ωi = Ωi − Ωi and inserting (7.6) ˙ By isolating ri from (7.4) Notice the sign change due to Ωi = −Ωi .
and the INS navigation equations. this equation is used both to develop a simulation model.13) In the following chapters. .31 CHAPTER 7. EQUATIONS OF MOTION ˙ vn = an − (Ωn + 2Ωn ) vn − gn en ie (7.
Chapter 8 Earth Simulation Model In this chapter. 7. accelerometer and gyroscope. Restating the equation of motion (7. but welldeﬁned conditions such as erroneous sensor outputs.1) b ˙ ab = Cn (vn + (Ωn + 2Ωn ) vn + gn ) en ie (8. This chapter is divided into two sections. an earth simulation model is derived using the equations of motion derived in Ch.1 Accelerometer Output In this section the accelerometer output from the earth simulation model is derived. As two sensors are used. This model enables one to create repeatable test data as well as test the system under suboptimal. the earth simulation model should output both the acceleration and rate sensed by the sensors when the vehicle is moving.13): ˙ vn = an − (Ωn + 2Ωn ) vn − gn en ie Rearranging and rotating yields the wanted output (8. 6 as 0 0 3 2 1 − 4 J2 (1 − 3 cos(2l)) − rωe cos2 (l) gn = µ r2 (8. 8.2) The plumbbob gravity term gn was calculated in Ch.3) The Ωn term is obtained by rotating the rotation rate matrix in earth coordinates into ie the navigation frame using the similarity transform 32 .
8) (8. ˙ α = L sin(l) ˙ n The Cb matrix is calculated using the body rotations Ωbn supplied by the user n (8.7) First.6) ˙ Cb = Cb Ωn n n bn At start of simulation.9) + h) Where [Savage. By doing this.4) cos(l) cos(α)ωe sin(l)ωe (8.2) is initialized at 0 and follows the following equation[Rogers. The bold ellipse is the earth surface.e h rmer Figure 8. g vn g ωeg. Figure 8. en (8. n Left is to ﬁnd the vn and Ωn terms.5) = cos(l) sin(α)ωe The wander angle (Fig. the dashed line is the meridional radius of curvature. the vertical term of vg is equal to the derivative of the height h. 2000] g ωeg = ˙ ˙ −l˙ L cos(l) L sin(l) T (8.n (rnor + h) g −ωeg.3.10) As the navigation frame is locally level.e (rmer (8. the horizontal parts of vn is found. EARTH SIMULATION MODEL Ωn ie n ωie e = Cn Ωie Ce e n T (8. This is done by ﬁrst observing the horizontal parts of the ground speed in the geodetic frame as a function of the horizontal angular rates of the geodetic frame.33 CHAPTER 8.1 shows the relationship for the north velocity. 2007]. the Cb matrix is initialized by the user. g ve g vn = = g ωeg.1: Relationship between north velocity and east angular rate of the geographic frame. one can ﬁnd a relationship between the horizontal angular rates and the horizontal velocity of the geographic frame. .
(8.5). but it does not exhibit this jump in value during navigation. ˙ ˙ L(rnor + h) cos(l) cos(α) + l(rmer + h) sin(α) ˙ ˙ vn = Cn vg = −L(rnor + h) cos(l) sin(α) + l(rmer + h) cos(α) g ˙ h (8. this earth simulation model is limited to navigation away from the poles. longitude is generally deﬁned as being within −180◦ to 180◦ . But it has been chosen to use longitude/latitude as it is more intuitive for the user than inputting a DCM. (8.14) T With n ωgn = 0 ˙ 0 −L sin(l) (8. can be eliminated. Latitude is also deﬁned as being within −90◦ to 90◦ .2 Gyro Output The outputs from the gyros are the components of the Ωb matrix which is given as ib Ωb = Cb (Ωn + Ωn + Ωn ) ib n ie en nb Which is calculated by inserting (8. This can cause problems.11) T Hence. and the horizontal terms are obtained by rotating the horizontal parts of the geographic rotation rate through the wander angle.2). GYRO OUTPUT 34 g ˙ vu = h (8.13) and (8. (8.13) n The remaining term is Ωen .3). the vertical part is set to 0. Also. however.8.3). vg = ˙ ˙ ˙ L(rnor + h) cos(l) l(rmer + h) h (8. (8. In the wander azimuth conﬁguration.17) given by the To use this earth simulation model L. One way of removing this problems from the earth simulation model is to used DCM’s instead of longitude/latitude. by removing the longitude constraint so it can attain any value.16) is inserted into (8.5).16) and the body rotations user. n 8. Height is also not expected to have jumps. . Hence. which will also cause a sudden jump. as longitude changes instantaneously at pole traversals. it is assumed that simulations will not include pole traversals. This jump.7).15) Yields n n ωen = Cg ˙ −l˙ L cos(l) 0 T ˙ −l˙ cos(α) + L cos(l) sin(α) ˙ = l˙ sin(α) + L cos(l) cos(α) 0 (8. l and h need to be diﬀerentiable. and the rotation matrix Cb is calculated using (8.2. n g g ωen = Cn (ωeg + ωgn ) g (8.12) This is then rotated into the navigation frame through the wander angle using (3.16) To get the outputs from the accelerometers. Ωn nb (8. Furthermore.
The last section describes the idea of dividing the integration of the attitude matrix into a fast and slow part.Chapter 9 Navigation Equations During this chapter the navigation equations needed for inertial navigation are derived. These equations describe how the velocity of the vehicle changes which can be integrated to give the position of the INS. The Cb matrix is calculated from gyro inputs as follows: b b n n ˙ Cn = Cn Ωnb = Cn (Ωb − Cn (Ωn + Ωie )Cb ) b b b ib en (9. this section seeks to derive expressions for the terms in the equation of motion (9. As explained in Ch. n The measured acceleration ab . This is used if the processor power is not suﬃcient to integrate the whole part with the high sample rate.3) and earth rotation Ωie (8. the transport rate is also used to calculate the Cn matrix. this matrix is used to e 35 .1 General Navigation Equations In order to determine the velocity and position of the INS. n ˙ vn = Cn ab − (Ωn + 2Ωie ) vn − gn b en (9. heading. Many of the expressions needed have already been derived in the previous chapter. The remaining ib n term in both (9.1) At start of navigation. so vn is initialized at 0.1) and (9. Apart from being used to calculate the derivative of the ground speed. 9. and earth rotation is known.1). The navigation equation continuously integrate accelerometer and gyro measurements and output attitude. The next section describes the problem with the unstable vertical channel of the INS and proposes a solution to stabilize this channel.2) Where Ωb is the input from the gyros.2) is an expression for the transport rate Ωen .5) are all n known.3. the IMU is not moving relative to the earth surface. gravity gn (8. The ﬁrst section derives the general navigation equations. velocity and position.
1. which is not deﬁned at the poles.8) (9. This reduces the computational demands of the system. and then rotating back to the navigation frame. one obtains g ωeg = g −vn (rmer +h) g ve (rnor +h) ˙ L sin(l) T (9. this expression contains the wander angle α. longitude and wander angle of the system.10) . The rest of this section will rewrite F to use the entries in the Cn matrix e instead. GENERAL NAVIGATION EQUATIONS 36 calculate latitude. so v g = Cg v n n (9. This is done as follows: Using the trigonometric identity sin2 (α) + cos2 (α) = 1 gives 1 1 sin(α) cos(α)( rnor +h − rmer +h ) 1 rnor +h − 1 1 1 − rnor +h +( rnor +h − rmer +h ) cos2 (α) 0 1 1 − sin(α) cos(α)( rnor +h − rmer +h ) 0 F= ( 1 1 rnor +h − rmer +h ) sin2 (α) 0 0 0 (9.4) (9.10).9. overcoming this limitation.6) With g ωgn = 0 ˙ 0 −L sin(l) T (9.6) and isolating vn yields the relationship between ground speed and transport rate: n ωen = = Fvn „ « sin2 (α) cos2 (α) 1 1 sin(α) cos(α)( rnor +h − rmer +h ) 0 rnor +h − rmer +h n « „ v sin2 (α) cos2 (α) 1 1 − sin(α) cos(α)( rnor +h − rmer +h ) 0 − rnor +h − rmer +h 0 0 0 (9. From there on e it follows the following equation ˙n Ce = −Ωn Cn en e which is continuously integrated by the INS.7) Substituting (9.5) and (9. As can be seen. The remainder of this section deals with the derivation of the transport rate.5) n This is used to calculate ωen using g g n ωen = Cn ωeg + ωgn g (9.9).3) By rearranging (8.8) and (8.9) n Note that the z part of ωen is always zero because of the wander angle mechanization.7) into (9. At the start of navigation the Cn matrix is given initial values by the initial alignment algorithm. The transport rate matrix Ωn is obtained from vn by rotating vn into the geographic en frame and using the relationship between ground speed and rotation rate in the geographic frame. and inserting it into the two ﬁrst positions of (8.
9.ins ˙n +  +  vz ˙n +  h c3 c2 c1 δh +  haid Figure 9. As h increases. n vz vz. yielding an increased upward acceleration.14) (9. 3. As a general rule when navigation time exceed 10 minutes. external measurements must be used to keep the vertical channel within acceptable values. relying only on inertial measurements for calculation of the vertical channel will render it exponentially unstable[Rogers.15) (h − haid )dt . which can be used in navigation without the wander angle constraint F= C21 C22 1−(C23 )2 1 rnor +h − 1 1 ( rnor +h − rmer +h ) 2 1 1 1 22 − rnor +h +( rnor +h − rmer +h ) 1−(C 21 − 1−(C C C22 2 23 ) 1 1 ( rnor +h − rmer +h ) (C )2 2 23 ) 0 0 0 (C21 ) 1 1 ( rnor +h − rmer +h ) 1−(C23 )2 (9. This is because of the dependence of g on h.13) However.1: Vertical channel control loop n ˙ h = vz − c1 (h − haid ) n vz = an − ((Ωn + 2Ωn ) vn )n − gz − c2 (h − haid ) − c3 ˙n ˙z en ie z (9. hence: n ˙ h = −vz (9. A common way of aiding the vertical channel is by implemented the following PID servo control loop.12) 0 0 9.37 CHAPTER 9. The purpose of the loop is to drive the error δh towards 0.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 deﬁnition always pointing downwards. [Savage.1. g decreases. the sin(α) and cos(α) terms can e be denoted sin(α) = C21 = cos(l) C21 1 − (C23 )2 cos(α) = C22 = cos(l) C22 1 − (C23 )2 (9. NAVIGATION EQUATIONS Using the entries in the Cn matrix as done in Ch. 2007]. 2000]. which is depicted in Fig.11) Yielding the ﬁnal result.
18) The gravity model is approximated by an inverse square law as 2 g0 r0 r2 n gz (r) ≈ − (9.23) (9.22) s + c3 = 0 (9.17) The δ¨z term is obtained from (9.24) .20) where g is the gravity magnitude at the equator.21) which can be inserted into (9. c2 and c3 are selected using classical servo control theory. diﬀerentiating (9.15) as follows: Assuming that earth rotation and vn transport rate are negligibly small compared to the other terms. VERTICAL CHANNEL 38 The PID gains c1 . To analyze the vertical channel control loop. the Laplace transform of the error forms of the equations are derived as follows: n ˙ δ h = δvz − c1 (δh − δhaid ) (9.19) where g0 is the gravity at the surface of the earth.17) 2g ˙ ¨ ¨ δ h + c1 δ h + c2 − r Taking the Laplace transform s3 + c1 s2 + c2 − With no control (c1 = c2 = c3 = 0). it simpliﬁes to n ˙z δ vz = δ an + δgz − c2 (δh − δhaid ) − c3 ˙n (δh − δhaid )dt (9. 2007]. 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.18) yields z δ¨z ≈ vn 2g ˙ ˙ δ h − c2 (δ h − δhaid ) − c3 (δh − δhaid ) r (9.16) Assuming that the error of the aiding device is constant [Rogers.2. Assuming δan to be constant.9. the second derivative is: ˙ ¨ ¨ δ h + c1 (δ h) = δ¨z vn (9. The error form is derived as n δgz = − n dgz (r) 2g δr = δh dr r (9. s3 − 2g =0 r 2g r ˙ δ h + c3 δh = c3 δhaid (9.
28) (9. However.29) The velocities in north and east can be expressed as vN vE n n = vy cos(α) + vx sin(α) (9. but includes fast changes. 3: L l a = arctan C13 C11 C22 − C21 C12 = arcsin(C23 ) C21 = arctan C22 (9. The c1 . the PID gains are usually chosen as follows s3 + 3λs2 + 4λ2 s + 2λ3 = 0 Such that c1 = 3λ c2 = 4λ2 + 2g r c3 = 2λ3 (9.20 KHz). 2007].26) (9. c2 and c3 PID gains should be chosen so that the resulting h contains both the stable characteristics of the aiding device. one . systems either used a ﬁrst order algorithm at a very high update rate (10 .39 CHAPTER 9. yielding an unstable system because of the positive root.25) Where λ is usually chosen to be 0. the processing power needed to execute the relatively complex algorithms must not exceed the available processing power.100 Hz). NAVIGATION EQUATIONS which has the solutions 0 and ± 2g/r. 9. or a higher order algorithm at lower update rates (50 .30) (9. The integration algorithms must be executed at a rate fast enough to include all major dynamics of the system.31) = n vx cos(α) − n vy sin(α) 9.4 Discrete Integration Algorithms The navigation equations are integrated by discrete digital algorithms in the navigation computer. whereas the height measured by the INS are unstable. the relatively high dynamics experienced during vehicle motion causes some problems regarding the bandwidth of the system. digital integration is accomplished by dividing the integration algorithm into two parts [Savage. 2000]. In order to obtain this.27) (9. but have stable characteristics. and the high dynamics of the height captured by the INS.01 [Rogers. The aiding device is unable to capture fast changes in height. Conversely. Earlier. longitude and height can be extracted from the Cn matrix as described e in Ch. In many systems today.3 Navigation Output The latitude.
The test was performed by strapping the Kearfott unit to the backseat of a car and driving a route of approximately 1. as it can be solved by integrating all the terms in the navigation equations in the navigation computer. the navigation algorithm is compared to an already existing INS unit. but the sensor data is available to the user at 50Hz.5. The same longitude/latitude and velocity was used for both systems. seems to follow the reference system good enough to justify a successful validation of the navigation algorithm. Looking at the Kearfott position it is clear that at the end it makes the same hooklike turn. It can be seen that the navigation developed in this chapter followed the reference system quite accurately during the ﬁrst minutes of the navigation. although this cannot be veriﬁed. This approach is known as a twospeed approach. VALIDATION OF NAVIGATION ALGORITHM 40 for the parts with high dynamics. 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. however. as the processing power available in modern microcomputers has increased dramatically during the last decades. What is also interesting is that the car was stopped at the starting point.5km. This is quite possibly because of the lower update rate available (50 Hz) compared to the operating speed of the reference system (normally >2kHz). the integration problem have decreased. The drive time was approximately 5.100 Hz) are executed by the navigation computer software. Furthermore any sudden turns were avoided. The Kearfott unit makes it possible to log both longitude/latitude determined by the Kearfott and the raw sensor data. The parts running with a lower iteration rate (50 . The results are shown in Fig. the navigation developed in this thesis drifts away from the reference system and at the end the navigation developed in this chapter drifts South. After a while. The curve. The parts running with a high iteration rate (1 2 KHz) are lower order integration algorithms.2. By applying the sensor outputs obtained from the Kearfott unit to the navigation algorithms derived in this chapter. the latitude and longitude results was compared to those of the Kearfott. Investigating the position just before it starts to drift East reveals that the position is 88m further East and 22 meters . 9. However. which needs to be run at a high iteration rate and one for the parts with low dynamics.6 minutes. It goes South and turns East but does not stop at the same point and furthermore starts to drift east. which are run by specialized hardware or highly optimized software algorithms.5 Validation of Navigation Algorithm In order to validate the navigation algorithm derived in this chapter.9. which can be run at a lower rate. so the route should end where it started. The speed did not exceed 60km/h and modest acceleration was utilized. As a reference INS the Kearfott T16 medium precision INS has been chosen. The initial rotation matrix Cn is the one determined by the Kearfott used by the navigation equations b derived in this chapter. This makes it possible to compare the performance with respect to a commercial product. It does not say at which sample rate the sensor data is available to the INS algorithms running within the Kearfott. 9.
The sensor frame was assumed equal to the navigation frame.085 −2.8 shows how various errors within an INS inﬂuences the longterm (48 [hour]) performance of the system.08 −2. one can investigate how the errors propagate through the system.188 Latitude 57. Kearfott T16 57.078 −2. By introducing errors in the initial INS states or sensor measurements. as well as initial tilt.41 CHAPTER 9.187 57. This diﬀerence is due to the drift of the INS when no aiding device is present.084 −2.3 through Fig. 9. In the simulations.184 57. 9.182 −2.189 57.079 −2. NAVIGATION EQUATIONS Our system vs.082 −2.186 57.192 57. Gyro and accelerometer bias errors are examined. 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.191 57.185 57.086 Our system Kearfott T16 −2. Sensor errors were introduced by adding a bias between the simulation model and the navigation equations.081 Longitude −2. velocity and position errors.191 [deg] latitude 0 [deg] longitude. where the INS was set to being stationary at 57. the gyro and accelerometer output is calculated by the earth simulation model. Figure 9.19 57.2: Comparison between our INS and the Kearfott INS.183 57.083 −2. further North than the start point.077 Figure 9. . It is more diﬃcult 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.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. heading.
all errors are bounded.3: Error propagation with an accelerometer bias of 50 [µg]. ANALYSIS OF LONG TERM ERROR PROPAGATION 42 Accelerometer bias error 15 Error [mrad] 10 5 0 −5 x 10 −3 Roll 0.05 −0. It can be seen that apart from the heading. The heading drift is so small it can be assumed negligible.9.6.05 0 −0.1 0 10 Velocity East North 0 x 10 −3 10 20 Pitch 30 40 20 30 40 Latitude 100 Error [m] 50 0 −50 5 Error [mrad] 0 −5 −10 −15 0 x 10 −3 10 20 Yaw 30 40 0 10 20 30 40 Longitude 200 Error [m] 100 0 −100 5 Error [mrad] 0 −5 −10 −15 0 10 20 30 Time [h] 40 0 10 20 30 Time [h] 40 Figure 9. .1 Error [m/s] 0.
05 0 −0.1 Error [mrad] Error [m/s] 0.4: Error propagation with an gyro bias of 0.1 Error [mrad] Error [m] 0. It can be seen.5 0 −0.05 −0. .1 0 10 20 Pitch 0. that longitude and heading error grow unbounded.5 Velocity East North 0 10 20 30 40 Latitude 0 x 10 4 10 20 30 40 Longitude 5 5 0 0 10 20 30 Time [h] 40 0 0 10 20 30 Time [h] 40 Figure 9.05 0 −0.05 −0.43 CHAPTER 9.01 [◦ /hr]. NAVIGATION EQUATIONS Gyro bias error Roll 0.1 0 10 20 Yaw 10 Error [mrad] Error [m] 10 30 40 5000 0 −5000 −10000 30 40 1 0.
02 −0. All errors are bounded.04 0 10 Velocity East North 0 x 10 −3 10 20 Pitch 30 40 20 30 40 Latitude 200 Error [m] 100 0 −100 −200 0 10 20 30 40 2 Error [mrad] 1 0 −1 −2 0 10 20 Yaw 30 40 Longitude 500 Error [m] 0 −500 −1000 −0.03 −0.05 −0.6.04 Error [m/s] 0.02 0 −0.02 Error [mrad] −0.06 0 10 20 30 Time [h] 40 0 10 20 30 Time [h] 40 Figure 9. .5: Error propagation with an initial heading error of 25E6 [rad]. ANALYSIS OF LONG TERM ERROR PROPAGATION 44 Initial heading error −3 2 Error [mrad] 1 0 −1 −2 x 10 Roll 0.9.04 −0.
5 −1 0 10 20 30 Time [h] 40 Figure 9.5 −1 x 10 Roll 2 Error [m/s] 1 0 −1 −2 x 10 −4 Velocity East North 0 x 10 −5 10 20 Pitch 30 40 0 10 20 30 40 Latitude 2 Error [m] 1 0 −1 −2 0 10 20 30 40 1 Error [mrad] 0.5 0 −0. All errors are bounded.5 0 −0.5 −1 0 x 10 −4 10 20 Yaw 30 40 Longitude 4 Error [m] 2 0 −2 −4 0 10 20 30 Time [h] 40 1 Error [mrad] 0.6: Error propagation with an initial position error of 1 [m]. NAVIGATION EQUATIONS Initial position error −5 1 Error [mrad] 0.5 0 −0.45 CHAPTER 9. .
04 0. All errors are bounded.05 0 10 20 Yaw 0.02 0 10 20 30 Time [h] 40 30 40 Longitude 2000 1000 0 −1000 −2000 0 10 20 30 Time [h] 40 Figure 9.02 0 −0. ANALYSIS OF LONG TERM ERROR PROPAGATION 46 Initial pitch error Roll 0.7: Error propagation with an initial pitch and roll error of 25E6 [rad].15 Error [mrad] 0.05 0 −0.5 Velocity East North 0 0 −0.06 Error [mrad] Error [m] 0.05 0 10 20 Pitch 30 40 −0.9. .05 Error [mrad] Error [m/s] 0.5 0 10 20 30 40 Latitude 1000 Error [m] 500 0 −500 −1000 0 10 20 30 40 0.1 0.6.
05 0 −0.01 0 −0. The heading drift is so small it can be assumed negligible.8: Error propagation with an initial velocity error of 0.02 0 10 20 Yaw 0.02 Error [mrad] Error [m] 0.01 −0.47 CHAPTER 9.02 0 10 20 Pitch 0.02 Error [mrad] Error [m/s] 0.01 −0.1 0 10 Velocity East North 20 30 40 Latitude 20 30 40 Longitude 20 30 Time [h] 40 Figure 9.02 0 10 20 30 Time [h] 40 200 100 0 −100 −200 0 10 30 40 100 50 0 −50 −100 0 10 30 40 0. .1 [m/s].05 −0. NAVIGATION EQUATIONS Initial velocity error Roll 0.01 −0.01 0 −0.1 0.02 Error [mrad] Error [m] 0.01 0 −0.
9. The platform is tilted with respect to the earth. This shows the importance of good alignment of an INS. one needs to know the initial attitude and heading of the system. their contributions are very small compared to that of the gyro bias. It is also the main cause of the requirement of 1 [nm/h] INS to have a gyro bias of 0. and will give an acceleration in the east direction. but not as signiﬁcant as gyro errors. which will be explained in the following chapters. it is seen that the INS will measure an acceleration to the west.4 and Fig. Because of the acceleration. 9.01 [◦ /h] or less[Savage.7 is it seen that even small gyro bias and tilt error give rise to position errors in kilometers. At a time between (a) and (b). This gives rise to an oscilation because of gravity being resolved incorrectly. Although both accelerometer bias and initial velocity error contribute to an unbounded yaw error.6. the platform will be level relative to earth. What is also very important to notice is that the unbounded yaw error is also mainly due to gyro bias. to obtain attitude. 9. is the system initiated with an tilt error of θ. An INS continuously integrates the equations derived in this chapter.9(a) is the initial time where navigation is started. 1997]. From Fig. and Fig. Interesting is it to see. Position errors are not very signiﬁcant. This can be recognized as being the frequency of a pendulum with the same length as the earth radius. velocity and position of an INS. Figure 9. the velocity will decrease.4 [min]. . As the INS assumes that the system is moving east. However. This will result in gravity being resolved wrongly. In (a). All the other errors give rise to a position error with a maximum of 500m and most of them below. but will still have a velocity to the east. so it will continue to torque the platform. although the system is actually not moving. it will torque the Cn matrix (the platform) e around the north axis using the transport rate.9.4 [min]. The Schuler oscillations apparent in the ﬁgures arise from the torquing of the platform in order to keep it level. This is done in a process called alignment. Figure 9. ANALYSIS OF LONG TERM ERROR PROPAGATION 48 One general characteristic on all the ﬁgures are oscillations with a period of 84.9: Schuler oscilation. Assumed velocity Assumed acceleration Torquing due to transport rate θ n gu n n Assumed acceleration Torquing due to transport rate θ n gu g g n ge n ge (a) (b) Figure 9. before navigation. whereas initial velocity errors should be kept to a minimum. that initial tilt errors and gyro bias are by far the biggest contributor to the errors in the system.9(b) is at a later time. Accelerometer bias does give rise to errors. In (b). and the result is an oscillation with the Schuler period of 84. but the INS will assume it still has an east velocity component.9 shows an INS with an exaggerated initial tilt error.
Where the normal direct Kalman ﬁlter seeks to estimate the states directly the indirect Kalman ﬁlter estimates the error in these states and corrects the real states. 10. position. This means that instead of estimating the states directly. Normally the system model describes how the states.1) . In indirect Kalman ﬁltering the system model describes how the errors in the states develop over time. For this reason is the model used by the Kalman ﬁlter an error model. etc. velocity.Chapter 10 Error Estimation Using Indirect Kalman Filtering The process of alignment is to determine attitude and heading of the INS.1 Kalman Filtering The main diﬀerence between a normal Kalman ﬁlter and the indirect Kalman ﬁlter approach is how the system is modeled. e. The states in an indirect Kalman ﬁlter is hence called error states. The Kalman ﬁlter technique proposes an optimal way of estimating states why this approach is also used in this thesis. a simple one dimensional example is presented in order to better explain the technique. The diﬀerences between the normal direct approach and the indirect approach are outlined. As the attitude and heading is not directly measurable these are to be estimated. only the errors in the states are estimated by the Kalman ﬁlter. With this in mind the system model is described as follows: ˙ x = A(t)x(t) + Gp (t)np (t) where x = Error state vector A = Error state dynamic matrix np = Process noise vector Gp = Process noise dynamic matrix 49 (10. Last.. describing the error propagation of the system. This chapter introduces the reader to the principles of and the notation used in indirect Kalman ﬁltering.g. develops over time. However it is not a standard direct Kalman ﬁlter but an indirect Kalman ﬁlter which is used in this thesis.
(10. etc. An example of an indirect Kalman ﬁlter is depicted in Fig. With these estimates the observation equation is: zobs = rins − rgps With error free measurement this observation equation is indeed zero. velocity. 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.) ξaid = Aiding device navigation states (position.10. The measurement equation is a linearized version of the general nonlinear observation equation zobs which describes the actual.1.1. nonlinear observation which is deﬁned as follows: zobs = f(ξins . KALMAN FILTERING 50 with the measurement equation as follows: z(t) = H(t)x(t) + Gm (t)nm (t) 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 ﬁlter to estimate the error states x.) f() = Function which compares the parameters from the INS and the aiding device.5) (10. attitude etc. ξaid ) where ξins = INS navigation states (position. As the estimate from the Kalman ﬁlter is in fact the best estimate of the errors in the states. 10. it seems obvious to use this information to adjust the states and thus also the error states (as they depend directly on the states).4) (10. The INS system calculates the INS states (position. The Kalman ﬁlter outputs the optimal estimate of the errors in the INS states. the input to the ﬁlter must also be errors. The error states are reset as follows: ˆ xn (+c ) = xn (+e ) + unc 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. As the ﬁlter estimates error states. velocity. etc. Hence. velocity.3) (10. it can be seen that the indirect Kalman ﬁlter is allowed to adjust the states of the system through a process called “control reset”.2) .). 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. where some are compared to the aiding device states to generate the observations for the Kalman ﬁlter. Looking at the ﬁgure.
This x control vector would reset the errors to zero at every kalman ﬁlter cycle. The measurements are parsed through the INS algorithms in order to get the ﬁlter input in the correct form. . This correction is applicable when correcting states like position and velocity.1: Indirect Kalman ﬁlter with control reset The ideal control vector uc is of course ˜n (+e ) as it can be seen from (10. ERROR ESTIMATION USING INDIRECT KALMAN FILTERING The control reset operation will prevent errors from being build up at the integrators. as xn (+e ) is the best estimate of the error.6) Measurements INS System  Filter Input Kalman Filter Error State Estimate Control Reset of INS States/Indirect Reset of Error State Figure 10. ucn ) where gins () = Functional operator to apply uc to the INS parameters The gins () operation diﬀers depending on which type of state that should corrected. This be either position error. 10. In this report two diﬀerent types of states are to be corrected. Aiding Device Foreground Equations + (10.5). The implementation of correcting the INS parameters involves applying un to the INS parameters in the following way: ξinsn (+c ) = gins (ξinsn (−). The parts which are special for the indirect Kalman ﬁlter is indented in order to compare it with the ’normal’ Kalman ﬁlter. velocity error. First of all the normal direct Kalman equations are listed: Measurement update Kn = Pn (−)HT (Hn Pn (−)HT + Rn)1 n n ˆ ˆ xn (+) = xn (−) + Kn (˜n − ˆn ) z z Pn (+) = (I − Kn Hn)Pn (−) Time update ˆ ˆ xn (−) = Φn xn1 (+) ˆ ˆ zn = Hn xn (−) Secondly the indirect Kalman ﬁlter equations are listed. as the errors are set to 0 at each time step. In order to point out the diﬀerences between a ”normal” direct Kalman ﬁlter and an indirect Kalman ﬁlter the Kalman equations for both ﬁlters are listed. But when correcting the rotation matrices subtraction is not applicable and they are corrected as seen in (12. One type of correction is to simply subtract the two states from each other. The idea of resetting the error states by control resetting the actual states is illustrated in Fig. etc. The Kalman ﬁlter estimates the error states and these are used to correct the INS states and thus indirectly resetting the error states.1.51 CHAPTER 10. With the INS error model the ﬁlter input should also be an error.41).
Velocity measured by aiding device vref  Measured a acceleration Ts z1 v + Kalman filter Observation set for estimate the Kalman filter zobs = v . Furthermore another speedometer is used which delivers velocity measurements.7) 10.2: One dimensional indirect Kalman ﬁlter As seen in the ﬁgure. . 10.2. ucn ) Time update ˆ ˆ xn (−) = Φn xn1 (+c ) ˆ ˆ zn = Hn xn (−) where (+e ) = Designation for parameter after estimation control but before control reset (+c ) = Designation for parameter after control reset Kn = Kalman gain (10. The Kalman ﬁlter is to estimate the errors in the velocity in order to get a better estimate of the velocity.10. the measured acceleration is integrated to give the velocity. The correction inside the integrator is shown below. The reason for this is to get the observation set to contain velocity errors which is the driving parameter for the Kalman ﬁlter.2. The accelerometer delivers noisy acceleration measurements and the system consist of an integrator in order to determine the velocity. This error is of course fed back to the integrator to correct the velocity. ξaidn ) Measurement update Kn = Pn (−)HT (Hn Pn (−)HT + Rn)1 n n ˆ ˆ z xn (+e ) = xn (−) + Kn (zobsn − ˆn ) Pn (+) = (I − Kn Hn)Pn (−) x ucn = −ˆ n (+e) ˆ xn (+c ) = xn (+e ) + unc ξinsn(+c ) = gins (ξinsn (−). MOTIVATING EXAMPLE 52 zobsn = f(ξinsn . The reference velocity measured by the aiding speedometer is subtracted from the systems estimated velocity to give the observation set. The indirect Kalman ﬁlter is presented in Fig.2 Motivating Example For this example a one dimensional accelerometer is used. The Kalman ﬁlter then estimates errors as the system model is an error model.vref Kalman v filter Control reset Figure 10.
3 the integrator is a discrete integrator with an addition block inside the integration loop. then the Kalman ﬁlter cycle has to be very fast. Two guidelines can be used to when the IKF should be considered. For these reasons a normal linear Kalman ﬁlter can be used with a much lower Kalman cycle setting much lower demands to the processor.g. This might pose a problem when implementing on a microprocessor. ERROR ESTIMATION USING INDIRECT KALMAN FILTERING Control reset v Measured acceleration a Add_1 Tn + + 1 z Add_2 + + Error corrected v velocity Discrete time delay Figure 10. This sets demands to the processor in order to do the Kalman calculations before the next Kalman cycle. Choosing a standard direct Kalman ﬁlter approach requires the use of an extended Kalman ﬁlter which should be run at 2kHz.1 Choice of Kalman Filter When using Kalman ﬁltering it is normally quite simple to choose between e.53 CHAPTER 10. .2. thus avoiding residual error buildup in the integrator. This method ensures that the correct value is store. Secondly. This leads to even more complicated calculation which sets even higher demands to the processor chosen. 10. For this reason. a normal linear. the model should be run at more than 2kHz and furthermore the is model nonlinear. If the system has high dynamics and one is trying to estimate the states.3: Illustration of correction of states inside a discrete integrator As seen in Fig. This might pose a problem when a system should be implemented on a microprocessor. it is only necessary to run when new data is available from the aiding device (1 Hz for typital GPS receivers). This error model can be approximated to be linear and in INS. It might not be possible to do all the calculations within each Kalman cycle. extended or unscented Kalman ﬁlter. Using the indirect Kalman ﬁlter. It might not be quite as intuitive when it could be advantageous to do the extra work to derive an error model in order to implement the indirect Kalman ﬁlter. the indirect Kalman ﬁlter is chosen. normally has much lower dynamics than the real system. if the system is also nonlinear then a nonlinear Kalman ﬁlter should be chosen. In order to describe the motion with high precision. 10. As mentioned earlier the use of indirect Kalman ﬁltering requires the system model to be an error model. The reasons for choosing the IKF for this project is that the model of the INS has very high dynamics.
the indirect Kalman ﬁlter technique has been described and the diﬀerences and advantages with respect to the direct Kalman ﬁlter have been outlined. SUMMARY 54 10.3 Summary During this chapter.3. In order to utilize the indirect Kalman ﬁlter an error model of the system needs to be developed and this error model of the INS is developed in the next chapter. As described. . the indirect Kalman ﬁlter is advantageous if the system is characterized by high dynamics and/or nonlinearity.10.
The navigation equations derived in Sec.1 describe the motion of a vehicle relative to the earth instrumented in a strapdown inertial navigation system.1 are simpliﬁed for use in the error equations. These errors are position. This error model is developed as it is required as model for the indirect Kalman ﬁlter.Chapter 11 Error Equations During this chapter is the error model of the INS developed which describes the propagation of errors within the INS.1 Derivation of Error Equations The purpose of the error model is to describe the propagation of the errors in the navigation equations.1) Where un = 0 0 1 is a vertical unit vector and ρR is the magnitude of the zn transport rate around the vertical. 9. attitude and heading error of the INS. The errors to be deﬁned 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. Certain approximations and assumptions has been made deriving the navigation equa55 . 9. rotation. The errors are deﬁned as the angle between the actual DCMs and the DCMs computed in the navigation computer and the velocity diﬀerence between the actual velocity of the INS and the velocity used by the navigation computer. velocity. The earth is considered a sphere instead of an ellipsoid which yields the following equations: b ˙n Cb = Cn Ωib − (Ωb + Ωb )Cn b ie en b n e ωie = Cn ωie e 1 n n ωen = (uzn × vn ) + ρR un zn R n n n n n n ˙ v = Cb a + g − (ωen + 2ωie ) × vn e ˙ C = C e Ωn n n en ˙ h = vn · u n zn (11. The navigation equations derived in Sec. 11.
2) as deﬁned by (11. DERIVATION OF ERROR EQUATIONS 56 tions above which lead to one type of errors. only the errors from the sensors is considered as the computer errors can be minimized by choosing suﬃcient length of word. These errors arise when the INS computer evaluates the navigation equations. ˆ (11.1). This means that the error in Cn matrix and the error in height is important. but with a hat to indicate that they include the second type of errors: ˙ ˆn ˆn ˜ ˆ ˆ ˆn Cb = Cb Ωb − (Ωb + Ωb )Cb ib ie en ˆn ˆe ˆn ωie = Ce ωie 1 ˆ en ˆ ωn = (un × vn ) + ρR un ˆ zn ˆ zn R ˙ ˆ n˜ ˆ ˆ ˆ ˆ ˆ vn = C an + gn − (ω n + 2ω n ) × vn b en ie ˙ ˆ ˆe ˆ C e = C n Ωn n en ˙ ˆ = vn · un h ˆ zn In order to develope the error mode an error is deﬁned as follows: (11.2) δa = a − a ˆ Where a is the computed state and a is the actual state.4) The error equations can be obtained by subtracting (11.1) from the navigation equations used by the onboard navigation computer (11. Before deriving the error model the errors of interest are stated.1) due to several sources of error.2) and neglecting the parts which are a multiplication of two errors as they are assumed to be very small. These errors arise because of measurement inaccuracies from the sensors and computer errors due to ﬁnite precision and integration errors.1) from (11. the errors e in the velocity of the INS is necessary in order to estimate the position of the vehicle when navigating. In this chapter. This leads to the following errors which are of interest: n ˆ δCb = Cn − Cn b b n ˆ δv = vn − vn ˆ δCe = Ce − Ce n n n ˆ δh = h − h (11. The navigation equations in the navigation computer are equal to (11. As the INS is to be used in two modes. During alignment. b In navigation mode it is mainly the position error which is of interest. This is achieved by subtracting the true navigation equations (11. two sets of errors are of interest. These errors can be minimized by making a suﬃciently accurate model.3). The equations used in the computer are not exactly equivalent to the equations in (11. another type of errors is introduced. alignment mode and navigation mode. Secondly. the primary error of interest is the error in the Cn matrix.3) Using this error deﬁnition an error model of the INS can be developed.11.1. Furthermore. This yields the following expression for the errors of interest: .
8) The gravity error model in the δvn term from (11. Gravity can be expressed as a simple inverse square law model plus a term taking into account the deviations from the simple model: gn = −g0 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.57 CHAPTER 11.5) these equations can be transformed into a suitable form.6) where: Γ.5) is the variation from the true gravity to the one estimated by the computer. These approximations are. E = Skew symmetric form of γ n and ǫn . variation of gravity is deﬁned to be gn diﬀerentiated with respect to R. n γ n = The rotation vector associated with δCb ǫn = The rotation vector associated with δCe n (11. 2 R0 n u + ∆gn R2 zn (11. of course.7) With this small angle approximation the error equations are rewritten. ERROR EQUATIONS n n n ˙ ˜ δ Cn = δCn Ωb + Cn δΩb − (δΩn + δΩen )Cn − (Ωn + Ωen )δCb b b ib b ib ie b ie n n n n ˙ δ vn = δCn ab + Cn δab − (2δωie + δωen) × vn − (2ωie + ωen ) × δvn + δgn b˜ b ˙n δ Ce = δCe Ωn + Ce δΩn n en n en n e δωie = δCn ωie e 1 δR n δωen = (un × δvn ) − 2 (un × vn ) + δρR un zn zn zn R R ˙ = δvn · un δh (11. Using (11. The small angle approximation is deﬁned as follows: n ˆb Cn = (I − Γn )Cn ⇒ δCn = −Γn Cb b b n ˆe Cn = (I − En )Cn ⇒ δCe = −En Cn e e ˆe ˆe Cn = Cn T = Ce (I + En ) ⇒ δCe = Ce En n n n (11.6) n δωie is rewritten: n e n δωie = −En Cn ωie = −ǫn × ωie e (11. As gravity is dependent on the parameter R. This gravity error is inﬂuenced both by errors on the altitude estimation and errors in the actual gravity to the gravity model in the computer.5) zn Although these equations describe the error propagation it does not directly describe the errors of interest. so the error equations derived here can only be used in such cases. only applicable when the error angles are small.9) . The gravity error δgn is found using a special approach. 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.
(11.11.5) which yields: n n ˙ ˙ Ce En + Ce En = Ce En Ωn + Ce δΩn n en n en n n e ˙n e n n e n Cn E = Cn E Ωen − Cn Ωen En + Ce δΩn ⇓ n en ˙ n = (En Ωn − Ωn En ) + δΩn E en en en (11.11) δ vn from (11. The error in the gravity is then simply the variation of gravity multiplied with the error in the position as follows: gn = var dgn dR dgn n δg = δR dR (11.10) Using this approach and the gravity model from (11.12) ˙ ˙ In order to rewrite δ Ce (11. here shown for three arbitrary vectors: v1 × (v2 × v3 ) = (v1 · v3 )v2 − (v1 · v2 )v3 (11. As the vector v is an arbitrary vector this will be chosen as identity: n (En Ωn − Ωn En ) = (ǫn × ωen )∗ ⇓ en en ˙n E = (ǫn × ω n )∗ + δΩn en en (11. DERIVATION OF ERROR EQUATIONS 58 This deﬁnition is used to deﬁne the error in gravity.2) is rewritten: n n ˙ δ vn = −γ n × fn + Cn δfb − (2δωie + δωen ) × vn − b 2g n n (ωen − 2ǫn × ωie ) × δvn + δhun + δgn zn M R 2 R0 2g δR un + δ∆gn ≈ 0 δh un + δgn zn zn M R3 R (11.16) Applying the triple vector product identity yields: n n n n (En Ωn − Ωn En )v = (ǫn · v)ωen − (ǫn · ωen )v − (ωen · v)ǫn + (ωen · ǫn )v en en n n = (ǫn · v)ωen − (ωen · v)ǫn (11.17) Using the triple vector product to collect term again: n (En Ωn − Ωn En )v = (ǫn × ωen ) × v en en (11.1.11) (11.15) Multiplying the term in brackets from (11.9) δgn is as follows: δgn = 2g0 where δgn = Unmodeled gravity error M ˙ Using (11.13) (11.19) Where the ()∗ operator brings the vector enclosed in the brackets to skew symmetric form.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.20) .6).6) is substituted into δ Ce from (11.18) Converting this into matrix form where the term in the bracket should be represented by the skew symmetric counterpart: n (En Ωn − Ωn En )v = (ǫn × ωen )∗ v en en (11.8) and (11.14) with an arbitrary vector (v) and switching to vector form: n n (En Ωn − Ωn En )v = ǫn × (ωen ) × v) − ωen × (ǫn ) × v) en en (11.
1) yielding: ˜ ˙ Γn Cn = −Γn Cn Ωb − (Ωn + Ωn )Cn ie en b b ib b n n ˜ + Γn Cn Ωb − Cn δΩb + (δΩn δΩn )Cb − (Ωn + Ωen )Γn Cn b ib b ib ie en ie b n = Γn (Ωie + Ωn )Cn − (Ωn + Ωn )Γn Cn b ie en b en − Cn δΩb + (δΩn + δΩn )Cn b ib ie en b Which can be simpliﬁed further by right side multiplying with (Cn )−1 b ˙ Γn = Γn (Ωn + Ωn ) − (Ωn + Ωn )Γn − Cn δΩb Cb + δΩn + δΩn ie en ie en b ib n ie en n n b = [γ n × (ωie + ωen )] − Cn δΩb Cn + δΩn + δΩn b ib ie en ∗ (11.21) ˙n ˙ In the same manner as with Ce . 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.12).1) in terms of the sensor errors δab and δωib : n n b n n ˙ γ n = γ n × (ωie + ωen ) − Cn δωib + δωie + δωen b n n n n ˙ δ vn = −γ n × ˜n + Cn δab − (2δωie + δωen ) × vn − (ωen − 2ǫn × ωie ) × δvn + a b n n ˙ ǫn = ǫn × ωen + δωen 1 δh n δωen = (un × δvn ) − 2 (un × vn ) + δρR un zn zn R zn R e ˙ δ h = δCn ωie e 2g δh un + δgn zn M R (11. The redundant information is in ǫn where the vertical part equals the azimuth error of the navigation frame. suﬀer from one disadvantage.26) n ˙ Equation (11. During the navigation mode where it is the position error vector which is of interest. The position error is described in form of four parts: ǫn and h.27) However. described by the equations (11.23) (11. 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.g. (11. h from (11.59 CHAPTER 11. Cn is rewritten by using the expression from (11. when aligning with the use of a Kalman ﬁlter as the position is assumed to be known precisely. this error model.5) and the approximation b δR = δh constitute the error model for (11. this model is inconvenient as it does not described this position error directly.21).22) ˙ In order to get rid of the derivative of Cn this is substituted with the expression from b (11. ERROR EQUATIONS which can be represented in vector form: n n ˙ ǫn = ǫn × ωen + δωen (11.26) along with δωen .27).25) which can be written in vector form: n n n b n n ˙ γ n = γ n × (ωie + ωen) − Cb δωib + δωie + δωen (11. This means that there is some redundant information as the position error can be described by three parts. This situation is e.5): ˙ ˙ ˙ δ Cn = −Γn Cn − Γn Cn b b b n n˜b b n n = −Γ Cb Ωib + Cn δΩib − (δΩn + δΩen )Cn + (Ωn + Ωn )Γn Cb b ie b ie en (11.6) b and then replacing with the expression from (11. .24) Using the similarity transform simpliﬁes the expression to: n n ∗ b ˙ Γn = [γ n × (ωie + ωen )] − (Cn δωib )∗ + δΩn + δΩn b ie en (11. (11.
36) . velocity and position error is deﬁned now in the earth frame: ˆe Cb = (I − Ψe )Ce ⇒ δCe = −Ψe Ce b b b ˆ δve = ve − ve ˆ δre = re − re where Ψ = The skew symmetric counterpart of ψ which is equal to the angular error in Ce b r = The position vector from the earth’s center to the INS The task is now to relate these new earthframe error equations to the navigationframe error equation already developed.33) into δve from (11.32) ˆn Ce = Ce + δCe n n (11. using the deﬁnition of δvn from (11.30) (11.4) and neglecting any second order errors as they are assumed insigniﬁcant yields: e ˆ ˆ δve = Ce vn − Cn vn = (Ce + δCe )ˆ n − Ce vn n n n v n = Ce (ˆ n − vn ) + δCe vn = Ce δvn + δCe vn n v n n n Using small angle approximation yields: δve = Ce (δvn + En vn ) = Ce (δvn + ǫn × vn ) n n (11.11. First of all it should be noted that: e δCb = δCe Cn + Ce δCn n b n b (11.34) (11. DERIVATION OF ERROR EQUATIONS 60 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.28) as: n Ψe = −(δCe Cb + Ce δCn )Ce T n n b b n n = −(Ce Cb En Cb − Ce Γn Cn )(Cn )T (Ce )T n n b b n = Ce (Γn − En )Cn n e This can be rearranged into: e (Cn )T Ψe Ce = Γn − En n (11.33) Substituting (11.1.28).28) (11.29) which makes it possible to describe the Ψ part of (11.35) As the error model should be expressed in navigation frame the velocity error in navigation frame is deﬁned: δυ n ˆ Cn δve e (11.31) Using the similarity transformation yields the following: ψ n = γ n − ǫn In order to relate δve with δvn it is ﬁrst recognized that: ve = Ce vn n ˆ ˆ ˆ ve = Ce vn n (11.
27) is substituted and diﬀerentiated.41) (11.42) To this point the old error model has been related to the new error states and is summarized here: ψ n = γ n − ǫn δυ n = δvn + ǫn × vn n δrn = R(ǫn × un ) + δRuzn zn n δrH = R(ǫn × un ) zn δR = δh (11.40) From this position error vector the horizontal and vertical parts are easily found.44) ˙ The δ υ n diﬀerential equation is derived by diﬀerentiating the δυ n expression from (11.35) it becomes: δυ n = δvn + ǫn × vn A relation from δre with ǫn and h is obtained by ﬁrst deﬁning re : re = Rue ze (11.45) . The n vertical parts is the dot product of (11.39) Transforming this into the navigation frame yields: δrn = REn un + δRun zn zn which can be represented in vector form: δrn = R(ǫn × un ) + δRun zn zn (11. and ue is the unit vector pointing from the center of the ze earth to the INS in earth frame.43): ˙ ˙ ˙ ˙ δ υ n = δ vn + ǫn × vn + ǫn × vn (11. ERROR EQUATIONS With deﬁnition (11.43) In order to obtain the diﬀerential equation for the new error model the equivalent parameters from (11. The error is deﬁned in the standard way and neglecting second order errors yield: ˆu δre = Rˆ e − Rue ze ze e e = (R + δR)(Ce + δCn )un − RCn ue n zn ze ≈ RδCe un + δRCe un n zn n zn (11. ˙ ˙ ˙ The Ψn equation is obtained by subtracting ǫn from γ n and substituting the parts with their equivalent from (11.38) (11. The vertical part is simply found by subtracting the vertical part from (11.61 CHAPTER 11.41) with uzn and as expected it is equal to δR.37) Where R is the length of re .43): n n b ˙ Ψn = Ψn × (ωie + ωen ) − Cn δωib b (11.41) and is equal to: δrn = R(ǫn × un ) H zn (11.
47) Furthermore (γ n − ǫn ) can be substituted with Ψn . Deﬁning Ω = 2Ωn + Ωn the triple vector product is used on the three en ie last parts and summarized with the following result: Ω × (ǫ × v) + (ǫ × Ω) × v − ǫ × (Ω × v) ǫ(Ω · v) − v(Ω · ǫ) − ǫ(Ω · v) + Ω(ǫ · v) − Ω(ǫ · v) + v(ǫ · Ω) v(ǫ · Ω − Ω · ǫ) = 0 (11. vn and ǫn with expressions from (11. Lastly the ∆gn part from gn from (11.43): H ǫn × gn = −g(ǫn × un ) zn g n = − δrH R ˙ All of these rearrangements result in the ﬁnal expression for δ υ n : n n ˙ ˜ δ υ n = Cn δab − Ψn × an − (2ωie + ωen ) × δυ n + b (11.52) with the expression of Cn from (11.15).9) can be neglected as it is very small compared to gn .1) and (11. As gn point the same way n as uzn the following expression can be substituted using δrn from (11.50) and (11.48) g 2g δRun − δrn + δgn (11.46) The three last parts can be shown to sum to zero by using the triple vector product from (11.1. υ n and rn is summarized below: n n b ˙ Ψn = Ψn × (ωie + ωen ) − Cn δωib b n n ˙ ˜ δ υ n = Cn δab − Ψn × an − (2ωie + ωen ) × δυ n + b n ˙ δ rn = δυ n − ωen × δrn g 2g δRun − δrn + δgn zn M R R H (11.53) ˙ ˙ ˙ The new error model with the new error states Ψn .27) and substituting ˙ δvn with an rearranged δ υ n expression from (11.51): ˙ ˙ δ rn = Cn δre + Cn δ re e e ˙ (11.52) (11.51) into (11.51) (11.1) yields: ˙ δ rn = −Ωn Cn δre + Cn δve e en e n = δυ n − ωen × δrn (11.54) .43) yields: n n ˙ ˜ δ υ n = Cn δab − γ n × an − (2ωie + ωen ) × (δυ n − ǫn × vn ) b 2g n n n n − (δωen − 2ǫn × ωie ) × vn + δhun + δgn + (ǫn × ωen + δωen ) × vn zn M R n n n ˜ + ǫn × Cb ab − (2ωie + ωen ) × vn + gn n n ˜ = Cn δab − (γ n − ǫn ) × an + ǫn × gn − (2ωie + ωen) × δυ n + b 2g δhun zn R n n n n n n n n n + δgM + (2ωie + ωen ) × (ǫ × v ) + [ǫ × (2ωie + ωen )] × v n n − ǫ × [(2ωie + ωen) × vn ] (11.50) ˙e Substituting (11.49) zn M R R H What remains is to relate the new position vector rn to the old states ǫn and h. DERIVATION OF ERROR EQUATIONS 62 ˙ ˙ Substituting δ vn .11. By deﬁnition: ˙ δ re = δve and furthermore: δrn = Cn δre e Diﬀerentiating (11.
When in navigation mode the second error model. These two error models describe the error propagation of the INS but has diﬀerent characteristics as they describe diﬀerent error states of the INS. With the error model developed and the indirect Kalman ﬁlter technique described the alignment algorithm is ready to be developed. 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.27) with the relation found in (11.43). describe the errors in Cb along with velocity and position errors which makes this error model preferable when doing alignment as alignment tries to estimate these states. Two diﬀerent approaches has been used which leads to two diﬀerent error models.63 CHAPTER 11. (γ) error model. is simpler to calculate enabling a faster Kalman ﬁlter and thus making it preferable. (ψ) error model. . ERROR EQUATIONS This new error model is of course equivalent to the one from (11. The ﬁrst n error model.2 Summary During this chapter has the error model of the INS been developed. The ψerror model is shown below in matrix form: ψ 0 ˙x −Ωc ψy Ω +Ω ψz b β δvx 0 δv an y= U δvz −an δrx 0 N δry δrz 0 0 Ωc −Ωb −Ωβ 0 0 0 0 0 0 0 Ωa +Ωα 0 0 0 0 0 0 −Ωa −Ωα 0 0 0 0 0 0 0 −an an 0 2Ωc −2Ωb +Ωβ −g/r 0 0 U N 0 −an −2Ωc 0 2Ωa +Ωα 0 −g/r 0 E an 0 2Ωb +Ωβ −2Ωa −Ωα 0 0 0 2g/r E 0 0 1 0 0 0 0 −Ωβ 0 0 0 1 0 0 0 Ωα 0 0 0 0 1 Ωβ −Ωα 0 ψx ψy ψz δvx δv y δvz δrx δry δ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(α) g ˙ = −v /(r + h) l N ˙ L = vg /(r + h) cos(l) E 11. The reason for developing two error models is that each of them are preferable in two diﬀerent situations.
attitude and heading before navigation can begin. Coarse alignment is an analytical alignment which suﬀers from a lack of precision as measurement errors and other noise sources are not considered. and aeroplane on the ground which vibrates due to fueling. passengers or gust of wind. The last section is a veriﬁcation of the alignment where it is tested against both simulated and realworld measurements. 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. estimate the attitude and heading accurately enough to justify the small angle approximations made in the error model. The position is assumed known but the attitude and heading needs to be determined. The ﬁrst section will describe the coarse alignment. This chapter will explain the alignment procedure. It does. in this thesis the navigation frame. however. This corresponds to forcing the wander angle of the navigation frame to 0. and hence allow ﬁne alignment to use an indirect Kalman ﬁlter using the error model to obtain a more precise alignment. 64 . Choosing the navigation frame equal to the geodetic frame simpliﬁes the coarse alignment as the earth rotation is only present on two axes. but close to with small variations. The second section describes the ﬁne alignment procedure which uses the indirect Kalman ﬁltering technique and the error model developed in the preceding chapter. But the position is always relative to the starting position and for this reason the INS needs to know both the position. Alignment is achieved using sensor measurements when the platform is in a quasistationary situation.Chapter 12 Alignment As mentioned earlier an INS simply integrates measured acceleration and rotation rate to determine the position of the body frame. In this thesis is the navigation frame and geodetic frame the same during alignment. This is e. This is the process of alignment. The Alignment process is divided into two parts: Coarse alignment and ﬁne alignment.g. Quasistationarity is a state where the body is not stationary. a boat at dockside pitching with the waves.
1 Comparison between Gimballed and Strapdown Alignment Understanding the concept of alignment when using a strapdown INS is easier when visualizing the gimballed situation. So throughout this thesis the concept of adjusting Cn will be called leveling in b order to gain greater understanding. n n . ALIGNMENT 12. C31 = C32 C33 gx g gy = g gz = g ωx gx − tan(l) ωie cos(l) g ωy gy C22 = − tan(l) ωie cos(l) g ωz gz C13 = − tan(l) ωie cos(l) g C21 = (12. 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. 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. (Cb )−1 = (Cb )T . When using a strapdown INS the sensors are mounted directly to the body and can not be rotated so this is achieved analytically. C22 . The objective of this section is to determine the direction cosine matrix Cb relating the n navigation frame with the body frame.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. When aligning a gimballed INS the gimbal is physically rotated until two of the the sensor axes are level with respect to the earth. By adjusting Cn until the accelerometer measurements b equals [0 0 g]T in the navigation frame.1) (12. Knowing the quan tities g = and = from the measurements of the accelerometer and gyroscope. results in the same as physically rotating the gimbal.2) (12.. Comparing these quantities makes it possible to determine the direction cosine matrix relating the navigation frame and body frame. and earth rotation is also known due to the knowledge of the b latitude. this system is easily solved. In the nframe gravity has only one component which is the vertical. The last three elements are easily determined with the use of the orthogonality property of the direction cosine matrix. These quantities are related in the following way: g b = Cb g n n b n ωie = Cb ωie n where gn = b 0 b gx 0 g b gy T b gz n and ωie = T b ωie 0 ωie cos(l) ωie sin(l) b ωx b ωy b ωz T T .3) b where C21 . C33 are the elements of the direction cosine matrix Cn . . The measured quantities are gb and ωie which are the measurements from the accelerometer and gyroscope. 12..65 CHAPTER 12.
5) (12. ˙n Cb = Cn Ωb − (Ωn + Ωn )Cn b ib ie en b n e ωie = Cn ωie e 1 n n ωen = (uzn × vn ) + ρR un zn R n n n n n n ˙ v = Cb a + g − (ωen + 2ωie ) × vn ˙e C = C e Ωn n n en ˙ h = vn · un zn (12. and does not need to be estimated. the attitude and azimuth heading is b estimated with greater accuracy than possible with coarse alignment. any position diﬀerent from 0 will be an error. FINE ALIGNMENT 66 C21 = −C12 C33 + C13 C32 C22 = C11 C33 − C31 C13 C23 = −C11 C32 + C31 C12 (12. The reason for this is that the coarse alignment is a one shot alignment procedure which suﬀers from measurement noise which prevents it from calculating the true value of Cn . the remaining errors in the Cn matrix consists of small roll. 12.3. The indirect Kalman ﬁlter then estimates the errors in the rotation matrix from body frame to n navigation frame Cn . After coarse alignment. Fine alignment uses the navigation equations to obtain position information of the INS. The vertical part of earth rotation is known analytically as a function of latitude.3 Fine Alignment Fine alignment uses an indirect Kalman ﬁlter with the result from the coarse alignment as the initial Cn . pitch b and yaw angles.6) As seen from these equations this DCM is only uniquely deﬁned if the latitude is not equal to ±90◦ . which enables the use of the b previously derived error equations as this error model uses small angle approximations. At ﬁne alignment completion. A precise estimate of earth rotation is also needed in order to cancel the eﬀects of earth rotation on platform tilt. Roll and pitch errors are also denoted tilt errors.4) (12. error in earth rotation ωie . This error is used as the driving parameter for the indirect Kalman ﬁlter. However. so the platform is leveled. This means that this alignment procedure is not possible if the INS is positioned at the North. This will become evident later in this section.12. The yaw error is the deviation of the y axis of the navigation frame from pointing true North. During ﬁne alignment. error in velocity vn and error in the b diﬀerence in position from the initial position ∆rn . b the errors in the Cn matrix can be assumed to be small. The yaw error can be calculated b n from the horizontal parts of the estimated earth rotation ωie .7) As ﬁne alignment is performed when the INS is in a quasistationary situation the .1 is restated below. as they correspond to the platform being not truly level.or South Pole. The navigation equations from Sec. the estimates of roll and pitch errors are used as control resets to correct the Cn matrix. As the system is quasistationary. 11.
First of all.1. As mentioned before.67 CHAPTER 12. an estimation error of H n ωie will tilt the INS from the correct leveled attitude and thus again lead to a nonzero ∆rn . in order to bring Cn to steady state. any tilt error will thus lead to a nonzero ∆rn . This vector is a measure of the attitude error Ψn and estimation H n error of ωie and can thus be used to estimate these two parameters. Cn needs to b be leveled in order to cancel out the gravitation force in the horizontal plane. This n n is performed by introducing a quantity called ωtilt used to correct Cb . the earth rotation rate needs to be estimated. Applying these simpliﬁcations gives the following simpliﬁed navigation equations: ˙n C = C n Ωb − Ωn C n b b ib ie b n e ωie = Cn ωie e ˙H vn = (Cn an )H b ∆˙ n = vn rH H ∆rn H (12. vn and ∆rH .8) is called the foreground equation as it is not within the Kalman ﬁlter. The quantities calculated in this foreground equation is not to be mistaken for the error states estimated within the Kalman ﬁlter. the Kalman ﬁlter is used not only to estimate the error states. estimation error of ωie or both.9) . Last. The navigation equations (12. 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 . Four Kalman gains are H needed to correct the four parameters. As any tilt from horizontal will give rise to a component of the earth gravitation in the horizontal plane. the driving parameter for the Kalman ﬁlter is the position divergence vector ∆rn . the Kalman gains are introduced. H Using the approach described in Ch. Equation (12. Furthermore. In order to interlink ∆rH with the four parameters which are to be corrected. the Kalman ﬁlter is allowed to reset n n the errors in the last two INS states. but also to correct errors in the INS states. 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. In order to level the body frame analytically Cn is torqued so that no components are b n ˙H present in vn . ALIGNMENT n navigation equations can be simpliﬁed as both ωen and vn are zero.10. This means that the INS is torqued only in the horizontal plane but with no rotation around the vertical axis. the estimated earth rotation ωie b b b needs to correspond to the measurements from the gyroscope Ωib = Ωie .8) where H indicate the horizontal parts and is the position divergence deﬁned as the diﬀerence in position from the initial position. Secondly. n n n Both ωie and ωtilt are directly connected to ∆rH as any error in ∆rn is either due to an H n attitude error. n Furthermore.8) are now expanded into the form utilized during ﬁne alignment: b n n ˙n Cb = Cn Ωib − (ωie + ωtilt )Cn b b n ωtilt = k2 un × ∆rn zn H n e n ωie = ωie H + uzn ωie sin l n n ˙ ωie H = k1 uzn × ∆rn H ˙H vn = (Cn an )H − k3 ∆rn b H ∆˙ n = vn − k4 ∆rn rH H H (12. The Kalman gains describe how ∆rn is correlated with the system states. 10. which correspond to horizontal tilt of Cb . This equation is the mapping from inertial measurements to velocities and positions which is called ”INS System” in Fig. As mentioned two parameters n needs to be corrected in order to bring Cb in steady state.
12. g = 9.2 shows the ﬁne alignment scheme.1(a) the body has not been rotated and in Fig. The accelerations are double integrated in order to get the divergence position which is the ﬁlter input.82 z z y x y a x b Figure 12.1. (12. k2 .3.1(b) the body has been rotated positive around x.12. k4 = Kalman gains n ωtilt = The angular rate feedback used to correct the attitude error associated with Cn b n The cross product with uzn has been introduced in order to assign the correct sign and gain to the correction. These error states are then used to correct the states in the INS system. This ﬁlter input is multiplied with the Kalman gains in order to get estimates of the errors states. Looking at the vector product this property will be revealed. In ﬁgure Fig. It can be realized by inspecting Fig. 12. This means that an positive acceleration on the yaxis should result in an negative rotation about the xaxis.2. Having established the initial understanding of ﬁne alignment a further description of the derivation of the Kalman gains is done. 12. Figure 12.10) . As can be seen in Fig. k3 . 12.1: Illustration of the accelerometer measurements due to an rotation of the body.1 if the frame is rotated positive about the xaxis then the accelerometer with sensitivity axis y will measure a positive acceleration. un = [0 0 1]T zn un × [x y z]T = [y x 0]T zn A block diagram of the ﬁne alignment procedure can be seen in Fig. As can be seen the input to the INS system is the measured accelerations and rotation rates. FINE ALIGNMENT 68 where un = Unit vector along the nframe vertical axis (z) zn k1 . 12.
2: Illustration of the ﬁne alignment procedure.69 CHAPTER 12. . ALIGNMENT INS System b ib n b n b b ib n tilt n ie n b Vertical part n of ie n Kalman Filter ieH C C ( )C K1 1 s n Cb n tilt K2 ab n vH (Cb)Ha n b uzn vH n n 1 s vH 1 s rH n n r n H K3 K4 Filter Input Figure 12.
As mentioned earlier. the error model for the Kalman ﬁlter is simpliﬁed to only contain the horizontal parts. 12.8) when ǫn is constant. ﬁne alignment uses an indirect Kalman ﬁlter to estimate the error states which means that the model used by the Kalman ﬁlter should be the error model which is derived in Ch.12.13) These equations should be expanded to include the position divergence as this is the driving parameter of the Kalman ﬁlter. INDIRECT KALMAN FILTER APPLIED TO QUASISTATIONARY FINE ALIGNMENT 70 12. 10 the observation set needs to be deﬁned.12) e This error model is simpliﬁed as certain parameters are not relevant when performing alignment.15) Remembering that it is only the horizontal parameters which is of interest.4 Indirect Kalman Filter Applied to QuasiStationary Fine Alignment In order to describe ﬁne alignment within the Kalman ﬁlter framework as described in Ch. As there is no aiding apart from the knowledge of the quasistationarity. This is easily included by recognizing that the error in the position divergence vector is: δ∆˙ n = δvn r (12. This error model is restated below: n n n b n n ˙ γ n = γ n × (ωie + ωen ) − Cb δωib + δωie + δωen n n n n ˙ δ vn = −γ n × an + Cn δab − (2δωie + δωen ) × vn − (ωen − 2ǫn × ωie ) × δvn + b 2g δhun + δgn zn M R n n n n ˙ ǫ = ǫ × ωen + δωen δh 1 n δωen = (un × δvn ) − 2 (un × vn ) + δρR un zn zn R zn R e ˙ δ h = δCn ωie (12.27). the observation set is: zobs = ∆rn − ∆rn H H ref (12.11) where ∆rn H is the reference position for the position divergence which is approximated ref to zero due to quasistationarity. 11. the rate of the earth rotation error is zero: ˙n δ ωie = − d n e (ǫ × ωie ) = 0 dt (12.4. This is done as follows: First of all the quasistationary error model from Sec. The error model used when deriving the Kalman gains for ﬁne alignment is the model from (11. Using the fact that the platform is quasistationary zeroes the velocity vn n and the transport rate ωen giving the following expression: n n ib ˙ γ n = γ n × ωie − Cn δ ωb + δωie b ˜ n ˙ δ vn = −γ n × an + Cn δab − 2δωie × δvn + b 2g δhun + δgn R M R (12.14) As can be seen from (11.4 is repeated below: .
71
CHAPTER 12. ALIGNMENT
n ib n ˙ γ n = γ n × ωie − Cn δ ωb + δωie b ˜
2g δh un + δgn (12.16) R M R Certain assumptions are made when reducing the error model to only containing horizontal parts. First of all, the horizontal tilt/earth rate product from the ﬁrst equation is assumed negligibly small[Savage, 2000]. Secondly, the vertical part of γ n is deﬁned to be zero at the beginning of ﬁne alignment. This can be achieved as the local level navigation frame n has an arbitrarily chosen heading which can be chosen to coincide with Cn heading at ﬁne alignment initialization. As the ﬁne alignment is performed b 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:
n ˙ δ vn = −γ n × fn + Cn δfb − 2δωie × δvn + b
0 0 n 0 1 ˙ δ ωie γ n = 0 ˙ 0 ˙ δ vn 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 fn U 0 −fn N 0 0 0 −c12 −c22 −c32 0 0 0
0 0 0 0 0 0 −fn U 0 fn E
0 0 0 0 0 0 fn N −fn E 0 0 0 0 0 0 0 c11 c21 c31
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
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 0 n 0 ˙ δ ωie 1 0 0 0 γn = ˙ 0 1 0 0 n ˙ δv H 0 0 0 −fn U n 0 0 0 fU 0 0 0 0 0 0 −c11 −c12 0 + −c21 −c22 0 0 0 c11 0 0 c21 0 0 0 0 0 0 0 0 0 0 c12 c22 0 0 n δω 0 nie γ 0 δvn H 0 0 b δ ωib ˜b δf ib
H
0 0 0 −c11 + −c21 −c31 0 0 0
0 0 0 −c13 −c23 −c33 0 0 0
0 0 0 0 0 0 c12 c22 c32
0 0 0 0 δ ωib ˜b 0 δfb ib 0 c13 c23 c33
0 0 0 n 0 δωie 0 γ n 0 δvn 0 0 0
(12.17)
(12.18)
(12.19)
(12.20)
12.4. INDIRECT KALMAN FILTER APPLIED TO QUASISTATIONARY FINE ALIGNMENT 72
This yields the following model:
n n x = [δωie H , γH , δvn , δ∆rn ] H H b np = [δωib , δfb , 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 1 0 0 0 0 0 A = 0 0 0 −g 0 0 0 0 0 g 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 −C11 −C12 0 0 −C21 −C22 0 0 Gp = 0 C11 C12 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 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
(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 ﬁlter: z = δzobs = δ∆rn − δ∆rn H H ref where δ∆rn = The position divergence error vector H δ∆rn H = Is the error due to the assumption of ∆rn H equal to zero ref ref If δ∆rn H is deﬁned to be the diﬀerence between the true value and the value used by ref the INS: δ∆rn H = ∆rn H − ∆rn H ref INS true where ∆rn H = 0 INS The true value can be recognized as the vibrating part of the position. This means that δ∆rn H can be written as: ref δ∆rn H = −∆rn H ref vib (12.24) (12.23) (12.22)
73
CHAPTER 12. ALIGNMENT
where ∆rn H = The horizontal position error due to the quasistationary situation of the INS vib With these deﬁnitions, the measurement equation is written as: z = δ∆rn + ∆rn H H vib (12.25)
With both the state and measurement equation deﬁned the ﬁrst thing to notice is that the error state dynamic matrix is constant which enables an oﬄine 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 = (12.26) g 2 0 −gTn 1 0 0 0 g 0 2 − 2 Tn Tn 0 gTn 0 0 1 0 0 2 g 2 g 3 0 0 − 2 Tn Tn 0 1 0 − 6 Tn g 3 0 gT2 Tn 0 0 0 1 n 6 Tn 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 (12.27) Q1n = Gp QpDens GT Tn Qn ≈ Q1n + Φn Q1n ΦT n p 2 2 where: Q1n = A ﬁrst 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 noise density is deﬁned as: 0 0 0 0 QpDens = 0 0 0 0 First of all Q1n is calculated: Q1n 0 0 0 0 = 0 0 0 0 0 0 0 0 0 0 0 0 the sensor errors in np are equal and the process 0 0 0 0 0 qω 0 0 0 0 0 0 0 0 0 0 0 0 0 qω 0 0 0 0 0 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
(12.28)
0 0 qω 0 0 0 0 0
0 0 0 qω 0 0 0 0
0 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
Tn
(12.29)
36) (12.35) (12. Due to this property the integrated process noise is also constant and can therefore be calculated oﬄine.21) the measurement matrix.32) . attitude and position. With both the state and measurement equations established.34) (12.12. 10. 10.31) (12.1. INDIRECT KALMAN FILTER APPLIED TO QUASISTATIONARY FINE ALIGNMENT 74 n The reason for Q1n being equal to pDens is due to the orthogonality principle of Cb Tn n nT where Cb Cb = I. the Kalman ﬁlter gain and error covariance matrices can be calculated according to the following general Kalman ﬁlter equations: Pn = Φn Pn1 (+)ΦT + Qn n Kn = Pn (−)HT Hn Pn (−)HT + Gmn Rn GT n m n n 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. 2000]: Rn = E(nmn nT n ) = prvib H I2x2 m where prvib H = Horizontal quasistationary position random motion variance E = Is the expected value operator (12. (12. Gmn and Rn are independent of time.11) the observation is: zobsn = ∆rn H This leads to the calculation of the control vector: unc = −Kn ∆rn H (12.30) Again it should be noted that both Hn . Q The ﬁnal task is to construct the output equations. measurement noise vector and measurement dynamic noise matrix is as follows: Hn = [0 0 0 0 0 0 I2x2 ] Gmn = I2x2 nmn = ∆rn H vib If we deﬁne the elements of nmn to have equal and uncorrelated variance the measurement noise covariance matrix can be written as follows [Savage. Using the deﬁnition of the measurement equation in Sec. As mentioned in Sec.1 the ideal reset is: x unc = −ˆ n (−e) Using this reset the discrete Kalman equations from (10.7) is as follows: ˆ xn (+c) = xn (+e) + un = 0 ˆ ˆ xn (−) = Φn xn1 (+c) = 0 ˆ ˆn = Hn xn (−) = 0 z ˆ ˆ ˆ xn (+e) = xn (−) + Kn (zobsn − zn ) = Kn zobsn From (12.4.25) and (12.33) −1 (12.
39) This error can also be interpreted as the correction to Cn (−) why the corrected Cn (+c ) b b is: Cn (+c ) = Cn (−) + δCn b b b Cn (+c ) = (I − Γn )Cn (−) b H b (12. the alignment schemes are veriﬁed by manually inputting acceleration and gyroscopic measurements which would be measured by gyroscopes and accelerometers at a speciﬁc and known orientation in space. This is done by deﬁning the individual parts of the control vector: n n uc = [δωie Hc .37) where c = Designation for the uc component which are to be applied to the foreground equations (12. Secondly to verify the ﬁne alignment the same inputs are used while selecting the initial rotation matrix Cn with a small error. Applying horizontal attitude error n n γH to Cn is performed as follows. while applying the control vector. as these error states are directly accessible from the control vector. δ∆rn c ]T H H (12.32). ALIGNMENT Equation (12. This way the coarse alignment should make a perfect one shot alignment. As deﬁned in (11.8).6) errors in the rotation matrix Cb b is: n n δCb = −Γn Cb (12.37). Veriﬁcation of alignment is divided into two steps which are described below.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 (−) vn n (+c) = vn n (−) + δvn nc H H H ∆rn n (+c) = ∆rn n (−) + δ∆rn nc H H H (12.9).41) To sum up. Step One: In step one. This resembles a b real situation where the coarse alignment estimates Cn with errors. velocity and position is straightforward.32) constitute the complete Kalman ﬁlter and the last thing to develop is the method to apply the control vector to the foreground equations from (12.36) using Kn from (12.75 CHAPTER 12. they are to be veriﬁed. the ﬁne alignment process consist of integrating the foreground equations. (12. which has been calculated by the use of the Kalman gain (12. (12.38) The last thing to determine is how to correct the INS states with the control vector from (12. 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.37). 12. The ﬁne alignment b . γHc .5 Veriﬁcation of Alignment As the coarse and ﬁne alignment has been developed. Subtracting the control vector from the earth rotation. δvn c .
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. 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.4. Step Two: In step two.7◦ in roll and pitch error.5. By due to the n misalignment. at b which point the position is zero. Testing with other rotations of the body frame showed that the coarse alignment worked. 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. Cn should be unity as the body frame is b coincident with the nframe and a test showed that the coarse alignment worked.axis can be calculated as: sin(0. As can be seen in Fig. This means that the acceleration and position should be driven to zero.5◦ . the expected rotation would be unity with a yaw angle error.4◦ ] in [roll. pitch and yaw angles after it has performed coarse and ﬁne alignment while accessing the raw measurements from the accelerometer and gyroscope.3 the initial north acceleration due to the gravitation projected onto the x. the position changes until a correct estimate of Cn and ωie is given. pitch. The system tested against is a Kearfott INS system. The error in the initial Cn has been chosen to b b be [0. 0.1 Step One Coarse Alignment: The coarse alignment calculates the rotation matrix Cn and by inputting accelerometer b and gyroscope measurement which resembles a leveled INS pointing north. pitch and yaw angles with and already existing system.5◦ . With this INS it is possible to get both the roll. As mentioned in Sec. 12. it is possible to compare the results and indicate the system developed in this thesis does work correctly. The position starts out being zero. This error should be driven to zero as Cb and earth n rotation ωie is estimated more and more precisely. horizontal b accelerations will be present due to the gravitationally force.7◦ . Due to the errors in the initial Cn . The next thing to verify is that the rotation matrix Cn converges to the expected value. it will converge towards zero slower and slower. as it is the best guess to where the platform is positioned. The same goes for the east acceleration. It can also be seen that it converges to zero an expected. . This originate from the fact that ﬁne alignment only torque horizontally to level the platform. Due to the fact that the closer the position gets to zero the smaller the torquing signal. 12. 12.5◦ ) · g = . the alignment scheme developed in this report is tested against an already existing INS system. Simultaneously it estimates the earth rotation which is used to estimate the yaw angle. Fine Alignment: With the same acceleration and gyroscopic inputs as with the special case under coarse alignment. VERIFICATION OF ALIGNMENT 76 ﬁlter should correct Cn and at ﬁne alignment completion it should once again resemble b the perfect rotation matrix from coarse alignment. −0.086 which coincides with the graph. It is again seen that the north position starts out going positive north and negative east due to the components of gravity. This way it is possible to compare the roll. yaw].4 the resets and torquing signals to correct Cn is driven by the b n error in the horizontal position. b With the same accelerometer and gyroscopic measurement. Although it is not possible to distinguish which of the two systems is the most correct. 12. The same characteristics can be seen by investigating Fig. Cn should b be unity. ﬁne alignment should decrease this error and b estimate the Cn with greater accuracy.12. but with an initial error in Cn . and 0.
3: Acceleration due to misalignment x 10 4 3 2 1 Position [m] 0 −1 −2 −3 −4 −5 −6 0 −4 East Position North Position 0. ALIGNMENT 0.15 0 0.05 Acceleration [m/s2] 0 −0.4: Position divergence due to misalignment .2 0.4 0.6 Time [min] 0.2 0.05 −0.1 −0.8 1 Figure 12.8 1 Figure 12.77 CHAPTER 12.1 East Acceleration North Acceleration 0.6 Time [min] 0.4 0.
Step Two: Step two shows the result from the test against an already existing INS system.6 ωie starts out being initialized as zero.5 0 −0. But as soon as there is an error in the position divergence vector the horizontal earth rate is changed. VERIFICATION OF ALIGNMENT 78 1 Error [°] 0.6 Time [min] 0.5.2 0. they cannot be compared directly as there is some uncertainty to how the Kearfott data should be interpreted. The ﬁne alignment then levels the platform by torquing the horizontal parts to zero.5 0 1 Error [°] 0. As can be seen in Fig.6 0. But the results imply that the ﬁne alignment developed does function with realworld measurements. a rotation of cos(57. 12.5.2 0.1: As can be seen the Kearfott and the ﬁne alignment used in this thesis estimate Cn to b somewhat the same values.4◦ but with a small component on the x axis.5 0 −0. As n can be seen in Fig. the rotation b n around x (roll).2 0. reveals that the rotation around y converges to around 3.4 0.9e5 and small rotation approximately zero around x. 12. 12. To test if the . which can be seen in Fig. 12.5 0 −0. So at latitude 57.12.12.5 0 1 Error [°] 0.4 0.5 0 0.8 1 Roll Error Figure 12. In order to take the misalignment around z into account the yaw angle needs to be n calculated.7. The Kearfott system was positioned on a table and completed a 15min ﬁne alignment. As can be seen. 12.5: Euler rotation associated with Cn b The Euler rotation associated with Cn is shown in Fig.5 only the roll and pitch rotation is converging to zero whilst the yaw error is not torqued.6 0.95e−5 is expected on the y axis and much smaller on the x axis.8 1 Pitch Error 0.8 the yaw angle is stationary at −0. which is the latitude at the test site at Aberdeen. which is unacceptable for an actual usage.4 0. This was compared to a 15min ﬁne alignment with the equations developed in this thesis. The results are compared directly in Tab.8 1 Yaw Error 0. ◦ As can be seen in Fig.19◦.19) · ωie = 3. It is expected that the yaw angle converges towards the yaw error in Cb .4 . A closer inspection. Although the diﬀerences are in tenth of a degree. 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. y (pitch) and z (yaw) starts out being the initial errors in the Cb .
79 CHAPTER 12.7: Estimate of the earth rotation rate .6 0.2 0.65 0.5 −1 −1.5 −2 −2.9 0. ALIGNMENT x 10 1 0.8 1 Figure 12.5 0 Angular Rate [rad/s] −0.6 Time [min] 0.7 0.6: Estimate of the earth rotation rate x 10 10 −5 Earth Rotation Rate Around X Earth Rotation Rate Around Y 8 Angular Rate [rad/s] 6 4 2 0 0.75 0.4 0.85 Time [min] 0.5 −3 −3 Earth Rotation Rate Around X Earth Rotation Rate Around Y 0 0.8 0.95 1 Figure 12.
where the b fn x fn y fn z 2. If the INS is leveled. 12.2: Measured acceleration rotated into the nframe measurements is performed can be calculated with (6.5.9 the rotated acceleration with x and y sensitive axes is located around zero with the z sensitive axis around 9.3166◦ 12.55347332e05 6. As can be seen from Fig.2752◦ 8. then the b rotated accelerations measured in body frame should be [0 0 9.8171]T as the nframe is leveled. VERIFICATION OF ALIGNMENT 80 100 Yaw Angle [ °] 50 0 −50 −100 0 2 4 6 8 10 0 Yaw Angle [ °] −0.8. 12. n And this component is very close to the theoretical.2219◦ 12.9. 12.1: Comparison of a 15min ﬁne alignment estimated Cn is a good estimate the accelerometer data is rotated through Cn in order b b to verify that Cn actually levels the INS analytically. The result are seen in Fig.12. The reason for not including the time before 5min is because ﬁne alignment needs some time to estimate Cn . 12.9) to be [0 0 9.8: Wander angle at ﬁne alignment completion Role Pitch Yaw Kearfott 81. The results are seen in Fig.81699624 Table 12.2 the accelerations is rotated so only a component is in the zdirection. This means that Cb does actually level the INS.0990◦ Table 12. The gravity in Aberdeen.81710728]T.2.5 −1 2 3 4 5 6 7 Time [min] 8 9 10 Figure 12.04109◦ 8.11895069e04 9. .0859◦ This report 81. A mean of the values from time 5min to 15min is presented below. As seen from Tab.
Two diﬀerent error sources are dominant in the alignment phase: Sensor errors and errors due to a nonquasistationary state when assuming quasistationary. it was shown that the alignment developed in this thesis did not converge to the exact same value as the Kearfott. In practice. The alignment is of course nor perfect but also the theoretical gravity.9: Accelerations rotated into nframe 12. Due to sea currents.81 CHAPTER 12.2 0. 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. 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.2 an x 0 −0. does not take the local gravity anomalies into account. the . So the important thing is not the convergence time but the fact that is actually converges to the correct values which it does. This diﬀerence stems from diﬀerent sources.2 an y 0 −0. In step two when testing alignment on realworld measurements. The test cases in step one where alignment is tested with simulated measurements can of course have inﬁnite convergence rate as the Kalman gains can be inﬁnitely high.6 Discussion As has been shown in the veriﬁcation section. ALIGNMENT 0.5 an z 10 9. Horizontal velocity and rotation around yaw.2 10.9). it is investigated what eﬀects arise from nonquasistationarity. two movements diﬀers from the quasistationary state. calculated from (6. First. This situation might be a boat aligning at sea.1mg. the alignment scheme developed in the preceding chapter works and the rotation matrices is converging to the correct values. As with the navigation equations.5 9 0 2 4 6 Time [min] 8 10 0 2 4 6 8 10 0 2 4 6 8 10 Figure 12. it is interesting to investigate what might inﬂuence alignment. This indicate that it stem from noise and ﬁne alignment precision. The diﬀerence on the theoretical gravity in Aberdeen and the one measured is approximately 0.
2 3 2. The errors in the horizontal axes are due to the Coriolis eﬀect and the error in the vertical axis is due to the centripetal acceleration.4 3.10: Errors in accelerations and rotation rates due to horizontal velocity when assuming the INS to be stationary. It does not rotate around horizontal axis and does not have a vertical drift.10 shows the errors in both acceleration and rotation rate. Horizontal Drift Horizontal drift causes two changes. As expected there are accelerations errors on both horizontal and vertical axes. 12. In order to examine the alignment errors due to this horizontal velocity alignment has .2 −8.2 −2. Furthermore.4748 3.475 3.4145 Roration Rate [/s] −6.4135 −6.8 −2 −2. a Coriolis eﬀect is present which is due to the moving navigation frame.4 −8.4747 Roration Rate [/s] 0 x 10 −6 3.5 x 10 −9 East Axis 0 x 10 −6 5 10 North Axis 15 0 x 10 −9 5 10 North Axis 15 Acceleration [m/s2] 3.413 x 10 −6 East Axis −8. The errors in the rotation rate is caused by the INS following the curvature of the earth when moving NorthEast. This eﬀect causes accelerations on the horizontal axis.8 5 Up Axis 10 15 0 x 10 −10 5 Up Axis 10 15 Acceleration [m/s ] 2 Roration Rate [/s] 0 5 10 Time [min] 15 −1. DISCUSSION 82 boat drifts horizontally and rotates around the yaw axis. The sensor outputs are showed in Fig. Figure 12. All sensors outputs are in body frame.3 −8. These two situations have been investigated.4 10 9 8 7 6 0 5 10 Time [min] 15 Figure 12. 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.414 −6. Acceleration [m/s2] −6. First of all the centripetal acceleration due to the speed around the earth.10. The earth simulation model from Ch. This acceleration is only measurable on the vertical axis.6.4749 3.12.
the North axis rotation should become smaller with a positive yaw rotation and the East axis rotation becomes larger.4 shows the result: Alignment Error Heading Pitch Roll Magnitude [◦ ] 46 · 10−6 5 · 10−6 6 · 10−6 Table 12. the error in both heading and attitude are in millidegrees. Table 12. 12.4. whilst the horizontal yaw errors are the horizontal earth rotation components projected into the two horizontal axes.4: Comparison of alignment error when aligning with a yaw rotation. As can be seen in Fig. As can be seen. ALIGNMENT been performed with both quasistationary and nonquasistationary sensor outputs and the alignment result compared. the North axis rotation error looks like a linear increase whilst the East axis rotation error looks like an exponential increase. Table 12. The sensor outputs are shown in Fig.5◦ after 15 minutes. 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.12. rotation around yaw has been simulated. As expected. The important quantities are the heading error and attitude error. which results in 4. 12.3 shows the alignment errors: Alignment Error Heading Pitch Roll Magnitude [◦ ] 12 · 10−3 42 · 10−6 13 · 10−6 Table 12. This explains the negative North error and positive East error. and should be constant.3. A yaw rate at 1/200 ◦ /s has been chosen. Hence. This is projected with a sine and a sine at 90◦ change almost linear whilst a sine at 0◦ changes exponentially. The horizontal yaw errors both start at zero and increases with time. the acceleration errors are all zero as expected. Rotation Around Yaw A rotation around yaw could be caused by aligning on a ship yawing. What is interesting to see. 12. This is because the earth rotation which is around the North axis is projected onto the rotated North and East axes. The vertical yaw error is constant at 8. it can be concluded. aligning with horizontal velocity causes a heading error of 12 millidegree and attitude errors in the microdegree range.11. Highgrade inertial sensors today experience bias stabilities of 10−3 [m/s2 ] and 10−8 [rad/s]. It is assumed that the yaw errors increase on both horizontal axes as the boat yaws more and more from zero.7267 · 10−5 rad/s which is 1/200◦/s. Again the alignment error with yaw error is examined and compared to alignment without any motion and with horizontal velocity. is that the accelerations and angular rates arising from the nonquasistationarity are in the order of 10−6 [m/s2 ] and 10−9 [rad/s] for nonzero velocity. that sensor errors have a larger inﬂuence on .83 CHAPTER 12.3: Comparison of alignment error when aligning with horizontal velocity. As can be seen from Tab.11. As can be seen from Tab. As with the horizontal velocity.
5 −1 0 5 10 Time [min] 15 Roration Rate [/s] 1 8.5 −1 −1.7267 0 5 10 Time [min] 15 Figure 12.12.7267 8.7267 8. All sensors outputs are in body frame.5 −1 0 x 10 −6 Roration Rate [/s] 1 x 10 −6 East Axis 4 3 2 1 0 x 10 −6 East Axis 5 10 15 0 x 10 −7 5 10 15 North Axis Roration Rate [/s] 0 −0.7267 8.11: Errors in accelerations and rotation rate due to rotation around yaw when assuming the INS to be stationary. .6.5 0 −0.5 0 −0.5 −1 0 x 10 −6 5 Up Axis 10 15 0 x 10 −5 5 Up Axis 10 15 Acceleration [m/s2] 0.5 North Axis Acceleration [m/s2] 1 0.5 0 −0. DISCUSSION 84 Acceleration [m/s2] 0.
has the alignment procedure been developed and been tested with simulated and realworld measurements. . The alignment procedure is divided into two phases.7 Summary During this chapter. b Fine alignment uses an indirect Kalman ﬁlter in order to estimate attitude and heading of the INS. Table 12. ALIGNMENT the alignment.01 [◦ /hour]. the ﬁne alignment procedure is performed afterwards. In order to get a better estimate of Cn .01 [◦ /hour] which is equal to 8 · 10−8 [rad/s].85 CHAPTER 12. Quasistationary means that over time the position is zero. And vertical rotation does only inﬂuence alignment marginal.5 shows alignment result: Alignment Error Heading Pitch Roll Magnitude [◦ ] 71 · 10−3 5. 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. than a constant velocity during alignment.9 · 10−6 Table 12. 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. In order to perform a precise ﬁne alignment.1 · 10−6 7. The ﬁne alignment procedure has been tested with both simulated inputs and with realworld sensor measurements. First. What can be derived from these test is that gyro bias causes larger alignment errors than horizontal velocity. Both tests showed that the alignment procedure worked and ﬁne alignment was able to decrease the error in the one shot coarse alignment estimate. 12. This knowledge along with the knowledge of the sensor errors is used with the Kalman ﬁlter to estimate attitude and heading of the INS. the INS is assumed to be in quasistationary state.5: Comparison of alignment error when aligning with a gyro bias of 0.
position and velocity.1: 86 . the same procedure will be followed describing how to aid with a speedometer.Chapter 13 Inertial Navigation with Aiding Unaided INS navigation inevitably suﬀer from unbounded errors in both velocity. they measure diﬀerent quantities and suﬀer from diﬀerent error characteristics. For this reason some kind of aiding is needed to either bound or reduce these errors. A comparison of the two systems is described below in Tab. The main idea is that with independent information at hand the Kalman ﬁlter is able to estimate with greater accuracy. GPS measures position and velocity directly. This chapter will end with a description of the approach used in this report to aid the INS with a GPS. though experiencing short time error drift. 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. Many diﬀerent forms of aiding can be used giving various results.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 attitude which may be unacceptable in many applications. 13. a speedometer and GPS. An error model of the GPS will be derived leading to an error model of the INS/GPS system ready for Kalman ﬁltering. Where an INS system estimates position and velocity by measuring acceleration and angular rate and afterward double integrating these quantities to give the output. Secondly. This chapter will start out with a description of the GPS system and describe some diﬀerent ways to implement GPS with an INS system. This report will describe the use of two diﬀerent aiding devices. Some of the more popular aiding devices are Doppler Velocity Logs (DVL). 13. So where the INS will have an unbounded longtime error drift. Global Positioning System (GPS) and barometric measurements. the GPS is bounded.
For this reason. As the clock in the receiver is not synchronized with the satellite clock this clock oﬀset also needs to be determined. The code tracking loop is the algorithm which actually estimates the pseudo range.1 Description of GPS The GPS system consists of at least 24 satellites orbiting the earth. Although it might seem peculiar why four satellites are needed to describe a position in Cartesian space. The orbits of the satellites are arranged in six planes. The carrier loop is the algorithm which . With the use of at least four satellites both position and velocity of the GPS antenna can be estimated. As the satellites are not in geostationary orbit the GPS receiver has a clock which keep track on where the satellites are at a speciﬁc time. A general GPS scheme is illustrated below. The code correlation algorithm extracts the signal from the correct GPS which is then sent to the code. As all the satellites transmits on the same frequency the received signals are a mix up of all the visible satellites signals.87 CHAPTER 13.1. The pseudo range is the designation for the time the signal has traveled from the diﬀerent satellites to the receiver. They are arranged in such a way that at least four satellites are visible from anywhere on the earth.1: Illustration [Brown and Hwang.1: Comparison of GPS and INS These diﬀerent error characteristics makes for a beneﬁcial combination of the two measurements.and carrier tracking loop. 13. with at least four satellites in each plane. but the traveling time of the signals. four satellites are needed to determine the position.1 is the antenna and RF hardware which is used to receive the GPS signals. where normally only three parameters are needed. 13. Code tracking Loop Pseudo Range Antenna and RF Hardware Code Correlation Carrier Tracking Loop Delta Range Figure 13. This is the reason for the name pseudo range as it is not a distance. INERTIAL NAVIGATION WITH AIDING INS GPS Advantages Very precise estimates Provide attitude estimates Fast update rate Noise almost white Bounded errors Disadvantages Unbound errors Knowledge of gravity required No attitude estimate Slow update rate Coloured noise Table 13. 1997] of a simpliﬁed GPS receiver scheme The ﬁrst block of Fig. the reason is the clock oﬀset in the GPS receiver.
A simpliﬁed representation of this integration is seen below: As can be seen in Fig. 1997]: σ1 = σ2 = σ3 = σ4 = where σi = Pseudorange from the i’th satellite to the GPS receiver antenna.1. 13.1. So it is obvious that the more satellites the more precise estimate of the position. It uses the GPS measurements when these are available and uses the INS during GPS outages. With these four satellites. the GPS system still functions separately whilst the INS system uses the GPS measurements as a parameter in the measurement equation.1) 13. AIDING WITH GPS 88 estimates the velocity of the receiver. the output from the GPS algorithms and INS measurements are used as the input to the Kalman integration ﬁlter. loosely coupled and tightly coupled aiding. 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.1) describes four spheres with center at the i’th satellite where the radius is corrected with the clock oﬀset. This approach does not involve any changes to the two systems and they will function separately if one system fails. xsati . ygps . zsati = Position of the i’th satellite xgps . (xsat1 − xgps )2 + (ysat1 − ygps )2 + (zsat1 − zgps )2 + c∆t (xsat2 − xgps )2 + (ysat2 − ygps )2 + (zsat2 − zgps )2 + c∆t (xsat3 − xgps )2 + (ysat3 − ygps )2 + (zsat3 − zgps )2 + c∆t (xsat4 − xgps )2 + (ysat4 − ygps )2 + (zsat4 − zgps )2 + c∆t (13. The main advantages of this . the position is estimated by solving the following system[Brown and Hwang. in order to calculate the position of the receiver at least four satellites are needed.1. This means that it can always be implemented to an already existing INS system and with any kind of GPS receiver.2 Uncoupled The uncoupled aiding scheme is the simplest method. ysati . As mentioned earlier. The velocity of the satellites is known at any time so the receivers velocity can be estimated because of this Doppler eﬀect. Aiding with GPS can be divided into three diﬀerent approaches. It uses the fact that the carrier wave changes frequency when either the satellite or receiver is moving. If GPS measurements are missing over large time periods the errors within the INS will of course grow. A short introduction will describe the advantages and drawbacks of the diﬀerent approaches. The disadvantages is that it is not possible to gain the same precision as with the coupled integration as will be described below. So by solving these four equations. zgps = Position of the GPS receiver antenna c = Speed of light ∆t = Oﬀset between the clock in the satellite and the clock in the receiver The four equations in (13. Uncoupled.2. the intersection of these four spheres is the location of the receiver.3 Loosely Coupled With the loosely coupled approach. 13.13.
89
CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
GPS receiver GPS position and velocity estimates
Antenna and RF hardware
Correlation and tracking loops
GPS Kalman filter
Position INS Inertial sensors INS computation
INS/GPS Kalman Error estimations filter integration
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 integration is a cascade of two Kalman ﬁlters. As the output from the GPS Kalman ﬁlter is used as a measurement this is assumed to have white noise by the integration Kalman ﬁlter. This is not necessary true as the output from the GPS may be time correlated. This can happen if the integration ﬁlter samples data faster than the GPS can supply uncorrelated data. It can also happen due to multipath interference which happens when the same satellite signal is received several times due to reﬂections. Yet another problem is the fact that at least four satellites are needed to get position and velocity output from the GPS. In situations where less than four satellites are available, aiding is not possible. Furthermore is it a problem that the integration ﬁlter requires knowledge of the covariance matrix of the GPS ﬁlter output. This covariance matrix diﬀer 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 pseudorange and deltarange estimates
Antenna and RF hardware
Correlation and tracking loops
Position INS Inertial sensors INS computation
INS/GPS Kalman Error estimations filter integration
INS pseudorange and deltarange estimates
Figure 13.3: Tightly coupled INS/GPS approach
As can be seen in the ﬁgure, it is now the pseudorange and deltarange which are used in the integration ﬁlter. These quantities are the outputs from the GPS tracking loops and can be used to model errors in the INS system. As with the loosely coupled
13.2. GPS ERROR MODEL ANALYSIS
90
integration, the GPS tracking loops are aided with the INS states in the same manner as with the loosely coupled approach. The advantages of this approach is as follows: • The statistical problem with the output from the loosely coupled approach is eliminated by integrating the two Kalman ﬁlters in one. • This approach will function although four satellites are not visible to the GPS receiver though accuracy will decrease drastically. As can be seen from Fig. 13.3 the GPS position and velocity is not present. These estimates can be produced in a separate loop and error detection may be performed on these estimates. This approach is preferable to the loosely coupled approach but the implementation is more complicated and the pseudorange and deltarange 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 Integration
The observation for the GPS receiver is the i’th GPS measured range from the INS to the satellites (˜GPSi ) subtracted from the INS estimated range from the INS to the p satellite pi . 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 = pi − pGPSi (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: t t pGPSi = c(˜reci − ˜GPSi ) ˜ where ˜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 t c = The speed of light (13.3)
91
CHAPTER 13. INERTIAL NAVIGATION WITH AIDING
The measurement equation for the Kalman ﬁlter needs to be in a form suitable for the ﬁlter which is in error states. For this reason the measurement equation is the ˆ diﬀerenced version of (13.2), z = zobs − zobs which is: zi = δpi − c(δtreci − δtGPSi ) (13.4)
This is the measurement equation for the Kalman ﬁlter 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 diﬀerent error sources. First of all, the position estimated by the INS is errorprone and secondly, the position of the satellite, which is estimated from the ephemeris data, is errorprone. As the ephemeris data gives the satellite position in earth ﬁxed coordinate system(Eframe) the range is also deﬁned in the Eframe: ˆi r l pe = ˆei − ˆe − ˆe rs where ˆei = Position vector from the center of the earth to the i’th satellite rs ˆe = Position vector from the center of the earth to the INS r ˆe = Lever arm from GPS antenna to INS l In order to ﬁnd the range error the magnitude of (13.5) is deﬁned as: p2 = (ˆ e )T pe ˆi pi ˆ i In order to ﬁnd the range error δpi the diﬀerential of (13.6) is found: ˆi 2ˆi δˆi = (δpe )T pe + (ˆ i )T δpe = 2(ˆ e )T δpe p p pe pi i i i which can be rearranged into: δpi = (ˆ i )T δpe pe i pi ˆ (13.8) (13.7) (13.6) (13.5)
δpe can be substituted with the diﬀerential of (13.5) to give: i δpi = (ˆ i )T (δrei − δre − δle ) pe s pi ˆ (13.9)
As it can be seen from (13.9), δpi is calculated from a vector dot product. As a dot ˆ product is invariant pi can be evaluated in the navigation frame[Savage, 2000]: δpi = (Cn pe )T (Cn δrei − δrn − δln ) e ˆi e s pi ˆ (13.10)
The lever arm error can be expressed as follows: δln = δ(Cn lb ) = δCn lb + Cn δlb b b b Using (11.6) on (13.11) reformulates it to:
n n δln = −Γg Cb lb + Cn δlb = (Cn lb ) × γ + Cb δlb b b
(13.11)
(13.12)
With these rearrangement δ pi from (13.4) is: ˆ δpi =
n (Cn pe )T (Ce δrei − δrn − (Cn lb ) × γ + Cn δlb ) e ˆi s b b pi ˆ
(13.13)
14) As the lever arm is assume constant the lever used by the computer is a constant: ˆb = lb l const ˙ lb const = 0 (13.21) (13.4) which is needed this is derived as the diﬀerential of (13. it is deﬁned how a clock is normally constructed.20) plus a random term [Savage. it is customary to assume that δˆei and δtreci are zero as rs ephemeris data are very accurate and the atomic clock in the satellite is very precise.19) . This receiver clock error is modelled as follows. So the receiver clock time is given by: ˆrecn = ˆrecn1 + tosc0 = ˆrecn1 + t t t where ˆrec = Clock time t tosc0 = Nominal time between clock ticks fosc0 = Nominal clock frequency (13.20) (13.18) 1 fosc0 (13. GPS ERROR MODEL ANALYSIS 92 The last thing to do before implementation with the Kalman ﬁlter is to derive expressions for the lever arm error δlb . In order to describe these errors. 2000].2. The main source of error originate from the receiver clock error. A clock suﬀer from two kinds of errors. position error of the i’th satellite δˆsi and the clock re error δtreci . such as a crystal.13.15) The lever arm error is the estimated lever arm subtracted from the true lever arm: b δlb = δlconst − lb ﬂex ˙ δ lb const = 0 (13. Normally a clock is simply a matter of counting ticks from a oscillatory source. The true lever arm is modelled as a constant and a ﬂexing term: b lb = lb const + lﬂex ˙ lb const = 0 (13.16) For INS/GPS integration. A random error caused by jitter and quantization and an oscillatory error[Savage. 2000]: ˙ δ tf = δfosc fosc0 δtreci = δtf + nt (13.17) Rearranging this clock time equation and dividing with the actual time between clock ticks ∆t gives: ˆrecn − ˆrecn1 t t 1 1 1 = = ∆t fosc0 ∆t fosc0 ∆t ˆ Recognizing that the lefthand term is the derivative of trecn yields: ˜ ˙ ˆrec = fosc t fosc0 where ˜osc = 1/∆t = The actual clock frequency f As it is the error in the clock time δrreci from (13.δtreci .
13.24) The observation equation for the Kalman ﬁlter is the INS estimated velocity (ˆ n ) minus v ): the measured velocity (vn INS. nosc. 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.2.rand . For this reason the loosely coupled integration is used which is described in Sec.2 Error Model for the INS/Speedometer Integration Aiding with a speedometer diﬀers somewhat from aiding with a position.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: ˆ ˆ ˆn ˜b l ˆ zobs = vn − Cn vb − Cb (ωib × ˆb ) b ref The associated measurement equation is the diﬀerential of (13. INERTIAL NAVIGATION WITH AIDING 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 satellitetosatellite or from cycletocycle. This means that if the INS rotate the aiding device would output an erroneous velocity if not correct for the lever arm.26): (13. 13.aid = Cb vref + Ce l (13.Mark = Is the random and ﬁrst order Markov process error contribution Cosc.aid ˙ ˆ nˆe ˆn ˆ n ˆb ˆ zobs = vn − vn INS.rand for the ﬁrst order Markov process = Is the white noise error source for the oscillatory part of the receiver clock error (13.Mark δfosc.rand + δfosc.27) .Mark ˙ δ fosc.Mark = −Cosc. δfosc.93 CHAPTER 13. 2000]: δfosc = δfosc.rand ˙ δ fosc.aid = v − Cb vref − Ce l (13.3.22) This section described the derivation of the GPS error model needed when using tightly coupled integration. The oscillatory part can be modelled as a random varying parameter plus a ﬁrst order Markov process[Savage. As this involves modeling and testing the crystal of the speciﬁc GPS receiver this is quite time consuming and is not within the scope of this report. So the velocity of the INS measured by the aiding device is the measured velocity corrected for lever arm velocity in the following way: n b n ˙e vn INS. The last thing before implementation is to determine the numeric values of the clock error model.Mark where δfosc.Mark .Mark + nosc.26) n ˆ ref ˆ n ref ˆ b ˜ b z = δvn − δCb vb − Cb δvb − Cn (ωib × δlb ) (13.rand = nosc.23) (13.Mark = Is the correlation frequency and white noise error contribution nosc.
The only thing missing is an expression for the velocity error experienced by the speedometer δvb .29) ψ n = γ n − ǫn δυ n = δvn + ǫn × vn Using these properties gives the following: (13. 13.31) ˆ ˆ ˜b ˆ ˜b ˆ z = δυ n − vn × ψ n − Cn δvb − Cn (ωib × δlb ) + Cn (ωib × lb ) b ref b const b ﬂex (13. IMPLEMENTATION OF NAVIGATION WITH AIDING 94 b ˜b where δCn (ωib × lb ) and Cn (δ ωib × lb ) have been neglected as they are assumed very b ˜ b small [Savage.32) and (11.3. Furthermore. 2000].1 Loosely Coupled INS/GPS Integration As mentioned earlier. These two properties relating the two error models are restated here: (13. Using (11. the integration of a speedometer is almost ready to be implemented.37).3.27) reveals: b ˆ ˆ ˜b z =δvn + (γ n × Cn )ˆ b − Cn δvb − Cn (ωib × (δlb b vref b ref b const − lﬂex )) ˆ ˆ ˜b ˆ ˜b ˆ ref δvn − vn × γ n − Cn δvb − Cn (ωib × δlb ) + Cn (ωib × lb ) b ref b const b ﬂex (13. the measurement equation is modiﬁed into containing ψerror parameters rather than γ error parameters using (11. 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. the loosely coupled integration combines the INS and GPS measurements to estimate the velocity.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. 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 speciﬁc GPS which is used . 13. position and attitude error of the INS.28) An approximation for vb derived from (13.13.6) and the lever arm error model from (13. As this expression diﬀers according to which speedometer is used and it is quite ref cumbersome to develop the error model.16) on (13.32) At this point. this is outside the scope of this report.28) gives: ˆ ˜ b lb ˆ ˆ ˜b ˆ ˜b z = δvn − (ˆ n − Cn (ωib × ˆ )) × γ n − Cn δvb − Cn (ωib × δlb ) + Cn (ωib × lb ) v b b ref b const b ﬂex (13.30) b ˆb As the parameter ˆ can be set freely as long as δlb l const is taken account. l is chosen to be zero.24) gives the following: ˆref ˆ ˜ b lb ˆ ref ˆ vn = vn − Cn (ωib × ˆ ) b Using this expression in (13.
δvn = Is the true position and velocity true true With the measurement equation derived. the model for the Kalman ﬁlter is: ˙ x = Ax + Gp np z = Hx + Gm nm where x = ψ n δvn δrn T n δrn − δrn rn + δrn − rn − δrGPS true true INS GPS INS = n n n n δvn − δvn vtrue + δvINS − vtrue − δvGPS INS GPS rn − rn INS GPS vn − vn INS GPS (13. A ﬁgure of the INS/GPS integration is presented below. this approach can always be implemented in any INS system.4 the GPS measurements are subtracted from the INS measurements to form the observation set.33) (13.95 CHAPTER 13. The Kalman ﬁlter estimates the errors which are used to reset the INS. As can be seen from Fig. 13.35) (13. Derivation of Kalman Filter Equations The Kalman ﬁlter is developed using the same approach as in Sec. 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. The observation equation is: zobs = The measurement equation is: ˆobs z = zn − zn obs z= where δrn . INERTIAL NAVIGATION WITH AIDING INS n rINS n n rINSvINS n rn vn + n rn v Kalman Filter  GPS r n n GPS GPS v Figure 13.4: Illustration of the loosely coupled INS/GPS integration as measurement noise.54) and . δvn = Is the error in the INS estimation of position and velocity INS INS δrn . δvn = Is the error in the GPS estimation of position and velocity GPS GPS δrn .4. 12. The precision of the GPS is described in the data sheet and therefore.36) The error state dynamic matrix (A) is the time varying ψerror model from (11.34) (13.
δGPSposition = GPS velocity and position variance E = Is the expected value operator The last thing before implementation is to ﬁnd the discrete error state dynamic matrix and the integrated process noise matrix.5T2 aU −0.34) and is as follows: 0 0 0 H= 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 (13. 12.4 where: Q1n = Gp QpDens GT Tn p So the ﬁrst order process noise matrix is: Qn ≈ 1 1 Q + Φn Q1n ΦT n 2 1n 2 (13.3.5T2 aE 0.41) . 12.5T2 aN −0.5T2 aE 0 0.40) 0 0 2T g r −Ωβ 0 0 0 The integrated process noise matrix is found using the same approach as in Sec.4 nm is assumed to have equal and uncorrelated variance which leads to the following measurement covariance matrix: Rn = E(nmn nT n) = m where δGPSvelocity .5T2 aU 0 0.13.5T2 aN 1 Ωα −Ωα 1 (13. The discrete error state dynamic matrix is computed in Matlab shown below: I3x3 δGPSvelocity 0 0 I3x3 δGPSposition (13.4 and is as follows: Gmn = nmn = I3x3 0 δvn GPS δrn GPS 0 I3x3 (13. IMPLEMENTATION OF NAVIGATION WITH AIDING 96 the process noise dynamic matrix is derived from (13. The error model which should be used when in navigation mode is the one from (11.54).38) As in Sec.39) Φ= 1 T(Ωc ) T(−Ωb −Ωβ ) 1 T(Ωa +Ωα ) T(−Ωc ) 1 T(Ωb +Ωβ ) T(−Ωa −Ωα ) 0 0 0 1 0 0 0 T(2Ωc ) 0 0 0 T(−2Ωb −Ωβ ) 0 0 0 −T g r 0 0 1 0 Ωβ 0 0 0 0 −T g r 0 0 0 −TaU 0 TaE TaN TaU −TaN 0 −TaE 0 T(−2Ωc ) 1 T(2Ωa +Ωα ) T(2Ωb +Ωβ ) T(−2Ωa −Ωα ) 1 T 0 0 0 T 0 0 0 T −0. 12.37) The measurement noise is derived in the same way as in Sec.
13. Using the δrn relation from (11.4 which gives: uc = [ψ n .43) ǫn is calculated from δrn in the following way: Having constructed ǫn . δrv ]T (13. It has been deﬁned earlier that the n n navigation frame does not rotate with respect to the geodetic frame. INERTIAL NAVIGATION WITH AIDING Q1n 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 0 0 0 0 qf 0 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 0 0 0 0 0 0 0 0 Tn 0 0 0 0 (13. The control reset vector is deﬁned as in Sec. e n n n navigation to Earth rotation matrix Cn with ǫ and velocity v with δv . all the necessary parameters are available and the INS states are corrected as described in (12.97 CHAPTER 13. 12. But in order to extract γ n from ψ. As ψ contain both the error in Cn and n b b Ce . This means that all the heading error is in γ n . γ n can be calculated from the ψ n relation from (11.43) n The states which are corrected are the body to navigation rotation matrix Cb with γ n .44) At this point.43) in the following way: γ n = ψ n + ǫn (13. .4 Veriﬁcation of Navigation with Aiding In order to test the aiding scheme developed in the preceding section both a GPS and an INS is needed. δvv . ǫn is needed. In order to correct Cn and Ce some transformations are necessary.46) With these equations derived. 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. the Kalman ﬁlter is ready for implementation and veriﬁcation.42) The last thing to deﬁne the control reset vector and how it is used correct the INS states.41): n Cn (+c) = [I3x3 − Γn ]Cb b Ce (+c) = [I3x3 − En ]Ce n n vn (+c) = v(−c) + δvn (13. δr is needed as it describe the errors in Ce . If one look at uc the only state which can be directly corrected is the velocity.45) δrn = R(ǫn × un ) + δRun zn zn δrx /R ǫy δry /R = −ǫx + δRu δrz /R 0 (13.
Navigation with aiding is shown in Fig. the GPS data outage occurs.193 57.5 shows navigation without aiding. Two test scenarios are presented.188 57. Just before the North turn. As can be seen from Fig.6 Figure 13. One test scenario where the INS starts out with 15 [min] alignment followed by navigation without aiding.191 57.13. VERIFICATION OF NAVIGATION WITH AIDING 98 The test data used to verify navigation with aiding is the same as used in Sec. The same track is followed as in the ﬁrst test scenario but navigation is aided with the GPS data. 9. With a drive time of approximately 5.187 57. Figure 13. the GPS position and the aided navigation.19 57.5: This ﬁgure shows navigation on realworld measurements without aiding. and how the INS system keeps the system “on track” during GPS outages.6 [min]. These data have been used as the GPS position. The INS then navigate . the INS navigates without any aiding. 57. in the bottom right of the track. Aiding with a GPS should bound the position error.084 −2.082 −2. During this time.6 shows the reference track.086 Aided position Real position Latitude [°] −2.5. 13.08 Longitude [°] −2.194 57.4. 13.5.078 Figure 13. A GPS outage of 40 [s] duration is simulated after 3. The two test scenarios should show how aiding is able to bound the position error.5 the navigation starts out following the reference track but drifts away from the reference track unbounded. As can be seen the aided navigation follows the reference track.3 [min] driving time. the Kearfott does only experience minor drift why the Kearfott track can be used as the reference track. 9. 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.192 57. The track is the same track used to verify the navigation equations in Sec. The second test scenario is a scenario where the INS again starts with 15 [min] alignment.186 −2.189 57.
The precision of this position depends on how tightly the INS is “fused” with the GPS.6: This ﬁgure shows navigation on realworld measurements aiding with a GPS. What can be seen is that the fused position (dotted) does not suﬀer from the same sudden jumps to the same degree as the GPS (x).191 57. however.193 57.084 −2. 13. During this chapter. But the plots on the right column shows a detailed view of the latitude and longitude the ﬁrst minute of navigation. Just before the turn to the South the GPS data is valid again and thus pulls the navigation towards the reference track. It does. 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.194 57.189 57. some kind of aiding is preferable if INS is to be used over longer time periods.187 57.08 Longitude [°] −2.188 57. without aiding but the position starts to drift from the reference track.7 shows the latitude where the second row shows the longitude of the INS.6 is how the INS/GPS position variates around the reference track. In order to investigate that Fig. a GPS and a Doppler velocity log. As can be seen the “x” which indicate the GPS position is absent.7 shows a 2d plot of the latitude and longitude. where both the velocity and position are available bounds the position error.19 57. 13. two diﬀerent aiding devices have been presented. 13. The ﬁrst row of Fig.5 Summary As unaided INS inevitably suﬀers from unbounded position error. 13.192 57.078 Figure 13.086 Aided position GPS position Real position Latitude [°] −2.082 −2. INERTIAL NAVIGATION WITH AIDING 57. Aiding with a GPS. What can not be seen in Fig. Three diﬀerent approaches are described in this thesis where the loosely coupled .99 CHAPTER 13.186 −2. follow the turns made by the vehicle.
0822 0 0.0819 −2.4 0.191 57.1908 0 0.7: This ﬁgure shows the positions of the fused INS/GPS (dotted).5.1912 57.191 Latitude [°] 57.0821 −2.0818 −2.189 57.6 0.19 57.186 0 1 2 3 4 5 57.13.082 −2.082 −2.4 0.084 −2.187 57.6 Time [min] 0.1911 57.192 57.2 0.8 1 −2. .078 −2.8 1 Figure 13.086 0 1 2 3 Time [min] 4 5 −2.2 0.1909 57. the GPS positions (x) and the reference track (solid). SUMMARY 100 57.188 57.08 Longitude [°] −2.
under water. A drawback from aiding with a DVL is that the position error is not bounded but only decreased.g.101 CHAPTER 13. e. When in such a situation aiding with a DVL is a possibility. A disadvantage when aiding with GPS is that it is not possible when no GPS signal is present. INERTIAL NAVIGATION WITH AIDING approach have been chosen for this project. .
and the transit time t for both beams to go around the light path is the same.1 Light path Light source CW beam ω Beam splitter/ detector CCW beam r Figure 14. The basis of inertial navigation is the accelerometers and angular rate sensors which exist in a wide variety. This chapter will give a description of the sensors that are commonly used in strapdown inertial navigation systems. Also. ω is 0.Chapter 14 Inertial Sensors The next two chapters describe an approach which is necessary when implementing an INS in the real world. and split into two beams travelling clockwise and counterclockwise around the circular light path with the radius r. 14. 14. each exploiting diﬀerent physical phenomena to measure speciﬁc force or angular rate. the errors that are expected to be experienced in these kinds of sensors are explained. This is illustrated in Fig.1: The fundamental principle of optical angular rate sensors Light is generated by the light source. With no rotation.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 diﬀerent lengths if the path is rotated. 102 .
the RLG outputs a pulse every time the gyro has rotated a certain angle.1.1.1 Ring Laser Gyro The Ring Laser Gyro (RLG) is the most common type of gyro used for precision strapdown inertial navigation systems. Along with also being a solid state sensor. appeals more to medium accuracy applications. 14. The fringes of the inference pattern moves at a rate proportional to the rate of rotation.1) A ﬁgure of an accelerometer is shown in Fig. 14. this makes it ideal for strapdown inertial navigation systems. As the gyro rotates. which can be detected. the need for high quality mirrors.01% >200 Hz >1000◦/s IFOG <0. A comparison of typical accuracy of RLG and IFOG sensors are shown in table Tab. if the light path is rotated. The performance of the IFOG. A prism combines the two light beams.1◦ /hour/g2 0. 14. 14. INERTIAL SENSORS However. however. 1997]. and the frequency diﬀerence forms an inference pattern. which makes the IFOG cheaper than the RLG. and the phase diﬀerence can be measured as a change of intensity by photodetectors.001 − 10◦ /hour insigniﬁcant insigniﬁcant few parts per million to 0. By having a proof mass suspended by springs.2 Interferometric Fibre Optic Gyro In an Interferometric Fibre Optic Gyro (IFOG). the length which the beams have to travel changes.5 − 50◦ /hour 1◦ /hour/g 0. F = ma (14. the displacement of the mass due to acceleration can be measured by the pickoﬀ. By outputting a pulse each time a fringe moves past a photodiode. which is spun around the sensitive axis. RLG <0. It works by letting a laser travel clockwise and counterclockwise around in a continuous light path constructed from three or more mirrors. This makes the RLG inherently digital and rateintegrating. two broadband light beams are transmitted CW and CCW through a ﬁbre optic coil.1: Comparison of typical gyro accuracy 14.05 − 0.103 CHAPTER 14. the path length for each of the beams are changed. One of the mirrors allows a small amount of light to exit the cavity. This makes the IFOG an analog rate sensor. . As the IFOG uses ﬁbre optic coils instead of a laser cavity.2. and precision machinery is avoided. As the IFOG is rotated. and the transit time diﬀerence for each beam can be measured.2 Accelerometers An accelerometer uses Newton’s Second Law of motion to measure speciﬁc force.5% >100 Hz >1000◦/s gindependent bias gdependent bias g2 dependent bias scale factor errors bandwidth maximum input rate Table 14.1[Titterton and Weston. the frequency of the two beams change.
Random noise/random walk Stochastic elements arising from other unmodelled noise sources. both deterministic and nondeterministic. the current needed by the electromagnet to hold the proof mass in place changes.2 is known as openloop accelerometers. and this change is proportional to the acceleration experienced.3. the proof mass is held at the zero position by an electromagnet. I.e. To overcome this.2: Conceptual drawing of a accelerometer Displacement pickoﬀ It is important to notice that gravity inﬂuences the proof mass by attraction. These include Bias The output from the sensor when no input is applied. . Accelerometers working as shown in Fig. the diﬀerence from a 1:1 input/output mapping. as the springs are not linear. This means that the input the sensor receives (angular rate. When the sensor accelerates. These suﬀers from nonlinearities at large accelerations. the most used type of accelerometer in inertial navigation systems today is the forcerebalance accelerometer. Temperature sensitivity The inﬂuence of temperature on the output of the sensor. SENSOR ERROR MODELS 104 Acceleration Case Spring Proof mass Spring Figure 14. Misalignment The inaccuracies in sensor alignment. Scale Factor The scaling the sensor applies from input to output. Quantization noise The noise introduced by converting an analog signal into a ﬁnite precision digital signal. gsensitivity The inﬂuence of acceleration on the output of the sensor. 14.14. so they output a pulse when a certain change in velocity have been measured. when 3 1dof sensors are mounted in a frame. so the acceleration measured by the accelerometer will be in the opposite direction of the attraction. 14. acceleration) does not correspond to the output the sensor sends. Here. Accelerometers for INS applications are of the integrating type.3 Sensor Error Models Many sources of errors arise from sensor imperfections.
the MA are the misalignments (with the subscripts deﬁning which axes the misalignment applies to). Therefore. the B’s are the biases. The most ˙ dominant terms are the linear bias. This model will be used in chapter 15 to calibrate inertial sensors. 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) Bz (T ) MAzx (T ) MAzy (T ) SFz (T ) (14.105 CHAPTER 14. the SF’s are the scale factors. Hence. bias. Normally.2) Where i are the measured input. scale factor. o are the output. gsensitivity and temperature sensitivity can be regarded as random constants with a process model of c = 0. INERTIAL SENSORS All of these can be nonlinear. misalignments. scalefactor misalignments and temperature dependency. these can be found by testing the sensor with various inputs. . and T is the temperature.
2 Calibration Equations In order to ﬁnd some of the sensor misalignment errors. Thus. which might be rotated with respect to the body frame. a weighing between sensor compensation (cost and time) and INS accuracy must be made. A rotation and a measurement can be done in 5 to 20 seconds depending on environmental eﬀects. however. In this error model is the earth rotation neglected due to the short time between each rotation making the earth rotation very small. This simpliﬁed error model of the system is: 106 . it is chosen to identify constant but temperature dependent errors. With no translatory motion. these will be called misalignment angles and must be estimated during calibration. The term “sensor error” will throughout this chapter signify only a repeatable sensor error.1 Introduction While it is possible to identify all the sensor errors.Chapter 15 Calibration of Sensors The performance of the INS is dependant on the errors that are present in the accelerometers and gyros. only gravity and the rotation of the earth inﬂuences the measurements. 15. In this project. pitch and yaw angles of this rotation is not known. The repeatable errors. this can take much time and requires expensive calibration equipment. Depending on the required accuracy of the ﬁnal INS. This is done by identifying the constant errors at various temperatures by rotating the IMU through a series of 180◦ rotations[Diesel. These errors cannot be compensated for without inﬂuencing the signal itself. 1987]. it is necessary to deﬁne a sensor frame. The magnitude of the random errors depends on the quality of the sensor. a simpliﬁed error model has been developed. As the roll. can be identiﬁed through a process called calibration and thus be compensated for. 15.
15.1: Rotation sequences used for calibration Where T are the time after a rotation.3) 0 aMAxy aSFy aMAzy aMAxz aMAyz ab (t) aSFz (15. Set 1 zb Set 2 yb Set 3 xb 3 (180◦ ) 2 (180◦ ) xb zb 3 (180◦ ) 2 (180◦ ) yb 3 (180◦ ) 2 (180◦ ) 1 (180◦ ) yb 1 (180◦ ) xb 1 (180◦ ) zb Figure 15. The rotations needed are shown in Fig. any velocity is an error. Thus. the system is rotated through 3 sets of 3 rotations each. 14 is used.107 CHAPTER 15.1)): n n −gθy (T ) −gθy (0) n n ˙ ˙ δ vn (T ) − δ vn (0) = δan (T ) + gθx (T ) − δan (0) + gθx (0) (15.2) 0 0 n −g∆θy n = ∆δan + g∆θx (15. From this.4) Where δab are the errors in the measured accelerations. the equations that need to be solved are of the following type (after adding time dependency on (15. The ∆ preﬁx denotes that the following term is the diﬀerence of the term from 0 to T . CALIBRATION OF SENSORS n n Where θy and θx are the misalignment angles from the sensor to the body frame around the y and x axis and δan are the acceleration errors. As ˙ the system should not be translating. all given in the navigation frame. so the vn term obtained ˙ from the navigation equation (9.1) To excite all the sensor errors. The accelerometer error model is aBx aSFx δan (t) = Cn (t)δab (t) = Cn (t) aBy + aMAyx b b aBz aMAzx ˙ To be able to analytically identify the errors δ vn is logged just before and after the rotations. and 0 is the time before a rotation. aMA are misalignments and ab is the acceleration due to gravity. ∆δan is obtained as ∆δan = δan (T ) − δan (0) (15. aSF are scale factors. 0 n ˙ δ vn = δan + θz n θy n θz 0 n θx n n −θy −gθy 0 n n θx 0 = δan + gθx 0 g 0 (15. To obtain ∆δan and ∆θ n .1. the aB terms are a constant bias.5) . the sensor error model from chapter Ch.1) is equal to a velocity error δ vn .
2. CALIBRATION EQUATIONS 108 Identically.9) Where φ(t) and ψ(t) are the rotation angles at time t.13) (15.2) for rotation 1 gives − sin(ψ(t)) cos(ψ(t)) 0 0 0 1 0 − sin(φ(t)) 1 0 0 cos(φ(t)) (15.14) .11) (15. solving (15.6) ∆θ n = 0 gSFx Cn (t) gMAyx b gMAzx gMAxy gSFy gMAzy gMAxz gMAyz ω b (t)dt gSFz (15.1.7) Where the gyro bias errors are disregarded as they are expected to contribute only a fraction of an arc second to the error during the short interval from 0 to T . where for brevity.15. the following deﬁnitions are made 1 0 0 0 n Cb (0) = 0 1 0 ab (0) = 0 0 0 1 g −1 0 0 0 n Cb (T ) = 0 1 0 ab (T ) = 0 0 0 −1 −g −2aBx aMAxz g aMAxz g −2aBx = −2aMAyzg 0 ∆δan = −aMAyzg − aMAyzg + −2aBz −2aBz aSFz aSFz (gMAxy cos(φ) + gMAzy sin(φ)) −2gMAzy π dφ = −πgSFy gSFy ∆θ n = − 0 (gMAxy cos(φ) + gMAzy sin(φ)) 2gMAxy gSFy πg −2aBx ˙ ˙ δ vn (T ) − δ vn (0) = −2aMAyzg + −2gMAzyg −2aBz 0 (15.12) (15. one obtains the measurements given in table 15. Solving for the remaining rotations. the gyro error model is gBx gSFx n δω n (t) = Cn (t)δω b (t) = Cb (t) gBy + gMAyx b gBz gMAzx To obtain ∆θ n . The rotation matrix from body to navigation corresponding to rotation set 1 is Rotation 1 and 2 cos(φ(t)) Cn (t) = 0 b sin(φ(t)) Rotation 3 cos(ψ(t)) Cn (t) = sin(ψ(t)) b 0 With this. δω n is integrated T gMAxy gSFy gMAzy gMAxz gMAyz ω b (t) gSFz (15.10) (15.8) (15.
17) (15. with the x accelerometer corresponding to the xaxis. which is equal to deﬁning the sensor frame so that the xy plane is spanned by the x and y accelerometer. 15. The expressions are seen in Tab. the x and y gyro bias is found from set 1 rotation 3: gBx = gBy δ¨y (T ) − δ¨y (0) vn vn 2g δ¨x (T ) − δ¨x (0) vn vn = −2g (15.23) n n The rotation matrix from sensor to body frame is found by calculating θx and θy as n well as the heading angle θz .16) (15.1: Table of observation equations for calibration coeﬃcients From these equations.22) And the z gyro bias is found from set 2 rotation 3: gBz = δ¨y (T ) − δ¨y (0) vn vn 2g (15. βk = δ vxy (T ) − δ vy (0) ˙n ˙n ˙n ˙n k (15. The missing accelerometer misalignments are set to 0. accelerometer biases and misalignments are obtained. expressions for the gyro scale factors. All are calculated from set 1 rotation 3: .2.15) Set 1 α1 =−2aBx +gSFy πg 1 1 β1 =−2(aMAyz +gMAzy )g α1 =2aBx +gSFy πg 2 1 β2 =2(aMAyz +gMAzy )g α1 =−2aBx −2(aMAxz −gMAxz )g 3 1 β3 =−2aBy −2(aMAyz −gMAyz )g Set 2 α2 =−2aBz +gSFx πg 1 2 β1 =−2(aMAxy +gMAyx )g Set 3 α3 =−2aBy +gSFz πg 1 3 β1 =−2(aMAzx +gMAxz )g α2 =2aBz +gSFx πg 2 2 β2 =2(aMAxy +gMAyx )g α3 =2aBy +gSFz πg 2 3 β2 =2(aMAzx +gMAxz )g α3 =−2aBy −2(aMAyx −gMAyx )g 3 3 β3 =−2aBz −2(aMAzx −gMAzx )g α2 =−2aBz −2(aMAzy −gMAzy )g 3 2 β3 =−2aBx −2(aMAxy −gMAxy )g Table 15. This is done by assuming the following approximations: ˙n ˙n δ¨x = δ ax − g θy ≈ −g θy vn ˙n ˙ ˙ δ¨n = δ an + g θn ≈ g θn v ˙ y y x x (15. The accelerometer scale factors can be determined by getting the vz component at the start of the ﬁrst rotation and at the end of the second ˙n rotation in each set after subtracting the corresponding bias term.21) (15.19) (15. For the aSFz term: 1 For set 1 rotation 1 : γ1 = δ vz (0) ˙n 1 For set 1 rotation 2 : γ2 = δ vz (T ) ˙n 1 1 γ + γ2 − 2aBz aSFz = 1 2g (15.18) Remaining is to ﬁnd the rotation matrix from the sensor frame to the body frame and the gyro bias.109 CHAPTER 15. CALIBRATION OF SENSORS j Set j. In this way. Rotation k: αj = δ vx (T ) − δ vx (0).20) These approximations are valid as the accelerometer errors will be very small after just a few iterations of the compensation procedure.
15.3. SMOOTHING FILTER
110
δ vy (T ) + δ vy (0) ˙n ˙n 2g δ vx (T ) + δ vx (0) ˙n ˙n n θy = −2g δ¨y (T ) + δ¨y (0) vn vn n θz = −2gΩn y
n θx = n Where Ωy is the earth rate y component in the navigation frame.
1 α1 +α1 2 2πg α1 −α1 aBx = 2 4 1 α1 +2a gMAxz = 3 2g Bx β 1 +2a gMAyz = 3 2g By α3 1 aMAzx = − 2g − gMAxz
(15.24) (15.25) (15.26)
gSFy =
α2 +α2 2 1 2πg 2 2 α −α aBz = 2 4 1 α2 +2a gMAxy = 3 2g Bx 2 β1 gMAyx = 2g aMAzy = α32+2aBz + gMAzy 2g
gSFx =
α3 +α3 2 1 2πg 3 3 α −α aBy = 2 4 1 −β 1 gMAzy = 2g1 β 3 +2a gMAzx = 3 2g Bz + aMAzx α3 +2a aMAyx = 3 2g By + gMAyx
gSFz =
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 horizontal acceleration errors, a smoothing ﬁlter is applied to the measurements with the dynamic equation ˙ δv 0 δv = 0 ˙ δ¨ v 0 1 0 δv 0 0 1 δv + 0 ˙ 0 0 δ¨ v wk
(15.27)
and measurement matrix
where
δv ˙ y = h δ v + wr δ¨ v
(15.28)
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 coeﬃcients 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,
111
CHAPTER 15. CALIBRATION OF SENSORS
misalignments, scale factors and random noise to simulate uncompensated sensors. Also, the navigation algorithm includes sensor compensation using the calibration coeﬃcients 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 montecarlo simulation of how the calibration coeﬃcients converge towards the true errors after a few iterations. Error gBx gBy gBz gSFx gSFy gSFz gMAxy gMAxz gMAyx gMAyz gMAzx gMAzy aBx aBy aBz aSFx aSFy aSFz aMAyx aMAzx aMAzy True −0.1E4 0.2E4 −0.3E4 1E4 −2E4 3E4 −0.1E4 0.2E4 −0.3E4 0.4E4 −0.5E4 0.6E4 0.01 −0.02 0.03 0.001 −0.002 0.003 −0.3E4 −0.5E4 0.6E4 Estimated −0.098728E04 0.20059E04 −0.29865E04 1.0072E04 −2.0099E04 3.0153E04 −1.8477E05 2.0168E05 −3.4114E05 3.0360E05 −6.1056E05 5.7603E05 0.01 −0.02 0.03 0.001 −0.002 0.003 −3.3082E05 −5.3109E05 5.6882E05
Table 15.3: True and estimated sensor errors
The sensor calibration algorithms have not been tested with reallife measurements. This is mainly because it is diﬃcult to obtain the sensor errors in a particular IMU, and thus, it is not possible to quantify the diﬀerence between the estimated errors and the actual errors.
15.4. CALIBRATION EXAMPLE
112
0 −1 −2 −3 −4 −5 −6 −7
x 10
−5
Gyro X bias estimate
estimate
1
2
3
4 5 iteration
6
7
8
Figure 15.2: Calibration estimation of gyro X bias error.
As the INS inevitably suﬀer from unbounded errors due to the double integration of noisy measurements. 10 shows the diﬀerences between the two ﬁlters an derives the equations used in later chapters to implement this ﬁlter. An indirect Kalman ﬁlter requires an error model of the INS. This part is very critical as even small errors in initial attitude leads to heavily decreased navigation precision. Although these error models model the same system they have diﬀerent properties as they describe the errors with diﬀerent parameters. In order to bound the errors the INS is aided with external reference measurements. The indirect Kalman ﬁlter is implemented using an error model of the nonlinear system which can be approximated linear. As the Kalman ﬁlter used in this thesis is an indirect variant of the normal direct Kalman ﬁlter Ch. Aiding with a speedometer only gives an observation set which include the velocity. Aiding with a GPS and with a speedometer. Using the γerror model the alignment algorithm has been developed in Ch. The ψerror model is preferable in the navigation mode as this model becomes simpler than the γerror model when moving. these errors need to be bounded if the INS is to be used over longer periods. The geodetic constants and equations was presented in Ch. The indirect Kalman ﬁlter is an advantageous approach when the system model has high dynamics and/or is nonlinear. Furthermore. Two approaches has been presented in this thesis in Ch. 12. 13. Two diﬀerent error models are derived. Alignment uses the knowledge of the gravity vector and earth rotation in a known frame and estimates attitude and heading using an indirect Kalman ﬁlter technique. 6 along with a simpliﬁed 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. Aiding with a GPS gives an observations set which include the position which bound the position error. Alignment is a matter of estimating the attitude and heading of the INS. With only the reference velocity the 113 . which is derived in Ch.Chapter 16 Summary Throughout this part has all the theory been derived which constitute the mathematical foundation of an inertial navigation system. the dynamics of the error model is usually much lower than that of the real system. 11. 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.
some of the sensor errors can be estimated under navigation in order to correct the sensor measurements online. Furthermore. 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.114 position error can only be decreased but is still unbounded. . The knowledge of these sensor errors facilitate a more precise alignment and navigation as the sensor measurements can be corrected for these sensor errors.
.
Part III Thesis Summary .
.
The equations developed were veriﬁed against measurements obtained from a medium accuracy INS system. a Kearfott T16. The alignment algorithm was able to estimate Cn precisely using the simulated test data and showed to estimate b Cn close to the one from the Kearfott T16. Tests showed that the output from the navigation equations developed in this thesis followed the output from the Kearfott T16 suﬃciently accurate to validate a veriﬁcation of the system. 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. Two diﬀerent error models of the INS have been developed. Veriﬁcation of the developed navigation equations also veriﬁed the earth simulation model. A γerror model suitable for alignment and a ψerror model suitable when in navigation mode. An alignment algorithm has been developed. This equation have been derived and form the foundation when developing the earth simulation model and the navigation equations. which uses an error model of the system. as the equations in the simulation model showed to be the inverse of the navigation equations. which were veriﬁed using both simulated test data and realworld data from the Kearfott T16 system. The alignment algorithm is based on an indirect Kalman ﬁlter. These tests concluded that a precise gyroscope is essential if high performance is required. The earth simulation model is not a part of an INS. alignment and aiding. The equation of motion translates measured accelerations into a velocity with respect to the surface of the earth. and these navigation equations are the most essential parts of an INS. 118 . but was developed in order to make reproducible test data simulating various sensor errors. This is accomplished by implementing algorithms for the main parts of an INS: Navigation. The development of the navigation equations is based on the equation of motion. Further tests with diﬀerent errors sources revealed that gyro bias was the predominant cause to unbounded position errors. This showed that the developed alignment b algorithm worked. This model was used to testing the precision of the INS when disturbed by diﬀerent noise sources. The basis when developing an inertial navigation systems consists of the equation of motion.Chapter 17 Conclusion The main purpose of this thesis were to develop algotihms for an inertial navigation system. the “ground speed”.
. In order to gain as high precision as possible the sensors needs to be calibrated.119 CHAPTER 17. two diﬀerent aiding approaches have been proposed: Aiding with a speedometer and aiding with a GPS. While aiding with a speedometer only decreases the position. A sensor model together with a calibration procedure is used in order to identify and estimate scale factors. to address the unbounded position error characterising of an unaided INS. misalignment and sensor biases. CONCLUSION In order to aid the navigation equations. The aiding algorithm was shown to be able to bound the errors in the INS system. error aiding with a GPS bounds the position error. the purpose of this thesis have been met. Aiding algorithms for loosely coupled INS/GPS integration were developed which enabled data from the navigation equations to be fused with data from the aiding device using an indirect Kalman ﬁlter. As all the equations needed for an inertial navigation system with aiding have been developed and have been veriﬁed to work.
However. the twospeed approach still oﬀers lower requirements to the processing power of the main computer. 2000]. 120 . This has also not been addressed in this thesis. A more precise discretization method could be used but in either case should the errors introduced by discretization be analyzed.Chapter 18 Improvements and Discussion Before the algorithms developed in this thesis can be implemented and used in the real world. The indirect Kalman approach has been chosen in this thesis to avoid running a nonlinear Kalman ﬁler with a Kalman cycle greater than 2kHz. The choice of discretization method aﬀect the accuracy of the discrete model. The earth simulation model can be used to test some performance aspects of the navigation algorithms by supplying it with various trajectory proﬁles. It has also been argued that the twospeed approach of the integration algorithms are no longer needed. A choice of execution rate of the navigation equations must also be made. It has not been investigated which performance improvements might be gained from running a direct nonlinear Kalman ﬁlter at this frequency. and several solutions have been presented. and in order to increase the accuracy a more advanced discretization method. A simple discretization method based on small time steps has been chosen in this thesis. The navigation algorithms need to be validated more rigorously than done in this report. As power consumption are of great concern in underwater vehicles. This choice depends heavily on the vibrational proﬁle of the sensor array [Savage. This introduces errors which could be avoided with a more advanced correction. INS will probably use a twospeed approach in systems used for underwater applications. Although the equations have been discretized and implemented as a matlab ﬁles in this thesis. the errors involved with the discretization procedure have not been analyzed. but tests in the real world are important to validate the navigation equations completely. and thus allows a lower power consumption. When correcting the rotation matrices a ﬁrst order approximation has been used throughout this thesis. This problem is documented rigorously in the literature. several aspect are to be considered. Before implementation on a microprocessor the equations need to be discretized. as modern computers are fast enough to execute the entire algorithms at full speed.
. The tightly coupled INS/GPS integration could prove more precise and more work should be done to implement that approach. and thereafter navigating using a calibrated IMU. however. If the navigation performance have improved. as the hardware needed for the calibration has not been available. The validation of the calibration equations were made using simulated sensor outputs. IMPROVEMENTS AND DISCUSSION When aiding with GPS only the loosely coupled integration approach have been implemented. the calibration equations work. This has not been done.121 CHAPTER 18. This was done as it is diﬃcult to obtain the values for the errors in a particular IMU. A validation of the calibration equations could be made by navigating using an uncalibrated IMU.
(2007). R. 2nd edition. D. W.. [Titterton and Weston. American Institute of Aeronautics and Astronautics. Strapdown inertial navigation technology. [Savage. Strapdown Associates. Strapdown inertial navigation technology. H. 3rd edition. (1987). (2005). (1971). G. [Diesel. P.. [Brown and Hwang. Y. G. John Wiley and Sons. P. 122 . H. In Biennial Guidance Test Symposium. Inertial Navigation Systems Analysis. 2005] Titterton. Peter Perengrinus Ltd. Technical report. Introduction to Random Signals and Applied Kalman Filtering. (1997). (2000). 3rd edition edition. 1st edition.. 1997] Brown. K. L. 1997] Titterton. P. and Weston. Strapdown inertial navigation . D. and Weston. Peter Perengrinus Ltd. C. number 13th in Biennial Guidance Test Symposium. Strapdown Analytics.lecture notes. J. Strapdown Associates. Applied Mathematics in Integrated Navigation Systems. [Titterton and Weston. [Savage. Calibration of a ring laser gyro inertial navigation system. John Wiley & Sons. [Rogers.Bibliography [Britting. G. J. R. R. (1997). 2000] Savage. (1997). L. 2007] Rogers. J. 1997] Savage. M. 1971] Britting. 1987] Diesel. and Hwang.
This action might not be possible to undo. Are you sure you want to continue?
Inertial Navigation Systems will be available on