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
.
navigering og aiding. Semester Sektion for Automation og kontrol Institut for Elektroniske Systemer Det Ingeniør. Derfor er der blevet udviklet en fejlmodel.aau. De navigeringsligninger som er nødvendige for navigering i nærheden af jorden er blevet udviklet og testet med referencesystemet. Disse algoritmer bruger også et indirect Kalman ﬁlter og en fejlmodel. Juni 2008 . For at veriﬁcere at algoritmerne også fungerer med data fra virkeligheden. En alignment algoritme er blevet udviklet som estimerer hældning og retning af systemet. er algoritmerne også blevet testet med et referencesystem. Aidingalgoritmer til navigeringsligningerne er blevet udviklet for en hastighedsmåler og en GPS. 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. en Kearfott T16 INS. Alignment algoritmen er blevet veriﬁceret op imod data fra referencesystemet. 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. som beskriver udbredelsen af fejl i INS systemet. Denne afhandling indeholder algoritmer til alle disse dele.AALBORG UNIVERSITY 8. En alignment algoritme bruger et indirect Kalman ﬁlter som estimerer fejlene i INS tilstandene. 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. og det er blevet vist at den var i stand til at estimere hældning og retning tilfredsstillende. Generelt består inertial navigation af tre dele: Alignment. Testene viste at de udviklede navigeringsligninger var i stand til at følge referencesystemet til en tilfredsstillende grad. Det blev også vist at INS systemet var i stand til at følge referencesystemet under manglende GPS signaler. hvilket gør at navigeringsligningerne er veriﬁcerede.dk/ Synopsis: Denne afhandling dokumenterer udviklingen af de algoritmer der bruges i inertial navigation systemer (INS). En simuleringsmodel af jorden er blevet lavet for at kunne lave reproducerbare test data til veriﬁkation og analyse of de udviklede algoritmer.control. Natur.
Throughout the project Matlab has been used for data processing and algorithm implementation.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 ﬁrst authors last name is written followed by “et al. The report is copyrighted by the members of the group and Aalborg University.0. 2007 Rolf Christensen Nikolaj Fogh . Aalborg University. Literature references are written according to the Harvardmethod: [Last name of author. A complete bibliography is found at page 121. 31st of May. For a complete understanding of the report.4. 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. If a literary work has more than two authors. The thesis and project is made at the Section of Automation and Control under the Deportment of Electronic Systems at Aalborg University. The version of Matlab used is 7.287 (R2007a). a technical and scientiﬁc level corresponding to that of 9th semester students at the Section of Automation and Control is required. year of publication].”.
.
. . . . . . . . . . . .1 Accelerometer Output . . . . . . . . . . . . . . . . . . 10. 9. . . . . . 6. . . . . . . . . . . . . . . . . . . . .6 Discussion . . . . . . . . . . .1 Comparison between Gimballed and Strapdown Alignment . . . . . . . . . . . . . . . . . . . 11 Error Equations 11. . . . . . .2 Summary . . . . . . . . . . . . . . . . . . . . . . .4 Indirect Kalman Filter Applied to QuasiStationary Fine Alignment 12. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .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. . . . 9. . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 . . . . . . . . . . . . .4 Discrete Integration Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . 9 Navigation Equations 9. . . . . . . . . . . .1 Geodetic Variables and Constants . . . . . . . . . . . . . . . . . . . 8 . . . . . . . . . . 12 Alignment 12. . . .2 Gyro Output . . . . . . . . . .2 Gravity Model . . . . . . . . . . . . . . .1 General Navigation Equations . . . . . . . . . . . . . 9. 11. . . . . . . . . .1 Kalman Filtering . . . . . . . . . . . . . . . . . . 9. 12. . . . . 12.3 Fine Alignment .3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . .6 Analysis of Long Term Error Propagation 10 Error Estimation Using Indirect 10. . . . . . . . . . . .3 Navigation Output . . . . . . . . . . . . . . . . . . . . . . . . .5 Validation of Navigation Algorithm . . . . . 9. . . . . .2 Coarse Alignment . . . . .5 Veriﬁcation of Alignment . . . . . . . . . . . . . . .2 Vertical Channel . . 10. . 8. . . . . . . . . . . .1 Derivation of Error Equations . . . . Kalman Filtering . . . . . . . . . . . . .2 Motivating Example . . . . . . 12. . . . . . . . . . . . . . . . 7 Equations of Motion 8 Earth Simulation Model 8. . . . . . . . . . . . . .
. . . . . . . . . . III Thesis Summary 116 118 120 122 17 Conclusion 18 Improvements and Discussion Bibliography . . . . . . . . . . . . . . . . . . . . . . 13 Inertial Navigation with Aiding 13. . .1 Aiding with GPS . . . . . .7 Summary . . . . . .3 Smoothing Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 86 86 90 94 97 99 . . . . . . . 15. . . . .2 Accelerometers . . . . . . . . . . . . . . . . .3 Implementation of Navigation with Aiding 13.4 Calibration Example . . . . . . . . . . . . . 16 Summary 106 106 106 110 110 113 . . . . . . . . . . . . . . . . . . . . . . . . . . .3 Sensor Error Models . . . . . . .5 Summary .2 GPS Error Model Analysis . . . . . . . . . . . . . . 102 14.1 Introduction . . . . . . . . . . . . . . . . . 13. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 Optical Angular Rate Sensors . . . . 15. . . . . . 103 14. . . . . . 104 15 Calibration of Sensors 15. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 Inertial Sensors 102 14. . . . . . . . . . . . . . . . . . . 13.4 Veriﬁcation of Navigation with Aiding .2 Calibration Equations 15. . . . . . . 13. . . . . . . . . .12. . . . . .
.
Part I Introduction .
.
Likewise if the vehicle is located under water or under ground the GPS signal is not present. but which are only airborne for a few seconds. the accuracy of the INS degrades with time due to measurement inaccuracies. An INS consists of accelerometers which measure the translatory acceleration and gyroscopes which measure the angular rotation of the system. But used in a short time span. This sensor array is called an Inertial Measurement Unit (IMU). and thus will always work regardless of external inﬂuences. however. Through time several diﬀerent approaches has been used to locate and navigate to a destination. This makes INS very suitable for missiles where precision is crucial. velocity and position of the system starting from some known initial point. like GPS. e. Lastly the precision using GPS may be very poor. Using the stars. the sun or special marks in the landscape. the INS can calculate the current attitude. Furthermore. Nowadays GPS is the most widespread navigation system but in certain applications GPS is not a viable solution. This makes it suitable for navigation where GPS signal is not present or if the signals are being jammed. the GPS signal is either not present or very inaccurate because of multipath interference. 4 . This means that INS does not depend on any third party applications. and not suitable for precision navigation needed by e. An INS is a navigation system which depends entirely on inertial measurements for navigation. High precision INS are also suitable for submarines where high precision is needed and external positioning aids are not readily available. navigation using GPS is dependent on the satellites governed by the USA which might not always be accessible during a war situation. Using the measurements from the IMU. When relying only on inertial measurements. in mountainous surroundings. a missile or a war ship. 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.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. In certain areas.g. INS gives a much higher precision than GPS.g.
making them ideally suitable for strapdown INS. . The main part of this task is to derive the navigation algorithms which translate the IMU measurements into attitude. and thus. hence the name “strapdown”. With the invention and maturing of the laser and optical gyro. velocity and position. expensive and errorprone moving mechanical parts. The lack of computer processing power postponed the introduction of SINS system until the 1980’s. In gimballed INS. 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 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. By disposing of bulky. This requires the strapdown gyros to have a much higher dynamic range than the gyros employed in gimballed systems. The gimbaled system still achieved better precision but the SINS had an precision which made it applicable in lowercost applications. This made INS solutions applicable to military aircraft and the ﬁrst commercial aircraft Boeing 757. the sensors are rigidly mounted to the body of the vehicle. the IMU is isolated from vehicle rotations by the use of a gimbal. The RLG and the FOG both have both ﬁne accuracy and a large dynamic range. The problem with strapdown inertial navigation systems is. The principal advantage is apparent from the name “strapdown”. INTRODUCTION Historical Development of INS The ﬁrst applications using inertial measurements for navigation purposes appeared at the end of the 19th century. Strapdown INS The strapdown implementation oﬀers numerous advantages over its gimballed counterpart. the reliability of the system is increased and the cost and size is reduced.5 CHAPTER 1. Further development of the gyros lead to even more precise INS during the 1950’s. Inserting the parameters in the developed algorithms will yield the ﬁnal software for the INS. SINS have even obtained an accuracy which at a much lower price tag rivals highend gimballed systems. and the properties of the selected sensors will be determined using system identiﬁcation. The advantages of a nonmechanic system with low price and low weight was the source of this development. Under WW2 the development of the gimballed INS was reﬁned. A general sensor model will be derived. These gyros eventually enabled strapdown INS to obtain a degree of accuracy comparable to lowend gimballed systems but with a lower price tag. and has made it the preferred type of inertial navigation systems today. Scope of This Project This project seeks to implement algorithms for a lowcost FOG based INS. In a SINS. the gyros must be able to capture these rotations. and the Germans used a gimballed INS to navigate their V2 rockets. They were simple gyro compasses were able to determine the direction of true north. that the sensor assembly is not mechanically isolated from the rotations of the body.
This model is used during the initial alignment phase. a conclusion and discussion will review the results obtained in the system test. Thereafter. which veriﬁes the system against a reference system. In the next chapter inertial navigation with aiding is described. the application of the indirect Kalman ﬁlter to the alignment and aiding problem will be explained. and an error model is derived. Geodesy deals with the parameters concerning the earth. In particular. . which will estimate the initial attitude. To the authors knowledge. Part II derives the equations needed for the inertial navigation algorithms. This thesis includes results from a simulation. which describes the speed of a vehicle moving relative to earth. a topic which is missing in most literature. These equations. while still keeping the derivations relatively simple. The error equations are used to “fuse” the INS with an external aiding device.6 Our Contributions The authors have found it diﬃcult to ﬁnd literature about INS which are both comprehensive and easily understandable. the subject of geodesy is introduced. Part III will include a system test. Additionally. This thesis seeks to provide algorithms for all the steps necessary to implement an INS. First. as parameters such as the oblateness of the earth and gravity has major inﬂuences on INS. 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. the equations of motion are derived. Lastly. The following chapters shows how the eﬀects of measurement uncertainties aﬀect the system. along with the geodetic variables. where the quasistationary requirement are violated. some of the mathematical techniques used throughout the report will be introduced. are used by the navigation equations to compute the position and velocity of a vehicle using measured angular velocity and translatory acceleration. The last two chapters models the sensors used in this project and the determines the errors they suﬀer from. and is a key aspect of inertial navigation. azimuth and position of the system. An indirect Kalman ﬁlter is used both in alignment mode and to “fuse” the INS with an aiding device in an optimal way. there have been conducted no analysis of the errors arising from assuming the system to be quasistationary during the ﬁne alignment process. and an analysis of the errors that arise from these violations.
1.1) It is often needed to represent a vector in diﬀerent coordinate frames. y and z being the principal axes of the frame. iz . A vector r referenced in the b frame will be denoted rb . All frames used in this report will span a three dimensional Euclidean space. consisting of three orthogonal basis vectors. by and bz .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. jz vectors are coincident and invisible. 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. In the ﬁgure. rb = b rx b ry b rz T (2. iy and iz . y and z components of the vector. The j coordinate frame has been rotated i j relative to the i frame through the iz axis by an angle α. Such a rotation around the y axis is shown in Fig. The basis vectors of the b frame will be denoted by bx . 2. rz . and the j coordinate frame spanned by jx . spanned by ix . as they protrude out through the page. Scalars are noted with nonbold letters. with x. The subscript notion will be used to extract the x. jy and jz . Using the nomenclature 7 . One coordinate frame is created from another by a series of rotations around the principal axes of the frame.
The rotation i matrix is also called the Direction Cosine Matrix (DCM). rx . rz . jz i rx j rx. Cj is the rotation matrix transforming r from frame i to frame j. The DCM’s corresponding to rotations around all principal axis are stated here for completeness . rx .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 . rz . Figure 2.1 ξ ix Figure 2. The components of a vector i i i r in the i frame is found in the ﬁgure as the bold lines rx .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. and the angle α.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.1 i = rx cos(α) i = ry sin(α) j i i = rx. iz . and in the j frame as j j j the bold lines rx .1 can now be used to ﬁnd the equations transforming the vector r from the i frame to the j frame. ry .
2) (2. Relative Angular Velocity of Frames When describing the relative angular velocity of two frames.9 CHAPTER 2.5) It is important to note. that the order in which the rotations around the principal are applied are not arbitrary.x j ωij. 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.y j ωij. 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.z T . it is necessary to describe both which frames are involved and in which coordinate system the angular velocity is expressed.3) (2. An XYZ rotation is not in general the same as an YXZ rotation. 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.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.g. If e. 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.
10 Computed and Measured Quantities In order to distinguish between actual measurements from sensors and computed quantities used in the computer. they are denoted diﬀerently. the skew symmetric matrix is introduced.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.5).9) in (2. Using the skew symmetric matrix. ∆θγ is the small angles that the i frame has been rotated through. i By deﬁnition. 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. During the i time ∆t. 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 relation of frame i and j is expressed by the DCM Cj (t). 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. The skew symmetric form is due to the small angle simpliﬁcations applied to the cosine and sine parts of the DCM from (2. it is sometimes convenient to change between a vector representation and a matrix representation. In doing so.9) ∆θβ −∆θα 0 where ∆θα .6) where A is the skew symmetric matrix form of a.10) . ∆θβ . The result is: 0 ∆θγ −∆θβ i 0 ∆θα ∆θ = −∆θγ (2.8) yields: ∆θ i ˙ Cj = Cj lim i i δt→0 ∆t (2. 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.
11 CHAPTER 2.7). 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.11) These two rotational representations will be used interchangably throughout the report. i (2.10) can be written as: ˙ Cj = Cj Ωi = −Ωj Cj i i ji ij i (2.12) . 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. (2.
azimuth and orientation. Ideally. diﬀerent ones are used. a frame in which there are no forces acting. EarthCentered Inertial Frame In INS applications. one is primarily interested in knowing longitude. it is important to deﬁne the coordinate frames used throughout this report as they are essential to the derivation of the INS equations. the inertial frame should include all forces exerted on the IMU. 3. In order to model the output of the sensors.e. it can be shown. and one axis parallel to the direction of earth rotation. an inertial frame is needed as a reference frame. An INS uses accelerometers and gyros that register known physical phenomena caused by the acceleration or rotation of an Inertial Measurement Unit (IMU). north velocity. i. the iy vector is chosen. An IMU in this thesis deﬁned as a collection of three gyroscopes and three accelerometers arranged as an orthogonal triad. However. most inertial navigation systems today use an additional set of frames. latitude. as they are an order of 10−7 [g] or less.1. In terrestrial navigation. but in practice this is impossible. The choice of elementary vector representing this axis (ix .Chapter 3 Frames of Reference and Position Determination Before explaining the principles of INS. while the ix completes the righthand coordinate system. In this thesis. 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. Also the gravitational pull of other celestial bodies can be disregarded. east velocity. Fortunately. with the iz vector going through the equator at the longitude where navigation was started. that as the resolution of most inertial sensors today are limited. The frame is seen in Fig. as this would require knowledge about the entire universe. 1971]. and in the literature. 12 . These could be obtained using only the inertial frame. iy or iz ) is a matter of preference. the inertial frame i is chosen as an earthcentered inertial frame (ECIF) which has origin at the center of the earth. height.
1: The inertial frame is spanned by ix . The geodetic frame is the earth frame rotated around the y axis through the longitude angle L. . gy U. gx iz Equator ix ez λ L l ex Figure 3. and thereafter rotated negatively around the new x axis through the latitude angle l. gz E. ey ωie N.CHAPTER 3. FRAMES OF REFERENCE AND POSITION DETERMINATION 13 iy . The earth frame is the inertial frame rotated around the y axis through the angle λ. iy and iz .
North. using the geodetic frame as a navigation frame introduces a singularity at the North pole. to be rotating with the earth at the earth spin rate (ωe ). One other popular formulation is the NorthEastDown (NED) frame. y and z axes are also called e. As two of the axes are horizontal. 3.1. This rotation is seen in Fig. Using (2. Up frame (ENU). the x. As will be evident later in this thesis. with the last completing the righthand coordinate frame (pointing east or west).3) and (2.14 EarthCentered. At time t = 0 the ECEF and ECIF are coincident. where λ = ωe t. 3. With the geographic frame. n and u axes in the case of an ENU frame. one can still integrate the accelerations to obtain velocities. earthﬁxed frame (ECEF) e is deﬁned. the local geodetic frame is always locally level. the rotation matrix from the ECEF to the NF is . To overcome this limitation.1.4). Navigation Frame As with the local geodetic frame.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. where North is undeﬁned. However. the rotation matrix from ECIF to ECEF is cos(λ) 0 Ce = i sin(λ) 0 1 0 − sin(λ) 0 cos(λ) (3. The ECEF frame is seen in Fig. so gx = ge . but as time progresses. where North has inﬁnitely many solutions. The diﬀerence from the geodetic frame is that the navigation frame can be rotated about the vertical axis. gy = gn and gz = gu .2). Height is obtained by integrating acceleration in the down axis. the ECEF frame is rotated at an angle λ through the ey axis.2. A common geodetic frame. one can simply integrate the acceleration resolved in the North and East axis to obtain velocity in north. which is also the geodetic frame in this thesis. 2000]. one pointing vertically down or up. Using this mechanization. and then through a latitude angle l through the new ex axis. an NED frame is chosen as a navigation frame. 1971]. As with the ENU frame. and the South pole. In [Britting.3). The choice of navigation frame is also known as a mechanization. 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. and east respectively. but they must be rotated through α to obtain it in North and East components. Height is still a double integration of acceleration in the nz . current INS typically uses a mechanization known as wander angle (WA) [Savage. Using (2. the wander angle mechanization also simpliﬁes the navigation equations which reduces the computational demand of the system. which is produced by rotating the ECEF frame around the ey axis at a longitude angle L. This frame is seen in Fig. (2. with one axis pointing north. EarthFixed Frame The earthcentered. 3. 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. is the East. 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.
FRAMES OF REFERENCE AND POSITION DETERMINATION 15 ny α gy nx gx Figure 3. INS today uses either quaternions parameters or DCM’s internally to keep track of position. they tend to be unintuitive and signiﬁcant calculations are needed for outputting the variables of interest (latitude. this suﬀers from the same problems as the NED navigation frame mechanization. However.CHAPTER 3. However. the choice between DCM’s and quaternions is more a matter of . longitude. it is not possible to restore longitude information after a pole traversal. Position Tracking An INS could keep track on position by keeping track of longitude. The sensor frame is deﬁned so that the xy plane is spanned by the x and y accelerometer. At the poles. and thus the singularity is not an issue.3) Body Frame The body frame (BF) is ﬁxed to the casing of the IMU. The rotation matrix from the sensor frame to the body frame is found by the sensor calibration algorithm. with the x accelerometer corresponding to the xaxis. Although one might argue that navigation involving pole traversal are not common. latitude and height of the system. and rotated with respect to the navigation frame by a roll.). Historically. 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. but longitude is not uniquely deﬁned. Today.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. Therefore. the latitude is 90◦ . 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. and the navigation information is destroyed.2: The wander angle α is the angle which the navigation frame is rotated around the z axis with respect to the geodetic frame. quaternions have been used because they required less processing power during navigation as compared to DCM’s. etc. 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.
l and a terms. From (3. latitude and wander angle can be extracted by n isolating the L.6) (3.2). n Omitting the calculation of the third column reduces the processing power needed for the navigation equation. only the ﬁrst two columns of Ce are needed to obtain the wanted information.7) As seen.16 preference[Savage. longitude. In this thesis. 1997].4) Ce = C12 C22 C32 C13 C23 C33 Yields L = l a = = C13 C11 C22 − C21 C12 arcsin(C23 ) C21 arctan C22 arctan (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. .5) (3. the Cn DCM is used to keep track of the poe sition on earth.
we would like to know our current position relative to some other point on the earth surface. 17 . 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.Chapter 4 Principle of a Strapdown Inertial Navigation System In order to get an idea of the concepts and principles of inertial navigation systems. 4. In many applications. When navigating on the earth.2. Being a 2D INS. one uses the notion of longitude. The derivative of h is equal to the velocity of the body in the vertical direction. the height h. First. the variables of interest is the latitude l. This system is allowed to tilt by an angle θ at a rate ω. as well as accelerate in the by and bz direction. and the tilt θ of the system. assume that the earth is not rotating. our system is only able to navigate around the meridional (northsouth) plane. By introducing the navigation frame. consider a 2 dimensional YZ IMU as depicted in Fig. This two dimensional system is depicted in Fig. so the inertial frame is equal to the earth frame. Many of the concepts in the two dimensional case can be directly transferred into the three dimensional case. 4. latitude and height above the earth surface to indicate where one is positioned relative to the earth. bz bx ab z by ω b ay Accelerometers Gyro Figure 4. Here.1: Inertial Measurement Unit measuring in two dimensions. the navigation frame is chosen as a ENU geodetic frame. a simple example is given in two dimensions. it is assumed that the sensor frame and the body frame are coincident.1. In this system. For simplicity. Also.
This is also known as the coriolis force. 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. vn n and vu . The acceleration measured by the accelerometers is comprised of both gravity and mass movement. At start of navigation. 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. ez Figure 4. Recognizing that the rotation rate of the navigation frame with respect to the . gravity only inﬂuences the u axis As the navigation is rotating with respect to the inertial frame.2: INS system around the meridian plane.3) n The vn /(R0 + h) term is also called the “transport rate”.1) n To ﬁnd these we need to ﬁnd the velocities of the system in the navigation frame. ˙ θ=ω− n vn R0 + h (4. The platform is tilted by an angle θ with respect to the earth surface. one can obtain the velocity as an integration of the acceleration. Because the navigation frame is level on earth. ey nn by l h bz nu θ R0 iz . one must take into account the accelerations arising from this rotation. As these measurements are given in the body frame. and the above equation can then be used to calculate it at any point from there on.18 iy . n vn R0 + h n ˙ h = vu l˙ = (4. Having obtained the accelerations in the navigation frame.2) Where the tilt of the platform θ needs to be calculated. These are obtained using the measured accelerations. It is the rate at which one needs to tilt the platform in order to keep it level on the ground. the attitude θ is initialized at some initial value (0 if the system is level on the ground at the start of navigation).
At start of navigation. (4. The job of the navigation computer is to integrate (4.19 CHAPTER 4.5) to yield the latitude. The principles explained in this chapter are used in the next part to derive the navigation equations for a full three dimensional case.5) These are integrated to yield the velocity of the system. PRINCIPLE OF A STRAPDOWN INERTIAL NAVIGATION SYSTEM inertial frame is the transport rate of (4.4) and (4.4) (4.1). 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.3). .3). the system must be stationary so the initial velocities are 0. height and tilt of the system. (4.
.
.
Part II Inertial Navigation Systems .
.
the roll. When the alignment phase has obtained a suﬃciently accurate estimate of the roll. 6). Before inertial sensors are incorporated into an INS. pitch and yaw of the system. height. navigation and aiding. longitude. pitch and yaw. During navigation. The structure of the part is as follows: As terrestrial navigation occurs in the vicinity of the earth. 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. which can bound the position and velocity of the “fused” INS/GPS system whilst keeping the precision of the INS. In this part. the geometric aspects of the earth. By requiring the IMU to be stationary during alignment. the alignment algorithm estimates the initial value of the body frame with respect to the navigation frame. a navigation algorithm continuously calculate the roll. 7) derives the equation of motion of an INS which are used by 24 . and the magnitude of gravity have a large inﬂuence on the INS. The next chapter (Ch. During the alignment phase. some of which are reproducible and hence correctable. This is used by the navigation algorithm to transform the measurements from the body frame to the navigation frame. these errors are identiﬁed by a calibration algorithm. this can be done by measuring gravity and earth rotation. velocity and attitude outputs. mathematical formulae needed by the above algorithms are derived. because of sensor errors and the double integration from acceleration to position.Chapter 5 Introduction Inertial navigation consists of three major parts: Alignment. that is. so acceleration measurements can be resolved in the navigation frame and integrated to yield velocity and subsequently positions with respect to the earth. this results in the latitude. pitch and yaw from inputs from the gyroscopes. 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. inertial navigation systems tend to drift exponentially from the true values. The important aspects of this topic is introduced in the ﬁrst chapter (Ch. Ultimately. the navigation phase can begin. A good example is GPS aiding. However. The sensors used in INS all have errors.
Before developing the alignment and aiding algorithms. Indirect Kalman ﬁltering uses an error model of the INS which is derived in Ch. Using this error model. 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. latitude. INTRODUCTION both an earth simulation model and the navigation equations.1: Simulation and INS The earth simulation model and navigation equations are developed in the two following chapters. Longitude. latitude height and attitude trajectories INS Figure 5. 12. Two diﬀerent aiding approaches is presented in Ch.1. chapter 10 gives an introduction to indirect Kalman ﬁltering. So the last two chapters describes how to calibrate the sensors. 9. The navigation equations reverses the process as seen in Fig. Ch. indirect Kalman ﬁltering equations and the navigation equations. 8 and Ch.25 CHAPTER 5. Chapter 15 shows how the sensor errors are identiﬁed using a test ﬁxture to rotate the IMU through a series of rotations. 11. the alignment algorithm is derived in Ch. The earth simulation model is derived in order to test the developed algorithms under controlled conditions. 5. GPS aiding and speedometer aiding. . 14. latitude height and attitude trajectories Accelerations Angular Rates Simulation model Longitude. 13. height and attitude inputs to accelerations and angular rates that the accelerometers and gyros would experience during navigation. The sensor calibration equations are based on sensor error models which are presented in Ch. The earth simulation model translates longitude. The last two chapters of this part present a somewhat diﬀerent part of this project than the preceding chapters.
3) [ m ] (6.0 · 10−11 Expressed in earth coordinates e ωie = 0 ωe 0 (6. if one continued ones current curve. also known as the semimajor axis radius re = 6378137 The polar radius of the earth.5) Two important geometric aspects of geodecy is the meridional and the normal radius of curvature. knowledge is needed about the geometric aspects of the earth as well as the gravitational ﬁeld surrounding the earth. as well as a gravity model. also known as the semiminor axis radius rp = 6399592 The eccentricity of the earth is deﬁned as ǫ= 1− 2 rp = 0. These values are known as geodetic variables and constants.4) (6. which deﬁnes many of the constants that are used in INS.1) ωe = 7292115.2) The equatorial radius of the earth. 6. The meridional radius of curvature is the radius of the circle one would follow if one travelled directly north or south.Chapter 6 Geodesy In order to develop the equations of motion of a vehicle in the vicinity of the earth. 6. This is depicted in Fig. and can be expected to be constant [ rad/s ] (6. The most widely used model of the earth is the World Geodetic System (WGS84). The normal radius of 26 . These are deﬁned as the radius of the circle one would travel in.0818191908426 2 re [ m ] (6.1.1 Geodetic Variables and Constants The rotational rate of the earth have been well known for many years.
7) 6.9) µ 3 2 1 − 4 J2 (1 − 3 cos(2l)) − rωe cos2 (l) r2 From [Savage.8) Where J2 . The equations giving the radii is a function of geographic latitude.2 Gravity Model From [Britting. . Meridional (northsouth) radius of curvature re (1 − ǫ2 ) (1 − ǫ2 sin2 (l))3/2 re (1 − ǫ2 sin2 (l)) rmer = (6.1: Figure showing the diﬀerence between the circle one would follow if one traveled directly north compared to the earth’s curvature. As apparent gravity is always pointing vertically. the gravitational magnitude can be calculated approximately using the following formula.6) Normal (eastwest) radius of curvature rnor = (6.27 CHAPTER 6. GEODESY North rmer Earth Surface Circle of curvature Figure 6. r is approximated as: r = re 1 − ǫ sin2 (l) + h (6. 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. and µ are empirical constants calculated by geodetic measurements and r is the radius from earth surface to the point of navigation. curvature is the radius of the circle if one travelled directly east or west. 1971]. 1997]. the gravity in the navigation frame is 0 0 gn = (6. µ r2 3 2 1 − J2 (1 − 3 cos(2l)) − rωe cos2 (l) 4 g= (6.10) The geodetic values and gravity model described in this chapter is used throughout this thesis.
GRAVITY MODEL 28 Before developing the earth simulation model and navigation algorithms the equations of motion need to be derived. .2. of a vehicle in the vicinity of the earth.6. These equations are derived in the following chapter and describe the motion. relative to the earth.
Derivation of Equations of Motion Acceleration is comprised of two terms: The acceleration due to mass movement and the acceleration due to gravity. ˙ The dependence of re on ri is obtained as follows 29 . Acceleration due to mass movement is the double derivative of the position vector in the inertial frame.1) (7. it is convenient to derive this equation in terms of the geographically referred earth referenced velocity.2) The result of this chapter is an expression relating vn to the accelerometer measurements given in the navigation frame an . 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. In later chapters. 4. also known as the “ground speed”. In this chapter. Due to the principle of equivalence. 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. This is the velocity of ri seen from the earth frame transformed into the navigation frame. this equation is integrated to yield velocity and subsequently position of the INS. deﬁned as follows vn n ˙ Ce re (7. an = Cn¨i + gn ir Where an is the acceleration in the navigation frame. As the output from an INS is velocity and position relative to earth. these cannot be distinguished and are both measured by the accelerometers [Britting. The acceleration due to gravity is given in the navigation frame as gn . 1971].
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.11) yields the ground speed of a vehicle as a function of accelerometer measurements.4) Notice the sign change due to Ωi = −Ωi .3) (7.6) and rearranging the terms yields n ˙ v n = Ci i Ωi − Ωie ni (7. the rotation of the navigation frame relative to earth and the apparent gravity: .11) It is customary to include the centripetal term Ωi Ωi ri in the equation of gravity.12) Rearranging and inserting the expression for gn into (7.9) By isolating the inertial acceleration r and multiplying by of (7.5) = Cn i r Ωi − Ωi ri + ¨i − Ωi Ωi ri ni ie ni ie ˙ (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.6) ˙ By isolating ri from (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.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.10) Inserting into (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.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.4) one obtains ˙ ri = Ci re + Ωi ri e˙ ie Inserting this into (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.7) r Ci re + Ωi ri + ¨i − Ωi Ωi ri ni ie e˙ ie (7. the rotation rate of earth.
13) In the following chapters. EQUATIONS OF MOTION ˙ vn = an − (Ωn + 2Ωn ) vn − gn en ie (7. this equation is used both to develop a simulation model. and the INS navigation equations.31 CHAPTER 7. .
As two sensors are used. an earth simulation model is derived using the equations of motion derived in Ch. 8.1) b ˙ ab = Cn (vn + (Ωn + 2Ωn ) vn + gn ) en ie (8. but welldeﬁned conditions such as erroneous sensor outputs. 7. Restating the equation of motion (7.Chapter 8 Earth Simulation Model In this chapter.13): ˙ vn = an − (Ωn + 2Ωn ) vn − gn en ie Rearranging and rotating yields the wanted output (8. This model enables one to create repeatable test data as well as test the system under suboptimal.1 Accelerometer Output In this section the accelerometer output from the earth simulation model is derived. accelerometer and gyroscope.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 . 6 as 0 0 3 2 1 − 4 J2 (1 − 3 cos(2l)) − rωe cos2 (l) gn = µ r2 (8.2) The plumbbob gravity term gn was calculated in Ch. This chapter is divided into two sections. the earth simulation model should output both the acceleration and rate sensed by the sensors when the vehicle is moving.
5) = cos(l) sin(α)ωe The wander angle (Fig.2) is initialized at 0 and follows the following equation[Rogers.1 shows the relationship for the north velocity. 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. Figure 8. g ve g vn = = g ωeg. ˙ α = L sin(l) ˙ n The Cb matrix is calculated using the body rotations Ωbn supplied by the user n (8.33 CHAPTER 8. n Left is to ﬁnd the vn and Ωn terms.3.n (rnor + h) g −ωeg. EARTH SIMULATION MODEL Ωn ie n ωie e = Cn Ωie Ce e n T (8. the dashed line is the meridional radius of curvature. The bold ellipse is the earth surface. 2000] g ωeg = ˙ ˙ −l˙ L cos(l) L sin(l) T (8.4) cos(l) cos(α)ωe sin(l)ωe (8. g vn g ωeg. 2007].6) ˙ Cb = Cb Ωn n n bn At start of simulation.e (rmer (8.e h rmer Figure 8. the vertical term of vg is equal to the derivative of the height h.10) As the navigation frame is locally level.7) First. the horizontal parts of vn is found.8) (8. one can ﬁnd a relationship between the horizontal angular rates and the horizontal velocity of the geographic frame. en (8. the Cb matrix is initialized by the user.9) + h) Where [Savage.1: Relationship between north velocity and east angular rate of the geographic frame.
7).13) n The remaining term is Ωen . it is assumed that simulations will not include pole traversals. Ωn nb (8. longitude is generally deﬁned as being within −180◦ to 180◦ .11) T Hence. however. but it does not exhibit this jump in value during navigation. vg = ˙ ˙ ˙ L(rnor + h) cos(l) l(rmer + h) h (8.5). by removing the longitude constraint so it can attain any value.2. Furthermore. .3).14) T With n ωgn = 0 ˙ 0 −L sin(l) (8. (8. and the horizontal terms are obtained by rotating the horizontal parts of the geographic rotation rate through the wander angle.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. Latitude is also deﬁned as being within −90◦ to 90◦ . (8. can be eliminated.13) and (8.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. But it has been chosen to use longitude/latitude as it is more intuitive for the user than inputting a DCM.16) is inserted into (8. l and h need to be diﬀerentiable. One way of removing this problems from the earth simulation model is to used DCM’s instead of longitude/latitude. Hence. n g g ωen = Cn (ωeg + ωgn ) g (8. (8.5). Also. which will also cause a sudden jump. ˙ ˙ 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.2). and the rotation matrix Cb is calculated using (8. GYRO OUTPUT 34 g ˙ vu = h (8. this earth simulation model is limited to navigation away from the poles. the vertical part is set to 0. (8. This can cause problems.12) This is then rotated into the navigation frame through the wander angle using (3. Height is also not expected to have jumps. as longitude changes instantaneously at pole traversals. n 8.17) given by the To use this earth simulation model L.8.16) To get the outputs from the accelerometers. In the wander azimuth conﬁguration. This jump.3).16) and the body rotations user.
9.1). 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.1) and (9. The last section describes the idea of dividing the integration of the attitude matrix into a fast and slow part. n The measured acceleration ab . n ˙ vn = Cn ab − (Ωn + 2Ωie ) vn − gn b en (9.5) are all n known. Apart from being used to calculate the derivative of the ground speed. 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. so vn is initialized at 0. this matrix is used to e 35 .3) and earth rotation Ωie (8.2) is an expression for the transport rate Ωen . heading. gravity gn (8. The navigation equation continuously integrate accelerometer and gyro measurements and output attitude. the transport rate is also used to calculate the Cn matrix.1 General Navigation Equations In order to determine the velocity and position of the INS. velocity and position. this section seeks to derive expressions for the terms in the equation of motion (9. The remaining ib n term in both (9. and earth rotation is known. As explained in Ch. the IMU is not moving relative to the earth surface.2) Where Ωb is the input from the gyros. Many of the expressions needed have already been derived in the previous chapter. The next section describes the problem with the unstable vertical channel of the INS and proposes a solution to stabilize this channel.3.Chapter 9 Navigation Equations During this chapter the navigation equations needed for inertial navigation are derived.1) At start of navigation. This is used if the processor power is not suﬃcient to integrate the whole part with the high sample rate.
3) By rearranging (8.7) Substituting (9.4) (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. This reduces the computational demands of the system. and then rotating back to the navigation frame. and inserting it into the two ﬁrst positions of (8. At the start of navigation the Cn matrix is given initial values by the initial alignment algorithm. As can be seen.5) n This is used to calculate ωen using g g n ωen = Cn ωeg + ωgn g (9. one obtains g ωeg = g −vn (rmer +h) g ve (rnor +h) ˙ L sin(l) T (9.10) .9.8) and (8.8) (9. The rest of this section will rewrite F to use the entries in the Cn matrix e instead.6) With g ωgn = 0 ˙ 0 −L sin(l) T (9. overcoming this limitation.1.7) into (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. The remainder of this section deals with the derivation of the transport rate. which is not deﬁned at the poles.5) and (9. GENERAL NAVIGATION EQUATIONS 36 calculate latitude. 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. so v g = Cg v n n (9.10). this expression contains the wander angle α.9). longitude and wander angle of the system.9) n Note that the z part of ωen is always zero because of the wander angle mechanization. From there on e it follows the following equation ˙n Ce = −Ωn Cn en e which is continuously integrated by the INS.
14) (9. 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. As a general rule when navigation time exceed 10 minutes. A common way of aiding the vertical channel is by implemented the following PID servo control loop. hence: n ˙ h = −vz (9. 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.37 CHAPTER 9.12) 0 0 9. n vz vz. 3. As h increases. This is because of the dependence of g on h. external measurements must be used to keep the vertical channel within acceptable values. yielding an increased upward acceleration. [Savage.11) Yielding the ﬁnal result. 2007]. 2000]. which is depicted in Fig.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. NAVIGATION EQUATIONS Using the entries in the Cn matrix as done in Ch.1. The purpose of the loop is to drive the error δh towards 0.ins ˙n +  +  vz ˙n +  h c3 c2 c1 δh +  haid Figure 9. g decreases. relying only on inertial measurements for calculation of the vertical channel will render it exponentially unstable[Rogers. 9.13) However.15) (h − haid )dt .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.
9.15) as follows: Assuming that earth rotation and vn transport rate are negligibly small compared to the other terms. s3 − 2g =0 r 2g r ˙ δ h + c3 δh = c3 δhaid (9. 2007].23) (9. it simpliﬁes to n ˙z δ vz = δ an + δgz − c2 (δh − δhaid ) − c3 ˙n (δh − δhaid )dt (9. VERTICAL CHANNEL 38 The PID gains c1 .17) The δ¨z term is obtained from (9.19) where g0 is the gravity at the surface of the earth. the Laplace transform of the error forms of the equations are derived as follows: n ˙ δ h = δvz − c1 (δh − δhaid ) (9.16) Assuming that the error of the aiding device is constant [Rogers. diﬀerentiating (9. The error form is derived as n δgz = − n dgz (r) 2g δr = δh dr r (9.21) which can be inserted into (9.17) 2g ˙ ¨ ¨ δ h + c1 δ h + c2 − r Taking the Laplace transform s3 + c1 s2 + c2 − With no control (c1 = c2 = c3 = 0).2.24) . To analyze the vertical channel control loop.18) The gravity model is approximated by an inverse square law as 2 g0 r0 r2 n gz (r) ≈ − (9.20) where g is the gravity magnitude at the equator. Assuming δan to be constant.22) s + c3 = 0 (9. r0 is the radius of the earth at the point directly under the INS and r is the length from the center of the earth to the INS. the second derivative is: ˙ ¨ ¨ δ h + c1 (δ h) = δ¨z vn (9. c2 and c3 are selected using classical servo control theory.18) yields z δ¨z ≈ vn 2g ˙ ˙ δ h − c2 (δ h − δhaid ) − c3 (δh − δhaid ) r (9.
digital integration is accomplished by dividing the integration algorithm into two parts [Savage.20 KHz). c2 and c3 PID gains should be chosen so that the resulting h contains both the stable characteristics of the aiding device. Earlier.01 [Rogers. yielding an unstable system because of the positive root.25) Where λ is usually chosen to be 0. 9.30) (9. the relatively high dynamics experienced during vehicle motion causes some problems regarding the bandwidth of the system. 3: L l a = arctan C13 C11 C22 − C21 C12 = arcsin(C23 ) C21 = arctan C22 (9. 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. In many systems today.29) The velocities in north and east can be expressed as vN vE n n = vy cos(α) + vx sin(α) (9.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. 2007]. The c1 . Conversely.28) (9.27) (9. but have stable characteristics.26) (9. NAVIGATION EQUATIONS which has the solutions 0 and ± 2g/r.3 Navigation Output The latitude. or a higher order algorithm at lower update rates (50 . systems either used a ﬁrst order algorithm at a very high update rate (10 . whereas the height measured by the INS are unstable. the processing power needed to execute the relatively complex algorithms must not exceed the available processing power.39 CHAPTER 9. longitude and height can be extracted from the Cn matrix as described e in Ch. However. 2000]. and the high dynamics of the height captured by the INS. In order to obtain this. The aiding device is unable to capture fast changes in height. The integration algorithms must be executed at a rate fast enough to include all major dynamics of the system. one .100 Hz). but includes fast changes.
5.5km.100 Hz) are executed by the navigation computer software. as the processing power available in modern microcomputers has increased dramatically during the last decades. as it can be solved by integrating all the terms in the navigation equations in the navigation computer. This makes it possible to compare the performance with respect to a commercial product. however. Looking at the Kearfott position it is clear that at the end it makes the same hooklike turn. The parts running with a lower iteration rate (50 . The drive time was approximately 5. the latitude and longitude results was compared to those of the Kearfott. which can be run at a lower rate. The parts running with a high iteration rate (1 2 KHz) are lower order integration algorithms. 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.5 Validation of Navigation Algorithm In order to validate the navigation algorithm derived in this chapter. VALIDATION OF NAVIGATION ALGORITHM 40 for the parts with high dynamics.2. but the sensor data is available to the user at 50Hz. 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 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. Investigating the position just before it starts to drift East reveals that the position is 88m further East and 22 meters . After a while. the navigation algorithm is compared to an already existing INS unit. The Kearfott unit makes it possible to log both longitude/latitude determined by the Kearfott and the raw sensor data. The same longitude/latitude and velocity was used for both systems. It goes South and turns East but does not stop at the same point and furthermore starts to drift east. 9. By applying the sensor outputs obtained from the Kearfott unit to the navigation algorithms derived in this chapter. The results are shown in Fig. which are run by specialized hardware or highly optimized software algorithms.6 minutes.9. seems to follow the reference system good enough to justify a successful validation of the navigation algorithm. The speed did not exceed 60km/h and modest acceleration was utilized. the integration problem have decreased. It does not say at which sample rate the sensor data is available to the INS algorithms running within the Kearfott. What is also interesting is that the car was stopped at the starting point. It can be seen that the navigation developed in this chapter followed the reference system quite accurately during the ﬁrst minutes of the navigation. The test was performed by strapping the Kearfott unit to the backseat of a car and driving a route of approximately 1. so the route should end where it started. The curve. This approach is known as a twospeed approach. the navigation developed in this thesis drifts away from the reference system and at the end the navigation developed in this chapter drifts South. although this cannot be veriﬁed. which needs to be run at a high iteration rate and one for the parts with low dynamics. However. 9. Furthermore any sudden turns were avoided.
. By introducing errors in the initial INS states or sensor measurements.189 57. as well as initial tilt.188 Latitude 57. The sensor frame was assumed equal to the navigation frame.086 Our system Kearfott T16 −2.077 Figure 9.192 57.191 [deg] latitude 0 [deg] longitude.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. 9.083 −2. 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. NAVIGATION EQUATIONS Our system vs. This diﬀerence is due to the drift of the INS when no aiding device is present.187 57. one can investigate how the errors propagate through the system.079 −2.082 −2.081 Longitude −2. In the simulations.182 −2.41 CHAPTER 9.084 −2.8 shows how various errors within an INS inﬂuences the longterm (48 [hour]) performance of the system.183 57. where the INS was set to being stationary at 57. the gyro and accelerometer output is calculated by the earth simulation model. further North than the start point. 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. 9. Kearfott T16 57.185 57.19 57.08 −2. velocity and position errors. Figure 9.191 57.184 57. heading. Gyro and accelerometer bias errors are examined.2: Comparison between our INS and the Kearfott INS. Sensor errors were introduced by adding a bias between the simulation model and the navigation equations.078 −2.085 −2.186 57.3 through Fig.
9. ANALYSIS OF LONG TERM ERROR PROPAGATION 42 Accelerometer bias error 15 Error [mrad] 10 5 0 −5 x 10 −3 Roll 0.05 −0.1 Error [m/s] 0. The heading drift is so small it can be assumed negligible.3: Error propagation with an accelerometer bias of 50 [µg]. . It can be seen that apart from the heading.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.05 0 −0.6. all errors are bounded.
1 Error [mrad] Error [m] 0.01 [◦ /hr]. .05 0 −0.4: Error propagation with an gyro bias of 0.1 0 10 20 Yaw 10 Error [mrad] Error [m] 10 30 40 5000 0 −5000 −10000 30 40 1 0.5 0 −0.05 0 −0.1 0 10 20 Pitch 0. that longitude and heading error grow unbounded.05 −0.05 −0. NAVIGATION EQUATIONS Gyro bias error Roll 0.43 CHAPTER 9. It can be seen.1 Error [mrad] Error [m/s] 0.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.
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. All errors are bounded.5: Error propagation with an initial heading error of 25E6 [rad].04 Error [m/s] 0. ANALYSIS OF LONG TERM ERROR PROPAGATION 44 Initial heading error −3 2 Error [mrad] 1 0 −1 −2 x 10 Roll 0.02 0 −0.04 −0.6.06 0 10 20 30 Time [h] 40 0 10 20 30 Time [h] 40 Figure 9.05 −0.03 −0.02 −0. .02 Error [mrad] −0.9.
6: Error propagation with an initial position error of 1 [m].45 CHAPTER 9. .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. NAVIGATION EQUATIONS Initial position error −5 1 Error [mrad] 0.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 10 20 30 Time [h] 40 Figure 9.
05 Error [mrad] Error [m/s] 0.06 Error [mrad] Error [m] 0.9.1 0.05 0 10 20 Pitch 30 40 −0.02 0 −0.6.05 0 10 20 Yaw 0.15 Error [mrad] 0.05 0 −0. .5 Velocity East North 0 0 −0. All errors are bounded.7: Error propagation with an initial pitch and roll error of 25E6 [rad].5 0 10 20 30 40 Latitude 1000 Error [m] 500 0 −500 −1000 0 10 20 30 40 0. ANALYSIS OF LONG TERM ERROR PROPAGATION 46 Initial pitch error Roll 0.04 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 10 20 Yaw 0.02 Error [mrad] Error [m] 0.1 [m/s].01 0 −0.01 −0.02 Error [mrad] Error [m/s] 0.01 0 −0.01 −0.01 −0. .01 0 −0.05 −0.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 0 10 Velocity East North 20 30 40 Latitude 20 30 40 Longitude 20 30 Time [h] 40 Figure 9.02 0 10 20 Pitch 0.47 CHAPTER 9. NAVIGATION EQUATIONS Initial velocity error Roll 0.8: Error propagation with an initial velocity error of 0. The heading drift is so small it can be assumed negligible.05 0 −0.1 0.02 Error [mrad] Error [m] 0.
At a time between (a) and (b). This can be recognized as being the frequency of a pendulum with the same length as the earth radius.9(b) is at a later time. it is seen that the INS will measure an acceleration to the west. In (a). This will result in gravity being resolved wrongly. but will still have a velocity to the east. the platform will be level relative to earth. but the INS will assume it still has an east velocity component. Position errors are not very signiﬁcant. it will torque the Cn matrix (the platform) e around the north axis using the transport rate. This is done in a process called alignment. 9. ANALYSIS OF LONG TERM ERROR PROPAGATION 48 One general characteristic on all the ﬁgures are oscillations with a period of 84. Interesting is it to see. and the result is an oscillation with the Schuler period of 84. velocity and position of an INS.9(a) is the initial time where navigation is started.9. Because of the acceleration. one needs to know the initial attitude and heading of the system. However. that initial tilt errors and gyro bias are by far the biggest contributor to the errors in the system.4 [min]. is the system initiated with an tilt error of θ. Although both accelerometer bias and initial velocity error contribute to an unbounded yaw error. Figure 9.9: Schuler oscilation. In (b). The platform is tilted with respect to the earth.6. and will give an acceleration in the east direction. before navigation. so it will continue to torque the platform. the velocity will decrease. their contributions are very small compared to that of the gyro bias. . What is also very important to notice is that the unbounded yaw error is also mainly due to gyro bias. The Schuler oscillations apparent in the ﬁgures arise from the torquing of the platform in order to keep it level. to obtain attitude. but not as signiﬁcant as gyro errors. From Fig.4 [min]. 1997]. Figure 9. This gives rise to an oscilation because of gravity being resolved incorrectly. As the INS assumes that the system is moving east. whereas initial velocity errors should be kept to a minimum.9 shows an INS with an exaggerated initial tilt error. 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. An INS continuously integrates the equations derived in this chapter.01 [◦ /h] or less[Savage. although the system is actually not moving.4 and Fig. Accelerometer bias does give rise to errors. which will be explained in the following chapters.7 is it seen that even small gyro bias and tilt error give rise to position errors in kilometers. This shows the importance of good alignment of an INS. and Fig. 9. It is also the main cause of the requirement of 1 [nm/h] INS to have a gyro bias of 0. All the other errors give rise to a position error with a maximum of 500m and most of them below. 9.
However it is not a standard direct Kalman ﬁlter but an indirect Kalman ﬁlter which is used in this thesis. 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. As the attitude and heading is not directly measurable these are to be estimated.g.Chapter 10 Error Estimation Using Indirect Kalman Filtering The process of alignment is to determine attitude and heading of the INS. e. velocity. Last. only the errors in the states are estimated by the Kalman ﬁlter.. 10. 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. This means that instead of estimating the states directly.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. position. This chapter introduces the reader to the principles of and the notation used in indirect Kalman ﬁltering. In indirect Kalman ﬁltering the system model describes how the errors in the states develop over time. etc. develops over time. describing the error propagation of the system. The diﬀerences between the normal direct approach and the indirect approach are outlined.1) . 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.
where some are compared to the aiding device states to generate the observations for the Kalman ﬁlter. The measurement equation is a linearized version of the general nonlinear observation equation zobs which describes the actual.) f() = Function which compares the parameters from the INS and the aiding device. 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 estimate from the Kalman ﬁlter is in fact the best estimate of the errors in the states.1. attitude etc. velocity. it seems obvious to use this information to adjust the states and thus also the error states (as they depend directly on the states).) ξaid = Aiding device navigation states (position. etc.3) (10. nonlinear observation which is deﬁned as follows: zobs = f(ξins . velocity. ξaid ) where ξins = INS navigation states (position.2) . Looking at the ﬁgure. the input to the ﬁlter must also be errors.5) (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”.).1.10.4) (10. The INS system calculates the INS states (position. With these estimates the observation equation is: zobs = rins − rgps With error free measurement this observation equation is indeed zero. velocity. As the ﬁlter estimates error states. 10. 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. Hence. 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. 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. An example of an indirect Kalman ﬁlter is depicted in Fig. The Kalman ﬁlter outputs the optimal estimate of the errors in the INS states. etc. (10.
This x control vector would reset the errors to zero at every kalman ﬁlter cycle.5). But when correcting the rotation matrices subtraction is not applicable and they are corrected as seen in (12. 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. . velocity error. ERROR ESTIMATION USING INDIRECT KALMAN FILTERING The control reset operation will prevent errors from being build up at the integrators. This correction is applicable when correcting states like position and velocity. In this report two diﬀerent types of states are to be corrected. 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. The implementation of correcting the INS parameters involves applying un to the INS parameters in the following way: ξinsn (+c ) = gins (ξinsn (−).1. With the INS error model the ﬁlter input should also be an error.1: Indirect Kalman ﬁlter with control reset The ideal control vector uc is of course ˜n (+e ) as it can be seen from (10.6) Measurements INS System  Filter Input Kalman Filter Error State Estimate Control Reset of INS States/Indirect Reset of Error State Figure 10. 10. The Kalman ﬁlter estimates the error states and these are used to correct the INS states and thus indirectly resetting the error states. Aiding Device Foreground Equations + (10. 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. as the errors are set to 0 at each time step. The parts which are special for the indirect Kalman ﬁlter is indented in order to compare it with the ’normal’ Kalman ﬁlter.41). 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.51 CHAPTER 10. The measurements are parsed through the INS algorithms in order to get the ﬁlter input in the correct form. as xn (+e ) is the best estimate of the error.
7) 10. .2 Motivating Example For this example a one dimensional accelerometer is used. The Kalman ﬁlter is to estimate the errors in the velocity in order to get a better estimate of the velocity. The accelerometer delivers noisy acceleration measurements and the system consist of an integrator in order to determine the velocity. The reason for this is to get the observation set to contain velocity errors which is the driving parameter for the Kalman ﬁlter. MOTIVATING EXAMPLE 52 zobsn = f(ξinsn . Furthermore another speedometer is used which delivers velocity measurements.10.vref Kalman v filter Control reset Figure 10. The reference velocity measured by the aiding speedometer is subtracted from the systems estimated velocity to give the observation set. The correction inside the integrator is shown below. The Kalman ﬁlter then estimates errors as the system model is an error model.2.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. ξ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 (−). 10.2: One dimensional indirect Kalman ﬁlter As seen in the ﬁgure. the measured acceleration is integrated to give the velocity. The indirect Kalman ﬁlter is presented in Fig. Velocity measured by aiding device vref  Measured a acceleration Ts z1 v + Kalman filter Observation set for estimate the Kalman filter zobs = v . This error is of course fed back to the integrator to correct the velocity.
if the system is also nonlinear then a nonlinear Kalman ﬁlter should be chosen. As mentioned earlier the use of indirect Kalman ﬁltering requires the system model to be an error model. This leads to even more complicated calculation which sets even higher demands to the processor chosen. Choosing a standard direct Kalman ﬁlter approach requires the use of an extended Kalman ﬁlter which should be run at 2kHz.g. If the system has high dynamics and one is trying to estimate the states.3 the integrator is a discrete integrator with an addition block inside the integration loop. 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. then the Kalman ﬁlter cycle has to be very fast.53 CHAPTER 10. 10. 10. This method ensures that the correct value is store. 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 indirect Kalman ﬁlter is chosen. normally has much lower dynamics than the real system.2. This might pose a problem when a system should be implemented on a microprocessor. extended or unscented Kalman ﬁlter. The reasons for choosing the IKF for this project is that the model of the INS has very high dynamics. a normal linear. This might pose a problem when implementing on a microprocessor. In order to describe the motion with high precision. For this reason.1 Choice of Kalman Filter When using Kalman ﬁltering it is normally quite simple to choose between e. thus avoiding residual error buildup in the integrator. This error model can be approximated to be linear and in INS. Secondly. . 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 model should be run at more than 2kHz and furthermore the is model nonlinear.3: Illustration of correction of states inside a discrete integrator As seen in Fig. 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). It might not be possible to do all the calculations within each Kalman cycle. Two guidelines can be used to when the IKF should be considered.
. 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. the indirect Kalman ﬁlter is advantageous if the system is characterized by high dynamics and/or nonlinearity. SUMMARY 54 10. As described.3.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.
This error model is developed as it is required as model for the indirect Kalman ﬁlter. The navigation equations derived in Sec. velocity. Certain approximations and assumptions has been made deriving the navigation equa55 .Chapter 11 Error Equations During this chapter is the error model of the INS developed which describes the propagation of errors within the INS. The navigation equations derived in Sec. These errors are position. attitude and heading error of 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. 11.1 describe the motion of a vehicle relative to the earth instrumented in a strapdown inertial navigation system. 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 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. 9. 9.1 are simpliﬁed for use in the error 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. rotation.
two sets of errors are of interest. 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 because of measurement inaccuracies from the sensors and computer errors due to ﬁnite precision and integration errors.1). Secondly. The equations used in the computer are not exactly equivalent to the equations in (11. These errors can be minimized by making a suﬃciently accurate model.1. ˆ (11.2) as deﬁned by (11. alignment mode and navigation mode. the primary error of interest is the error in the Cn matrix.4) The error equations can be obtained by subtracting (11. the errors e in the velocity of the INS is necessary in order to estimate the position of the vehicle when navigating.3). Before deriving the error model the errors of interest are stated. This means that the error in Cn matrix and the error in height is important.1) from (11. In this chapter. Furthermore. another type of errors is introduced.2) and neglecting the parts which are a multiplication of two errors as they are assumed to be very small.1) due to several sources of error. As the INS is to be used in two modes. These errors arise when the INS computer evaluates the navigation equations. During alignment. 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. DERIVATION OF ERROR EQUATIONS 56 tions above which lead to one type of errors. The navigation equations in the navigation computer are equal to (11. 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.1) from the navigation equations used by the onboard navigation computer (11. This is achieved by subtracting the true navigation equations (11. This yields the following expression for the errors of interest: .2) δa = a − a ˆ Where a is the computed state and a is the actual state. b In navigation mode it is mainly the position error which is of interest.11.3) Using this error deﬁnition an error model of the INS can be developed.
6) where: Γ. 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. By introducing small angle approximations to (11.5) these equations can be transformed into a suitable form. As gravity is dependent on the parameter R.9) .8) The gravity error model in the δvn term from (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. n γ n = The rotation vector associated with δCb ǫn = The rotation vector associated with δCe n (11. E = Skew symmetric form of γ n and ǫn . 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. 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.5) zn Although these equations describe the error propagation it does not directly describe the errors of interest. 2 R0 n u + ∆gn R2 zn (11. variation of gravity is deﬁned to be gn diﬀerentiated with respect to R. so the error equations derived here can only be used in such cases. These equations show the derivative of the error DCM and not the derivative of the error angle of the DCM.5) is the variation from the true gravity to the one estimated by the computer.7) With this small angle approximation the error equations are rewritten.6) n δωie is rewritten: n e n δωie = −En Cn ωie = −ǫn × ωie e (11. of course.57 CHAPTER 11. These approximations are. The gravity error δgn is found using a special approach. only applicable when the error angles are small.
19) Where the ()∗ operator brings the vector enclosed in the brackets to skew symmetric form.1. here shown for three arbitrary vectors: v1 × (v2 × v3 ) = (v1 · v3 )v2 − (v1 · v2 )v3 (11.20) .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.6).11) δ vn from (11.6) is substituted into δ Ce from (11.8) and (11. (11.13) (11.17) Using the triple vector product to collect term again: n (En Ωn − Ωn En )v = (ǫn × ωen ) × v en en (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. 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.11.12) ˙ ˙ In order to rewrite δ Ce (11.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.11) (11.15) Multiplying the term in brackets 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. 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.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.9) δgn is as follows: δgn = 2g0 where δgn = Unmodeled gravity error M ˙ Using (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. DERIVATION OF ERROR EQUATIONS 58 This deﬁnition is used to deﬁne the error in gravity.
59 CHAPTER 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.25) which can be written in vector form: n n n b n n ˙ γ n = γ n × (ωie + ωen) − Cb δωib + δωie + δωen (11. this error model. h from (11. The position error is described in form of four parts: ǫn and h.22) ˙ In order to get rid of the derivative of Cn this is substituted with the expression from b (11.26) n ˙ Equation (11.26) along with δωen . Cn is rewritten by using the expression from (11. .12).21). This situation is e. 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 position error directly.21) ˙n ˙ In the same manner as with Ce . 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.27) However. This means that there is some redundant information as the position error can be described by three parts.23) (11. described by the equations (11.5) and the approximation b δR = δh constitute the error model for (11. (11. ERROR EQUATIONS which can be represented in vector form: n n ˙ ǫn = ǫn × ωen + δωen (11. suﬀer from one disadvantage. 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.6) b and then replacing with the expression from (11. The redundant information is in ǫn where the vertical part equals the azimuth error of the navigation frame.g.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.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.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.27). (11.
32) ˆn Ce = Ce + δCe n n (11.28) (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. First of all it should be noted that: e δCb = δCe Cn + Ce δCn n b n b (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.1.28).11.34) (11.33) Substituting (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.36) .33) into δve from (11. 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. 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.30) (11. using the deﬁnition of δvn from (11.29) which makes it possible to describe the Ψ part of (11.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.
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. ˙ ˙ ˙ 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.43): n n b ˙ Ψn = Ψn × (ωie + ωen ) − Cn δωib b (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.41) (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.27) is substituted and diﬀerentiated. ERROR EQUATIONS With deﬁnition (11.41) with uzn and as expected it is equal to δR.43): ˙ ˙ ˙ ˙ δ υ n = δ vn + ǫn × vn + ǫn × vn (11.37) Where R is the length of re .61 CHAPTER 11.44) ˙ The δ υ n diﬀerential equation is derived by diﬀerentiating the δυ n expression from (11.41) and is equal to: δrn = R(ǫn × un ) H zn (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. The n vertical parts is the dot product of (11. and ue is the unit vector pointing from the center of the ze earth to the INS in earth frame.45) .43) In order to obtain the diﬀerential equation for the new error model the equivalent parameters from (11.40) From this position error vector the horizontal and vertical parts are easily found.
49) zn M R R H What remains is to relate the new position vector rn to the old states ǫn and h.1) yields: ˙ δ rn = −Ωn Cn δre + Cn δve e en e n = δυ n − ωen × δrn (11. Lastly the ∆gn part from gn from (11.50) ˙e Substituting (11. vn and ǫn with expressions from (11. υ 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. As gn point the same way n as uzn the following expression can be substituted using δrn from (11. 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.27) and substituting ˙ δvn with an rearranged δ υ n expression from (11.11. DERIVATION OF ERROR EQUATIONS 62 ˙ ˙ Substituting δ vn .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.9) can be neglected as it is very small compared to gn .53) ˙ ˙ ˙ The new error model with the new error states Ψn .1.46) The three last parts can be shown to sum to zero by using the triple vector product from (11.15).51) (11.51) into (11.47) Furthermore (γ n − ǫn ) can be substituted with Ψn .52) with the expression of Cn from (11.50) and (11.51): ˙ ˙ δ rn = Cn δre + Cn δ re e e ˙ (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.54) .52) (11.1) and (11. By deﬁnition: ˙ δ re = δve and furthermore: δrn = Cn δre e Diﬀerentiating (11.48) g 2g δRun − δrn + δgn (11.
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. . (ψ) error model. With the error model developed and the indirect Kalman ﬁlter technique described the alignment algorithm is ready to be 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. 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. 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.43). When in navigation mode the second error model.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. is simpler to calculate enabling a faster Kalman ﬁlter and thus making it preferable. (γ) error model.2 Summary During this chapter has the error model of the INS been developed.27) with the relation found in (11. The ﬁrst n error model. ERROR EQUATIONS This new error model is of course equivalent to the one from (11. The reason for developing two error models is that each of them are preferable in two diﬀerent situations.63 CHAPTER 11.
a boat at dockside pitching with the waves. It does. in this thesis the navigation frame. The second section describes the ﬁne alignment procedure which uses the indirect Kalman ﬁltering technique and the error model developed in the preceding chapter. attitude and heading before navigation can begin. however. passengers or gust of wind. and aeroplane on the ground which vibrates due to fueling. 64 . Alignment is achieved using sensor measurements when the platform is in a quasistationary situation. The last section is a veriﬁcation of the alignment where it is tested against both simulated and realworld measurements. Quasistationarity is a state where the body is not stationary. The ﬁrst section will describe the coarse alignment. but close to with small variations. Coarse alignment is an analytical alignment which suﬀers from a lack of precision as measurement errors and other noise sources are not considered.g. estimate the attitude and heading accurately enough to justify the small angle approximations made in the error model. 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.Chapter 12 Alignment As mentioned earlier an INS simply integrates measured acceleration and rotation rate to determine the position of the body frame. The Alignment process is divided into two parts: Coarse alignment and ﬁne alignment. This chapter will explain the alignment procedure. and hence allow ﬁne alignment to use an indirect Kalman ﬁlter using the error model to obtain a more precise alignment. This is e. 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. But the position is always relative to the starting position and for this reason the INS needs to know both the position. The position is assumed known but the attitude and heading needs to be determined. This is the process of alignment. Choosing the navigation frame equal to the geodetic frame simpliﬁes the coarse alignment as the earth rotation is only present on two axes.
65 CHAPTER 12.3) b where C21 . . 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. results in the same as physically rotating the gimbal..2) (12. n n . When aligning a gimballed INS the gimbal is physically rotated until two of the the sensor axes are level with respect to the earth. 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 objective of this section is to determine the direction cosine matrix Cb relating the n navigation frame with the body frame.1 Comparison between Gimballed and Strapdown Alignment Understanding the concept of alignment when using a strapdown INS is easier when visualizing the gimballed situation. (Cb )−1 = (Cb )T . 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. and earth rotation is also known due to the knowledge of the b latitude. C33 are the elements of the direction cosine matrix Cn .1) (12. By adjusting Cn until the accelerometer measurements b equals [0 0 g]T in the navigation frame. C22 . this system is easily solved. 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. In the nframe gravity has only one component which is the vertical. So throughout this thesis the concept of adjusting Cn will be called leveling in b order to gain greater understanding. When using a strapdown INS the sensors are mounted directly to the body and can not be rotated so this is achieved analytically. ALIGNMENT 12.. The last three elements are easily determined with the use of the orthogonality property of the direction cosine matrix. The measured quantities are gb and ωie which are the measurements from the accelerometer and gyroscope. Comparing these quantities makes it possible to determine the direction cosine matrix relating the navigation frame and body frame. Knowing the quan tities g = and = from the measurements of the accelerometer and gyroscope. 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 .
The yaw error can be calculated b n from the horizontal parts of the estimated earth rotation ωie . Roll and pitch errors are also denoted tilt errors. ˙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. so the platform is leveled. As the system is quasistationary.or South Pole. At ﬁne alignment completion. error in velocity vn and error in the b diﬀerence in position from the initial position ∆rn . 11. During ﬁne alignment. A precise estimate of earth rotation is also needed in order to cancel the eﬀects of earth rotation on platform tilt. as they correspond to the platform being not truly level. This error is used as the driving parameter for the indirect Kalman ﬁlter. The yaw error is the deviation of the y axis of the navigation frame from pointing true North. 12. pitch b and yaw angles. which enables the use of the b previously derived error equations as this error model uses small angle approximations. b the errors in the Cn matrix can be assumed to be small. However. This will become evident later in this section. Fine alignment uses the navigation equations to obtain position information of the INS.3 Fine Alignment Fine alignment uses an indirect Kalman ﬁlter with the result from the coarse alignment as the initial Cn . The vertical part of earth rotation is known analytically as a function of latitude. The indirect Kalman ﬁlter then estimates the errors in the rotation matrix from body frame to n navigation frame Cn . 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 attitude and azimuth heading is b estimated with greater accuracy than possible with coarse alignment. FINE ALIGNMENT 66 C21 = −C12 C33 + C13 C32 C22 = C11 C33 − C31 C13 C23 = −C11 C32 + C31 C12 (12.3.7) As ﬁne alignment is performed when the INS is in a quasistationary situation the . The navigation equations from Sec. and does not need to be estimated.12.5) (12.6) As seen from these equations this DCM is only uniquely deﬁned if the latitude is not equal to ±90◦ . error in earth rotation ωie . the remaining errors in the Cn matrix consists of small roll. the estimates of roll and pitch errors are used as control resets to correct the Cn matrix.4) (12.1 is restated below. This means that this alignment procedure is not possible if the INS is positioned at the North. any position diﬀerent from 0 will be an error. After coarse alignment.
The quantities calculated in this foreground equation is not to be mistaken for the error states estimated within the Kalman ﬁlter. Furthermore. First of all. the estimated earth rotation ωie b b b needs to correspond to the measurements from the gyroscope Ωib = Ωie . 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 .9) . 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. Four Kalman gains are H needed to correct the four parameters. the Kalman ﬁlter is used not only to estimate the error states.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. ALIGNMENT n navigation equations can be simpliﬁed as both ωen and vn are zero. Secondly. n Furthermore. estimation error of ωie or both. vn and ∆rH . the earth rotation rate needs to be estimated. the Kalman ﬁlter is allowed to reset n n the errors in the last two INS states. H Using the approach described in Ch.1. 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. Equation (12. 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. any tilt error will thus lead to a nonzero ∆rn .67 CHAPTER 12. the driving parameter for the Kalman ﬁlter is the position divergence vector ∆rn . which correspond to horizontal tilt of Cb . As mentioned two parameters n needs to be corrected in order to bring Cb in steady state. Last. This means that the INS is torqued only in the horizontal plane but with no rotation around the vertical axis.8) is called the foreground equation as it is not within the Kalman ﬁlter. the Kalman gains are introduced. The navigation equations (12. As any tilt from horizontal will give rise to a component of the earth gravitation in the horizontal plane. In order to level the body frame analytically Cn is torqued so that no components are b n ˙H present in vn . As mentioned before.8) where H indicate the horizontal parts and is the position divergence deﬁned as the diﬀerence in position from the initial position. In order to interlink ∆rH with the four parameters which are to be corrected. This equation is the mapping from inertial measurements to velocities and positions which is called ”INS System” in Fig.10. an estimation error of H n ωie will tilt the INS from the correct leveled attitude and thus again lead to a nonzero ∆rn . Cn needs to b be leveled in order to cancel out the gravitation force in the horizontal plane. The Kalman gains describe how ∆rn is correlated with the system states. 10. but also to correct errors in the INS states. This n n is performed by introducing a quantity called ωtilt used to correct Cb . in order to bring Cn to steady state. 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.
12. 12. FINE ALIGNMENT 68 where un = Unit vector along the nframe vertical axis (z) zn k1 .2.1: Illustration of the accelerometer measurements due to an rotation of the body. k2 . 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. (12.82 z z y x y a x b Figure 12. Figure 12.12. 12. 12.1. As can be seen in Fig.1(b) the body has been rotated positive around x. 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. It can be realized by inspecting Fig. k3 .1 if the frame is rotated positive about the xaxis then the accelerometer with sensitivity axis y will measure a positive acceleration.2 shows the ﬁne alignment scheme. These error states are then used to correct the states in the INS system. As can be seen the input to the INS system is the measured accelerations and rotation rates. 12. 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.10) . g = 9. Having established the initial understanding of ﬁne alignment a further description of the derivation of the Kalman gains is done.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. This ﬁlter input is multiplied with the Kalman gains in order to get estimates of the errors states.3. In ﬁgure Fig.
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.2: Illustration of the ﬁne alignment procedure.69 CHAPTER 12. .
12) e This error model is simpliﬁed as certain parameters are not relevant when performing alignment. This is done as follows: First of all the quasistationary error model from Sec.12. 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.4.27). the rate of the earth rotation error is zero: ˙n δ ωie = − d n e (ǫ × ωie ) = 0 dt (12.11) where ∆rn H is the reference position for the position divergence which is approximated ref 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. The error model used when deriving the Kalman gains for ﬁne alignment is the model from (11. 12. the error model for the Kalman ﬁlter is simpliﬁed to only contain the horizontal parts. 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.15) Remembering that it is only the horizontal parameters which is of interest.8) when ǫn is constant.13) These equations should be expanded to include the position divergence as this is the driving parameter of the Kalman ﬁlter.4 is repeated below: . As there is no aiding apart from the knowledge of the quasistationarity. the observation set is: zobs = ∆rn − ∆rn H H ref (12. This is easily included by recognizing that the error in the position divergence vector is: δ∆˙ n = δvn r (12.14) As can be seen from (11. 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. 10 the observation set needs to be deﬁned. 11.
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)
32) . 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. 10.30) Again it should be noted that both Hn . attitude and position. Gmn and Rn are independent of time. Using the deﬁnition of the measurement equation in Sec. As mentioned in Sec.36) (12.21) the measurement matrix. With both the state and measurement equations established.25) and (12.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. 10. (12.33) −1 (12. 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.34) (12.1 the ideal reset is: x unc = −ˆ n (−e) Using this reset the discrete Kalman equations from (10. Due to this property the integrated process noise is also constant and can therefore be calculated oﬄine.4.35) (12. Q The ﬁnal task is to construct the output equations. 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.1.31) (12. 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.11) the observation is: zobsn = ∆rn H This leads to the calculation of the control vector: unc = −Kn ∆rn H (12.12.
Step One: In step one. This resembles a b real situation where the coarse alignment estimates Cn with errors. (12. they are to be veriﬁed. while applying the control vector.38) The last thing to determine is how to correct the INS states with the control vector from (12. which has been calculated by the use of the Kalman gain (12. (12. Applying horizontal attitude error n n γH to Cn is performed as follows.37). as these error states are directly accessible from the control vector.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. This is done by deﬁning the individual parts of the control vector: n n uc = [δωie Hc .5 Veriﬁcation of Alignment As the coarse and ﬁne alignment has been developed. Secondly to verify the ﬁne alignment the same inputs are used while selecting the initial rotation matrix Cn with a small error.41) To sum up. δvn c . 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.36) using Kn from (12.37) where c = Designation for the uc component which are to be applied to the foreground equations (12. δ∆rn c ]T H H (12. This way the coarse alignment should make a perfect one shot alignment.6) errors in the rotation matrix Cb b is: n n δCb = −Γn Cb (12.75 CHAPTER 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. The ﬁne alignment b . Veriﬁcation of alignment is divided into two steps which are described below. Subtracting the control vector from the earth rotation.8). As deﬁned in (11.32).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. γHc .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).37). ALIGNMENT Equation (12. velocity and position is straightforward. the ﬁne alignment process consist of integrating the foreground equations. 12.
The same characteristics can be seen by investigating Fig. As mentioned in Sec.12. Although it is not possible to distinguish which of the two systems is the most correct.5◦ ) · g = . and 0. 12. Cn should b be unity. Due to the errors in the initial Cn . Simultaneously it estimates the earth rotation which is used to estimate the yaw angle.086 which coincides with the graph. 12.5◦ . ﬁne alignment should decrease this error and b estimate the Cn with greater accuracy. 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. Due to the fact that the closer the position gets to zero the smaller the torquing signal. yaw]. This means that the acceleration and position should be driven to zero. 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.7◦ .5.5◦ . it will converge towards zero slower and slower. Step Two: In step two. With this INS it is possible to get both the roll. The system tested against is a Kearfott INS system.5. horizontal b accelerations will be present due to the gravitationally force. It is again seen that the north position starts out going positive north and negative east due to the components of gravity. −0. as it is the best guess to where the platform is positioned. This error should be driven to zero as Cb and earth n rotation ωie is estimated more and more precisely. Cn should be unity as the body frame is b coincident with the nframe and a test showed that the coarse alignment worked. 12. The same goes for the east acceleration. pitch. b With the same accelerometer and gyroscopic measurement.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.axis can be calculated as: sin(0.3 the initial north acceleration due to the gravitation projected onto the x. The error in the initial Cn has been chosen to b b be [0. It can also be seen that it converges to zero an expected. but with an initial error in Cn . 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. it is possible to compare the results and indicate the system developed in this thesis does work correctly. By due to the n misalignment. . at b which point the position is zero.4 the resets and torquing signals to correct Cn is driven by the b n error in the horizontal position. pitch and yaw angles with and already existing system. The position starts out being zero. 12.7◦ in roll and pitch error.4◦ ] in [roll. Fine Alignment: With the same acceleration and gyroscopic inputs as with the special case under coarse alignment. the expected rotation would be unity with a yaw angle error. The next thing to verify is that the rotation matrix Cn converges to the expected value. the alignment scheme developed in this report is tested against an already existing INS system. This originate from the fact that ﬁne alignment only torque horizontally to level the platform. 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. As can be seen in Fig. 0.4. the position changes until a correct estimate of Cn and ωie is given. 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.
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.15 0 0.4 0.8 1 Figure 12.77 CHAPTER 12.2 0.1 −0.4: Position divergence due to misalignment .05 Acceleration [m/s2] 0 −0.4 0.1 East Acceleration North Acceleration 0.2 0.8 1 Figure 12.6 Time [min] 0.6 Time [min] 0. ALIGNMENT 0.05 −0.
Step Two: Step two shows the result from the test against an already existing INS system.6 0. 12. As n can be seen in Fig.5 0 −0.5 0 −0. But as soon as there is an error in the position divergence vector the horizontal earth rate is changed. 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.19◦.5 0 0.5 0 1 Error [°] 0. So at latitude 57.8 1 Pitch Error 0. 12.7.6 Time [min] 0. A closer inspection. the rotation b n around x (roll). y (pitch) and z (yaw) starts out being the initial errors in the Cb .5 0 −0.1: As can be seen the Kearfott and the ﬁne alignment used in this thesis estimate Cn to b somewhat the same values. But the results imply that the ﬁne alignment developed does function with realworld measurements.8 1 Yaw Error 0.4 0. reveals that the rotation around y converges to around 3. 12.5 only the roll and pitch rotation is converging to zero whilst the yaw error is not torqued. ◦ As can be seen in Fig. a rotation of cos(57.5: Euler rotation associated with Cn b The Euler rotation associated with Cn is shown in Fig.4 0. which is unacceptable for an actual usage. As can be seen in Fig. The ﬁne alignment then levels the platform by torquing the horizontal parts to zero.2 0. 12. This was compared to a 15min ﬁne alignment with the equations developed in this thesis. VERIFICATION OF ALIGNMENT 78 1 Error [°] 0. 12.9e5 and small rotation approximately zero around x.19) · ωie = 3. As can be seen.5. The results are compared directly in Tab.6 ωie starts out being initialized as zero.8 1 Roll Error Figure 12.4 .8 the yaw angle is stationary at −0.95e−5 is expected on the y axis and much smaller on the x axis.4 0. The Kearfott system was positioned on a table and completed a 15min ﬁne alignment. In order to take the misalignment around z into account the yaw angle needs to be n calculated.5.5 0 1 Error [°] 0.4◦ but with a small component on the x axis. It is expected that the yaw angle converges towards the yaw error in Cb .12.2 0. they cannot be compared directly as there is some uncertainty to how the Kearfott data should be interpreted.6 0.12. which is the latitude at the test site at Aberdeen. To test if the .2 0. which can be seen in Fig. Although the diﬀerences are in tenth of a degree.
5 0 Angular Rate [rad/s] −0.7: Estimate of the earth rotation rate . ALIGNMENT x 10 1 0.75 0.6 0.6 Time [min] 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.2 0.5 −2 −2.8 0.5 −3 −3 Earth Rotation Rate Around X Earth Rotation Rate Around Y 0 0.5 −1 −1.4 0.85 Time [min] 0.7 0.79 CHAPTER 12.65 0.9 0.8 1 Figure 12.95 1 Figure 12.
55347332e05 6.12.2: Measured acceleration rotated into the nframe measurements is performed can be calculated with (6. then the b rotated accelerations measured in body frame should be [0 0 9.2752◦ 8. The gravity in Aberdeen. If the INS is leveled.2219◦ 12. The result are seen in Fig. As can be seen from Fig. 12.2.0859◦ This report 81.11895069e04 9.9.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. VERIFICATION OF ALIGNMENT 80 100 Yaw Angle [ °] 50 0 −50 −100 0 2 4 6 8 10 0 Yaw Angle [ °] −0.5 −1 2 3 4 5 6 7 Time [min] 8 9 10 Figure 12.04109◦ 8. 12. This means that Cb does actually level the INS.9) to be [0 0 9. . The results are seen in Fig.3166◦ 12.8: Wander angle at ﬁne alignment completion Role Pitch Yaw Kearfott 81.0990◦ Table 12. The reason for not including the time before 5min is because ﬁne alignment needs some time to estimate Cn .9 the rotated acceleration with x and y sensitive axes is located around zero with the z sensitive axis around 9.8171]T as the nframe is leveled. As seen from Tab. 12.81699624 Table 12. where the b fn x fn y fn z 2. 12. n And this component is very close to the theoretical. A mean of the values from time 5min to 15min is presented below.5.2 the accelerations is rotated so only a component is in the zdirection.81710728]T.8.
does not take the local gravity anomalies into account. 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. The alignment is of course nor perfect but also the theoretical gravity.9: Accelerations rotated into nframe 12.2 0. The diﬀerence on the theoretical gravity in Aberdeen and the one measured is approximately 0. calculated from (6. Due to sea currents.1mg. ALIGNMENT 0. This situation might be a boat aligning at sea. In practice.5 9 0 2 4 6 Time [min] 8 10 0 2 4 6 8 10 0 2 4 6 8 10 Figure 12. the . 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. Horizontal velocity and rotation around yaw. it was shown that the alignment developed in this thesis did not converge to the exact same value as the Kearfott. First.6 Discussion As has been shown in the veriﬁcation section.81 CHAPTER 12.2 an x 0 −0.2 an y 0 −0. 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 alignment scheme developed in the preceding chapter works and the rotation matrices is converging to the correct values. As with the navigation equations. In step two when testing alignment on realworld measurements. two movements diﬀers from the quasistationary state. This indicate that it stem from noise and ﬁne alignment precision. 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.2 10. it is investigated what eﬀects arise from nonquasistationarity. 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.5 an z 10 9.9).
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.10: Errors in accelerations and rotation rates due to horizontal velocity when assuming the INS to be stationary. The sensor outputs are showed in Fig. All sensors outputs are in body frame.2 −2. As expected there are accelerations errors on both horizontal and vertical axes.414 −6. The errors in the rotation rate is caused by the INS following the curvature of the earth when moving NorthEast.12. DISCUSSION 82 boat drifts horizontally and rotates around the yaw axis. This acceleration is only measurable on the vertical axis.3 −8.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. It does not rotate around horizontal axis and does not have a vertical drift. Furthermore. In order to examine the alignment errors due to this horizontal velocity alignment has .6. This eﬀect causes accelerations on the horizontal axis.8 −2 −2.10 shows the errors in both acceleration and rotation rate.4 −8.10. Horizontal Drift Horizontal drift causes two changes. a Coriolis eﬀect is present which is due to the moving navigation frame.4 10 9 8 7 6 0 5 10 Time [min] 15 Figure 12. 12. Figure 12.475 3. First of all the centripetal acceleration due to the speed around the earth. Acceleration [m/s2] −6. These two situations have been investigated.2 −8.413 x 10 −6 East Axis −8.4145 Roration Rate [/s] −6.4747 Roration Rate [/s] 0 x 10 −6 3.4749 3.4748 3.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.4 3. The earth simulation model from Ch. 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.2 3 2.4135 −6.
83 CHAPTER 12. 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.7267 · 10−5 rad/s which is 1/200◦/s. Table 12.5◦ after 15 minutes. whilst the horizontal yaw errors are the horizontal earth rotation components projected into the two horizontal axes. The important quantities are the heading error and attitude error.4: Comparison of alignment error when aligning with a yaw rotation. 12.11. 12. As can be seen from Tab. Highgrade inertial sensors today experience bias stabilities of 10−3 [m/s2 ] and 10−8 [rad/s].4 shows the result: Alignment Error Heading Pitch Roll Magnitude [◦ ] 46 · 10−6 5 · 10−6 6 · 10−6 Table 12. As expected. The sensor outputs are shown in Fig.3: Comparison of alignment error when aligning with horizontal velocity. Table 12. As can be seen from Tab. the North axis rotation should become smaller with a positive yaw rotation and the East axis rotation becomes larger. the North axis rotation error looks like a linear increase whilst the East axis rotation error looks like an exponential increase. It is assumed that the yaw errors increase on both horizontal axes as the boat yaws more and more from zero.4. This is because the earth rotation which is around the North axis is projected onto the rotated North and East axes. that sensor errors have a larger inﬂuence on . This explains the negative North error and positive East error. 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. which results in 4. As with the horizontal velocity. This yaw error only causes errors in the rotation rates and not in the acceleration as the boat does not have any translatory motion.12.3 shows the alignment errors: Alignment Error Heading Pitch Roll Magnitude [◦ ] 12 · 10−3 42 · 10−6 13 · 10−6 Table 12. it can be concluded. What is interesting to see. 12. This is projected with a sine and a sine at 90◦ change almost linear whilst a sine at 0◦ changes exponentially. and should be constant.3. The vertical yaw error is constant at 8. As can be seen in Fig. As can be seen. The vertical yaw error is directly connected to the yaw rate of the ship. Again the alignment error with yaw error is examined and compared to alignment without any motion and with horizontal velocity. Hence. rotation around yaw has been simulated. ALIGNMENT been performed with both quasistationary and nonquasistationary sensor outputs and the alignment result compared. aligning with horizontal velocity causes a heading error of 12 millidegree and attitude errors in the microdegree range. A yaw rate at 1/200 ◦ /s has been chosen. the error in both heading and attitude are in millidegrees.11.
7267 0 5 10 Time [min] 15 Figure 12.7267 8.7267 8.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. DISCUSSION 84 Acceleration [m/s2] 0.5 −1 0 5 10 Time [min] 15 Roration Rate [/s] 1 8.6.5 North Axis Acceleration [m/s2] 1 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 −1 −1. All sensors outputs are in body frame. .5 0 −0.7267 8.12.11: Errors in accelerations and rotation rate due to rotation around yaw when assuming the INS to be stationary.5 0 −0.
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]. What can be derived from these test is that gyro bias causes larger alignment errors than horizontal velocity. Quasistationary means that over time the position is zero. b Fine alignment uses an indirect Kalman ﬁlter in order to estimate attitude and heading of the INS. 12. .01 [◦ /hour]. has the alignment procedure been developed and been tested with simulated and realworld measurements.85 CHAPTER 12. In order to perform a precise ﬁne alignment. 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 alignment procedure is divided into two phases. the ﬁne alignment procedure is performed afterwards.5 shows alignment result: Alignment Error Heading Pitch Roll Magnitude [◦ ] 71 · 10−3 5.9 · 10−6 Table 12. And vertical rotation does only inﬂuence alignment marginal.7 Summary During this chapter.5: Comparison of alignment error when aligning with a gyro bias of 0. the INS is assumed to be in quasistationary state. than a constant velocity during alignment. Both tests showed that the alignment procedure worked and ﬁne alignment was able to decrease the error in the one shot coarse alignment estimate. The ﬁne alignment procedure has been tested with both simulated inputs and with realworld sensor measurements. ALIGNMENT the alignment. First. In order to get a better estimate of Cn . 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.1 · 10−6 7. Table 12.
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. This chapter will end with a description of the approach used in this report to aid the INS with a GPS. the GPS is bounded. position and attitude which may be unacceptable in many applications. GPS measures position and velocity directly. though experiencing short time error drift. 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. they measure diﬀerent quantities and suﬀer from diﬀerent error characteristics. The main idea is that with independent information at hand the Kalman ﬁlter is able to estimate with greater accuracy. 13. For this reason some kind of aiding is needed to either bound or reduce these errors.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. A comparison of the two systems is described below in Tab. So where the INS will have an unbounded longtime error drift. a speedometer and GPS. Where an INS system estimates position and velocity by measuring acceleration and angular rate and afterward double integrating these quantities to give the output.1: 86 . Global Positioning System (GPS) and barometric measurements. the same procedure will be followed describing how to aid with a speedometer. This report will describe the use of two diﬀerent aiding devices. An error model of the GPS will be derived leading to an error model of the INS/GPS system ready for Kalman ﬁltering. Some of the more popular aiding devices are Doppler Velocity Logs (DVL). Secondly.Chapter 13 Inertial Navigation with Aiding Unaided INS navigation inevitably suﬀer from unbounded errors in both velocity. 13.
A general GPS scheme is illustrated below.1. 1997] of a simpliﬁed GPS receiver scheme The ﬁrst block of Fig. but the traveling time of the signals. As all the satellites transmits on the same frequency the received signals are a mix up of all the visible satellites signals. They are arranged in such a way that at least four satellites are visible from anywhere on the earth. four satellites are needed to determine the position. 13. 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.1 Description of GPS The GPS system consists of at least 24 satellites orbiting the earth.1: Comparison of GPS and INS These diﬀerent error characteristics makes for a beneﬁcial combination of the two measurements. The orbits of the satellites are arranged in six planes. This is the reason for the name pseudo range as it is not a distance. the reason is the clock oﬀset in the GPS receiver. The code tracking loop is the algorithm which actually estimates the pseudo range.1 is the antenna and RF hardware which is used to receive the GPS signals. Code tracking Loop Pseudo Range Antenna and RF Hardware Code Correlation Carrier Tracking Loop Delta Range Figure 13. With the use of at least four satellites both position and velocity of the GPS antenna can be estimated. with at least four satellites in each plane. The carrier loop is the algorithm which .and carrier tracking loop. 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.87 CHAPTER 13. 13.1: Illustration [Brown and Hwang. The code correlation algorithm extracts the signal from the correct GPS which is then sent to the code. where normally only three parameters are needed. The pseudo range is the designation for the time the signal has traveled from the diﬀerent satellites to the receiver. Although it might seem peculiar why four satellites are needed to describe a position in Cartesian space. 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.
1. The velocity of the satellites is known at any time so the receivers velocity can be estimated because of this Doppler eﬀect.1) 13. As mentioned earlier. AIDING WITH GPS 88 estimates the velocity of the receiver. 13.3 Loosely Coupled With the loosely coupled approach. ygps . 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. The disadvantages is that it is not possible to gain the same precision as with the coupled integration as will be described below. Aiding with GPS can be divided into three diﬀerent approaches.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. A simpliﬁed representation of this integration is seen below: As can be seen in Fig. So it is obvious that the more satellites the more precise estimate of the position. in order to calculate the position of the receiver at least four satellites are needed. It uses the fact that the carrier wave changes frequency when either the satellite or receiver is moving. zsati = Position of the i’th satellite xgps . With these four satellites. 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. A short introduction will describe the advantages and drawbacks of the diﬀerent approaches. the GPS system still functions separately whilst the INS system uses the GPS measurements as a parameter in the measurement equation.1.1) describes four spheres with center at the i’th satellite where the radius is corrected with the clock oﬀset. (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 intersection of these four spheres is the location of the receiver. This means that it can always be implemented to an already existing INS system and with any kind of GPS receiver. loosely coupled and tightly coupled aiding.1. If GPS measurements are missing over large time periods the errors within the INS will of course grow.13. This approach does not involve any changes to the two systems and they will function separately if one system fails. the position is estimated by solving the following system[Brown and Hwang. the output from the GPS algorithms and INS measurements are used as the input to the Kalman integration ﬁlter. Uncoupled. 13. 1997]: σ1 = σ2 = σ3 = σ4 = where σi = Pseudorange from the i’th satellite to the GPS receiver antenna. So by solving these four equations. The main advantages of this . xsati . ysati .2.
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)
it is deﬁned how a clock is normally constructed.4) which is needed this is derived as the diﬀerential of (13. 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) . The true lever arm is modelled as a constant and a ﬂexing term: b lb = lb const + lﬂex ˙ lb const = 0 (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.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. such as a crystal.20) plus a random term [Savage. In order to describe these errors.16) For INS/GPS integration.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. This receiver clock error is modelled as follows. A random error caused by jitter and quantization and an oscillatory error[Savage.δtreci .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 .2.18) 1 fosc0 (13. Normally a clock is simply a matter of counting ticks from a oscillatory source. 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. position error of the i’th satellite δˆsi and the clock re error δtreci . The main source of error originate from the receiver clock error.20) (13. 2000]: ˙ δ tf = δfosc fosc0 δtreci = δtf + nt (13. 2000].21) (13. A clock suﬀer from two kinds of errors.
Mark = Is the correlation frequency and white noise error contribution nosc.rand ˙ δ fosc. The last thing before implementation is to determine the numeric values of the clock error model.Mark = −Cosc.24) The observation equation for the Kalman ﬁlter is the INS estimated velocity (ˆ n ) minus v ): the measured velocity (vn INS. 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.93 CHAPTER 13.aid = Cb vref + Ce l (13.Mark ˙ δ fosc. 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.Mark + nosc.26) n ˆ ref ˆ n ref ˆ b ˜ b z = δvn − δCb vb − Cb δvb − Cn (ωib × δlb ) (13.2.22) This section described the derivation of the GPS error model needed when using tightly coupled integration.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 . nosc. δfosc.26): (13.Mark = Is the random and ﬁrst order Markov process error contribution Cosc.2 Error Model for the INS/Speedometer Integration Aiding with a speedometer diﬀers somewhat from aiding with a position.27) . For this reason the loosely coupled integration is used which is described in Sec.rand .rand + δfosc. 13.rand = nosc.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.3. 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.23) (13. 2000]: δfosc = δfosc. 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. This means that if the INS rotate the aiding device would output an erroneous velocity if not correct for the lever arm. 13. The oscillatory part can be modelled as a random varying parameter plus a ﬁrst order Markov process[Savage.Mark δfosc.aid = v − Cb vref − Ce l (13.Mark where δfosc.
l is chosen to be zero. this is outside the scope of this report.31) ˆ ˆ ˜b ˆ ˜b ˆ z = δυ n − vn × ψ n − Cn δvb − Cn (ωib × δlb ) + Cn (ωib × lb ) b ref b const b ﬂex (13. These two properties relating the two error models are restated here: (13. 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 .1 Loosely Coupled INS/GPS Integration As mentioned earlier. The only thing missing is an expression for the velocity error experienced by the speedometer δvb .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. 13.13.24) gives the following: ˆref ˆ ˜ b lb ˆ ref ˆ vn = vn − Cn (ωib × ˆ ) b Using this expression in (13. 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.37). the integration of a speedometer is almost ready to be implemented. 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. 2000].3.28) An approximation for vb derived from (13.6) and the lever arm error model from (13.3. Furthermore. the measurement equation is modiﬁed into containing ψerror parameters rather than γ error parameters using (11. position and attitude error of the INS.16) on (13.30) b ˆb As the parameter ˆ can be set freely as long as δlb l const is taken account. Using (11.32) At this point. As this expression diﬀers according to which speedometer is used and it is quite ref cumbersome to develop the error model. 13.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.32) and (11.29) ψ n = γ n − ǫn δυ n = δvn + ǫn × vn Using these properties gives the following: (13. 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.
34) (13. A ﬁgure of the INS/GPS integration is presented below. 12. 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 precision of the GPS is described in the data sheet and therefore.95 CHAPTER 13.54) and . 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.4 the GPS measurements are subtracted from the INS measurements to form the observation set.4. δvn = Is the error in the GPS estimation of position and velocity GPS GPS δrn . The observation equation is: zobs = The measurement equation is: ˆobs z = zn − zn obs z= where δrn . δvn = Is the error in the INS estimation of position and velocity INS INS δrn . As can be seen from Fig. The Kalman ﬁlter estimates the errors which are used to reset the INS.36) The error state dynamic matrix (A) is the time varying ψerror model from (11.33) (13.35) (13. 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. this approach can always be implemented in any INS system. Derivation of Kalman Filter Equations The Kalman ﬁlter is developed using the same approach as in Sec. 13. δvn = Is the true position and velocity true true With the measurement equation derived.
The error model which should be used when in navigation mode is the one from (11. 12.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.40) 0 0 2T g r −Ωβ 0 0 0 The integrated process noise matrix is found using the same approach as in Sec. 12.3. IMPLEMENTATION OF NAVIGATION WITH AIDING 96 the process noise dynamic matrix is derived from (13. 12.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.38) As in Sec.5T2 aE 0 0.54).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. The discrete error state dynamic matrix is computed in Matlab shown below: I3x3 δGPSvelocity 0 0 I3x3 δGPSposition (13.5T2 aU 0 0.37) The measurement noise is derived in the same way as in Sec.4 and is as follows: Gmn = nmn = I3x3 0 δvn GPS δrn GPS 0 I3x3 (13.5T2 aE 0.5T2 aN −0.5T2 aU −0.5T2 aN 1 Ωα −Ωα 1 (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.13.41) .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 .
As ψ contain both the error in Cn and n b b Ce . δr is needed as it describe the errors in Ce . 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.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.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. This means that all the heading error is in γ n . It has been deﬁned earlier that the n n navigation frame does not rotate with respect to the geodetic frame. 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. In order to correct Cn and Ce some transformations are necessary. the Kalman ﬁlter is ready for implementation and veriﬁcation. all the necessary parameters are available and the INS states are corrected as described in (12.44) At this point. The control reset vector is deﬁned as in Sec. But in order to extract γ n from ψ. Using the δrn relation from (11. 12.43) ǫn is calculated from δrn in the following way: Having constructed ǫn .43) in the following way: γ n = ψ n + ǫn (13. e n n n navigation to Earth rotation matrix Cn with ǫ and velocity v with δv . δvv . ǫn is needed.45) δrn = R(ǫn × un ) + δRun zn zn δrx /R ǫy δry /R = −ǫx + δRu δrz /R 0 (13. 13.43) n The states which are corrected are the body to navigation rotation matrix Cb with γ n .97 CHAPTER 13. If one look at uc the only state which can be directly corrected is the velocity. δrv ]T (13. γ n can be calculated from the ψ n relation from (11.46) With these equations derived.4 which gives: uc = [ψ n .
the INS navigates without any aiding.5. 9. During this time.5.6 [min].6 shows the reference track. As can be seen the aided navigation follows the reference track.3 [min] driving time.187 57. The two test scenarios should show how aiding is able to bound the position error.191 57. These data have been used as the GPS position.5 shows navigation without aiding.082 −2.19 57.188 57. 13. and how the INS system keeps the system “on track” during GPS outages.189 57. 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. Aiding with a GPS should bound the position error. 57. One test scenario where the INS starts out with 15 [min] alignment followed by navigation without aiding. The INS then navigate . the GPS position and the aided navigation.13. As can be seen from Fig. With a drive time of approximately 5.194 57. Navigation with aiding is shown in Fig.5 the navigation starts out following the reference track but drifts away from the reference track unbounded. VERIFICATION OF NAVIGATION WITH AIDING 98 The test data used to verify navigation with aiding is the same as used in Sec. Figure 13. the GPS data outage occurs.08 Longitude [°] −2.193 57. The track is the same track used to verify the navigation equations in Sec.078 Figure 13.086 Aided position Real position Latitude [°] −2.6 Figure 13.4. Two test scenarios are presented.192 57. A GPS outage of 40 [s] duration is simulated after 3. the Kearfott does only experience minor drift why the Kearfott track can be used as the reference track. The second test scenario is a scenario where the INS again starts with 15 [min] alignment. Just before the North turn.5: This ﬁgure shows navigation on realworld measurements without aiding. 13. in the bottom right of the track.186 −2. The same track is followed as in the ﬁrst test scenario but navigation is aided with the GPS data.084 −2. 9.
Just before the turn to the South the GPS data is valid again and thus pulls the navigation towards the reference track.086 Aided position GPS position Real position Latitude [°] −2.5 Summary As unaided INS inevitably suﬀers from unbounded position error.186 −2.188 57. 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.7 shows the latitude where the second row shows the longitude of the INS. 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). follow the turns made by the vehicle. The precision of this position depends on how tightly the INS is “fused” with the GPS. Aiding with a GPS.078 Figure 13. 13. As can be seen the “x” which indicate the GPS position is absent.187 57. It does.084 −2.19 57.189 57. a GPS and a Doppler velocity log. What can not be seen in Fig.6: This ﬁgure shows navigation on realworld measurements aiding with a GPS.192 57.7 shows a 2d plot of the latitude and longitude.191 57. some kind of aiding is preferable if INS is to be used over longer time periods. two diﬀerent aiding devices have been presented. In order to investigate that Fig. where both the velocity and position are available bounds the position error. without aiding but the position starts to drift from the reference track. During this chapter. INERTIAL NAVIGATION WITH AIDING 57. however.193 57.99 CHAPTER 13. Three diﬀerent approaches are described in this thesis where the loosely coupled . 13.6 is how the INS/GPS position variates around the reference track.082 −2. 13.194 57.08 Longitude [°] −2. The ﬁrst row of Fig. 13.
078 −2.188 57.187 57.8 1 Figure 13.191 57.192 57.191 Latitude [°] 57.1908 0 0.0819 −2. .6 Time [min] 0.1912 57. the GPS positions (x) and the reference track (solid).0822 0 0.082 −2.4 0.8 1 −2.7: This ﬁgure shows the positions of the fused INS/GPS (dotted).086 0 1 2 3 Time [min] 4 5 −2.13.189 57.0821 −2.1909 57.2 0.4 0. SUMMARY 100 57.19 57.2 0.082 −2.08 Longitude [°] −2.6 0.1911 57.084 −2.186 0 1 2 3 4 5 57.5.0818 −2.
. under water. A drawback from aiding with a DVL is that the position error is not bounded but only decreased. When in such a situation aiding with a DVL is a possibility.g. INERTIAL NAVIGATION WITH AIDING approach have been chosen for this project.101 CHAPTER 13. e. A disadvantage when aiding with GPS is that it is not possible when no GPS signal is present.
and split into two beams travelling clockwise and counterclockwise around the circular light path with the radius r.1: The fundamental principle of optical angular rate sensors Light is generated by the light source. This is illustrated in Fig. 102 . and the transit time t for both beams to go around the light path is the same.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.1 Light path Light source CW beam ω Beam splitter/ detector CCW beam r Figure 14. This chapter will give a description of the sensors that are commonly used in strapdown inertial navigation systems. Also.Chapter 14 Inertial Sensors The next two chapters describe an approach which is necessary when implementing an INS in the real world. the errors that are expected to be experienced in these kinds of sensors are explained. 14. 14. ω is 0. each exploiting diﬀerent physical phenomena to measure speciﬁc force or angular rate. With no rotation. The basis of inertial navigation is the accelerometers and angular rate sensors which exist in a wide variety.
the RLG outputs a pulse every time the gyro has rotated a certain angle.2 Interferometric Fibre Optic Gyro In an Interferometric Fibre Optic Gyro (IFOG). As the gyro rotates. 1997].1◦ /hour/g2 0. the path length for each of the beams are changed. This makes the RLG inherently digital and rateintegrating. which is spun around the sensitive axis. A prism combines the two light beams. INERTIAL SENSORS However.5 − 50◦ /hour 1◦ /hour/g 0. 14. As the IFOG is rotated. the length which the beams have to travel changes.1.1.001 − 10◦ /hour insigniﬁcant insigniﬁcant few parts per million to 0. 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. As the IFOG uses ﬁbre optic coils instead of a laser cavity.103 CHAPTER 14. and the frequency diﬀerence forms an inference pattern. which makes the IFOG cheaper than the RLG. The performance of the IFOG.1: Comparison of typical gyro accuracy 14. and the transit time diﬀerence for each beam can be measured. 14. two broadband light beams are transmitted CW and CCW through a ﬁbre optic coil. the need for high quality mirrors. It works by letting a laser travel clockwise and counterclockwise around in a continuous light path constructed from three or more mirrors. and precision machinery is avoided. 14. F = ma (14. By outputting a pulse each time a fringe moves past a photodiode. and the phase diﬀerence can be measured as a change of intensity by photodetectors.2 Accelerometers An accelerometer uses Newton’s Second Law of motion to measure speciﬁc force.2. which can be detected. This makes the IFOG an analog rate sensor. One of the mirrors allows a small amount of light to exit the cavity. By having a proof mass suspended by springs. the frequency of the two beams change. if the light path is rotated.1[Titterton and Weston. the displacement of the mass due to acceleration can be measured by the pickoﬀ.1 Ring Laser Gyro The Ring Laser Gyro (RLG) is the most common type of gyro used for precision strapdown inertial navigation systems.5% >100 Hz >1000◦/s gindependent bias gdependent bias g2 dependent bias scale factor errors bandwidth maximum input rate Table 14. RLG <0. appeals more to medium accuracy applications. . Along with also being a solid state sensor.01% >200 Hz >1000◦/s IFOG <0. 14. A comparison of typical accuracy of RLG and IFOG sensors are shown in table Tab. however.05 − 0. this makes it ideal for strapdown inertial navigation systems.
Accelerometers working as shown in Fig. Temperature sensitivity The inﬂuence of temperature on the output of the sensor. both deterministic and nondeterministic. the current needed by the electromagnet to hold the proof mass in place changes. Scale Factor The scaling the sensor applies from input to output. when 3 1dof sensors are mounted in a frame. Quantization noise The noise introduced by converting an analog signal into a ﬁnite precision digital signal.3 Sensor Error Models Many sources of errors arise from sensor imperfections. as the springs are not linear.14. Accelerometers for INS applications are of the integrating type. 14. SENSOR ERROR MODELS 104 Acceleration Case Spring Proof mass Spring Figure 14. the most used type of accelerometer in inertial navigation systems today is the forcerebalance accelerometer.e. so they output a pulse when a certain change in velocity have been measured. .2: Conceptual drawing of a accelerometer Displacement pickoﬀ It is important to notice that gravity inﬂuences the proof mass by attraction. Misalignment The inaccuracies in sensor alignment.3. 14. When the sensor accelerates. These suﬀers from nonlinearities at large accelerations. This means that the input the sensor receives (angular rate. and this change is proportional to the acceleration experienced. Here.2 is known as openloop accelerometers. so the acceleration measured by the accelerometer will be in the opposite direction of the attraction. I. These include Bias The output from the sensor when no input is applied. Random noise/random walk Stochastic elements arising from other unmodelled noise sources. the diﬀerence from a 1:1 input/output mapping. To overcome this. gsensitivity The inﬂuence of acceleration on the output of the sensor. the proof mass is held at the zero position by an electromagnet. acceleration) does not correspond to the output the sensor sends.
Therefore. the MA are the misalignments (with the subscripts deﬁning which axes the misalignment applies to). 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. the SF’s are the scale factors. The most ˙ dominant terms are the linear bias.2) Where i are the measured input. misalignments. these can be found by testing the sensor with various inputs. gsensitivity and temperature sensitivity can be regarded as random constants with a process model of c = 0. and T is the temperature. Hence. the B’s are the biases. Normally. . scalefactor misalignments and temperature dependency. This model will be used in chapter 15 to calibrate inertial sensors. bias. o are the output. scale factor. INERTIAL SENSORS All of these can be nonlinear.105 CHAPTER 14.
These errors cannot be compensated for without inﬂuencing the signal itself.Chapter 15 Calibration of Sensors The performance of the INS is dependant on the errors that are present in the accelerometers and gyros. however. As the roll. In this project. The term “sensor error” will throughout this chapter signify only a repeatable sensor error. 1987]. it is necessary to deﬁne a sensor frame. This simpliﬁed error model of the system is: 106 . The magnitude of the random errors depends on the quality of the sensor. Thus. This is done by identifying the constant errors at various temperatures by rotating the IMU through a series of 180◦ rotations[Diesel.1 Introduction While it is possible to identify all the sensor errors. this can take much time and requires expensive calibration equipment. a simpliﬁed error model has been developed. Depending on the required accuracy of the ﬁnal INS. 15. can be identiﬁed through a process called calibration and thus be compensated for. In this error model is the earth rotation neglected due to the short time between each rotation making the earth rotation very small. these will be called misalignment angles and must be estimated during calibration.2 Calibration Equations In order to ﬁnd some of the sensor misalignment errors. it is chosen to identify constant but temperature dependent errors. The repeatable errors. a weighing between sensor compensation (cost and time) and INS accuracy must be made. which might be rotated with respect to the body frame. 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. With no translatory motion. 15. pitch and yaw angles of this rotation is not known.
aMA are misalignments and ab is the acceleration due to gravity.1) is equal to a velocity error δ vn . As ˙ the system should not be translating. the equations that need to be solved are of the following type (after adding time dependency on (15. the sensor error model from chapter Ch. aSF are scale factors.2) 0 0 n −g∆θy n = ∆δan + g∆θx (15. so the vn term obtained ˙ from the navigation equation (9. The rotations needed are shown in Fig. and 0 is the time before a rotation. To obtain ∆δan and ∆θ n . 15. 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.1.1: Rotation sequences used for calibration Where T are the time after a rotation.1) To excite all the sensor errors. all given in the navigation frame. any velocity is an error. From this.4) Where δab are the errors in the measured accelerations. The ∆ preﬁx denotes that the following term is the diﬀerence of the term from 0 to T . Thus.107 CHAPTER 15.3) 0 aMAxy aSFy aMAzy aMAxz aMAyz ab (t) aSFz (15. the system is rotated through 3 sets of 3 rotations each. 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.5) .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. the aB terms are a constant bias. 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. 14 is used. ∆δan is obtained as ∆δan = δan (T ) − δan (0) (15. 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.
2. where for brevity. 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.1.9) Where φ(t) and ψ(t) are the rotation angles at time t.2) for rotation 1 gives − sin(ψ(t)) cos(ψ(t)) 0 0 0 1 0 − sin(φ(t)) 1 0 0 cos(φ(t)) (15.10) (15.14) .6) ∆θ n = 0 gSFx Cn (t) gMAyx b gMAzx gMAxy gSFy gMAzy gMAxz gMAyz ω b (t)dt gSFz (15.11) (15. one obtains the measurements given in table 15.8) (15. 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. solving (15.15. CALIBRATION EQUATIONS 108 Identically.13) (15. the gyro error model is gBx gSFx n δω n (t) = Cn (t)δω b (t) = Cb (t) gBy + gMAyx b gBz gMAzx To obtain ∆θ n . Solving for the remaining rotations.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 .12) (15.
1: Table of observation equations for calibration coeﬃcients From these equations. which is equal to deﬁning the sensor frame so that the xy plane is spanned by the x and y accelerometer. 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.22) And the z gyro bias is found from set 2 rotation 3: gBz = δ¨y (T ) − δ¨y (0) vn vn 2g (15.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. 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.20) These approximations are valid as the accelerometer errors will be very small after just a few iterations of the compensation procedure. The expressions are seen in Tab.17) (15. 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.19) (15.16) (15. with the x accelerometer corresponding to the xaxis. Rotation k: αj = δ vx (T ) − δ vx (0). The missing accelerometer misalignments are set to 0. All are calculated from set 1 rotation 3: .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 . In this way. 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. CALIBRATION OF SENSORS j Set j.21) (15. 15. accelerometer biases and misalignments are obtained. expressions for the gyro scale factors.109 CHAPTER 15.2. βk = δ vxy (T ) − δ vy (0) ˙n ˙n ˙n ˙n k (15.
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.
12. Two diﬀerent error models are derived. With only the reference velocity the 113 . 13. An indirect Kalman ﬁlter requires an error model of the INS. Two approaches has been presented in this thesis in Ch. 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. Aiding with a GPS and with a speedometer. Furthermore. Alignment is a matter of estimating the attitude and heading of the INS. The indirect Kalman ﬁlter is an advantageous approach when the system model has high dynamics and/or is nonlinear. 10 shows the diﬀerences between the two ﬁlters an derives the equations used in later chapters to implement this ﬁlter. 11. Using the γerror model the alignment algorithm has been developed in Ch. 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 geodetic constants and equations was presented in Ch. these errors need to be bounded if the INS is to be used over longer periods. The ψerror model is preferable in the navigation mode as this model becomes simpler than the γerror model when moving. As the INS inevitably suﬀer from unbounded errors due to the double integration of noisy measurements. Aiding with a GPS gives an observations set which include the position which bound the position error. 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. 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. 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. Aiding with a speedometer only gives an observation set which include the velocity. The indirect Kalman ﬁlter is implemented using an error model of the nonlinear system which can be approximated linear. the dynamics of the error model is usually much lower than that of the real system. As the Kalman ﬁlter used in this thesis is an indirect variant of the normal direct Kalman ﬁlter Ch. 6 along with a simpliﬁed gravity model with an accuracy suitable for INS. In order to bound the errors the INS is aided with external reference measurements.
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. some of the sensor errors can be estimated under navigation in order to correct the sensor measurements online. Furthermore. 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 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. the “ground speed”. 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. a Kearfott T16. This showed that the developed alignment b algorithm worked. These tests concluded that a precise gyroscope is essential if high performance is required. The equation of motion translates measured accelerations into a velocity with respect to the surface of the earth. The equations developed were veriﬁed against measurements obtained from a medium accuracy INS system. The alignment algorithm is based on an indirect Kalman ﬁlter.Chapter 17 Conclusion The main purpose of this thesis were to develop algotihms for an inertial navigation system. Further tests with diﬀerent errors sources revealed that gyro bias was the predominant cause to unbounded position errors. which were veriﬁed using both simulated test data and realworld data from the Kearfott T16 system. A γerror model suitable for alignment and a ψerror model suitable when in navigation mode. This is accomplished by implementing algorithms for the main parts of an INS: Navigation. as the equations in the simulation model showed to be the inverse of the navigation equations. 118 . 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. which uses an error model of the system. 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. This model was used to testing the precision of the INS when disturbed by diﬀerent noise sources. but was developed in order to make reproducible test data simulating various sensor errors. alignment and aiding. 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. and these navigation equations are the most essential parts of an INS. An alignment algorithm has been developed. The basis when developing an inertial navigation systems consists of the equation of motion. The development of the navigation equations is based on the equation of motion.
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. . the purpose of this thesis have been met. While aiding with a speedometer only decreases the position. two diﬀerent aiding approaches have been proposed: Aiding with a speedometer and aiding with a GPS. The aiding algorithm was shown to be able to bound the errors in the INS system. As all the equations needed for an inertial navigation system with aiding have been developed and have been veriﬁed to work. misalignment and sensor biases. to address the unbounded position error characterising of an unaided INS.119 CHAPTER 17. error aiding with a GPS bounds the position error. CONCLUSION In order to aid the navigation equations. 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.
2000]. It has not been investigated which performance improvements might be gained from running a direct nonlinear Kalman ﬁlter at this frequency. The earth simulation model can be used to test some performance aspects of the navigation algorithms by supplying it with various trajectory proﬁles. A simple discretization method based on small time steps has been chosen in this thesis.Chapter 18 Improvements and Discussion Before the algorithms developed in this thesis can be implemented and used in the real world. and several solutions have been presented. However. but tests in the real world are important to validate the navigation equations completely. the twospeed approach still oﬀers lower requirements to the processing power of the main computer. 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. The indirect Kalman approach has been chosen in this thesis to avoid running a nonlinear Kalman ﬁler with a Kalman cycle greater than 2kHz. INS will probably use a twospeed approach in systems used for underwater applications. A more precise discretization method could be used but in either case should the errors introduced by discretization be analyzed. This introduces errors which could be avoided with a more advanced correction. as modern computers are fast enough to execute the entire algorithms at full speed. Before implementation on a microprocessor the equations need to be discretized. It has also been argued that the twospeed approach of the integration algorithms are no longer needed. several aspect are to be considered. When correcting the rotation matrices a ﬁrst order approximation has been used throughout this thesis. The choice of discretization method aﬀect the accuracy of the discrete model. As power consumption are of great concern in underwater vehicles. 120 . The navigation algorithms need to be validated more rigorously than done in this report. This has also not been addressed in this thesis. This problem is documented rigorously in the literature. and thus allows a lower power consumption. and in order to increase the accuracy a more advanced discretization method. 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.
This was done as it is diﬃcult to obtain the values for the errors in a particular IMU.121 CHAPTER 18. IMPROVEMENTS AND DISCUSSION When aiding with GPS only the loosely coupled integration approach have been implemented. as the hardware needed for the calibration has not been available. however. This has not been done. If the navigation performance have improved. The validation of the calibration equations were made using simulated sensor outputs. the calibration equations work. . and thereafter navigating using a calibrated IMU. A validation of the calibration equations could be made by navigating using an uncalibrated IMU. The tightly coupled INS/GPS integration could prove more precise and more work should be done to implement that approach.
Applied Mathematics in Integrated Navigation Systems... and Weston. Strapdown Analytics. H. 1997] Titterton. H. J. John Wiley & Sons. and Weston. 1971] Britting. number 13th in Biennial Guidance Test Symposium. R. 122 . Peter Perengrinus Ltd. (2007). G. Strapdown Associates. American Institute of Aeronautics and Astronautics. Strapdown inertial navigation technology. In Biennial Guidance Test Symposium. Introduction to Random Signals and Applied Kalman Filtering. Technical report. R. (1987). Inertial Navigation Systems Analysis. Peter Perengrinus Ltd. [Diesel. 2nd edition. [Brown and Hwang. (1971). P. L. L.Bibliography [Britting. 3rd edition. P. (1997). Strapdown inertial navigation . J. [Savage. P. Strapdown inertial navigation technology. G.. Strapdown Associates. (2000). 2005] Titterton. 1987] Diesel. D. Y. G. W. [Savage. K. (1997). [Rogers. 2007] Rogers. and Hwang. Calibration of a ring laser gyro inertial navigation system. 1997] Brown. John Wiley and Sons. 1st edition. C.lecture notes. M. 2000] Savage. D. (1997). 1997] Savage. [Titterton and Weston. 3rd edition edition. (2005). J. R. [Titterton and Weston.
This action might not be possible to undo. Are you sure you want to continue?
Use one of your book credits to continue reading from where you left off, or restart the preview.