Embed Size (px)
Citation preview
A Low-Cost GPS Aided Inertial Navigation
System for Vehicular Applications
ISAAC SKOG
Master of Science ThesisStockholm, Sweden 2005-03-09
IR-SB-EX-0506
1
Abstract
In this report an approach for integration between GPS and inertial navigationsystems (INS) is described. The continuous-time navigation and error equationsfor an earth-centered earth-fixed INS system are derived. Using zero order holdsampling, the set of equations is discretized. An extended Kalman filter forclosed loop integration between the GPS and INS is derived. The filter propa-gates and estimates the error states, which are fed back to the INS for correctionof the internal navigation states. The integration algorithm is implemented ona host PC, which receives the GPS and inertial measurements via the serial portfrom a tailor made hardware platform, which is briefly discussed. Simulationresults of the system are presented.
Contents
1 Introduction 41.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.2 System Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.3 Thesis Outline and Contributes . . . . . . . . . . . . . . . . . . . 6
2 Coordinate Systems 82.1 Coordinate Frame Definition . . . . . . . . . . . . . . . . . . . . 8
2.1.1 Earth-centered earth-fixed frame (ECEF, e-frame) . . . . 82.1.2 Local geodetic frame (t-frame) . . . . . . . . . . . . . . . 82.1.3 Inertial frame (i-frame) . . . . . . . . . . . . . . . . . . . 102.1.4 Body frame (b-frame) . . . . . . . . . . . . . . . . . . . . 10
2.2 Coordinate Frame Transformation . . . . . . . . . . . . . . . . . 102.2.1 Projection . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 Plane Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . 122.3.2 Rotation matrices . . . . . . . . . . . . . . . . . . . . . . 142.3.3 Small angle rotation . . . . . . . . . . . . . . . . . . . . . 14
2.4 Rotating Coordinate Frame . . . . . . . . . . . . . . . . . . . . . 16
3 Inertial Navigation Equation 183.1 Fundamental Equations of Inertial Navigation . . . . . . . . . . . 18
3.1.1 Gravity force . . . . . . . . . . . . . . . . . . . . . . . . . 183.1.2 Inertial force . . . . . . . . . . . . . . . . . . . . . . . . . 193.1.3 Equations of motion . . . . . . . . . . . . . . . . . . . . . 19
3.2 Navigation Equations . . . . . . . . . . . . . . . . . . . . . . . . 203.2.1 General Navigation Equations . . . . . . . . . . . . . . . . 203.2.2 ECEF-frame navigation equations . . . . . . . . . . . . . 21
3.3 ECEF Navigation Error Model . . . . . . . . . . . . . . . . . . . 223.3.1 ECEF navigation error equations . . . . . . . . . . . . . . 22
4 Discretization 274.1 Discretization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.1.1 Discrete time navigation equations . . . . . . . . . . . . . 274.1.2 Discrete time error equations . . . . . . . . . . . . . . . . 29
5 Integration of GPS and INS 305.1 The Extend Kalman Filter . . . . . . . . . . . . . . . . . . . . . . 30
CONTENTS 3
6 Hardware 336.1 The Inertial Measurement Unit . . . . . . . . . . . . . . . . . . . 336.2 Data Acquisition . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
7 Simulation Results 357.1 Driving Scenario . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
8 Conclusions and Further Work 398.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 398.2 Further Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
A The Kalman Filter 42A.1 Derivation of the Kalman Filter Equations . . . . . . . . . . . . . 42
Chapter 1
Introduction
1.1 Background
Today many vehicles are equipped with global positioning system (GPS) re-ceivers that constantly can provide the driver with information about the ve-hicle’s position with an accuracy in the order of 15-100 meters [1]. However,the GPS receiver has two major weaknesses. The slow update rate, only once asecond for most receivers, and the sensitivity to blocking of the satellite signals.This can cause problems when for example plotting a vehicle’s movements on amap.
Imagine a car that is travelling at high speed an suddenly makes a fast turnaround a corner of a house. If the position of the car is only determined oncea second and out of this position estimates of the car’s movement is plotted, itmost likely will look as the car drove through the corner of the house. After theturn the car drives into a tunnel and all of a sudden the position of the car can’tbe determined at all.
Inertial navigation systems (INS) can provide position, velocity and attitudeestimates at a high rate, typically 100 times per second and are, to the oppositeof the GPS receiver, self contained [2]. Instead it relies on Newton’s laws ofmotion, from which it can be concluded that if an object’s initial position,velocity and attitude are known all further positions, velocities and attitudescan be determined by integrations of the accelerations and angular rates of theobject. However, due to the integrative nature of the INS, low frequency noiseand sensor biases are amplified. The unaided INS may therefore have unboundedposition,velocity and attitude errors [3]. These complementary properties makean integration of the two systems suitable. Indeed it has been shown that bycombining the short time accuracy and high update rate of the INS with thelong term accuracy of the GPS, it is possible to obtain a navigation system withnot only high update rate but also good accuracy [2].
Until recently inertial measurement units (IMU) have been to expensive forprivate persons. But lately some low cost micro electro mechanical systems(MEMS) gyros and accelerometers have been introduced on the market. Thishas opened the door for development of low cost inertial navigation systems,suitable for integration with GPS-receivers.
1.2 System Overview 5PSfrag replacements
INS
GPS
Integration
satellite signals
accelerations
rotations
positionvelocityattitude
Figure 1.1: A schematic description of the proposed system with its input andoutput signals.
1.2 System Overview
The purpose of the system described in this thesis is to combine GPS andINS data in an optimal way to obtain a navigation system with both higherupdate rate and smaller position error than the stand alone GPS-receiver. InFigure 1.1 a system overview with its different input and output signals is shown.A common integration method is to let the INS be the major navigation systemand use the position estimates of the GPS-receiver to estimate and correct theerrors in the INS. The integration between INS and GPS is refereed to as a GPSaided INS.
Before integrating the two systems the provided data must be transformedinto a common coordinate system, suitable for integration. The GPS-receiverprovides position estimates in the earth-centered earth-fixed (ECEF) coordinateframe. This is a cartesian coordinate system that rotates with earth and has itsorigin at the earth center of mass. In the case of the INS it is more complicated.The accelerations and angular rates are measured with respect to the inertialframe of reference and resolved in the instrumental frame of the accelerometersand the gyros, respectively. An inertial frame is a coordinate frame in whichNewton’s laws of motion applies, that is in rest or in linear motion. From the in-strumental frame the measurements must be transformed to the platform-frame,which is the coordinate system associated with the platform onto which the ac-celerometers and gyros are mounted. The platform is referred to as the inertialmeasurement unit (IMU). Finally the angular rates, resolved in platform coor-dinates are used to transform the accelerations into ECEF-coordinates, wherethey are processed and integrated with the GPS data.
There are several approaches on how to design a GPS aided INS: tightlycoupled or loosely coupled, closed or open loop, pseudorange or position aided.In this thesis a loosely coupled position aided method is proposed. Essentiallythis means that GPS and INS works independently and that there is no directdata exchange between them, i.e loosely coupled, and that GPS-receiver positionestimates and not the estimated distances to the satellites are used (pseudor-anges). The loosely coupled approach allows the use of a low cost of-the-shelfGPS receiver. The difference between the GPS and INS position estimate isused to drive an extended Kalman filter, housing a model of how the errors inthe INS develops with time. The error states estimated by the filter are fedback to the INS for calibration of its internal states, resulting in a closed loopdesign; See Figure 1.2. A comparison between the tightly and loosely coupledapproach can be found in [4].
1.3 Thesis Outline and Contributes 6
KF
H
INS
+
-
INS input
GPS input
Navigationoutput
Figure 1.2: Loosely coupled position aided closed loop implementation of a GPSaided INS system. KF denotes the extended Kalman filter, and H the mapbetween navigation output and GPS data.
1.3 Thesis Outline and Contributes
The thesis is divided into 8 chapters:
• Chapter 2 gives an introduction to the coordinate frames used throughoutthis thesis.
• Chapter 3 gives a short introduction to the basic physics behind iner-tial navigation. From this the general navigation equations are derived.Further the ECEF navigation dynamics are presented.
• Chapter 4 describes the discretization of the continuous-time ECEF navi-gation equations and the corresponding error model, using zero-order-holdsampling.
• Chapter 5 gives the mathematical background for the integration betweenthe GPS and INS, using an extended Kalman filter.
• Chapter 6 gives a short description of the tailor made hardware that hasbeen developed within this project.
• Chapter 7 presents simulations results of the integrated navigation systemfor a typical driving scenario.
• Chapter 8 presents conclusions and future work.
The main contributions in this thesis are:
• Presentation of discrete ECEF navigation equations suitable for imple-mentation in an ECEF inertial navigation system.
• Presentation of a discrete state-space model for the error dynamics of anECEF inertial navigation system.
• Presentation of an explicit extended Kalman filter algorithm for a GPSaided INS.
• Development of the hardware described in Chapter 6.
1.3 Thesis Outline and Contributes 7
Within the field of integration between INS and GPS data using Kalman fil-ter technics a lot of work has been done and the author would like to highlighttwo of them, which are closely related to the content of this report. First, theconference paper [1] which describes an integration method where the Kalmanfilter propagates the navigation stages and not the errors. Secondly, the techni-cal report [9] in which a closed loop, pseudo range aided approach for integrationbetween GPS and INS data is evaluated.
Chapter 2
Coordinate Systems
In inertial navigation systems the measured quantities is obtained in differentcoordinate frames and need to be transformed into a coordinate frame suitablefor processing. A basic inertial navigation system involves at least four differentframes. The accelerometers measures the platform accelerations with respect tothe inertial frame of reference. The accelerations are resolved in the instrumentalframe of the accelerometers and need to be transformed into platform coordinateframe by a fixed rotation matrix. Gyros measure the platform angular ratesrelative to the inertial frame of reference and resolves this in the instrumentalframe of the gyros, which are transformed into angular rates in the platformframe by a fixed rotation matrix. From the gyro measurements a rotation matrixis calculated which transform the accelerations in the platform-frame into theused navigation frame, where they are processed to determine the velocity andposition of the navigation system.
2.1 Coordinate Frame Definition
This section defines the different coordinate frames and corresponding notationused in this thesis. All coordinate systems are orthogonal Cartesian systems ifnot otherwise stated.
2.1.1 Earth-centered earth-fixed frame (ECEF, e-frame)
This coordinate system has its origin at the center of the earth and rotates withthe earth. The axes directions are defined as follows: the x-axis points towardsthe intersection between the prime meridian and the equator, the z-axis pointsin the direction of the mean polar axis and the y-axis completes the right handcoordinate; See Figure 2.1. The ECEF frame is denoted by the superscript e.
2.1.2 Local geodetic frame (t-frame)
This is the coordinate system often referred to in our daily life as the north, eastand down direction. It will be denoted by the superscript t. It’s determined byfitting at tangent plane to a fixed point of the geodetic reference ellipse. Thispoint will be the origin of the coordinate system and the x-axis points towards
2.1 Coordinate Frame Definition 9
PSfrag replacements
equator
latitude
longitudereferencemeridian
local geodetic frame
North
East
Down
g
λ
ϕ
zi ≡ ze
yi
xi
ye
xe
ωie
Figure 2.1: Relations between ECEF(e-frame)-, local geodetic(t-frame)- andinertial(i-frame)-frame.
2.2 Coordinate Frame Transformation 10
the true north, the y-axis to the east and the z-axis completes the right handcoordinate system by pointing towards the earth’s interior.
2.1.3 Inertial frame (i-frame)
An inertial frame is a coordinate frame in which Newton’s laws of motions apply.Therefore the inertial frame is not accelerating, but can be in a linear motion.The origin of the inertial coordinate system is arbitrary, and the coordinate axismay point in any three perpendicular directions. It’s often preferable to let theorigin of the inertial frame consist with the earth center of mass. This framewill be referred to as the i-frame and will be denoted by the superscript i andshould not be confused with the ideal inertial frame described above since thegravity force applies. As will be shown in the sequel (Section 2.2.1) a vector p inthe i-frame is linearly related (by a rotation matrix) to a vector in the e-frame,and vice verse.
2.1.4 Body frame (b-frame)
The body frame is the coordinate system associated with the vehicle. The co-ordinate system has its origin at the center of gravity of the vehicle and thex-axis points in the forward direction, the z-axis down through the vehicle andthe y-axis completes the right hand coordinate system. See Figure 2.2. Thisframe will be denoted by the superscript b. In some applications the instrumen-tation platform is not aligned with the body frame, and thus a (orthonormal)platform frame is needed as well. The platform frame is the coordinate systemof the platform which the accelerometers and the gyros are mounted on. How-ever throughout this thesis the platform is assumed to have its coordinate axesperfectly aligned with the body coordinate axes, and therefore no distinctionbetween the two frames will be made.
The inertial sensors resolves their measurements along the sensitivity axes ofthe instruments. The instrument sensitivity axes of the gyros and accelerometersspans the axes of the gyro and accelerometer instrumental frames, respectively.The instrumental frames can not be assumed to be orthogonal and identificationof the misalignment matrices for transformation between the instrumental andplatform frame is the primary objective of the IMU alignment, see [6].
2.2 Coordinate Frame Transformation
In navigation systems it’s often necessary to transform a vector from one co-ordinate system into another. This section describes two different methods toderive a mathematical expression for the rotation matrix relating two Cartesian,orthogonal coordinate systems. Further, small angle rotations and derivativesof rotation matrices are studied.
2.2.1 Projection
The first method presented on how to find a mathematically expression for therotation matrix relating to coordinate systems is based upon projections of avector onto the orthonormal bases of the two coordinate systems. Mathematical
2.2 Coordinate Frame Transformation 11
PSfrag replacements
roll
yawpitch
yb
xb
zb
ωbibx
ωbiby
ωbibz
Figure 2.2: Body coordinate frame.
PSfrag replacements
p
ia
ib
ja
jb
ka
kb
Figure 2.3: By projection of vector p onto the bases of the two orthonormal coor-dinate systems the directional cosine matrix relating the two coordinate systemscan be found.
2.3 Plane Rotations 12
this can be described as follows. Let: ia, ja,ka and ib, jb,kb be the unitvectors spanning two orthonormal coordinate systems having the same origin,see Figure 2.3. Vector p can then be written in terms of the unit vectorsspanning coordinate system A, that is
p = xa ia + ya ja + za ka (2.1)
Projection of p onto the unit vectors spanning coordinate system B and usingthe fact that the bases of the coordinate system are orthogonal results in
xb =< ib,p >=< ia, ib > xa+ < ja, ib > ya+ < ka, ib > za
yb =< jb,p >=< ia, jb > xa+ < ja, jb > ya+ < ka, jb > za
zb =< kb,p >=< ia,kb > xa+ < ja,kb > ya+ < ka,kb > za
where scalar product < ik, jl >= cos(αik,jl). Here αik,jl denotes the anglebetween vector ik and jl. In matrix form this can be written as
pb = Rba pa (2.2)
where
Rba =
cos(αia,ib) cos(αja,ib) cos(αka,ib)cos(αia,jb) cos(αja,jb) cos(αka,jb)cos(αia,kb
) cos(αja,kb) cos(αka,kb
)
(2.3)
The rotation matrix Rba is a so called directional cosine matrix. Although Rb
a
has nine elements, it has only three degrees of freedom and can be uniquelydescribed by the three Euler angles, in the sequel gathered in the vector θ [5].
The directional cosine matrix is an orthonormal matrix, that is (Rba Rb
a
T=
Rba
TRb
a = I). Hence, the inverse of the rotation matrix is the same as itstranspose.
2.3 Plane Rotations
2.3.1 Introduction
Plane rotations are a convenient way to mathematically express the vector trans-formation between two coordinate frames with common origin, where the secondcoordinate system can be related to the first by a rotation around a vector v.In the case of v being one of the coordinate axes the rotation matrix takes asimplified form. The rotation matrix between two coordinate systems relatedby a sequences of plane rotations can be found by multiplying the plane rotationmatrices in the order of the rotations.
By plane rotations the directional cosine matrix relating coordinate systemA and B in Figure 2.4 can be found in the following manner. First rotatingcoordinate system A around its z-axis with α1 radians to align the new x′-axiswith the projection of xb-axis onto the plane spanned by x′-axis and the y′-axis.See Figure 2.5(a) and 2.5(d). The rotation can be described as
x′
y′
z′
=
cos(α1) sin(α1) 0− sin(α1) cos(α1) 0
0 0 1
xa
ya
za
(2.4)
2.3 Plane Rotations 13
PSfrag replacements
xb
yb
zb
xa
ya
za
Figure 2.4: Coordinate system A and B referred to in the derivation of therotation matrix Rb
a through of plane rotations.
2.3 Plane Rotations 14
Next, rotating the new coordinate system, denoted with prime with α2 radiansaround the y′-axis, resulting in that the x′′-axis becomes aligned with the xb-axis. This is illustrated in Figure 2.5(b) and 2.5(e). The rotation is describedby
x′′
y′′
z′′
=
cos(α2) 0 − sin(α2)0 1 0
sin(α2) 0 cos(α2)
x′
y′
z′
(2.5)
Then rotating the new coordinate system α3 radians around the x′′-axis re-sults in that the y′′- and z′′-axis aligns the yb- and zb-axis, respectively. SeeFigures 2.5(c) and 2.5(f). The last plane rotation is described by
xb
yb
zb
=
x′′′
y′′′
z′′′
=
1 0 00 cos(α3) sin(α3)0 − sin(α3) cos(α3)
x′′
y′′
z′′
(2.6)
The rotation matrix from coordinate system A to B is found by multiplyingthe rotation matrices in equations (2.4)–(2.6) in the order of rotations. Therotation matrix Rb
a reads
Rba = (2.7)
c(α1)c(α2) s(α1)c(α2) −s(α2)−s(α1)c(α3) + c(α1)s(α2)s(α3) c(α1)c(α3) + s(α1)s(α2)s(α3) c(α2)s(α2)s(α1)s(α3) + c(α1)s(α2)c(α3) −c(α1)s(α3) + s(α1)s(α2)c(α3) c(α2)c(α2)
Here s(·) and c(·) denotes the sine and cosine operation, respectively. Therotation matrices used throughout this thesis can be found below.
2.3.2 Rotation matrices
It is clear by now that coordinate rotations are essential parts of inertial nav-igation. Throughout this report the most extensively used coordinate rotationmatrix is the ECEF to body frame rotation matrix Rb
e. It is possible to find anexpression for the rotation matrix Rb
e directly in terms of the three Euler anglesrelating the ECEF and body coordinate systems. However, a more convenientmethod is to divide the rotation matrix Rb
e into two coordinate rotations asRb
e = Rbt Rt
e. That is, firstly a coordinate rotation from the ECEF frame to thelocal geodetic frame and secondly a coordinate rotation from the local geodeticframe to the body frame, described by Rt
e respectively Rbt . The definitions of
the local geodetic to body frame and the ECEF to local geodetic frame rotationmatrices are found in Table (2.1) and (2.2), respectively.
2.3.3 Small angle rotation
When two coordinate systems differ only by small angles rotations the corre-sponding directional cosine matrix can be approximated being a linear functionof the rotation angles. This is a very useful property in the derivation of the INSerror equations and derivative calculations of the rotation matrix. Let coordi-nate system A and B be differently oriented by the small angle rotations βx,βy
2.3 Plane Rotations 15
PSfrag replacements
xa
ya
za
y′
x′
α1
α1
(a) Rotation around z.
PSfrag replacementsx′
y′
z′
x′′
z′′α2
α2
(b) Rotation around y′.
PSfrag replacements
x′′
y′′
z′′y′′′
z′′′
α3
α3
(c) Rotation around x′′.
PSfrag replacements
xa
ya
y′
x′
α1
α1
(d) Rotation around z.
PSfrag replacements
x′
z′
x′′
z′′
α2
α2
(e) Rotation around y′.
PSfrag replacements y′′ z′′
y′′′
z′′′
α3
α3
(f) Rotation around x′′.
Figure 2.5: Plane rotations.
Rbt =
c(ψ) c(θ) s(ψ) c(θ) −s(θ)−s(ψ) c(φ) + c(ψ) s(θ) s(φ) c(ψ) c(φ) + s(ψ) s(θ) s(φ) c(θ) s(φ)s(ψ) s(φ) + c(ψ) s(θ) c(φ) −c(ψ) s(φ) + s(ψ) s(θ) c(φ) c(θ) c(φ)
where:
φ - roll [radians]
θ - pitch [radians]
ψ - yaw [radians]
c(·) - cosine operator
s(·) - sine operator
Table 2.1: Definition of local geodetic- to body- frame rotation matrix.
2.4 Rotating Coordinate Frame 16
Rte =
−s(λ) c(φ) −s(λ) s(φ) c(λ)−s(φ) c(φ) 0
−c(λ) c(φ) −c(λ) s(φ) −s(λ)
where:
λ - latitude [radians]
φ - longitude [radians]
c(·) - cosine operator
s(·) - sine operator
Table 2.2: Definition of ECEF- to local geodetic- frame rotation matrix.
and βz around the axis of system A. For small x the approximations cos(x) ≈ 1and sin(x) ≈ x are valid by aid of Taylor series expansion. Applying this to(2.7), the small angle rotation matrix can be approximated as
Rba ≈
1 βz −βy
−βz 1 βx
βy −βx 1
= I + Ξβ (2.8)
where Ξβ is the skew symmetric matrix representation of β = [βx, βy, βz]∗,
defined such that
p × β = Ξβ p (2.9)
Here p is an arbitrary 3 × 1 dimensional vector. The matrix Ξβ reads
Ξβ =
0 βz −βy
−βz 0 βx
βy −βx 0
(2.10)
Using the properties of the cross product, it is straight-forward to show thatΞβ p = −Ξp β, where Ξp is the skew symmetric matrix representation of thevector p.
2.4 Rotating Coordinate Frame
As the attitude of the vehicle changes, so does the relative angle between someof the coordinate frames in the navigation system. Hence, so does the rotationmatrices relating this coordinate frames. The rate of change of the rotationmatrix corresponds to the first order derivative of the directional cosine matrix.Assuming that angular velocity ωa
ba between coordinate system A and B isconstant in the interval t to t + ∆t, then using the result from small anglesrotations the rotation matrix at time t+ ∆t can be written as
Rba(t+ ∆t) = Rb
a(t) (I + Ωaba ∆t) (2.11)
where Ωaba is the skew symmetric representation of the angular rates ωa
ba. Thematrix Ωa
ba reads
2.4 Rotating Coordinate Frame 17
Ωaba =
0 −ωabaz
ωabay
ωabaz
0 −ωabax
−ωabay
ωabax
0
(2.12)
Using the definition of derivative, the rate of change in the rotation matrixbecomes
Rba(t) = lim
∆t→0
Rba(t+ ∆t) − Rb
a(t)
∆t
= lim∆t→0
Rba(t) (I + Ωa
ba ∆t) − Rba(t)
∆t
= Rba(t)Ωa
ba (2.13)
where (2.11) was used in the second equality. Not surprisingly the rate ofchange of the rotation matrix is a function of both the current orientation andthe angular rate between the coordinate systems.
Chapter 3
Inertial NavigationEquation
In this chapter the fundamental equation of inertial navigation is derived. To-gether with the relationships for frame rotations derived in Chapter 2 the funda-mental equation of inertial navigation is used to deduce the general continuous-time navigation equation. Further, the ECEF-frame navigation equations areintroduced and the physical interpretation and significance of the different termsare discussed. By mechanization of the ECEF-frame navigation equations a lin-ear model of how the sensor errors are related to the errors in the navigationoutput is found. The linear error model will later be used in the integrationbetween the INS and GPS-receiver.
3.1 Fundamental Equations of Inertial Naviga-tion
According to Newton’s first law of motion an object tends to keep its initialspeed and attitude unless affected by an unbalanced force. Hence, a changein motion is a result of a force being applied to overcome the objects inertia.Two types of forces determine the motion of a vehicle: gravity and inertial force.
3.1.1 Gravity force
Newton’s universal law of gravity states that the force fg, due to gravitionalattraction between two masses m1 and m2, see Figure 3.1 is
fg =Gm1m2
|r|3r (3.1)
where G denotes the universal gravity constant and r the position vector ofmass m2 with respect to the center of mass m1. Let mass m1 be equal to theearth mass M and m2 equal to the vehicle mass m. Assuming the vehicle is ata far distance from the earth, then the vehicle sensed gravity force becomes
3.1 Fundamental Equations of Inertial Navigation 19PSfrag replacements
m1 m2fg fg
|r|
Figure 3.1: Gravity attraction between two masses m1 and m2 at the distance rfrom each other
fg = −GM m
|r|3r (3.2)
Rewriting this as force per vehicle unit mass yields
g4=
fgm
= −GM
|r|3r (3.3)
where g denotes the acceleration of gravity.
3.1.2 Inertial force
Newton’s second law states that the acceleration of an object is directly propor-tional to the magnitude of the net force, in the same direction as the net forceand inversely proportional to the mass of the object. In the case the inertialforce being the only force acting upon the vehicle
fI = m s (3.4)
Here fI is the inertial force applied to the vehicle of mass m to produce theacceleration s. The acceleration s will henceforth be refereed to as the specificforce.
3.1.3 Equations of motion
In the previous sections the kinematic equations for an object were derivedin the absent of either the inertial force fI or the gravity force fg. In realitythe motion of an object is mostly a result of both these forces being present.The total force, ftot acting upon the object can be expressed as the sum of thegravitional and inertial force, that is
ftot = fg + fI (3.5)
Substituting the left hand side with the kinematic acceleration in the i-frametimes the mass m of the vehicle gives
m ri = fg + fI (3.6)
Replacing the right hand side of (3.6) with (3.2) and (3.4) gives
m ri = −GM m
|ri|3ri +m si (3.7)
Finally the fundamental equation of inertial navigation is obtained by dividingwith the mass m on both sides, resulting in
3.2 Navigation Equations 20
ri = gi + si (3.8)
The equation states that the kinematic acceleration ri is equal to the sum ofthe gravity acceleration gi and the specific force si.
3.2 Navigation Equations
The navigation equations are a set of differential equations describing the rela-tionship between the INS outputs (that is position, velocity and attitude) andthe inputs in terms of acceleration and angular rates. Below, they are first de-rived for an arbitrary navigation frame and then for the ECEF-frame, used inthe GPS aided INS approach.
3.2.1 General Navigation Equations
Let ra and ri be two position vectors in the arbitrary a-frame and the i-frame,respectively. Remember that the i-frame is an inertial frame with origin at theearth center of mass, so the gravity force applies. The position vectors can berelated to each other through the directional cosine matrix Ri
a as
ri = Ria ra + qi (3.9)
where qi is a vector pointing from the origin of the i-frame to the origin of thea-frame. Differentiating twice with respect to time yields
ri = Ria ra + 2Ri
a ra + Ria ra + qi (3.10)
The first order derivative of the directional cosine matrix was derived in Sec-tion (2.4), and reads
Ria = Ri
a Ωaia (3.11)
where Ωaia was derived in (2.12). Differentiating (3.11) once more yields
Ria = Ri
a Ωaia + Ri
a Ωaia
= Ria(Ωa
iaΩaia + Ωa
ia) (3.12)
Inserting equations (3.11) and (3.12) into (3.10) results in
ri = Ria ra + 2Ri
aΩaia ra + Ri
a(Ωaia + Ωa
iaΩaia) ra + qi (3.13)
Finally substituting ri with gi + si and multiplying both sides with rotationmatrix Ra
i yields
ga + sa = ra + 2Ωaia ra + (Ωa
ia + ΩaiaΩ
aia) ra + qa (3.14)
where pa = Rai pi for an arbitrary vector p was used. Further, Ra
i Ria = I. This
is the general differential equation for navigation in an arbitrary a-frame [5].Worth noting is that the derived equation is exact. Note also that in manycoordinate frames and navigation systems some terms are small and may beneglected, resulting in a simplified set of equations.
3.2 Navigation Equations 21
3.2.2 ECEF-frame navigation equations
An ECEF navigation coordinate system implementation of the navigation equa-tions has the advantage of producing position estimates in the same coordinatesystem as used by the GPS system, which simplifies the integration betweenthe two systems [1]. Substituting the arbitrary a-frame with the ECEF-frame,denoted by the superscript e results in
ge + se = re + 2Ωeie re + Ωe
ie re + ΩeieΩ
eier
e + qe (3.15)
where qe = 0 and the skew-symmetric matrix Ωeie takes the simplified form
Ωeie =
0 −ωie 0ωie 0 00 0 0
(3.16)
This is due to the fact that the ECEF-frame (e-frame) and i-frame only differsin a rotation around the z-axis of the ECEF-frame. The earth rotational raterelative to the inertial frame is approximated to ωie ≈ 7.292115 · 10−5 rad/s [5].
The physical interpretation and significance of the different terms in (3.15)are as follows. The first term of the right hand side is the acceleration asobserved in the ECEF-frame. The second term is the Coriolis acceleration [6].The third term is the Euler acceleration and can be omitted, since the change inthe earth rotational rate is negligible [6]. The last term is the centripetal forcedue to the earth rotation, which can be neglected, due to low sensitivity of theused sensors. The simplified ECEF navigation equation reads
re = −2Ωeie re + ge + se (3.17)
The specific force s is observed in the body frame and must be transformed tothe the ECEF-frame by the rotation matrix Re
b before used in the navigationequation. Actually, the specific force is measured in the platform coordinatesbut since the coordinate axis of the IMU-platform and the body coordinatesystem is assumed to be perfectly aligned the body and platform coordinateframes are equivalent. Introducing the velocity vector ve = re equation (3.17)can be written as two first order differential equations, which together withthe relationship for rotating coordinate frames derived in Section 2.4 gives theECEF navigation dynamics, that is
ve
ve
Reb
=
re
−2Ωeie ve + ge + Re
b sb
Reb Ωb
eb
(3.18)
These are the continuous-time versions of the equations implemented in theconsidered ECEF inertial navigation system. According to [9], the rotationrates, ωe
eb between the ECEF-frame and the body-frame can be obtained asfollows
ωbeb = ωb
ib − Rbe ω
eie (3.19)
where the angular rate ωbib between the body frame and the inertial frame is
measured by the gyros, and ωeie is the earth rotational rate relative to the inertial
frame. A graphical interpretation of how the ECEF-navigation equations areused in an ECEF INS implementation is shown in Figure 3.2 [10].
3.3 ECEF Navigation Error Model 22
+
++ -
PSfrag replacements
Acc
Gyros
sb se
ωbib
ωbeb
ωbie
ωeie, earth
ωeie, earth
rotation
rotation
re re re
ge
Reb
Rbe
Gravity
IMUCoriolis force
−2ωeie × ve
∫ t+∆t
t(·)dt
∫ t+∆t
t(·)dt
∫ t+∆t
t(·)dt
Navigation
Position
Navigation
Attitude
λ latitudeϕ longitudeh height
φ roll
θ pitch
ψ yaw
Figure 3.2: A block diagram of a ECEF INS
3.3 ECEF Navigation Error Model
Even though the inertial instruments have been calibrated the measured IMUsignals will be erroneous, due to environmental variations and instrument degra-dation. As a result, there are biases in the position and velocity estimates as wellas a misalignment between the estimated and true coordinate rotation matrix.In a GPS aided INS the position error is estimated by comparing the positioncalculated by the GPS-receiver with that of the INS. The estimated positionerror is then related to velocity, attitude and sensor errors through as set ofdifferential equations, refereed to as the navigation error equations. The errorequations are derived below.
3.3.1 ECEF navigation error equations
The ECEF navigation error equations will be derived by mechanization of theequations in (3.18). Mechanization means that all terms in the navigation equa-tions are substituted with the corresponding estimated or measured quantities.By Taylor series expansion and neglecting all second and higher order terms alinear model of how the different errors are related is found.
Let tilde, (·) denote a measured and hat (·) a computed value. Then themechanized version of the velocity equation in (3.18) reads
˙ve = Reb sb + ge − 2Ωe
ie ve (3.20)
Thus, the accelerations sb is measured by the accelerometers, and Ωeie is a
known constant matrix. All other quantities are calculated ones. Introducingthe vector ε denoting the small angles rotations that aligns the computed andtrue coordinate rotation matrix, the computed and true rotation matrix can berelated as
Reb = (I − Ωε)R
eb (3.21)
where the skew-symmetric matrix Ωε reads
3.3 ECEF Navigation Error Model 23
Ωε =
0 −ε3 ε2ε3 0 −ε1−ε2 ε1 0
(3.22)
Define the error as the actual value minus the computed value, respectively theactual value minus the measured value. Then the mechanized equation can bewritten in terms of true values and errors as
ve − δve = (I − Ωε)Reb (sb − δsb) + ge − δge − 2Ωe
ie (ve − δve) (3.23)
where δve denotes the error in velocity, et cetera. Expansion of (3.23) gives
ve − δve = se + ge − 2Ωeie ve
︸ ︷︷ ︸
=ve
−Ωε se − Reb δs
b + Ωε Reb δs
b
︸ ︷︷ ︸
≈0
−δge + 2Ωeie δv
e
(3.24)The first three terms on the right hand side of equation (3.24) can be identifiedto correspond to ve and thus cancels out. Neglecting the second order termΩε Re
b δsb, a linear differential equation for the velocity error is obtained, that
is
δve = Ωε se + Reb δs
b + δge − 2Ωeie δv
e (3.25)
The term Ωε se = −Se ε, where Se is the skew symmetric matrix representationof the specific fores se in ECEF coordinates. Hence, the velocity error can becalculated as
δve = −Se ε+ Reb δs
b + δge − 2Ωeie δv
e (3.26)
where
Se =
0 −sez se
y
sez 0 −se
x
−sey se
x 0
(3.27)
A linear model for the small angle errors ε in the computed coordinate rota-tion matrix can be found through mechanization of the ECEF attitude equation,that is the last equation in (3.18). The mechanized ECEF attitude equationreads
˙Re
b = Reb Ωb
eb (3.28)
Differentiating equation (3.21) once with respect to time and inserting on theleft hand side of the mechanized attitude equation yields
(I − Ωε) Reb − Ωε Re
b = Reb Ωb
eb (3.29)
Expansion of the right hand side results in
(I − Ωε) Reb − Ωε Re
b = (I − Ωε) Reb Ωe
eb︸ ︷︷ ︸
=Reb
−(I − Ωε︸ ︷︷ ︸
≈I
)Reb δΩ
eeb (3.30)
3.3 ECEF Navigation Error Model 24
Identifying Reb Ωe
eb to be equal to Reb and approximating I − Ωε ≈ I, results in
the following equation for the attitude errors.
Ωε = Reb δΩ
beb Rb
e (3.31)
Using the fact that skew-symmetric matrices Ωε and Ωbeb are totaly described
by the vector ε respectively ωbeb, the attitude error equation can be written in
vector form as [5]
ε = Reb δω
beb (3.32)
Here the attitude error is a linear function of the error in the computed ECEFto body frame angular rate δωb
eb. The angular rate error can be rewritten interms of the measurement error of the gyros δωb
ib and the error in the earthrotational rate, when expressed in the body coordinates δωb
ie. The relationshipreads
δωbeb = δωb
ib − δωbie (3.33)
The earth rotational rate is assumed to be perfectly known when expressedin ECEF-coordinates, but when converted to body-coordinates errors are intro-duced due to the misalignment between the computed and true rotation matrix.The error is found through the following mechanization
ωbie = Rb
e ωeie (3.34)
Here the mechanized ECEF to body frame rotation matrix is found as
Rbe = (Re
b)−1
= ((I − Ωε)Reb)
−1
= Rbe (I − Ωε)
−1 (3.35)
Expanding (I − Ωε)−1 into geometric series and neglecting second and higher
order terms the mechanized directional cosine matrix can be approximated as
Rbe ≈ Rb
e (I + Ωε) (3.36)
Inserting this into (3.34) and rewriting the equation in terms of actual valuesand errors results in
ωbie − δωb
ie = Rbe (I + Ωε)ω
eie
δωbie = −Rb
e Ωε ωeie
δωbie = Rb
e Ωeie ε (3.37)
Substituting this into (3.32) yields the final attitude error equation, that is
ε = Reb δω
bib − Ωe
ie ε (3.38)
Collecting the derived relationships for the attitude and velocity error the ECEF-navigation error equations are as follows
3.3 ECEF Navigation Error Model 25
δve
δve
ε
=
δre
−Se ε+ Reb δs
b + δge − 2Ωeie δv
e
Reb δω
bib − Ωe
ie ε
(3.39)
So far the equations describing how the errors in position, velocity and attitudeare related to measurement errors have been derived. Using the knowledge abouthow the accelerometer and gyro errors develops with time a model for the ECEFnavigation error dynamics can be found. There are several approaches for howthe measurement errors can be modelled, see [6]. Here the measurement errorsare modelled as a random level, and white Gaussian noise, describing the biasand the measurement noise, respectively. Assuming that the IMU sensors arethe only noise sources in the system the noise enters the system equations onlythrough the attitude and velocity state, that is the two last equations in (3.18).
A convenient way to describe a dynamic model is to write it in terms of astate space model. Defining the error state vector δx(t) and the measurementnoise vector uc(t) as
δx(t) =[δre ∗ δve ∗ ε∗ δf b∗ δωb ∗
ib
]∗(3.40)
uc(t) =[
u∗acc(t) u∗
gyro(t)]∗
(3.41)
where uacc(t) denotes the accelerometer noise and ugyro(t) the gyro noise. Ne-glecting gravity errors, the navigation error state model becomes
δx(t) = F(t) δx(t) + G(t)uc(t) (3.42)
where F(t) is the 15 × 15 matrix
F(t) =
03 I3 03 03 03
03 −2Ωeie −Se Re
b 03
03 03 −Ωeie 03 Re
b
03 03 03 03 03
03 03 03 03 03
(3.43)
and G(t) is of size 15 × 6
G(t) =
03 03
Reb 03
03 Reb
03 03
03 03
(3.44)
The error equations are time-varying, since Reb depends on the attitude and
Se on the acceleration of the vehicle. Recall that Reb is the rotation matrix,
transforming a vector from the body-frame into the ECEF-frame. Further Se
and Ωeie are the skew symmetric matrix representation of the specific force and
the earth rotational rate, respectively. In (3.43) I3 (03) denotes the unity (zero)matrix of order 3.
The constructed IMU platform houses three separate accelerometers andgyros, therefore the sensors’ noises are assumed uncorrelated. However, theaccelerometers respectively the gyros are of the same model and thus assumed
3.3 ECEF Navigation Error Model 26
to have the same noise characteristics. Let σ2acc and σ2
gyro denote the variance ofthe accelerometer and the gyro noise, respectively. Then the covariance matrix,Qc(t) of the Gaussian measurement noise uc(t) in (3.41) is defined as
Euc(t)u∗c(τ) =
[σ2
acc I3 03
03 σ2gyro I3
]
δ(t− τ)
4= Qc(t− τ) (3.45)
where δ(t) is the Kronecker delta.So far the continuous-time versions of the ECEF navigation equations have
been derived from Newton’s laws of gravity and motion. Through mechaniza-tion of the navigation equations a linear state space model was found, describ-ing how the errors in position, velocity, attitude and biases are related. Nextthe continuous-time ECEF navigation equations and the model for the errordynamics will be discretized, making them suitable for implementation in theconsidered GPS aided inertial navigation system.
Chapter 4
Discretization
4.1 Discretization
In this chapter the derived navigation equations and error dynamics are sampled,making them suitable for a discrete implementation. First the zero-order-holdsampling of the navigation equations are described, where special care is taken topreserve the properties of the rotation matrix. Next the model of the navigationerror dynamics is discretized.
4.1.1 Discrete time navigation equations
Introducing the state vector d(t) housing the position and velocity, and theinput vector n(t) housing the accelerations in ECEF coordinates, that is
d(t) =[
re ∗ ve ∗]∗
(4.1)
and
n(t) = −2Ωeie ve + ge + Re
b se (4.2)
Then the two first navigation equations in (3.18) can be written in state spaceform as
d(t) = Ad(t) + B(t)n(t) (4.3)
where A is the 6 × 6 matrix
A =
[03 I3
03 03
]
(4.4)
and B is the 6 × 3 matrix
B =
[03
I3
]
(4.5)
Having a continuous-time equation as in (4.3) with a known solution at time t0,the solution at a time t > t0 can be represented as [7, 8].
d(t) = Φ(t, t0)d(t0) +
∫ t
t0
Φ(t, τ)B(τ)n(τ)dτ (4.6)
4.1 Discretization 28
where the state transition matrix Φ(t, t0) is defined as the unique solution to
∂Φ(t, τ)
∂t= AΦ(t, τ) (4.7)
If the state transition matrix A is time invariant, the homogenous differen-tial equation (4.7) has the solution of the matrix exponential function, that isΦ(t, τ) = exp (A · (t − t0)) [8]. Using the power series definition of the matrixexponential, the state transition matrix between time kTs and (k+ 1)Ts can becalculated as
Φ((k + 1)Ts, kTs) = I6 + ATs +A2T 2
s
2!+ ...
= I6 + ATs (4.8)
where the last equality is a result of An = 06 for all n > 1. Having the solutionto the state transition matrix Φ((k+1)Ts, kTs) and assuming n(t) to be constantbetween the sample instances an approximative solution to the integral on theright hand side in (4.6) can be found as
∫ t
t0
Φ(t, τ)Bn(τ)dτ =
∫ (k+1) Ts
k Ts
(I + ATs)Bn(τ)dτ
≈ Ts Bn(k Ts) (4.9)
Hence, the zero-order-hold sampling of the position and velocity ECEF naviga-tion equations becomes
[re
k+1
vek+1
]
=
[I3 Ts I3
03 I3
] [re
k
vek
]
+ Ts
[03×1
Reb,k sb
k − 2Ωeie ve
k + ge
]
(4.10)
When discretizing the attitude equation in (3.18) care must be taken so thatthe orthogonality constrains of the directional cosine matrix are maintained.Let Ts denote the sample time and assume that Ωb
eb is constant. Then thematrix taking the solution of the attitude differential equation from time k Ts
to (k+1)Ts is exp(Ωbeb Ts). Hence, the attitude equations can be approximated
by
Reb,k+1 = Re
b,k eΩb
eb Ts (4.11)
By expanding the matrix exponential into a (n, n) Pade approximation theorthogonality constrains of the rotation matrix are preserved [1]. Using a (2, 2)Pade approximation the discrete attitude equation becomes
Reb,k+1 = Re
b,k(2I3 + Ωbeb Ts)(2I3 − Ωb
eb Ts)−1 (4.12)
Equations (4.10) and (4.12) are the discrete ECEF navigation equations imple-mented in the INS.
4.1 Discretization 29
4.1.2 Discrete time error equations
In the state space model for the ECEF navigation dynamics both F(t) andG(t) are time varying. If the sample rate is chosen high compared to the rateof change in F(t) and G(t), they may be approximated as the constant matri-ces between the samples. Applying the same procedure as for the navigationequations the discrete state space model for the error dynamics becomes
δxk+1 = Ψkδxk + ud,k (4.13)
where the state transition matrix is approximated as
Ψk = Φ((k + 1)Ts, kTs)
≈ eF(kTs)Ts (4.14)
Expanding the matrix exponential into a power series neglecting second andhigher order terms yields
Ψk ≈ I + F(kTs)Ts (4.15)
The discrete-time process noise, uk is
ud,k =
∫ (k+1)Ts
kTs
Φ((k + 1)Ts, s)G(s)uc(s)ds (4.16)
Since ud,k is a linear combination of Gaussian noise, it is Gaussian distributedand described by its first and second order moments. The mean of ud,k iszero, since uc(t) is assumed zero mean. Applying the definition of covarianceand assuming Ts small, the covariance of the discrete-time noise Qd,k can beapproximated as [5]
Qd,k ≈ G(kTs)Qc(kTs)G∗(kTs)Ts
= diag(03, σ2acc I3, σ
2gyro I3,06) (4.17)
where diag(·) denotes a block diagonal matrix. The last equality is a result oforthonormality property of the rotation matrix, Re
b and that Qc(t) is a diagonalmatrix.
The definition of the state observation equation is straightforward since theGPS position estimate is used, and not the pseudo ranges. Let δy be thedifference between the GPS and INS position estimate and wd,k the error in theGPS position estimates. Then the observation equation can be written as
δyk = Hkδxk + wd,k (4.18)
with the state observation matrix, Hk of size 3 × 15 defined as
Hk =
[I3,03×12], k = n `, n = 1, 2, 3...03×15, otherwise
(4.19)
where ` denotes the ratio between the INS and GPS sample frequency.
Chapter 5
Integration of GPS and INS
In this Chapter a method for combining GPS and INS data is derived. Theproposed method utilizes the complimentary properties of the two systems andcombine them, resulting in a navigation system with both higher update rate andaccuracy than the stand alone GPS-receiver. Further, the integrated navigationsystem will be able to provide the user with position estimates even in theabsences of satellite signals. In the proposed method the INS acts as the majornavigation system and the position estimates from the GPS is used to estimatethe errors in the INS. The integration method is referred to as a GPS aidedINS.
When GPS-data is available the system compares the position estimate fromthe GPS-receiver with the position calculated by the INS. The estimated po-sition errors are fed to a Kalman filter housing the error model for the ECEFnavigation error dynamics derived in Chapter 3. A derivation of the generalKalman filter equations can be found in Appendix A. The filter estimates thenavigation system errors, which are used to calibrate the INS. The system isshown in Figure 1.2. The described system is actually an indirect extendedKalman filter, where the navigation equations are linearized around the outputof the INS.
5.1 The Extend Kalman Filter
The discrete non-linear navigation equations (4.10) and (4.12) can be writtenas
zk+1 = c(zk,ak) + u′k (5.1)
where zk is the navigation system outputs: position, velocity and Euler anglesθk defining the rotation matrix Re
b, that is the 9-element vector
zk =[
re ∗k ve ∗
k θ∗k]∗
(5.2)
Further, the 6-element vector ak contains the inputs to navigation system, ac-celerations and angular rates, that is
ak =[
f b ∗k ωb ∗
ib,k
]∗(5.3)
5.1 The Extend Kalman Filter 31
The vector u′k is the noise in measurements of the navigation input. Lineariza-
tion of the navigation equations (5.1) are first done around a known nominaltrajectory, resulting in a linear model for the perturbations away from the truetrajectory. To the linear error equations the standard Kalman filter is applied.Then substituting the nominal trajectory with that of the INS estimated tra-jectory results in an extended Kalman filter. Consider the true state vector zk
and the measured input ak to the system written as
zk = znomk + δzk (5.4)
ak = anomk + δak (5.5)
where znomk and anom
k are the nominal trajectory and input. The quantity,δzk is the perturbation away from the true trajectory and δak the bias of themeasurements. Assuming δzk and δak are small and applying a first orderTaylor series expansion to c(z,a), equation (5.1) can be approximated as
znomk+1 + δzk+1 ≈ c(znom
k ,anomk )
+ C1,k δzk + C2,k δak + u′k (5.6)
where
C1,k =∂c(z,a)
∂z
∣∣∣∣z=znom
k
C2,k =∂c(z,a)
∂a
∣∣∣∣a=anom
k
(5.7)
The Jacobians’ of c(z,a) are updated with nominal trajectory and input foreach sample. Choosing znom
k and anomk to fulfill the deterministic differential
equation
znomk+1 = c(znom
k ,anomk ) (5.8)
and substituting this into equation (5.6) results in a linear model for the errorδzk, that is
δzk+1 = C1,k δzk + C2,k δak + u′k (5.9)
Noting that δxk =[δz∗k δa
∗k
]∗it becomes clear that C1,k and C2,k corresponds
to the upper part of the navigation error state transition matrix Ψk. The lower6 × 6 block matrix of Ψk is a description of how the IMU biases δak developswith time. Since this is a linear model the standard Kalman filter equations canbe applied to estimate δxk [8]. The Kalman filter equations read
[δz−k+1
δa−k+1
]
= Ψk
[δzk
δak
]
(5.10)
[δzk
δak
]
=
[δz−kδa−
k
]
+Kf,k
(
yk − Hk
[znom
k
anomk
]
− Hk
[δz−kδa−
k
])
(5.11)
Kf,k = P−k H∗
k (Hk P−k H∗
k + Rd,k)−1 (5.12)
Pk = P−k − Kf,k Hk P−
k (5.13)
P−k+1 = Ψk Pk Ψ∗
k + Qd,k (5.14)
5.1 The Extend Kalman Filter 32
Here δak denotes the estimated biases in the measurements, et cetera. Variableswith a minus sign, (·)− are predicted values. The matrix, Rd,k is the covariancematrix of the error, wd,k in the GPS position estimates, yk. Now adding znom
k
to both sides of equation (5.10) and substituting znomk with current estimate
in all equations results in an extended Kalman filter, where the time and filterupdate for the estimates are given below
z−k+1 = c(zk, ak) (5.15)
δa−k+1 = [Ψk]10:15, 10:15 δak (5.16)
[δzk
δak
]
=
[δz−kδa−
k
]
+ Kf,k
(
yk − Hk
[z−k
06×1
])
(5.17)
The solution to (5.15) is provided by the INS, since this corresponds to thenavigation equations. The vector ak is the estimate of the true IMU-signalobtained by subtracting the estimated bias from the measured IMU signal. Theonly problem is the time update of navigation state errors δzk. If the estimatednavigation error states are fed back to the INS for correction of the INS internalstages, the corresponding error states can be set to zero [1]. Hence, δz−k = 09×1.The final algorithm for the integration is given in Table 5.1.
No GPS data available. (k 6= 100, 200, ...)
a−k = ak + δa−
k
z−k+1 = c(z−k , a−k )
δa−k+1 = [Ψk]10:15,10:15 δa
−k
P−k+1 = Ψk P−
k Ψ∗k + Qd,k
GPS data available. (k = 100, 200, ...)
Kf,k = P−k H∗
k (Hk P−k H∗
k + Rk)−1
[δzk
δak
]
=
[09×1
δa−k
]
+ Kf,k
(
yk − Hk
[z−k
06×1
])
zk = z−k + δzk
ak = ak + δak
Pk = P−k − Kf,k Hk P−
k
z−k+1 = c(zk, ak)
δa−k+1 = [Ψk]10:15,10:15 δak
P−k+1 = Ψk Pk Ψ∗
k + Qd,k
Table 5.1: The algorithm for integration between GPS and INS data, with aratio between the sample rates equal to 100 times.
Chapter 6
Hardware
This chapter includes a short description of the tailor made hardware con-structed for the GPS aided inertial navigation system described in this thesis.It should be noted that the hardware and proposed calibration method have atthe time of writing not yet been tested and may be refined or changed. Thehardware consist of four distinguishable parts: the IMU housing the tree gyrosand tree accelerometers, the GPS-receiver providing position estimates accord-ing to the NMEA protocol via a RS232 serial interface, the PC onto whichthe proposed integration algorithm is implemented, and the micro-controllerresponsible for data acquisition.
6.1 The Inertial Measurement Unit
The inertial measurement unit comprises three state-of-the-art MEMS gyros andaccelerometers. The sensors are mounted with their instrumental sensitivityaxes pointing in three perpendicular directions, resulting in a six degree-of-freedom IMU. Even though the sensors and their sensitivity axes are mounted topoint in three perpendicular directions, they will never span a truly orthogonalcoordinate system. Procedures for finding this misalignment and compensationmethods can be found in [6]. Before the IMU is used the sensors must becalibrated. For the accelerometers this is straightforward since the norm ofthe accelerometer measurement output vector, when the IMU platform is atrest should be equal to the local earth acceleration. A common method forcalibration of gyros is to use the earth rotational rate as reference, but due tothe lack of sensitivity in the used sensors this is not possible and other methodsmust be consulted.
6.2 Data Acquisition
A micro-controller housing a ten bit analog to digital converter and a six toone analog multiplexer is used to sample the IMU sensors and control the dataacquisition. More, the micro-controller masters the GPS-receiver via a RS232serial interface and synchronizes the GPS and IMU data streams. The collecteddata is then transmitted over a second RS232 interface to a PC, housing theproposed integration algorithm. Further, the calibration parameters are stored
6.2 Data Acquisition 34
PSfrag replacements
µCGPS Host PCIMURS232 RS232
Figure 6.1: Block diagram of the hardware.
in the micro-controller and sent to the host PC at start up. Hence, calibrationsare not necessary, but maybe preferable when a new software is used. Using abattery operated PC the system is fully mobile and able to perform real-timesignal processing. A block diagram of the hardware is shown in Figure 6.1.
Chapter 7
Simulation Results
In this chapter simulation results of the proposed algorithm for integration be-tween GPS and INS data will be presented. To evaluate the integration al-gorithm a Monte Carlo simulation has been applied to simulation data corre-sponding to a typical driving scenario. The Monte Carlo simulation consists oftwenty different GPS and IMU sensors noise realizations generated under thesame conditions. To the different data realizations the integration algorithmhas been applied and the outputs have been averaged. The driving scenario andthe simulation results are shown and discussed below.
7.1 Driving Scenario
The driving scenario is illustrated in Figure 7.1. The car is stationary for thefirst 100 seconds, then it makes a wide turn and accelerates to 18 km/h whichit keeps until after the last turn. Finally the car slows down and stops. InFigure 7.1 the dashed trajectory is the position estimates generated by the GPSaided INS for one typical noise realization, i.e not a Monte Carlo simulation.The shown specks are the GPS position estimates and the solid line is the truetrajectory. A ratio of 100 times was used between the INS and GPS sample ratioand the GPS position estimates had a standard deviation of 13 meters. Thebiases of the accelerometers and gyros were in the order of 1-2 cm/s2 respectively5-10 /h. Not surprisingly, the GPS aided system clearly outperforms the GPS-system.
In Figure 7.2 the averaged errors in the estimated accelerometer biases areshown. On the average the accelerometer biases have converted already beforethe car starts to move after 100 seconds. Since no information about the attitudeof the navigation system is gained from the GPS position estimates when thecar is stationary the gyro biases and attitude errors estimates don’t starts toconverge until the car starts to move, which can be seen in Figure 7.3 andFigure 7.4. The average position errors in the x, y and z direction (ECEF-coordinates) are shown in Figures 7.5, 7.6 and 7.7, respectively. The maximumand root mean square position errors for the GPS aided INS respectively thestand alone GPS-receiver are presented in Table 7.1.
7.1 Driving Scenario 36
0 50 100 150 200 250
−50
0
50
100
150
PSfrag replacements
Nor
th[m
]
East [m]
True trajectory
INS/GPS estimate
GPS reading
Figure 7.1: Estimated and true trajectory of a typical driving sequence. Firstthe car is stationary for 100 seconds. Then it makes a wide turn and acceleratesto 18 km/h, which it keeps until after the last turn. Finally the car slows downand stops.
0 50 100 150 200 250 300−0.02
−0.015
−0.01
−0.005
0
0.005
0.01
0.015
0.02
0.025
PSfrag replacements Est
imat
ion
erro
rs[m/s
2]
Time [s]
x-axis
y-axis
z-axis
Figure 7.2: The average errors in the accelerometer bias estimates for the drivingsequence. The errors have already converged before the car starts moving after100 seconds.
7.1 Driving Scenario 37
0 50 100 150 200 250 300−0.2
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
PSfrag replacements
Est
imat
ion
erro
rs[degrees/s
]
Time [s]
x-axis
y-axis
z-axis
Figure 7.3: The average errors in the gyro bias estimates for driving sequenceone. The errors don’t starts to converge until the car begins moving and makeslarge turn after 100 seconds.
0 50 100 150 200 250 300−20
−15
−10
−5
0
5
10
15
20
PSfrag replacements
Est
imat
ion
erro
rs[degrees/s
]
Time [s]
x-axis
y-axis
z-axis
Figure 7.4: The average errors in the Euler angle estimates describing the bodyto ECEF frame rotation matrix Re
b. As for the gyro bias estimates, informationis only gained when the car is turning.
7.1 Driving Scenario 38
0 50 100 150 200 250 300−3
−2
−1
0
1
2
3
4
5
PSfrag replacements
Pos
itio
ner
rorx-d
irec
tion
[m]
Time [s]
Figure 7.5: The average position error in ECEF x-direction.
0 50 100 150 200 250 300−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
PSfrag replacements
Pos
itio
ner
rory-d
irec
tion
[m]
Time [s]
Figure 7.6: The average position error in ECEF y-direction.
0 50 100 150 200 250 300−4
−3
−2
−1
0
1
2
3
PSfrag replacements
Pos
itio
ner
rorz-d
irec
tion
[m]
Time [s]
Figure 7.7: The average position error in ECEF z-direction.
Peak error [m] RMSE [m]GPS 35.7090 13.7987
GPS aided INS 17.2451 2.0948
Table 7.1: Peak and root mean square errors for the GPS aided INS and thestand alone GPS-receiver, respectively.
Chapter 8
Conclusions and FurtherWork
8.1 Conclusions
The proposed algorithm for integration between GPS and INS data is sensitiveto its initial condition and tuning, since the extended Kalman filter linearizesthe navigation equations around the inertial navigation system outputs. Whenproperly initialized and tuned the GPS aided INS works well and clearly out-performs the stand alone GPS-receiver. At a ratio of 100 times between theINS and GPS sample ratio the integration reduces the error in the position es-timates with a factor 6 compared to the stand alone GPS-receiver. Simulationsshow that the convergence rate of the attitude errors and gyro biases estimatesdepends not only on the tunable filter parameters, but also on the trajectoryof the vehicle. For the attitude error and gyro bias states to be observable theIMU platform must rotate and therefore the beginning of a vehicle path shouldcontain turns to ensure fast convergence of the filter.
Even though a very simplified model of the GPS receiver was used in thesimulations, the results indicates that the proposed integration method alsoshould be applicable to real world data.
8.2 Further Work
Before the GPS aided INS system can be taken into use, there are severalproblems to be solved and further work is needed in the following areas:
• Development of a method for initial calibration of the IMU sensors scalefactors, biases, misalignment, et cetera.
• Investigation of methods for estimation of the delay between the actualmeasurement time and output time of the GPS-receivers position esti-mates.
• Development of a method for initial and online alignment of the IMUplatform.
8.2 Further Work 40
• Investigation of the possibilities of using model based filtering of the IMUsensors output.
• Investigation of suitable error models for the IMU sensors and the gravityerror.
• Construction and evaluation of the IMU platform and the hardware fordata acquisition.
• Development of a software package for processing, analysis and presenta-tion of the measured data.
Bibliography
[1] Q. Honghui and J. Moore, “Direct Kalman filtering approach for GPS/INSintegration,” IEEE Transactions on Aerospace and Electronic Systems, vol.38, Issue: 2, pp. 687–693, April. 2002.
[2] S. Hong, F. Harashima, S. Kwon, S.B Choi, M.H Lee and H.Lee “Estima-tion of errors in inertial navigation systems with GPS measurements,” inProc. of ISIE 2001, IEEE International Symposium ,12–16 June. 2001, vol.3, pp. 1477 – 1483
[3] S. Cooper and H. Durrant-Whyte, “A Kalman filter model for GPS navi-gation of land vehicles,” in Proc. of the IEEE/RSJ/GI International Con-ference on Intelligent Robots and Systems ’94. ’Advanced Robotic Systemsand the Real World’, IROS ’94. , 12–16 Sept. 1994, vol. 1, pp. 157 – 163.
[4] K.P. Schwarz and M. Wei, “Aided versus embedded- a comparison of twoapproaches to GPS/INS integration,” in Proc. of IEEE Position Locationand Navigation Symposium, pp. 314–322, April 11-15. 1994.
[5] J. Farrell and M. Barth, The Global Positioning System and Inertial Nav-igation. McGraw-Hill, New York, NY, 1999.
[6] A. Chatfield, Fundamentals of High Accuracy Inertial Navigation. Ameri-can Institute of Aeronautics and Astronautics, Reston, VA, 1997.
[7] K. Astrom, B. Wittenmark, Computer Controlled Systems: Theory andDesign. Prentice-Hall, Englewood Cliffs, NJ, 1984.
[8] T. Kailath, A.H. Sayed and B. Hassibi, Linear Estimation. Prentice-Hall,Englewood Cliffs, NJ, 2000.
[9] P. Stromback, “Integrating GPS and INS with a tightly coupled Kalmanfilter - using proximity information for compensating signal loss,” TechnicalReport, Dept. of Signal, Sensors and Systems., Royal Institut of Technol-ogy, Sweden, March 2003.
[10] R. Dorobantu, Ch. Gerlach, “Characterisation and Evaluation of aNavigation-Grade RLG SIMU,” European Journal of Navigation, nr 1, Feb.2004, vol. 2, pp. 63–78.
[11] F. Gusafsson, Adaptive Filtering and Change Detection. John Wiley &Sons, New York, NY, 2000.
Appendix A
The Kalman Filter
The Kalman filter, which was first introduced by R.E. Kalman in 1960 providesan optimal solution to the least-squares estimator problem for applications thatcan be represented as a linear system driven and disturbed by Gaussian noise.The Kalman filter is a recursive algorithm that solves the minimum least-squareserror problem using state-space methods. The Kalman filter equations will herebe derived using an innovation approach. More details about the Kalman filtercan be found in [8, 11]. The derivation below follows the derivation in [8].
A.1 Derivation of the Kalman Filter Equations
Define a discrete state-space model such as
xk+1 = Fk xk + Gk wk (A.1)
yk = Hk xk + vk (A.2)
where the xk is the n × 1 state vector. The processes vk and wk are assumedto be zero mean white noise process, with
E
[vk
wk
][
v∗l w∗
l
] =
[Qk Sk
S∗k Rk
]
δ(l − k) (A.3)
Let y−k denote the linear least square estimator of yk given the observation
sequence y0,y1,y2, ...,yk−1. Define the error between the one-step aheadpredicted value and true value as
ek = yk = yk − y−k (A.4)
Here ek can be seen as the new information brought into the system by yk, there-for referred to as the innovation. Since y−
k is the least square estimator, the errorper definition is orthogonal to all the subspace span by y0,y1,y2, ...,yk−1, de-noted L(yk−1); See Figure A.1. Since error ek is a linear combination of yk andy−
k not only will the innovations and the sequence y0,y1,y2, ...,yk−1 spanthe same subspace, i.e L(yk−1) = L(ek−1), the innovation sequence will also bea white sequence. Now raising the problem to find the least square estimatory−
k , which in terms of projections can be written as
A.1 Derivation of the Kalman Filter Equations 43
PSfrag replacements
yk
y−k
L(yk−1) = L(ek−1)
ek
subspace
Figure A.1: The projection of a vector yk onto the subspace L(yk−1) correspondsto the least square estimate of the vector yk given the sequence y0,y1, ...,yk−1.Note that the prediction error is orthogonal to the subspace.
y−k = Proj(yk|L(yk−1))
= Proj(Hk xk + vk|L(yk−1))
= Hk Proj(xk|L(yk−1)) + Proj(vk|L(yk−1))
= Hk x−k (A.5)
Here Proj(·|·) denotes the projection operation and the last equality is a resultof vk being a white sequence. Thus it turned out that finding the linear least-squares estimator of y−
k is equivalent to finding the state prediction x−k . Since
L(yk) = L(ek) and ek is a white sequence the state prediction can be writtenas
x−k+1 = Proj(xk+1|L(yk))
= Proj(xk+1|L(ek))
= Proj(xk+1| ek) + Proj(xk+1|L(ek−1))
= Exk+1 e∗kR−1e,k ek + Proj(xk+1|L(ek−1)) (A.6)
where the innovation covariance matrix is defined as Re,k4= Eek e∗k. This is
almost the desired form and would be exactly so if the term Proj(xk+1|L(ek−1))could be expressed in terms of x−
k and ek. Projecting xx+1 onto the linearsubspace Lyk−1 shows that
Proj(xk+1|L(yk−1)) = Proj(Hk xk + Gk vk|L(yk−1))
= Hk Proj(xk|yk−1) + Gk Proj(vk|L(yk−1))
= Hk x−k (A.7)
A.1 Derivation of the Kalman Filter Equations 44
where the last equality is a result of Evk y∗l = 0 for l < k. Thus a recursive
algorithm for the state prediction may be written as
ek = yk − Hk x−k (A.8)
x−k+1 = Fk x−
k + Kp,k ek (A.9)
Kp,k = Exk+1 e∗kR−1e,k (A.10)
e0 = y0, x−0 = 0 (A.11)
Now confronting the problem to find a recursion for Kp,k and Re,k, which arenon-random quantizes and determined only by our model assumptions. Intro-ducing the a posterior error covariance matrix P−
k defined as
P−k
4= Ex−
k x−∗k (A.12)
wherex−
k = xk − x−k (A.13)
The innovation covariance matrix Re,k can then be written in terms of Pk|k−1
as
Re,k = Eek e∗k
= E(yk − Hk x−k ) (yk − Hk x−
k )∗
= E(Hk x−k + vk) (Hk x−
k + vk)∗
= Hk P−k H∗
k + Rk (A.14)
and the prediction Kalman gain Kp,k as
Kp,k4= Exk+1 e∗kR−1
e,k
= (Fk Exk e∗k + Gk Evk e∗k)R−1e,k (A.15)
where
Exk e∗k = Exk (Hk x−k + Gk vk)∗
= Exk x−∗k H∗
k + Exk vkGk
= P−k H∗
k (A.16)
and
Evk e∗k = Evk (Hk x−k + wk)∗
= Ewk xkH∗k + Evk w∗
k
= Sk (A.17)
Thus the Kalman gain is becomes
Kp,k = (Fk P−k H∗
k + Gk Sk)R−1e,k (A.18)
What remains is to find a recursion for P−k . As soon shall be shown P−
k can becomputed via the discrete-time Riccati recursion. Using the expression for thestate predictor in (A.9) the prediction error may be written as
A.1 Derivation of the Kalman Filter Equations 45
x−k+1 = xk+1 − x−
k+1
= xk+1 − Fk x−k − Kp,k ek
= xk+1 − Fk x−k − Kp,k (Hk x−
k + wk)
= Fk x−k + Gk wk − Kp,k Hk x−
k − Kp,k wk
= (Fk − Kp,k Hk) x−k −
[Gk −Kp,k
][
vk
wk
]
(A.19)
Then it straightforward to show that the a posteriori error covariance matrixmay be written as
P−k+1 = Ex−
k+1 x−∗k+1
= (Fk − Kp,k Hk)P−k (Fk − Kp,k Hk)∗
+[Gk −Kp,k
][
Qk Sk
S∗k Rk
] [Gk
−Kp,k
]
(A.20)
After some algebra this reduces to
P−k+1 = Fk P−
k F∗k + Gk Qk G∗
k − Kp,k Re,k K∗p,k (A.21)
which indeed is the discrete-time Riccati recursion. Bring it all together theprediction form of the Kalman filter is shown in Table A.1.
ek = yk − Hk x−k
x−k+1 = Fk x−
k + Kp,k ek
Kp,k = (Fk P−k H∗
k + Gk Sk H∗k)R−1
e,k
P−k+1 = Fk P−
k F∗k + Gk Qk G∗
k − Kp,k Re,k K∗p,k
P−0 = Ex0 x∗
0 x−0 = Ex0
Table A.1: The Kalman filter recursions in prediction form.
In many applications it is of interest to obtain an estimate of xk given theobservations y0,y1,y2, ...yk, i.e the filtered estimate. The derived Kalmanfilter equations can easily be modified to output both the filtered and predictedstate estimate using the innovation approach once more. Starting by projectingxk onto the subspace L(ek) the estimator may be written as
xk = Proj(xk|L(ek))
= x−k + Exk e∗kR−1
e,k ek
= x−k + Kf,k ek (A.22)
where the Kalman filter gain Kf,k is defined as
A.1 Derivation of the Kalman Filter Equations 46
Kf,k4= Exk e∗kR−1
e,k
= Exk (Hk x−k + vk)∗R−1
e,k
= P−k H∗
k R−1e,k
= P−k H∗
k (Hk P−k H∗
k + Rk)−1 (A.23)
and the error covariance Pk as
Pk|k4= Exk x∗
k
= Exk x∗k
= E(xk − xk)x∗k
= Exk − x−k − Kf,k ek)x∗
k
= Ex−k x∗
k − Kf,k Eek x∗k
= P−k − Kf,k Hk P−
k (A.24)
This far expressions for updating the predicted estimate x−k to the filtered es-
timate xk and the covariance matrix P−k to Pk have been derived. If a update
from the filtered estimate xk to the predicted x−k+1, as well as for the covariance
matrix Pk to the a posterior covariance matrix P−k+1 could be found, then the
derived Kalman filter recursions could be rewritten to output the filtered esti-mate as well. Assuming no correlation between the process and measurementnoise, i.e Sk = 0, the time update can be found as
x−k+1 = Proj(xk+1|L(ek))
= Proj(Fk xk + Gk vk|L(ek))
= Fk xk (A.25)
and
P−k+1 = Ex−
k+1 x−∗k+1
= E(Fk xk + Gk vk)(Fk xk + Gk vk)∗
= Fk Pk F∗k + Gk Qk G∗
k (A.26)
If there are correlation between the measurement and process noise sources thetime update becomes quit complicated and expressions for this case may befound in [8]. Collecting the derived relation ships the Kalman filter recursioncan be written in terms of measurement and time updates as in Table A.2.
A.1 Derivation of the Kalman Filter Equations 47
Time update.
x−k+1 = Fk xk
y−k+1 = Hk x−
k+1
P−k+1 = Fk Pk F∗
k + Gk Qk G∗k
Measurement update.
Kf,k = P−k H∗
k (Hk P−k H∗
k + Rk)−1
xk = x−k + Kf,k (yk − Hk x−
k )
Pk = P−k − Kf,k Hk P−
k
Table A.2: The Kalman filter recursions in time and measurement update form.
