GPS-IMU-EKF Navigation System Documentation

GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
Issue: Issue 1
Page: 1 of 68
X
r
Z
f
B
A'
B'
C
A
C'
D'
D
Z
r
Y
r
X
f
Y
f
O
Project: Document No.:
Autonomous Aerospace Navigation System with integrated
Global Positioning System and Inertial Measurement Unit
CNPq Process 471381/2004-7
GPSIMU-INPE-
002
National Institute of Space Research - INPE
Av. dos Astronautas 1758 Phone: +55-12-3945-6198
Sao Jose dos Campos-SP 12227-010 Fax: +55-12-3945-6226
Brazil E-Mail: hkk@dem.inpe.br
Doc. No.: GPSIMU-INPE-002
Issue: Issue 1
Written: Gustavo Baldo Carvalho Date: October 26, 2007
Approved: Dr. Helio Koiti Kuga
INPE Coordinator Date: October 26, 2007
Dr. Stephan Theil
ZARM Coordinator
National Institute of Space Research - INPE Page 1 of 68
Issue: Issue 1
Page: 2 of 68
Contents
1. Introduction 6
2. System description 7
2.1. Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2. Inertial Measurement Unity . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.3. Global Positioning System - GPS . . . . . . . . . . . . . . . . . . . . . . 8
2.4. Estimation and data fusion . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3. Denitions 11
3.1. Reference Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2. Navigation variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4. Estimator 17
5. Navigation Mechanization 20
5.1. Chosing navigation frame . . . . . . . . . . . . . . . . . . . . . . . . . . 20
5.2. GPS measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
5.3. Platform ECI Navigation mechanization . . . . . . . . . . . . . . . . . . 25
5.4. Gravitational Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
5.5. Navigation transformation . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.5.1. Platform X Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . 31
5.5.2. Receiver Antenna X Vehicle . . . . . . . . . . . . . . . . . . . . . 32
5.5.3. Platform X Receiver Antenna . . . . . . . . . . . . . . . . . . . . 33
6. Ground Initialization and Alignment 34
6.0.4. Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
6.0.5. Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
6.0.5.1. Initialization with full priory vehicle information . . . . 38
6.0.5.2. Auto-Initialization . . . . . . . . . . . . . . . . . . . . . 40
7. Navigation Filter 41
7.1. State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
7.1.1. State Derivatives with respect to time . . . . . . . . . . . . . . . 42
7.2. State System Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.2.1. Derivative of Angular rate bias time Derivatives with respect to
State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
7.2.2. Derivative of Quaternions time Derivatives with respect to State
Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
Issue: Issue 1
Page: 3 of 68
7.2.3. Derivative of Specic force bias time Derivatives with respect to
State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
7.2.4. Derivative of Speed time Derivatives with respect to State Space . 48
7.2.5. Derivative of Clock drift speed time Derivatives with respect to
State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
7.2.6. Derivative of Position time Derivatives with respect to State Space 51
7.2.7. Derivative of Clock bias range time Derivatives with respect to
State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
7.3. System Noise Covariance Matrix . . . . . . . . . . . . . . . . . . . . . . . 53
7.4. Measurement coupling Matrix . . . . . . . . . . . . . . . . . . . . . . . . 54
7.4.1. Derivative of Pseudorange with respect to State Space . . . . . . 55
7.4.2. Derivative of Delta-Pseudorange with respect to State Space . . . 56
8. Bibliography 58
A. Physical Constants 61
B. Mechanics 62
B.1. Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
B.1.1. Scalar Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
B.1.2. Vectorial Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 63
B.2. Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
Issue: Issue 1
Page: 4 of 68
List of Figures
1. EKF Estimator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2. Pseudorange. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3. Satellite ranging. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4. Platform navigation mechanization. . . . . . . . . . . . . . . . . . . . . . 28
5. Platform/Antenna installation. . . . . . . . . . . . . . . . . . . . . . . . 30
6. Initialization and Alignment with vehicle priory information. . . . . . . . 34
7. Auto-Initialization and Alignment. . . . . . . . . . . . . . . . . . . . . . 36
8. Vectorial representation in cartesian reference frame. . . . . . . . . . . . 63
9. Vectorial relative representation. . . . . . . . . . . . . . . . . . . . . . . . 64
List of Tables
1. Summary of Continuous-Discrete EKF equations. . . . . . . . . . . . . . 19
2. WGS-84 denitions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
3. Earth zonal harmonics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
4. Physical constants. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
5. Mathematical constants. . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
Issue: Issue 1
Page: 5 of 68
Document Change Record
Issue Date Changed Pages /
Changed Chapters
Remarks Done
Issue 1 October 26,
2007
all Initial Version

Issue: Issue 1
Page: 6 of 68
1. Introduction
The aim of this document is the practical design and development for a navigation sys-
tem via the integration of a Global Positioning System (GPS) receiver ([10], [19], [21]
and [27]) and an Inertial Measurement Unity (IMU) ([3], [13], [16], [24], [28] and [31])
through an Extended Kalman Filter (EKF) ([4], [5], [6], [9], [20], [36] and [38]) for
simulation and application in a real Hardware system.
It presents schematics and mathematical models involving the integration, showing
strategies for their output processing in the direction of providing a blended naviga-
tion solution since system power-ON status, including initialization and alignment, up
to navigation status.
It also presents explanations about the navigation mechanization equations and rela-
tionship between reference frames used in the navigation solution.
The EKF constitutes the navigation lter, where the system model is developed with the
state space technique ([22]) used to fuse IMU measurements of specic force and angular
rate ([11]) to the GPS Pseudoranges and Doppler measurements aected by clock bias
and drift ([8]).
This document was composed with the support of a Scholarship CNPq/Embraer-Brazil .
Issue: Issue 1
Page: 7 of 68
2. System description
The system here developed is a navigation system which uses data fusion among dier-
ent sensors to obtain the navigation solution.
This navigation system is constituted of 2 sensors, GPS receiver and IMU, which are ca-
pable to sense some physical entities related to the desired navigation. However, because
of related biases, drifts and noises the sensors add errors to their measurements, making
that the navigation solution provided directly from the sensors would be inaccurate and
inviable for some applications.
Hence, in order to provide accurate navigation solution, the noisy measurements are
integrated in a state propagation/update process with a known mathematic model of
the system. This constitutes the data fusion process among the sensors and dynamic
model through an estimator.
2.1. Vehicle
The vehicle here is considered a ying Airspacecraft modeled as rigid body moving with
6 degree of freedom under the eect of Earth gravitation and resulting specic force (as
drag and throttle for example) which rotates around its center of gravity (CG).
The vehicle has only 2 navigation sensors installed: The IMU and GPS receiver with
one antenna. The installation is modeled as xed, that means, the platform and receiver
antenna do not present relative motion or rotation with respect to the reference frame
xed to the vehicle body CG. However it does consider distances from installation point
to the vehicle CG as well as angular displacements of each sensor reference frame with
respect to the vehicle reference frame.
Although inputs and measurements from external world are made by the sensors with
respect to their position and attitude in the inertial space, the navigation solution of the
vehicle reference frame with respect to the inertial space is the goal of this document.
2.2. Inertial Measurement Unity
One of the most used navigation sensors is the IMU (Inertial Measurement Unit) ([11]),
also known as platform. This sensor is capable to provide measurements of the three-
dimensional specic force, as well as its attitude rate, both with respect to the inertial
space measured in the platform reference frame.
In each orthogonal direction at least 2 types of primary sensors are required: an ac-
celerometer and a gyroscope. Hence, to provide a three-dimensional solution, 3 orthog-
onal accelerometers and 3 orthogonal gyroscopes are used embedded in the IMU.
Due to the fact that the physical laws are well known, and thus the related propa-
gating equations, the present position, velocity and attitude can be computed through
Issue: Issue 1
Page: 8 of 68
the time integration of the current kinematic accelerations and attitude rates. Since
the IMU senses the relative movement, its observations are used to propagate previous
state estimation to new state estimation, which are however highly dependent on initial
conditions. Since the IMU does not sense the states directly, but their dynamics, it is
considered a dead reckoning sensor.
While the attitude rates are sensed by the embedded gyros, the accelerometers are capa-
ble only of measuring the specic force. Since the kinematic acceleration, composed of
gravitational acceleration and specic force together, a gravitational model must be used
in function of position to calculate the gravitational part, thus allowing the integration.
Although these physical laws do not change with time, the primary sensors and grav-
itational models are not perfect. This causes that the measurements are aected by
various errors, drifts and misalignments during the navigation. Since the solution is a
propagation from previous state estimation, the solution inevitably has unbounded error
growth with the time leading to inaccurate navigation computation. It is also impor-
tant to remember that since the gravitational models are dependent on the position,
more strictly on the altitude, and that the position available comes from the navigation
solution itself, there is a positve error feedback loop that contributes to unbound the
pure solution obtained from the integration of measurements turning the platform into
a unbounded drifting sensor in the time.
Therefore, although the IMU has the advantage of having high sample rate and of be-
ing a stand alone sensor, it has its main disadvantages associated to the gyros drift,
accelerometers bias and gravitational models imperfections meaning poor long time ac-
curacy and unbounded error growth.
Hence, the fast navigation solution provided can not reach the accuracy requirements for
long duration missions. However, the measurements can be used in estimation processes,
where other sensors can help on correcting the states, then avoiding unbounded growth
of error while keeping the track to the actual position, velocity and attitude.
2.3. Global Positioning System - GPS
The Global positioning system ([10]), frequently known as GPS, is composed by a con-
stellation of 24 or more synchronized satellites in dierent orbits around the Earth.
A GPS receiver is a sensor capable of measuring the three-dimensional present position
and velocity with respect to Earth-Centered reference frames (ECI or ECEF) by means
of using signals sent by the GPS constellation.
Due to the fact that the GPS constellation orbits are well known, and thus the GPS
satellites position and speed, the present position and speed can be computed using the
incoming GPS signals.
Although the GPS is not a stand alone sensor, it can be thought as one, once it pro-
vides the vehicle a navigation solution, independently of where it is around the Earth.
Issue: Issue 1
Page: 9 of 68
Since the GPS receiver senses the states directly, it does not need information about
previous states in order to produce a solution, that means, it is dependent on initial
conditions. Hence the GPS receiver is considered a absolute sensor with bounded error
in time. This feature gives long operation time stability, what is not provided by a dead
reckoning sensor like IMU.
However, unlike the IMU, it can not provide a fast navigation solution because of its
low sample rate and the fact that the vehicle must have at least 4 GPS satellite signals
available.
Therefore, although the advantage of being a stand alone sensor and the long operation
time stability, it has also associated inaccuracies due to the path of the satellite signals
to the receiver, which suer the inuence of the atmosphere, multipath eects and clock
bias and drift.
Hence, the navigation solution provided by the GPS receiver can not reach high accuracy
requirements. However, the measurements can be used in estimation processes as aiding
sensor, where they help inertial sensors keeping the track to the actual position, velocity
and attitude.
2.4. Estimation and data fusion
As discussed by ([32]), there are 2 methods of integration: loosely coupled and tightly
coupled. While no information from the estimation lter is taken to the aiding sensor in
the loosely coupled method, in the tightly coupled method, the aiding sensor is feed with
navigation information, what enhances its measurement data integrity and performance.
However, its implementation is more dicult due the fact that the design must consider
more deep details about design and operation of each sensor. This poses a problem
mainly for the GPS aiding sensor, which must permit manipulation of its internal algo-
rithms for tracking satellite signals.
Since such algorithms dictate the performance of the GPS sensor working alone, they
congure market secrets for the manufacturers, causing then that the number of open
GPS receivers available for integration on tightly coupled systems is very small. That
is one of the reasons why most of the developments available in the literature follows
the loosely coupled method ([32]) besides the fact of its easier implementation when in
comparison with the tightly coupled.
Having these reasons in mind, this development intends the development of the nav-
igation lter implementing the tightly coupled method as a contribution using a low
cost open source hardware available in the market (GPS1005-PCI from GPS Creations:
www.gpscreations.com).
As already discussed, none of the sensors above can reach high accuracy and timing
requirements at the same time when used alone. Hence, to improve the accuracy, the
navigation dynamics can be modeled and propagated using IMU measurements and then
Issue: Issue 1
Page: 10 of 68
updated using GPS through an estimator that implements sensor data fusion. However
the GPS receiver is treated here purely as sensor instead of navigation system itself. This
approach then brings navigation information to the receiver algorithm for producing its
observations of satellite pseudoranges instead of the position. This implements then the
tightly coupled integration method between a dead reckoning sensor represented by the
IMU and an absolute sensor represented by the GPS receiver.
Since the propagation equations congure a non-linear system, the estimation is provided
through an Extended Kalman Filter (EKF) ([9]), working with data coming from IMU
and GPS receiver with dierent sampling rates in an improved update ([6]).
Issue: Issue 1
Page: 11 of 68
3. Denitions
3.1. Reference Frames
The reference frames used in this document are:
XY Z
i
- ECI Reference Frame - Earth-Centered-Inertial reference frame xed to
the distant stars ([7]).
XY Z
e
- ECEF Reference Frame - Earth-Centered-Earth-Fixed reference frame
xed to the rotating Earth ([7]).
XY Z
v
- Vehicle Reference Frame - reference frame xed to the moving vehicle
with X pointing forward in the longitudinal axis and Z down.
XY Z
r
- Receiver Antenna Reference Frame - reference frame xed to the receiver
antenna, which in turn is xed to the vehicle without relative movement with
respect to vehicle frame.
XY Z
p
- Platform Reference Frame - reference frame xed to the platform, which
in turn is xed to the vehicle without relative movement with respect to vehicle
frame.
3.2. Navigation variables
Here the variables treated in this document are dened.
Time information:
t - Real time (s)

t - Measurement of biased time (s)

E
- Sideral Time angle for Greenwich at t (rad)
T
e
i
- ECI to ECEF rotation matrix (rad)

i
ie
- Earth angular rate vector (rad/s)
Installation information:
Platform with respect to vehicle:
L
v
P
- Platform Lever Arm in Vehicle Frame (m)
Issue: Issue 1
Page: 12 of 68
T
p
v
- Platform Installation matrix (Vehicle Frame to Platform rotation matrix)
It is assumed:
d(L
v
P
)
dt
= 0
d
2
(L
v
P
)
dt
2
= 0
w
v
pv
= 0
d(w
v
pv
)
dt
= 0
d(T
p
v
)
dt
= 0
d
2
(T
p
v
)
dt
2
= 0
Vehicle with respect to platform:
L
p
V
- Vehicle Lever Arm in Platform Frame (m)
T
v
p
- Platform Frame to Vehicle rotation matrix
It is assumed:
d(L
p
V
)
dt
= 0
d
2
(L
p
V
)
dt
2
= 0
w
p
vp
= 0
d(w
p
vp
)
dt
= 0
d(T
v
p
)
dt
= 0
d
2
(T
v
p
)
dt
2
= 0
Receiver Antenna with respect to vehicle:
L
v
R
- Receiver Antenna Lever Arm in Vehicle Frame (m)
T
r
v
- Receiver Antenna Installation matrix (Vehicle Frame to Receiver An-
tenna rotation matrix)
It is assumed:
d(L
v
R
)
dt
= 0
d
2
(L
v
R
)
dt
2
= 0
w
v
rv
= 0
d(w
v
rv
)
dt
= 0
Issue: Issue 1
Page: 13 of 68
d(T
r
v
)
dt
= 0
d
2
(T
r
v
)
dt
2
= 0
Vehicle with respect to Receiver Antenna:
L
r
V
- Vehicle Lever Arm in Receiver Antenna Frame (m)
T
v
r
- Receiver Antenna Frame to Vehicle rotation matrix
It is assumed:
d(L
r
V
)
dt
= 0
d
2
(L
r
V
)
dt
2
= 0
w
r
vr
= 0
d(w
r
vr
)
dt
= 0
d(T
v
r
)
dt
= 0
d
2
(T
v
r
)
dt
2
= 0
Receiver Antenna with respect to Platform:
L
p
PR
- Receiver Antenna Lever Arm in Platform Frame (m)
T
r
p
- Receiver Antenna Installation matrix (Platform Frame to Receiver An-
tenna rotation matrix)
It is assumed:
d(L
p
PR
)
dt
= 0
d
2
(L
p
PR
)
dt
2
= 0
w
p
rp
= 0
d(w
p
rp
)
dt
= 0
d(T
r
p
)
dt
= 0
d
2
(T
r
p
)
dt
2
= 0
Platform with respect to Receiver Antenna:
L
r
RP
- Platform Lever Arm in Receiver Antenna Frame (m)
T
p
r
- Receiver Antenna Frame to Platform rotation matrix
It is assumed:
Issue: Issue 1
Page: 14 of 68
d(L
r
RP
)
dt
= 0
d
2
(L
r
RP
)
dt
2
= 0
w
r
pr
= 0
d(w
r
pr
)
dt
= 0
d(T
p
r
)
dt
= 0
d
2
(T
p
r
)
dt
2
= 0
Navigation States:
Vehicle:
r
i
V
- Vehicle position in ECI (m)
v
i
V
- Vehicle speed in ECI (m/s)
a
i
V
- Vehicle kinematic acceleration in ECI (m/s
2
)
q
v
i
- Vehicle attitude quaternions with respect to ECI
T
v
i
- Vehicle attitude matrix with respect to ECI

i
iv
- Vehicle angular rate with respect to ECI in ECI (rad/s)

v
iv
- Vehicle angular rate with respect to ECI in Vehicle frame(rad/s)

i
iv
- Vehicle angular acceleration with respect to ECI in ECI (rad/s
2
)

v
iv
- Vehicle angular acceleration with respect to ECI in Vehicle frame(rad/s
2
)
Platform:
r
i
P
- Platform position in ECI (m)
v
i
P
- Platform speed in ECI (m/s)
a
i
P
- Platform kinematic acceleration in ECI (m/s
2
)
q
p
i
- Platform attitude quaternions with respect to ECI
T
p
i
- Platform attitude matrix with respect to ECI

i
ip
- Platform angular rate with respect to ECI in ECI (rad/s)

p
ip
- Platform angular rate with respect to ECI in Platform frame(rad/s)

i
ip
- Platform angular acceleration with respect to ECI in ECI (rad/s
2
)

p
ip
- Platform angular acceleration with respect to ECI in Platform frame(rad/s
2
)
Receiver Antenna:
Issue: Issue 1
Page: 15 of 68
r
i
R
- Receiver Antenna position in ECI (m)
v
i
R
- Receiver Antenna speed in ECI (m/s)
a
i
R
- Receiver Antenna kinematic acceleration in ECI (m/s
2
)
q
r
i
- Receiver Antenna attitude quaternions with respect to ECI
T
r
i
- Receiver Antenna attitude matrix with respect to ECI

i
ir
- Receiver Antenna angular rate with respect to ECI in ECI (rad/s)

r
ir
- Receiver Antenna angular rate with respect to ECI in Receiver Antenna
frame(rad/s)

i
ir
- Receiver Antenna angular acceleration with respect to ECI in ECI
(rad/s
2
)

r
ir
- Receiver Antenna angular acceleration with respect to ECI in Receiver
Antenna frame(rad/s
2
)
Inputs and Measurements:
Platform:
Inputs:

f
p
P
- Measurement of biased platform specic force in Platform frame
(m/s
2
)

p
ip
- Measurement of biased platform angular rate in Platform frame
(rad/s)
Computed values:
f
p
P
- Platform real specic force in Platform frame (m/s
2
)
b
p
f
- Specic force bias in Platform frame (m/s
2
)

p
ip
- Platform real angular rate in Platform frame (rad/s)
b
p
- Angular rate bias in Platform frame (rad/s)

p
ip
- Platform real angular acceleration in Platform frame (rad/s
2
)
Receiver:
Measurements:

PR - Measurement of Pseudorange of satellite with respect to receiver
antenna (m)

dPR - Measurement of Delta-Pseudorange of satellite with respect to
receiver antenna (m/s)
Issue: Issue 1
Page: 16 of 68

t
R
- Satellite signal biased reception time (s)

t
T
- Satellite biased transmission time (s)

DCO - Measurement of carrier DCO frequency (Hz)
Computed values:

PR - Estimation of Pseudorange of satellite with respect to receiver an-
tenna (m)

dPR - Estimation of Delta-Pseudorange of satellite with respect to re-
ceiver antenna (m/s)
R
bc
- Range corresponding to receiver clock bias (s)
V
bd
- Speed corresponding to receiver clock drift (m/s)

b
SV
- Estimated satellite clock bias (s)

t
r
- Estimated satellite signal relativistic delay (s)

T
I
- Estimated satellite signal ionospheric delay (s)

T
T
- Estimated satellite signal tropospheric delay (s)

T
M
- Estimated satellite signal multipath delay (s)

T - Estimated satellite signal total extra receiver delay (s)

r
i
S
- Estimated satellite position with respect to ECI (From NAV message
and transmission time estimation) (m)

R - Estimation of receiver antenna geometric range to satellite (m)
v
i
S
- Estimated satellite speed with respect to ECI (From NAV message
and transmission time estimation) (m/s)
d(
T)
dt
- Variation of Satellite signal total extra receiver delay
Issue: Issue 1
Page: 17 of 68
4. Estimator
Since the equations treated here are non-linear, an estimator for non-linear process
must be used. This is here implemented by means of Continuous-discrete Extended
Kalman Filter EKF (continuous dierential equations in the system state propagation
and discrete measurements in the update process) ([9]).
The gure 1 represents a summary for the implementation of the EKF.
Estimator
Propagation
Update
States
Propagation
Covariance
Propagation
Observation
model
Kalman Gain
States
Update
Covariances
Update
Linear F
matrix
Linear H
matrix
R matrix
Q matrix
+
-
X(-)
P(-)
P(+)
X(+)
Z h
h
System
dynamics
Sensors
Dynamics
Noise
Measurements
Noise
+
+
Z X
w
v
U
Inputs
Figure 1: EKF Estimator.
Issue: Issue 1
Page: 18 of 68
Given the state initial conditions

X
0
, the estimator propagates the states X
through
the system dynamics dierential equations represented by f using noisy measurements
Z.
At the same time state covariances P
are also propagated using given initial conditions

P
0
based on the current knowledge of the system process associated noises Q and on
the linearized system dynamics F around the current state propagation X
. It shows
how accurate are the propagated states X
.
By using propagated states X
and observation model h, the measurements can be

predicted and then compared to real incoming measurements Z. This dierence is then
used to updated states X
+
using an weighting gain K. This gain is based on the
knowledge of current states covariance propagation P
, on the current knowledge of

noises associated to the measurement process R and on the linearized measurements
dynamics H around the current propagated state X
. This is known as the data fusion

process.
The data fusion is also used to result in the updated state covariance P
+
, which will be
then again propagated and used to fuse new measurements into the state estimation.
The state covariances update P
+
is calculated also based on the current knowledge of
the noises associated to the measurement process R, on the current states covariance
propagation P
and on the linearized measurements dynamics H around the current

propagated state X
.
Issue: Issue 1
Page: 19 of 68
The Table 1 summarizes the Continuous-discrete Extended Kalman Filter equations
used in the estimator development.
System Model

X(t) = f(X(t), U(t), t) +G w(t)
Measurement Model Z
k
= h
k
(X(t
k
)) +
k
, k = 1, ..., N
Assumptions
w(t) N(0, Q(t)),
k
N(0, R
k
), E[w(t),
T
k
] = 0
Initial Conditions

X(0) N(

X
0
, P
0
)
State Estimate Propa-
gation
X(t) = f(

X(t), U(t), t)
State Covariance
Propagation
P(t) = F(

X(t), U(t), t)P(t) +P(t)F
T
(

X(t), U(t), t)
+G.Q(t) G
T
State Estimate Up-
date
X
k
+
=

X
k
+K
k
_
Z
k
h
k
(

X
k
)
_
State Covariance Up-
date (Joseph form)
P
k
+
=
_
I K
k
H
k
(

X
k
)
_
P
k
_
I K
k
H
k
(

X
k
)
_
T
+K
k
R K
T
k
Gain Matrix
K
k
= P
k
H
T
k
(

X
k
)
_
H
k
(

X
k
) P
k
H
T
k
(

X
k
) +R
k
_
1
Linearization Deni-
tions
H
k
(

X
k
)
h
k
(X(t
k
))
X(t
k
)
X(t)=

X
k
F(

X
, U(t), t)
f(X(t),U(t),t)
X(t)
X(t)=

X
Table 1: Summary of Continuous-Discrete EKF equations.

Issue: Issue 1
Page: 20 of 68
5. Navigation Mechanization
The navigation solution is constituted by position, speed and attitude obtained through
time integrations of kinematic acceleration and angular rate.
For a integrated GPS/IMU navigation, there are 3 possible candidates for the navigating
origin: Vehicle CG, Platform reference origin and GPS antenna reference origin.
A natural thought would be to implement the equations using the Vehicle CG. However,
since the states to be integrated are measured by the platform and are not referred to
the vehicle, the use of the platform reference origin as navigating point is much more
suitable avoiding the complexity of the equations to be integrated when related to the
vehicle CG which can also increase round-o eect.
Related to the use of the GPS antenna reference origin, besides the complexity of the
equations and the round-o eect in the integration, there is also the fact of the dier-
ence in the rates of data delivered by the platform in comparison to the GPS receiver
discussed before. All these topics converge to the sense that such use is simply out
of question in terms of computational eciency. Furthermore, if the navigation solu-
tion is somehow needed for the antenna reference origin, the vehicle navigation (or even
platform navigation) can always be easily converted to it.
5.1. Chosing navigation frame
The choice for the reference frame to which the navigation mechanization is referred
to is in principle arbitrary and actually based on requeriment criteria as speed of com-
putation, accuracy, operational ease or required output coordinates. Usually, the last
criterion is chosen in navigation applications.
In most of the cases for near Earth navigation, local level reference frames as the North-
East-Down (NED) or East-North-Up (ENU) (see [7]) are used. However, their mech-
anization is far more complex then the mechanization in Earth-Centered-Earth-Fixed
(ECEF) cartesian system and not properly the best in terms of computational eciency.
As shown by [35], the navigation computation in terms of ECEF is slightly superior in
accuracy, more robust and is about 32% more ecient computationally than the local
level mechanization, already including computation of the local level navigation solution
from the cartesian mechanization in case it is somehow needed. The higher eciency
is due the elimination of angular velocity computations inside the propagation used to
bring the ECEF solution to local level frames, once gravitational eld computations have
similar eciency in both cases. Therefore the ECEF approach is more suitable for ap-
plications requiring GPS/IMU navigation solution, which preferably use this reference.
In case of space applications, a not rotating frame is more desireable then an Earth-
xed one. Therefore, an inertial reference frame like the Earth-Centered-Inertial (ECI)
reference frame is used, instead of ECEF, constituting this another contribution of this
Issue: Issue 1
Page: 21 of 68
development.
Considering that the only dierence between ECI and ECEF is the Earth rotation and
that this rotation is well known, the same advantages mentioned above can be given to
the use of ECI frame. Furthermore, although the navigation equations can be mecha-
nized in any reference frame, the natural reference frame for systems which take use of
an IMU is the inertial reference frame. This then adds as a further advantage, once the
measurements provided by the IMU are already related to the inertial space.
However, a question may arise about the use of GPS measurements in the mechanization,
which are given in ECEF as standard mechanization in the literature. This question can
be answered by remembering the dierence in the rates of data delivered by the IMU in
comparison to the GPS receiver. While a GPS receiver typically provides data at 10Hz,
the IMU typically provides data at 100Hz, 10 times more. From that, it becomes clear
that the use of ECI to propagate the navigation states is more suitable then the use of
ECEF. Furthermore, if the pseudorange approach is used, then the satellite positions can
be easily adapted to provide ECI solution (see [10]) in order to be used directly together
with the ECI position provided by the navigation mechanization in ECI. This eliminates
any further concern about some possible disadvantage of using GPS measurements in
ECI navigation mechanization. This is another contribution of this work.
Issue: Issue 1
Page: 22 of 68
5.2. GPS measurements
In absence of clock bias, three measurements are enough to determine the position in a
three-dimensional space. However, in the presence of clock bias, 3 measurements only
give a region which contains the position, without the possibility to determine the spe-
cic position. Thus, a fourth measurement is needed to determine the position within
this region.
The complete range description, which takes the clock bias into account is called pseu-
dorange.
Figure 2: Pseudorange.
From the signal correlation performed in the electronic, the GPS receiver is capable of
providing two types of measurements for GPS signals: Pseudorange and Doppler.
The raw measurement of the pseudorange and delta-pseudorange due doppler are
PR = (
t
R
t
T
) c (1)
dPR = (

DCO IF
L1
) c/L1 (2)
To determine the position and speed, a model for the GPS measurements is needed.
Issue: Issue 1
Page: 23 of 68
r

i
Sk
- r
R
i
r
R
i
Y
ECI
r

i
Sk
Z
ECI
X
ECI
Figure 3: Satellite ranging.
The measurement models for the pseudorange and delta-pseudorange due doppler are
PR =

R +

R
bc
T c
R = |r
i
S
r
i
R
|
T =

b
SV
+
t
r
T
I

T
T

T
M
(3)
dPR = ( v
i
S
v
i
R
)
T
(r
i
S
r
i
R
)
R
+ (V
bd
+
bd
)
d(
T)
dt
c
d(
T)
dt
=

T(t
k+1
)
T(t
k
)
t
k+1
t
k
(4)
where
bd
represent the associated noise around the real clock drift speed V
bd
such as
V
bd
= V
bd
+
bd
(5)
and the delays are:
t
r
- Relativistic delay
Issue: Issue 1
Page: 24 of 68
T
I
- Ionospheric delay
T
T
- Tropospheric delay
T
M
- Multipath eect delay
In the ltering process, the goal is to bring the model results as near as possible to the
raw measurements such as:

PR

PR and

dPR

dPR.
Note: The satellite clock bias is represented by

b
SV
which is not included in the clock
bias term R
bc
that represents only the receiver clock bias.
It is possible to note that the pseudorange equation contributes with the same 4 un-
knowns r
X
, r
y
, r
z
and R
bc
for each observed satellite while the delta-pseudorange equa-
tion contributes with the same 4 unknowns v
X
, v
y
, v
z
and V
bd
. Thus, if at least 4
satellites are available, the position, clock bias, speed and clock drift can be determined.
Please refer to [10] for computation algorithms of the satellite position and speed as well
as the involved delays.
Issue: Issue 1
Page: 25 of 68
5.3. Platform ECI Navigation mechanization
The platform measurements are divided in two groups: Accelerometer and Gyro mea-
surements.
Once the output is available, it is a very imperfect representation of the real desired
actuating specic force and angular rate.
The measurements in platform frame can be represented by:
f
p
P
= f
p
P
+b
p
f
+
p
f
(6)

p
ip
=
p
ip
+b
p
+
p
(7)
where b
p
and b
p
f
represent biases while
p
and
p
f
are the associated noises around the
real physical entities f
p
P
and
p
ip
.
Once the platform measurements are available, the platform inertial navigation solu-
tion can be computed by time integration. The navigation solution is constituted by
position r
i
P
, speed v
i
P
, kinematic acceleration a
i
P
, attitude q
p
i
, angular rate
i
ip
and an-
gular acceleration
i
ip
.
By integrating the platform angular rate, the attitude can be obtained. In quaternions
form
q
p
i
= q
0
p
i
+
_
t
t
0
q
p
i
dt (8)
where q
0
p
i
is the quaternion form of the platform initial inertial attitude T
0
p
i
obtained by
alignment.
Note: The resulting quaternion may not be norm 1 due round-o problems. This can
be solved by a simple renormalization after propagation. The same problem occurs at
update phase, where renormalization must also be used.
The quaternion derivative can be expressed as function of the angular rate
q
p
i
=
1
2

q
(
p
ip
) q
p
i
=
1
2

q
(
p
ip
b
p
) q
p
i

1
2

q
(
p
) q
p
i
(9)
where
q
is dened as ([7])
q
() =
_
_
0
z

y

x
z
0
x

y
y

x
0
z
x

y

z
0
_
_
(10)
Issue: Issue 1
Page: 26 of 68
Note that while the bias can be estimated and used in the integration, the noise can not.
However, by using its standard deviation in the lter covariances propagation process,
it can better guide the quaternions estimation. Thus in the equation above, only the
rst term is actually considered in the quaternions propagation.
From q
p
i
the platform attitude can be transformed to the rotation matrix form ([7])
T(q) =
_
_
q
2
1
q
2
2
q
2
3
+q
2
4
2(q
1
q
2
+q
3
q
4
) 2(q
1
q
3
q
2
q
4
)
2(q
1
q
2
q
3
q
4
) q
2
1
+q
2
2
q
2
3
+q
2
4
2(q
2
q
3
+q
1
q
4
)
2(q
1
q
3
+q
2
q
4
) 2(q
2
q
3
q
1
q
4
) q
2
1
q
2
2
+q
2
3
+q
2
4
_
_
(11)
The angular rate is
p
ip
=
p
ip
b
p
i
ip
= (T
p
i
)
T

p
ip
(12)
Since the platform does not provide angular acceleration measurements, an approxima-
tion can be done in order to obtain the angular acceleration:
p
ip
=
p
ip
(t + t)
p
ip
(t)
t
i
ip
= (T
p
i
)
T

p
ip
(13)
By integrating the platform kinematic acceleration, the speed can be obtained
v
i
P
= v
0
i
P
+
_
t
t
0
a
i
P
dt (14)
where v
0
i
P
is the platform initial inertial speed obtained by initialization.
Then, by integrating the obtained platform inertial speed, the position can be obtained
r
i
P
= r
0
i
P
+
_
t
t
0
v
i
P
dt (15)
where r
0
i
P
is the platform initial inertial position also obtained by initialization.
Since the platform is not able to measure the kinematic acceleration, but only spe-
cic force, a gravitational acceleration model (see section 5.4) applied at the platform
position must be used in order to compute the gravitational acceleration part contained
in the total kinematic acceleration needed in the integration. Furthermore, since the
Issue: Issue 1
Page: 27 of 68
specic force is measured in the platform reference frame, it must be converted to ECI
by using the platform ECI attitude. Thus the equation for kinematic acceleration is
a
i
P
= T
p
i
(q
p
i
)
T
f
p
P
+g
i
P
(r
i
P
)
= T
p
i
(q
p
i
)
T
(
f
p
P
b
p
f

p
f
) +g
i
P
(r
i
P
)
= T
p
i
(q
p
i
)
T
(f
p
P
b
p
f
) +g
i
P
(r
i
P
) T
p
i
(q
p
i
)
T

p
f
(16)
Also note that while the bias can be estimated and used in the integration, the noise
can not. However, by using its standard deviation in the lter covariances propagation
process, it can better guide the speed estimation (and indirectly the position by further
integration). Thus in the equation above, only the rst two terms are actually consid-
ered in the speed propagation.
The gure 4 represents the propagation for the platform navigation computation. It
is possible to note that in the kinematic acceleration computation there is a need for a
positive fedback on the position, which comes from previous integrations. For that rea-
son a stand alone inertial navigation system without external adding can drift mainly
in the vertical position because of the dependency on the gravitational model on the
position. It is here that comes the importance of using the GPS receiver with bounded
error to reset the states for next propagation.
In the attitude integration there is also a similar eect, which in turn causes the attitude
drift with the time.
There is also a further dependency of kinematic acceleration on previous integrations
of attitude, which represent also a source for positioning and speed drift with the time.
As shown later in section 7.4.1, the fact of the GPS antenna is installed with a lever
arm with respect to IMU, brings the possibility of also updating the attitude using the
dependencies of the pseudoranges with respect to the quaternions.
Issue: Issue 1
Page: 28 of 68
f
P
p
+
g
P
i
r
P
i
Gravitational
model
T
i
p
x
x
T
T
p
i
f
P
i
a
P
i
r
0P
i
v
0P
i
v
P
i
Rotation
matrix
ip
p
ip
p
q
i
p
x1/2
x
q
0i
p
q
i
p
.

Quaternion
T
0i
p
Figure 4: Platform navigation mechanization.
Once the platform inertial navigation solution is known, the vehicle inertial navigation
solution can be calculated by the installation equations as explained in next sections.
Issue: Issue 1
Page: 29 of 68
5.4. Gravitational Model
The gravitational acceleration in function of the position r, due geodetic eects, can be
regarded as:
g(r) =
E
r
3
r
3
2

J
2
E
A
2
E
r
5

_
_
_
1 5
_
r
z
r
_
2
_
r
x
_
1 5
_
r
z
r
_
2
_
r
y
_
3 5
_
r
z
r
_
2
_
r
z
_
_
(17)
Note the model above can be used for both ECEF or ECI. The derivatives of the model
with respect to the position are
g(r
i
)
r
i
=
_
g
r
x
g
r
y
g
r
z
_
(18)
g
r
x
=
_
E
r
3
+ 3
E
r
2
x
r
5

3
2
J
2
E
A
2
E
1
r
5
+
15
2
J
2
E
A
2
E
r
2
x
r
7
+
15
2
J
2
E
A
2
E
r
2
z
r
7

105
2
J
2
E
A
2
E
r
2
x
r
2
z
r
9
3
E
r
y
r
x
r
5
+
15
2
J
2
E
A
2
E
r
y
r
x
r
7

105
2
J
2
E
A
2
E
r
2
z
r
y
r
x
r
9
3
E
r
z
r
x
r
5
+
45
2
J
2
E
A
2
E
r
z
r
x
r
7

105
2
J
2
E
A
2
E
r
3
z
r
x
r
9
_
_
(19)
g
r
y
=
_
_
3
E
r
x
r
y
r
5
+
15
2
J
2
E
A
2
E
r
x
r
y
r
7

105
2
J
2
E
A
2
E
r
2
z
r
x
r
y
r
9
E
r
3
+ 3
E
r
2
y
r
5

3
2
J
2
E
A
2
E
1
r
5
+
15
2
J
2
E
A
2
E
r
2
y
r
7
+
15
2
J
2
E
A
2
E
r
2
z
r
7

105
2
J
2
E
A
2
E
r
2
y
r
2
z
r
9
3
E
r
z
r
y
r
5
+
45
2
J
2
E
A
2
E
r
z
r
y
r
7

105
2
J
2
E
A
2
E
r
3
z
r
y
r
9
_
_
(20)
g
r
z
=
_
_
3
E
r
x
r
z
r
5
+
45
2
J
2
E
A
2
E
r
x
r
z
r
7

105
2
J
2
E
A
2
E
r
3
z
r
x
r
9
3
E
r
y
r
z
r
5
+
45
2
J
2
E
A
2
E
r
y
r
z
r
7

105
2
J
2
E
A
2
E
r
3
z
r
y
r
9
E
r
3
+ 3
E
r
2
z
r
5

9
2
J
2
E
A
2
E
1
r
5
+ 45 J
2
E
A
2
E
r
2
z
r
7
105
2
J
2
E
A
2
E
r
4
z
r
9
_
_
(21)
Issue: Issue 1
Page: 30 of 68
5.5. Navigation transformation
Although the time integration of the kinematic acceleration and the angular rate gives a
navigation solution for the platform, what is of real interest is the navigation solution for
the vehicle, on which the platform is installed. Hence after the platform navigation was
obtained, it must be translated to the vehicle navigation. Furthermore, since the GPS
measurements are related to the antenna reference frame, there is also the need to relate
them with respect to the platform reference frame in order to enable the navigation lter
to update the platform navigation.
Hence, the system must be able to change among vehicle, receiver and platform naviga-
tion variables. This task is accomplished through the installation relationship equations.
Also, as shown later in section 7.4.1, the fact of using a lever arm for GPS antenna with
respect to IMU, brings the possibility of also updating the attitude using the dependen-
cies of the pseudoranges with respect to the quaternions.
Depending on the design of the user vehicle, the installation position of the platform or
receiver antenna and its angular alignment with respect to the vehicle reference frame
can be diverse. In the Figure 5, the installation relationship is sketched for platform and
receiver antenna.
r
P
i
Y
i
Z
i
X
i
X
v
P
X
p
Y
p
i
Z
p
Y
v
Z
v
L
P
v
V
r
V
i
r
R
i
Y
i
Z
i
X
i
X
v
R
X
r
Y
r
i
Z
r
Y
v
Z
v
L
R
v
V
r
V
i
Figure 5: Platform/Antenna installation.
Note that since the navigation is computed with respect to platform, it needs initial
conditions with respect to platform, what is although given with respect to vehicle.
Thus, the given vehicle initial conditions can be translated to the platform by using the
frame relationship as presented in the following section.
Issue: Issue 1
Page: 31 of 68
5.5.1. Platform X Vehicle
Since the platform navigation solution is inuenced by vehicle motions, and that the
platform is not necessarily installed on vehicle CG, the following relations must be estab-
lished (See Appendix B.1.2). The relationships between platform and vehicle navigation
are
Vehicle to Platform:
T
p
i
= T
p
v
T
v
i
i
ip
=
i
iv
i
ip
=
i
iv
r
i
P
= r
i
V
+ (T
v
i
)
T
L
v
P
v
i
P
= v
i
V
+ (T
v
i
)
T
(
v
iv
L
v
P
)
a
i
P
= a
i
V
+ (T
v
i
)
T
(
v
iv
v
iv
L
v
P
+
v
iv
L
v
P
)
(22)
where V and P are respectively the vehicle and platform reference origins, which coincide
with their respective CGs, L
v
P
is the platform displacement from vehicle CG, also known
as platform lever arm, and T
p
v
is the platform installation matrix with respect to the
vehicle reference frame.
It is assumed
d(L
v
P
)
dt
= 0,
d
2
(L
v
P
)
dt
2
= 0, w
v
pv
= 0,
d(w
v
pv
)
dt
= 0,
d(T
p
v
)
dt
= 0,
d
2
(T
p
v
)
dt
2
= 0.
Platform to Vehicle:
T
v
p
= (T
p
v
)
T
T
v
i
= T
v
p
T
p
i
L
p
V
= T
p
v
L
v
P
i
iv
=
i
ip
i
iv
=
i
ip
r
i
V
= r
i
P
+ (T
p
i
)
T
L
p
V
v
i
V
= v
i
P
+ (T
p
i
)
T
(
p
ip
L
p
V
)
a
i
V
= a
i
P
+ (T
p
i
)
T
(
p
ip
p
ip
L
p
V
+
p
ip
L
p
V
)
(23)
It is assumed
d(L
p
V
)
dt
= 0,
d
2
(L
p
V
)
dt
2
= 0, w
p
vp
= 0,
d(w
p
vp
)
dt
= 0,
d(T
v
p
)
dt
= 0,
d
2
(T
v
p
)
dt
2
= 0.
Issue: Issue 1
Page: 32 of 68
5.5.2. Receiver Antenna X Vehicle
Since the receiver antenna measurements are inuenced by vehicle motions, and that
the antenna is not necessarily installed on vehicle CG, the following relations must
be established (See Appendix B.1.2). The relationships between receiver antenna and
vehicle navigation are
Vehicle to Receiver Antenna:
T
r
i
= T
r
v
T
v
i
i
ir
=
i
iv
i
ir
=
i
iv
r
i
R
= r
i
V
+ (T
v
i
)
T
L
v
R
v
i
R
= v
i
V
+ (T
v
i
)
T
(
v
iv
L
v
R
)
a
i
R
= a
i
V
+ (T
v
i
)
T
(
v
iv
v
iv
L
v
R
+
v
iv
L
v
R
)
(24)
where V and R are respectively the vehicle and receiver antenna reference origins, which
coincide with their respective CGs, L
v
R
is the antenna displacement from vehicle CG,
also known as antenna lever arm, and T
r
v
is the antenna installation matrix with respect
to the vehicle reference frame.
It is assumed
d(L
v
R
)
dt
= 0,
d
2
(L
v
R
)
dt
2
= 0, w
v
rv
= 0,
d(w
v
rv
)
dt
= 0,
d(T
r
v
)
dt
= 0,
d
2
(T
r
v
)
dt
2
= 0.
Receiver Antenna to Vehicle:
T
v
r
= (T
r
v
)
T
T
v
i
= T
v
r
T
r
i
L
r
V
= T
r
v
L
v
R
i
iv
=
i
ir
i
iv
=
i
ir
r
i
V
= r
i
R
+ (T
r
i
)
T
L
r
V
v
i
V
= v
i
R
+ (T
r
i
)
T
(
r
ir
L
r
V
)
a
i
V
= a
i
R
+ (T
r
i
)
T
(
r
ir
r
ir
L
r
V
+
r
ir
L
r
V
)
(25)
It is assumed
d(L
r
V
)
dt
= 0,
d
2
(L
r
V
)
dt
2
= 0, w
r
vr
= 0,
d(w
r
vr
)
dt
= 0,
d(T
v
r
)
dt
= 0,
d
2
(T
v
r
)
dt
2
= 0.
Issue: Issue 1
Page: 33 of 68
5.5.3. Platform X Receiver Antenna
Since the receiver antenna does not coincide with platform installation on vehicle, the
following relations must be established (See Appendix B.1.2). The relationships between
platform and receiver antenna navigation are
Platform to Receiver Antenna:
T
r
p
= T
r
v
(T
p
v
)
T
T
r
i
= T
r
p
T
p
i
L
p
PR
= T
p
v
(L
v
R
L
v
P
)
i
ir
=
i
ip
i
ir
=
i
ip
r
i
R
= r
i
P
+ (T
p
i
)
T
L
p
PR
v
i
R
= v
i
P
+ (T
p
i
)
T
(
p
ip
L
p
PR
)
a
i
R
= a
i
P
+ (T
p
i
)
T
(
p
ip
p
ip
L
p
PR
+
p
ip
L
p
PR
)
(26)
where P and R are respectively the platform and receiver antenna reference origins,
which coincide with their respective CGs, L
p
PR
is the antenna displacement from platform
origin, also known as antenna lever arm with respect to platform, and T
r
p
is the antenna
installation matrix with respect to the platform reference frame.
It is assumed
d(L
p
PR
)
dt
= 0,
d
2
(L
p
PR
)
dt
2
= 0, w
p
rp
= 0,
d(w
p
rp
)
dt
= 0,
d(T
r
p
)
dt
= 0,
d
2
(T
r
p
)
dt
2
= 0.
Receiver Antenna to Platform:
T
p
r
= T
p
v
(T
r
v
)
T
T
p
i
= T
p
r
T
r
i
L
r
RP
= T
r
v
(L
v
P
L
v
R
)
i
ip
=
i
ir
i
ip
=
i
ir
r
i
P
= r
i
R
+ (T
r
i
)
T
L
r
RP
v
i
P
= v
i
R
+ (T
r
i
)
T
(
r
ir
L
r
RP
)
a
i
P
= a
i
R
+ (T
r
i
)
T
(
r
ir
r
ir
L
r
RP
+
r
ir
L
r
RP
)
(27)
It is assumed
d(L
r
RP
)
dt
= 0,
d
2
(L
r
RP
)
dt
2
= 0, w
r
pr
= 0,
d(w
r
pr
)
dt
= 0,
d(T
p
r
)
dt
= 0,
d
2
(T
p
r
)
dt
2
= 0.
Issue: Issue 1
Page: 34 of 68
6. Ground Initialization and Alignment
At the very start of operation, when the user vehicle is yet stopped on the ground, before
the platform can provide reasonable measurements to be used in navigation computa-
tions, the initial conditions for its navigation solution, that means position, speed and
attitude, must be dened in order to be used in its integration equations (See Section
5.3).
The process by which these values are dened and processed is divided in two parts:
Initialization and Alignment.
Although both process are here treated separated, they are coupled in the sense that the
calculation of initial attitude (alignment) is fully dependent on the position and that the
calculation of initial position and speed (initialization) is also dependent on the attitude.
Thus, the initialization and alignment can be seen as iterative processes, which can be
started using priory given reference states.
Initialization/Alignment
Loop
Alignment
Processing
Position
Initialization
Processing
Vehicle
Reference State
Calculation
0
,
0
, h
0
r
0V
e
,
0 e
v
L
p
v
, T
v
p
IMU
Reference State
Calculation
r
0P
e
R
0
,P
0
,Y
0 a
p
,
ip
p
0 e
p
(k) =
0 e
p
(k-1)
Matrix to
Euler 321
angles
Average
Average
Euler 321 angles
to Matrix
0 e
p
ECI to ECEF
Date,UTC
x
0 i
p
0 e
p
r
0P
e
x
r
0P
i
0 i
e
x
T
Initalize
Align
Pre-Alignment
Processing
r
0P
e
,
0 e
p
0 e
p
x
ie
i
v
0P
i
Figure 6: Initialization and Alignment with vehicle priory information.
However, its worth to observe that although the alignment and initialization are per-
Issue: Issue 1
Page: 35 of 68
formed for the platform and not for the vehicle, it is the vehicle initial conditions that
are available at the start of operation. Thus they must be translated to the platform
before the iterative process starts. The translation must also be performed for the GPS
antenna, in order to use GPS receiver as aiding in the process. Such transformations
are done by using the platform and antenna installation informations.
The process starts by taking the platform computed priori information and doing a rst
alignment with data from platform measurements. The alignment is the process, which
provides attitude information for the platform position calculation. In the loop, the
platform initial position is calculated based on alignment computed attitude and given
vehicle initial position. With the information of the platform position and platform
measurements, the alignment can calculate again the platform attitude, which will be
used in a new platform position calculation. After dened time period, all calculated
values are averaged in order to generate the platform initial position and attitude.
With these results and the current time information, the platform initial position and
attitude are obtained in ECI and by using the Earth angular rate cross product to the
position, the speed is easily obtained.
Note that although the rst priory platform attitude is calculated with basis on given
priory information in the process above, an auto initialization procedure based only
in the priory information of altitude and longitude and platform measurements is also
possible. In this case, the platform measurement components are used to calculate the
latitude, roll, pitch and yaw needed in the Initialization/Alignment loop.
Issue: Issue 1
Page: 36 of 68
Initialization/Alignment
Loop
Alignment
Processing
Position
Initialization
Processing
0
, h
0
L
p
v
, T
v
p
IMU Auto
Reference State
Calculation
r
0P
e
a
p
,
ip
p
0 e
p
(k) =
0 e
p
(k-1)
Matrix to
Euler 321
angles
Average
Average
Euler 321 angles
to Matrix
0 e
p
ECI to ECEF
Date,UTC
x
0 i
p
0 e
p
r
0P
e
x
r
0P
i
0 i
e
x
T
Initalize
Align
Pre-Alignment
Processing
r
0P
e
,
0 e
p
0 e
p
x
ie
i
v
0P
i
0
Figure 7: Auto-Initialization and Alignment.
However, if the priory information for altitude and longitude must be given, and that
the latitude is also generally known, the only advantage is that the roll, pitch and yaw
angles must not be given. Anyway, since the platform measurements contain errors, the
result of such procedure is worse for must of the cases where low precision platform is
used. For High precision platform, the auto-initialization procedure could be possible.
In both processes above, the ECI to ECEF transformation takes place only at the end
in order to avoid the time handling during the loop process.
Once the platform initial state conditions are obtained in ECEF, the transformation to
ECI is
r
0
i
P
= (T
0
e
i
)
T
r
0
e
P
(28)
T
0
p
i
= T
0
p
e
T
0
e
i
(29)
Issue: Issue 1
Page: 37 of 68
The transformation matrix from ECI to ECEF is dened as ([7])
T
0
e
i
(
0E
) =
_
_
cos(
0E
) sin(
0E
) 0
sin(
0E
) cos(
0E
) 0
0 0 1
_
_
(30)
where
0E
is the sideral time angle at initialization which is dened with respect to
current Julian Date ([7]).
6.0.4. Alignment
Alignment is the process by which the platform is initial attitude is calculated. Once
the platform is aligned, the vehicle initial attitude is completely dened by using the
platform installation matrix.
The method here described is called analytical Coarse alignment ([13]). This method
aligns the platform on the ground computationally by means of using the local gravity
and Earth angular rate.
Since the gravitational specic force acts together with the centrifuge eect of the Earth
angular rate, the resulting specic force is called local apparent gravity and is dened
as
g
e
A
(r
0
e
P
) = g
e
(r
0
e
P
) + (
E
e
ie
E
e
ie
r
0
e
P
),

E
e
ie
=
_
0 0

T
(31)
where r
0
e
P
is the platform initial position in ECEF and

E
is the Earth angular rate.
Considering the platform in absence of other external specic forces, its output will be
exactly the measurement of the local apparent gravity and the Earth angular rate in the
platform reference frame. Hence, by using the apparent gravity g
e
A
, the Earth angular
rate

E
e
ie
and the cross product between both g
e
A

E
e
ie
, a equation system can be
constructed, which relates these references to the platform outputs
_
_
( a
p
)
T
(
p
ip
)
T
( a
p
p
ip
)
T
_
_
=
_
_
(g
p
A
)
T
(

E
p
ie
)
T
(g
p
A
E
p
ie
)
T
_
_
=
_
_
(g
e
A
)
T
T
0
p
e
(

E
e
ie
)
T
T
0
p
e
(g
e
A
E
e
ie
)
T
T
0
p
e
_
_
(32)
where T
0
p
e
is the platform initial attitude with respect to ECEF. Note the negative sign
in the accelerometer measurement. That is because the accelerometer is only capable to
measure the specic force that results from the reaction to the apparent gravity.
This method can not be used at or near the Earth poles because of the co-linearity of
both gravity and Earth angular rate. The co-linearity would make the matrix inversion
undened, once the third line in the matrix would be a linear combination of the rst
two.
Issue: Issue 1
Page: 38 of 68
Solving for T
0
p
e
T
0
p
e
=
_
_
(g
e
A
)
T
(

E
e
ie
)
T
(g
e
A
E
e
ie
)
T
_
_
1
_
_
( a
p
)
T
(
p
ip
)
T
( a
p
p
ip
)
T
_
_
(33)
Note that, the alignment process is highly dependent on the platform initial position
r
0
e
P
because the gravitation specic force model.
It is worth to mention that the used values are platform measurements and not the nal
result obtained from the processing of its outputs (such process discounts gravitational
specic force from the accelerometers).
Note: This alignment method is only valid for platform with gyros capable of sensing
Earth rotation rate. In the case of low cost platforms, this method can not be applied
because of the abscense of the rotation rate vector needed in the equation shown before.
In this case the alignment is highly dependent on given initial conditions. Later when
the vehicle stars moving, the attitude can be corrrected by the estimation lter by using
GPS measurements and the fact that the platform and antenna are somehow displaced
from vehicle CG by their Lever arm installation.
6.0.5. Initialization
Initialization is the process by which the initial conditions for platform position r
i
P
,
speed v
i
P
and attitude T
0
p
i
are dened.
However, it is the vehicle initial conditions that are available and even in other reference
frame, like ECEF for example (This reference frame is more suitable for Earth positioning
and operation start).
Then, by means of using the installation information they can be transformed to the
platform conditions in ECEF, and later obtained in ECI with the time information.
As mentioned before, there are two ways of initializing the platform: initialization with
full priory vehicle information or auto-initialization.
6.0.5.1. Initialization with full priory vehicle information
Using the priory vehicle information available at the operation start, the initial condi-
tions are obtained for the platform.
The vehicle initial position in ECEF can be obtained from the geodetic position dened
in terms of in geodetic latitude
0
G
, longitude
0
and altitude h
0
with respect to the
Earth reference ellipsoid WGS-84 ([15]).
r
0
e
V
=
_
_
(R
N
+h
0
)cos
0
G
cos
0
(R
N
+h
0
)cos
0
G
sin
0
(R
N
(1 e
2
E
) +h
0
)sin
0
G
_
_
(34)
Issue: Issue 1
Page: 39 of 68
where e
E
is the Earth ellipsoid excentricity and
R
N
=
A
E
_
1 (e
E
sin
0
G
)
2
(35)
is the length of the local normal from the ellipsoid surface to its intersection with the
ECEF Z axis Z
ECEF
. A
E
is the Earth semimajor axis.
The geodetic latitude is related to the geographic latitude
0
by
0
G
= atan
_
A
2
E
B
2
E
tan(
0
)
_
(36)
For the initial attitude, the vehicle is normally assumed to be leveled. However, if the roll
and pitch angles are to be considered, the complete transformation from NED (North-
East-Down level reference frame) to vehicle reference frame is completely dened by the
roll R
0
, pitch P
0
and yaw Y
0
angles (c = cos, s = sin)
T
v
n
(Y
0
, P
0
, R
0
) =
T
321
(Y
0
, P
0
, R
0
) =
_
_
cP
0
cY
0
cP
0
sY
0
sP
0
cR
0
sY
0
+sR
0
sP
0
cY
0
cR
0
cY
0
+sR
0
sP
0
sY
0
sR
0
cP
0
sR
0
sY
0
+cR
0
sP
0
cY
0
sR
0
cY
0
+cR
0
sP
0
sY
0
cR
0
cP
0
_
_
(37)
The ECEF to NED transformation is function of the longitude and geodetic latitude as
T
0
n
e
(
0
G
,
0
) =
_
_
sin
0
G
cos
0
sin
0
G
sin
0
cos
0
G
sin
0
cos
0
0
cos
0
G
cos
0
cos
0
G
sin
0
sin
0
G
_
_
(38)
Then the vehicle attitude with respect to ECEF is completely established
T
0
v
e
= T
0
v
n
T
0
n
e
(39)
Through the installation information the ECEF reference state for platform is
r
0
e
P
= r
0
e
V
+ (T
0
v
e
)
T
L
v
P
(40)
T
0
p
e
= T
p
v
T
0
v
e
(41)
Once the Initialization/Alignment process was accomplished, the position and attitude
are transformed to ECI as already shown. To complete the initial conditions, the speed
is calculated directly from the ECI position and the Earth angular rate as
v
0
i
P
=
i
ie
r
0
i
P
,
i
ie
= [0 0
E
]
T
(42)
were
E
is the Earth angular rate.
Issue: Issue 1
Page: 40 of 68
6.0.5.2. Auto-Initialization
Using fewer priory vehicle information available, that means, only longitude and altitude,
the initial conditions can still be obtained by using platform measurements.
Remembering that the accelerometers provide measurements for the reaction to the
apparent gravity g
p
A
= a
p
P
, platform angles with respect to a local level reference frame
l with same heading can be obtained by using the apparent gravity components as
y
= sin
1
_
g
A
p
x
|g
p
A
|
_
,
x
= sin
1
_
g
A
p
y
|g
p
A
| cos(
y
)
_
,
z
= 0 (43)
These angles will result in a transformation matrix T
0
p
l
from the local level reference
frame to platform reference frame by using the same expression dened for the roll,
pitch and yaw in the last section, but making R =
x
, P =
y
, and Y =
z
.
This matrix can be used to bring the platform angular rate measurements to the local
level reference frame (yaw not aligned to North direction)

l
ip
= (T
0
p
l
)
T

p
ip
(44)
Then the components of the resulting angular rate can be used to calculate the geodetic
latitude and platform yaw as
l
xy
=
_
(w
l
y
)
2
+ (w
l
x
)
2
,
0
G
= tg
1
_
w
l
z
w
l
xy
_
, Y
p
= tg
1
_
w
l
y
w
l
x
_
(45)
Taking R =
x
, P=
y
, and Y = Y
p
it will result in a transformation matrix T
0
p
n
from
the local North-East-Down reference frame to platform reference frame.
By using the obtained geodetic latitude, the transformation matrix T
0
n
e
from ECEF to
local North-East-Down reference frame can be obtained as shown in last section.
Finally, the platform attitude is
T
0
p
e
= T
0
p
n
T
0
n
e
(46)
Also by using the obtained geodetic latitude and the given altitude and longitude, the
vehicle position in ECEF is obtained as already shown and from it the platform position
in ECEF. From here on, the platform ECI position, speed and attitude are obtained
exactly as already shown.
Issue: Issue 1
Page: 41 of 68
7. Navigation Filter
Here, the state space form of the navigation mechanization and update equations is
presented, as well as their linearization about the states. This represents the system
knowledge to be applied to the generical estimator.
7.1. State Space
Since the estimator used here requires Gaussian processes, the states to be estimated
can not be biased, otherwise the estimator can fail.
Because of that, besides the variables of the navigation solution themselves, some biases
should be estimated in order to be discounted from the corresponding variables in order
to improve estimator performance.
Thus the states with known dierential equation and required biases are grouped as
states in only one vector as the states of the estimator in the form:
X =
_
b
p
q
p
i
b
p
f
v
i
P
V
bd
r
i
P
R
bc
_
T
(47)
where the state initial conditions are represented by X
0
.
Note however that not all navigation variables take place in the state space vector once
the dierential equations that describe their dynamics are not known. The navigation
states that take no part on the state can still be computed as already shown in the
navigation mechanization section.
The Inputs that represent the Platform measurements are grouped in only one vector
in the form:
U =
_

p
ip
f
p
P
_
T
(48)
The Measurements that represent the GPS measurements are grouped in only one vector
in the form:
Z =
_

PR
1
...

PR
N

dPR
1
...

dPR
N
_
T
(49)
where N is the number of tracked satellites.
The state dynamics is represented by the time derivation for each the state grouped
in the form:
f(X, U) =

X =
_
b
p
q
p
i
b
p
f
v
i
P
V
bd
r
i
P
R
bc
_
T
(50)
The noises W associated to the system process are assumed independent and are dened
as Gaussian processes ([2]) with the diagonal covariance matrix Q such as:
W(t) N(0, Q(t)) (51)
Issue: Issue 1
Page: 42 of 68
The measurement model is represented in the form:
h(X) =

Z =
_

PR
1
...

PR
N

dPR
1
...

dPR
N
_
T
(52)
The noises V associated to the measurement process are assumed independent. and are
dened as Gaussian processes with the diagonal covariance matrix R such as:
V (t) N(0, R(t)) (53)
7.1.1. State Derivatives with respect to time
Angular rate bias:
b
p
w
= 0 (54)
Quaternions:
q
p
i
=
1
2

q
(
p
ip
b
p
) q
p
i
(55)
Specic force bias:
b
p
f
= 0 (56)
Speed:
v
i
P
= T
p
i
(q
p
i
)
T
(
f
p
P
b
p
f
) +g(r
i
P
) (57)
Clock drift Speed:
V
bd
= 0 (58)
Position:
r
i
P
= v
i
P
(59)
Clock bias Range:
R
bc
= V
bd
(60)
Issue: Issue 1
Page: 43 of 68
7.2. State System Matrix
The State system matrix F is given by the linearization of system dynamics around the
current estimation of state X applying the Jacobian to f function with respect to the
states. This procedure yields the linearized system dynamics matrix:
F =
f(X, U)
X
X=

X
=
_
_
(
b
p
)
b
p
b
p
)
q
p
i
(
b
p
)
b
p
f
(
b
p
)
v
i
P
(
b
p
)
V
bd
(
b
p
)
r
i
P
(
b
p
)
R
bc
( q
p
i
)
b
p
( q
p
i
)
q
p
i
( q
p
i
)
b
p
f
( q
p
i
)
v
i
P
( q
p
i
)
V
bd
( q
p
i
)
r
i
P
( q
p
i
)
R
bc
(
b
p
f
)
b
p
b
p
f
)
q
p
i
(
b
p
f
)
b
p
f
(
b
p
f
)
v
i
P
(
b
p
f
)
V
bd
(
b
p
f
)
r
i
P
(
b
p
f
)
R
bc
( v
i
P
)
b
p
( v
i
P
)
q
p
i
( v
i
P
)
b
p
f
( v
i
P
)
v
i
P
( v
i
P
)
V
bd
( v
i
P
)
r
i
P
( v
i
P
)
R
bc
(

V
bd
)
b
p
(

V
bd
)
q
p
i
(

V
bd
)
b
p
f
(

V
bd
)
v
i
P
(

V
bd
)
V
bd
(

V
bd
)
r
i
P
(

V
bd
)
R
bc
( r
i
P
)
b
p
( r
i
P
)
q
p
i
( r
i
P
)
b
p
f
( r
i
P
)
v
i
P
( r
i
P
)
V
bd
( r
i
P
)
r
i
P
( r
i
P
)
R
bc
(

R
bc
)
b
p
(

R
bc
)
q
p
i
(

R
bc
)
b
p
f
(

R
bc
)
v
i
P
(

R
bc
)
V
bd
(

R
bc
)
r
i
P
(

R
bc
)
R
bc
_
_
(61)
where the partial derivatives above are matrices containing derivatives of the respective
state time derivative with respect to each of the states.
Issue: Issue 1
Page: 44 of 68
7.2.1. Derivative of Angular rate bias time Derivatives with respect to State
Space
Angular rate bias:
(
b
p
)
b
p
= 0
33
(62)
Quaternions:
(
b
p
)
q
p
i
= 0
34
(63)
Specic force bias:
(
b
p
)
b
p
f
= 0
33
(64)
Speed:
(
b
p
)
v
i
P
= 0
33
(65)
Speed due clock drift:
(
b
p
)
V
bd
= 0
31
(66)
Position:
(
b
p
)
r
i
P
= 0
33
(67)
Range due clock bias:
(
b
p
)
R
bc
= 0
31
(68)
Issue: Issue 1
Page: 45 of 68
7.2.2. Derivative of Quaternions time Derivatives with respect to State Space
Angular rate bias:
( q
p
i
)
b
p
=
1
2

b
p
q
(b
p
) q
p
i
_
=
1
2
A
q
(q
p
i
) (69)
where
A
q
(q) =
_
_
q
4
q
3
q
2
q
3
q
4
q
1
q
2
q
1
q
4
q
1
q
2
q
3
_
_
(70)
Quaternions:
( q
p
i
)
q
p
i
=
1
2

q
(
p
ip
b
p
) (71)
Specic force bias:
( q
p
i
)
b
p
f
= 0
43
(72)
Speed:
( q
p
i
)
v
i
P
= 0
43
(73)
( q
p
i
)
V
bd
= 0
41
(74)
Position:
( q
p
i
)
r
i
P
= 0
43
(75)
Issue: Issue 1
Page: 46 of 68
( q
p
i
)
R
bc
= 0
41
(76)
Issue: Issue 1
Page: 47 of 68
7.2.3. Derivative of Specic force bias time Derivatives with respect to State
Space
Angular rate bias:
(
b
p
f
)
b
p
= 0
33
(77)
Quaternions:
(
b
p
f
)
q
p
i
= 0
34
(78)
Specic force bias:
(
b
p
f
)
b
p
f
= 0
33
(79)
Speed:
(
b
p
f
)
v
i
P
= 0
33
(80)
(
b
p
f
)
V
bd
= 0
31
(81)
Position:
(
b
p
f
)
r
i
P
= 0
33
(82)
(
b
p
f
)
R
bc
= 0
31
(83)
Issue: Issue 1
Page: 48 of 68
7.2.4. Derivative of Speed time Derivatives with respect to State Space
Angular rate bias:
( v
i
P
)
b
p
= 0
33
(84)
Quaternions:
( v
i
P
)
q
p
i
=

q
p
i
(T
p
i
(q
p
i
)
T
(
f
p
P
b
p
f
))
=
_
_
T
p
i
q
1
_
T
(
f
p
P
b
p
f
)
_
T
p
i
q
2
_
T
(
f
p
P
b
p
f
)
_
T
p
i
q
3
_
T
(
f
p
P
b
p
f
)
_
T
p
i
q
4
_
T
(
f
p
P
b
p
f
)
_
(85)
where
T
p
i
q
1
= 2
_
_
q
1
q
2
q
3
q
2
q
1
q
4
q
3
q
4
q
1
_
_
T
p
i
q
2
= 2
_
_
q
2
q
1
q
4
q
1
q
2
q
3
q
4
q
3
q
2
_
_
T
p
i
q
3
= 2
_
_
q
3
q
4
q
1
q
4
q
3
q
2
q
1
q
2
q
3
_
_
T
p
i
q
4
= 2
_
_
q
4
q
3
q
2
q
3
q
4
q
1
q
2
q
1
q
4
_
_
(86)
Specic force bias:
( v
i
P
)
b
p
f
=

b
p
f
((T
p
i
)
T
b
p
f
) = (T
p
i
)
T
(87)
Speed:
( v
i
P
)
v
i
P
= 0
33
(88)
( v
i
P
)
V
bd
= 0
31
(89)
Position:
Issue: Issue 1
Page: 49 of 68
( v
i
P
)
r
i
P
=

r
i
P
(g(r
i
P
)) =
_
g(r
i
P
)
r
i
P x
g(r
i
P
)
r
i
P y
g(r
i
P
)
r
i
P z
_
(90)
( v
i
P
)
R
bc
= 0
31
(91)
Issue: Issue 1
Page: 50 of 68
7.2.5. Derivative of Clock drift speed time Derivatives with respect to State
Space
Angular rate bias:
(

V
bd
)
b
p
= 0
13
(92)
Quaternions:
(

V
bd
)
q
p
i
= 0
14
(93)
Specic force bias:
(

V
bd
)
b
p
f
= 0
13
(94)
Speed:
(

V
bd
)
v
i
P
= 0
13
(95)
(

V
bd
)
V
bd
= 0 (96)
Position:
(

V
bd
)
r
i
P
= 0
13
(97)
(

V
bd
)
R
bc
= 0 (98)
Issue: Issue 1
Page: 51 of 68
7.2.6. Derivative of Position time Derivatives with respect to State Space
Angular rate bias:
( r
i
P
)
b
p
= 0
33
(99)
Quaternions:
( r
i
P
)
q
p
i
= 0
34
(100)
Specic force bias:
( r
i
P
)
b
p
f
= 0
33
(101)
Speed:
( r
i
P
)
v
i
P
= I
33
(102)
( r
i
P
)
V
bd
= 0
31
(103)
Position:
( r
i
P
)
r
i
P
= 0
33
(104)
( r
i
P
)
R
bc
= 0
31
(105)
Issue: Issue 1
Page: 52 of 68
7.2.7. Derivative of Clock bias range time Derivatives with respect to State
Space
Angular rate bias:
(

R
bc
)
b
p
= 0
13
(106)
Quaternions:
(

R
bc
)
q
p
i
= 0
14
(107)
Specic force bias:
(

R
bc
)
b
p
f
= 0
13
(108)
Speed:
(

R
bc
)
v
i
P
= 0
13
(109)
(

R
bc
)
V
bd
= 1 (110)
Position:
(

R
bc
)
r
i
P
= 0
13
(111)
(

R
bc
)
R
bc
= 0 (112)
Issue: Issue 1
Page: 53 of 68
7.3. System Noise Covariance Matrix
The System noise covariance matrix Q is given by the noise standard deviation acting
on the state space system dynamics and as function of the state X such as
Q =
_
_
Q
b
0 0 0 0 0 0
0 Q
q
0 0 0 0 0
0 0 Q
bf
0 0 0 0
0 0 0 Q
v
0 0 0
0 0 0 0 Q
V bd
0 0
0 0 0 0 0 Q
r
0
0 0 0 0 0 0 Q
Rbc
_
_
(113)
where the matrices above are diagonal matrices containing the respective system noise
standard deviations for each state. Denoting D(v) is the diagonal matrix resulting from
the elements of v the diagonal matrices are as following:
Q
b
= 0
33
(114)
Q
q
= D(
1
2

q
(
) q
p
i
) (115)
where
is determined by averaging sensor output.

Q
bf
= 0
33
(116)
Q
v
= D(T
p
i
(q
p
i
)
T

f
) (117)
where
f
is determined by averaging sensor output.
Q
V bd
= 0 (118)
Q
r
= 0
33
(119)
Q
Rbc
=
bd
(120)
where
bd
is determined by averaging clock drift speed estimation at receiver initializa-
tion.
Issue: Issue 1
Page: 54 of 68
7.4. Measurement coupling Matrix
The Measurement coupling matrix H is given by the linearization of measurement dy-
namics around the current estimation of state X applying the Jacobian to h function
with respect to the states. This procedure yields the linearized observation matrix:
H =
h(X, U)
X
X=

X
=
_
_
(

PR
k
)
b
p
(

PR
k
)
q
p
i
(

PR
k
)
b
p
f
(

PR
k
)
v
i
P
(

PR
k
)
V
bd
(

PR
k
)
r
i
P
(

PR
k
)
R
bc
...
(

dPR
k
)
b
p
(

dPR
k
)
q
p
i
(

dPR
k
)
b
p
f
(

dPR
k
)
v
i
P
(

dPR
k
)
V
bd
(

dPR
k
)
r
i
P
(

dPR
k
)
R
bc
...
_
_
(121)
where the partial derivatives above are matrices containing derivatives of the respective
measurement prediction with respect to each of the states that must be evaluated for
each allocced satellite k.
Issue: Issue 1
Page: 55 of 68
7.4.1. Derivative of Pseudorange with respect to State Space
Angular rate bias:
(

PR)
b
p
= 0
13
(122)
Quaternions:
(

PR)
q
p
i
=
_

PR
q
1

PR
q
2

PR
q
3

PR
q
4
_
(123)
where
(

PR)
q
i
=
(r
i
S
r
i
R
)
T
R

r
i
R
q
i
,
r
i
R
q
i
=
_
T
p
i
q
i
_
T
L
p
PR
(124)
Specic force bias:
(

PR)
b
p
f
= 0
13
(125)
Speed:
(

PR)
v
i
P
= 0
13
(126)
(

PR)
V
bd
= 0 (127)
Position:
(

PR)
r
i
P
=
(r
i
S
r
i
R
)
T
R
(128)
(

PR)
R
bc
= 1 (129)
Issue: Issue 1
Page: 56 of 68
7.4.2. Derivative of Delta-Pseudorange with respect to State Space
Angular rate bias:
(

dPR)
b
p
=
1
R
_
((T
p
i
)
T
(L
p
PR
))
T
(r
i
S
r
i
R
)
_
T
(130)
Quaternions:
(

dPR)
q
p
i
=
_

dPR
q
1

dPR
q
2

dPR
q
3

dPR
q
4
_
(131)
where
(

dPR)
q
i
=
(r
i
S
r
i
R
)
T
R

v
i
R
q
i
(v
i
S
v
i
R
)
T
R

r
i
R
q
i
+ 2
(v
i
S
v
i
R
)
T
(r
i
S
r
i
R
)
R
2

_
(r
i
S
r
i
R
)
T
r
i
R
q
i
_
v
i
R
q
i
=
_
T
p
i
q
i
_
T
((
p
ip
b
p
) L
p
PR
)
(132)
Specic force bias:
(

dPR)
b
p
f
= 0
13
(133)
Speed:
(

dPR)
v
i
P
=
(r
i
S
r
i
R
)
T
R
(134)
(

dPR)
V
bd
= 1 (135)
Position:
(

dPR)
r
i
P
=
_
(v
i
S
v
i
R
)
R
+ 2
(v
i
S
v
i
R
)
T
(r
i
S
r
i
R
)
R
2
(r
i
S
r
i
R
)
_
T
(136)
Issue: Issue 1
Page: 57 of 68
(

dPR)
R
bc
= 0 (137)
Issue: Issue 1
Page: 58 of 68
8. Bibliography
References
[1] P.F. Beer and E.R. Johnston Jr. Mecanica Vetorial para Engenheiros: Din amica.
McGraw-Hill, 1981.
[2] J.S. Bendat and A.G. Piersol. RANDOM DATA:Analysis and Measurement Proce-
dures. Wiley-Interscience, 1971.
[3] K. R. Britting. Inertial Navigation Systems Analysis. John Willey And Sons Inc.,
1971.
[4] R.G. Brown and P.Y.C. Hwang. Introduction to Random Signals and Applied
Kalman Filtering. John Wiley and Sons, 1997.
[5] G. B. Carvalho. Linear/Extended Continuous-Discrete Kalman Filter Model Ap-
plication Example. Technical Report INT-KF-TU-ZAR-002, ZARM Bremen, June
2003.
[6] G. B. Carvalho. Linear/Extended Continuous-Discrete Kalman Filter Tutorial.
Technical Report INT-KF-TU-ZAR-001, ZARM Bremen, June 2003.
[7] G. B. Carvalho. Reference Frame Denitions. Technical Report INT-GEN-DF-
ZAR-001, ZARM Bremen, November 2003.
[8] G. B. Carvalho. GPS-INS-EKF Integration Models Comparison. Technical Report
INT-NAV-TN-ZAR-001, ZARM, January 2004.
[9] G. B. Carvalho. Practical Approaches of the Extended Kalman Filter. Technical
Report INT-KF-TN-ZAR-001, ZARM, August 2004.
[10] G. B. Carvalho. Practical Approaches to the Global Positioning System. Technical
Report INT-GPS-TN-ZAR-001, ZARM, September 2004.
[11] G. B. Carvalho. Practical Approaches to the Inertial Measurement Unity. Technical
Report INT-IMU-TN-ZAR-001, ZARM Bremen, March 2005.
[12] G. B. Carvalho, S. Theil, and H.K. Kuga. Integration Model Comparison for
Coupled Navigation using an IMU and the GPS through the EKF. IV Simp osio
Brasileiro de Engenharia Inercial - INPE, november 2004.
Issue: Issue 1
Page: 59 of 68
[13] A.B. Chateld. Fundamentals of High Accuracy Inertial Navigation, volume 174 of
Progress in Astronautics and Aeronautics. American Institue of Astronautics and
Aeronautics, 1997.
[14] Pedro Ramon Escobal. Methods of Orbit Determination. Krieger Publishing Com-
pany, Malabar, Florida, 2nd edition, 1976.
[15] Eurocontrol, IfEN. WGS 84 Implementation Manual.
[16] J. L. Farrell. Integrated Aircraft Navigation. Academic Press, 1976.
[17] J. L. Farrell and M. Barth. The Global Positioning System and Inertial Navigation.
McGraw-Hill, 1998.
[18] Arthur Gelb, editor. Applied Optimal Estimation. The MIT Press, 1974.
[19] Mohinder S. Grewal, Lawrence R. Weill, and Angus P. Andrews. Global Positioning
Systems, Inertial Navigation and Integration. John Wiley and Sons, Inc., 2001.
[20] M.S. Grewal and A.P. Andrews. Kalman Filtering - Theory and Practice. Prentice
Hall, Englewood Clis, New Jersey, 1993.
[21] B. Hofmann-Wellenhof, H. Lichtenegger, and J. Collins. GPS Theory and Practice.
Springer-Verlag Wien New York, August 2000.
[22] O. Katsuhiko. Engenharia de Controle Moderno. Prentice/Hall do Brasil, 1993.
[23] Xiaoying Kong. Inertial Navigation System Algorithms for Low Cost IMU. PhD
thesis, Department of Mechanical and Mechatronic Engineering of The University
of Sidney, August 2000.
[24] A. Lawrence. Modern Inertial Technology: Navigation, Guidance and Control.
Springer Verlag, 1992.
[25] S. Lipschutz.

Algebra Linear. Cole cao Schaum. McGraw-Hill, 1971.
[26] P.S. Maybeck. Stochastic models, estimation, and control Vol. 1. Mathematics in
Science and Engineering. Academic Press Inc., 1979.
[27] B.W. Parkinson and J.J. Spilker Jr. Global Positioning System: Theory and Ap-
plications Volume I. Progress in Astronautics and Aeronautics, Vol. 163. AIAA,
1996.
[28] R. M. Rogers. Applied Mathematics in Integrated Navigation Systems. AIAA Edu-
cation Series, 2000.
Issue: Issue 1
Page: 60 of 68
[29] M. Schlotterer and S. Theil. Numerical Improvements of Navigation Algorithms
for RLV Demonstrator. In 53rd International Astronautical Congress - The World
Space Congress 2002, Houston, Texas. IAF, October 2002.
[30] M. Schlotterer, S. Theil, M. Wiegand, S. Voegt, and L. L ubke-Ossenbeck, B. Peters.
Navigation System for Auto-Landing of RLV. In 52nd International Astronautical
Congress, Toulouse, France. IAF, October 2001.
[31] S.H. Stovall. Basic Inertial Navigation. Technical Report NAWCWPNS TM 8128,
Naval-Air Warfare Center/Weapons Division, September 1997.
[32] Salah Sukkarieh. Low Cost, High Integrity, Aided Inertial Navigation Systems for
Autonomous Land Vehicles. PhD thesis, Department of Mechanical and Mecha-
tronic Engineering of The University of Sidney, March 2000.
[33] Catherine L. Thornton and Gerald J. Bierman. UDU Covariance Factorization for
Kalman Filtering. In Cornelius Thomas Leondes, editor, Control and Dynamic
Systems: Advances in theory and application, volume 16, pages 177248. Academic
Press, New York, 1980.
[34] S. Voegt, B. L ubke-Ossenbeck, and M. Schlotterer. Tool Concept Design for Nav-
igation System Performance Evaluation - Concept Design Description -. Techni-
cal Report ASTRA-IO64-TN-02, Astrium GmbH, OHB-System, ZARM Bremen,
March 2001.
[35] M. Wei and K. P. Schwarz. A Strapdown Inertial Algorithm Using an Earth-Fixed
Cartesian Frame. In Journal of The Institute of Navigation, Vol. 37, No. 2. The
University of Calgary, Calgary, Alberta, Canada, Summer 1990.
[36] G. Welch and G. Bishop. An Introduction to the Kalman Filter. ACM Inc., 2001.
[37] J.R. Wertz, editor. Spacecraft Attitude Determination and Control. Kluwer Aca-
demic Publishers, Dordrecht, The Netherlands, 1978.
[38] P. Zarchan and H. Muso. Fundamentals of Kalman Filtering: A Practical Ap-
proach. Progress in Astronautics and Aeronautics, Vol. 190. AIAA, 2003.
Issue: Issue 1
Page: 61 of 68
A. Physical Constants
The Earth WGS-84 (World Geodetic System-1984) model parameters are:
Denition Value
Earth mass (M
E
) 5.9742 10
24
(kg)
Earth semimajor axis (A
E
) 6378137.0 (m)
Earth semiminor axis (B
E
) 6356752.3142 (m)
Earth geoid eccentricity (e
E
) 0.08181919
Earth turn rate (
E
) 7.2921151467 10
5
(rad/s)
Earth gravitational constant (
E
) 3.986004418 10
14
(m
3
/s
2
)
Table 2: WGS-84 denitions.
The Earth aspherical zonal harmonics are ([14]):
Denition Value
J
2
(1082.68 0.0) 10
06
J
3
(2.3 0.2) 10
06
J
4
(2.12 0.5) 10
06
J
5
(0.2 0.1) 10
06
J
6
(1.0 0.8) 10
06
Table 3: Earth zonal harmonics.
Physical constants ([37]):
Denition Value
Speed of light in vacuum (c) 299792458 (m/s)
Universal gravitational constant (G) 6.6720 10
11
(m
3
/(kg s
2
))
gravitational acceleration at surface (g) 9.807 (m/s
2
)
Table 4: Physical constants.
Mathematical constants:
Denition Value
3.1415926535898
Table 5: Mathematical constants.
Issue: Issue 1
Page: 62 of 68
B. Mechanics
B.1. Kinematics
B.1.1. Scalar Kinematics
Dening r as the position of a given point, the speed v can be dened as the time
derivative of r, and the acceleration a as the time derivative of v or the second derivative
of r in time ([1]):
v =
dr
dt
= r
a =
dv
dt
= v =
d
2
r
dt
2
= r
(138)
From the denition above follows that the speed v is the integration of a over the time,
dv = a dt
_
v
v
0
dv =
_
t
0
a dt v = v
0
+
_
t
0
a dt (139)
and the position r is the integration of v over the time or the double integration of a
over the time:
dr = v dt
_
r
r
0
dr =
_
t
0
v dt r = r
0
+
_
t
0
v dt
r = r
0
+
_
t
0
(v
0
+
_
t
0
a dt) dt r = r
0
+v
0
t +
_
t
0
(
_
t
0
a dt) dt
(140)
If a is constant:
v = v
0
+at
r = r
0
+v
0
t +
at
2
2
(141)
From these equations follows:
v
2
= (v
0
+at)
2
v
2
= v
2
0
+ 2v
0
at +a
2
t
2
r = r
0
+v
0
t +
at
2
2
v
0
t = r r
0
at
2
2
v
2
= v
2
0
+ 2a(r r
0
at
2
2
) +a
2
t
2
v
2
= v
2
0
+ 2ar
(142)
Using the same relationships for angular motion:
=
0
+t
=
0
+
0
t +
t
2
2
2
=
2
0
+ 2
(143)
Issue: Issue 1
Page: 63 of 68
where is the angular position, is the angular speed and is the angular acceleration.
B.1.2. Vectorial Kinematics
If the position, speed and acceleration are regarded as 3D vectorial entities, the same
denitions shown before can be expanded to the vectorial case:
v =
dr
dt
a =
dv
dt
=
d
2
r
dt
2
(144)
If a reference frame a is dened with its base built by unity vectors
i,
j,
k in the direction
of its axes XY Z, the vectors above can be regarded as cartesian components in this frame
as ([7]):
r
a
=
_
_
r
x
r
y
r
z
_
_
, v
a
=
_
_
v
x
v
y
v
z
_
_
, a
a
=
_
_
a
x
a
y
a
z
_
_
(145)
where the vectorial components above multiply respectively the dened unity vectors:
Y
r
r
x r
y
r
z
Z
X
k
i
j
Figure 8: Vectorial representation in cartesian reference frame.
Issue: Issue 1
Page: 64 of 68
r
a
= r
x
i +r
y
j +r
z
k
v
a
= v
x
i +v
y
j +v
z
k
a
a
= a
x
i +a
y
j +a
z
k
(146)
If another frame b is dened with origin in a point B, them a third point P has its
relative vectors dened with respect to A and B by the following vectorial relationship:
r
P/A
= r
B/A
+r
P/B
v
P/A
= v
B/A
+v
P/B
a
P/A
= a
B/A
+a
P/B
(147)
r
P/A
r
P/B
Y
a
r
B/A
Z
a
X
a
Y
b
Z
b
X
b
P
B
A
X
c
Y
c
Z
c
Figure 9: Vectorial relative representation.
and its coordinates in reference frame a are:
r
a
P/A
= r
a
B/A
+r
a
P/B
v
a
P/A
= v
a
B/A
+v
a
P/B
a
a
P/A
= a
a
B/A
+a
a
P/B
(148)
Issue: Issue 1
Page: 65 of 68
To specify the relations among the dierent coordinates:
r
a
P/A
= r
a
B/A
+T
a
b
r
b
P/B
v
a
P/A
= v
a
B/A
+T
a
b
v
b
P/B
a
a
P/A
= a
a
B/A
+T
a
b
a
b
P/B
(149)
where T
a
b
is the rotation matrix that brings a vector expressed in b to its representation
in a and is equal to the transpose of the attitude matrix of b with respect to a, T
b
a
.
Considering the rotation of the frame b with respect to a, the relationship for the speed
can be regarded as:
r
a
P/A
= r
a
B/A
+

T
a
b
r
b
P/B
+T
a
b
r
b
P/B
= r
a
B/A
+T
a
b
(
b
ab
r
b
P/B
+ r
b
P/B
)
= r
a
B/A
+T
a
b
(
b
ab
r
b
P/B
+ r
b
P/B
)
(150)
where the relations
T
a
b
= T
a
b
b
ab
=
a
ba
T
a
b
(151)
b
ab
= (
b
ab
) =
_
_
0
z

y
z
0
x
y

x
0
_
_
(152)
were used in function of the angular speed
b
ab
([7]).
By observing equations 149 and 150, it is possible to note that the speed of P with
respect to B in this case is also dependent on the angular speed of b with respect to a:
v
b
P/B
=
b
ab
r
b
P/B
+ r
b
P/B
(153)
where the
b
ab
r
b
P/B
is the tangential speed of B due the rotation while r
b
P/B
is the
relative speed of B with respect to the origin A. Note that speed in a rotating frame is
dierent from the derivative of position.
By deriving equation 150 it is possible to obtain the relationship for the acceleration:
r
a
P/A
= r
a
B/A
+

T
a
b
(
b
ab
r
b
P/B
+ r
b
P/B
) +T
a
b
(
b
ab
r
b
P/B
+
b
ab
r
b
P/B
+ r
b
P/B
)
= r
a
B/A
+T
a
b
(
b
ab
b
ab
r
b
P/B
+ 2
b
ab
r
b
P/B
+
b
ab
r
b
P/B
+ r
b
P/B
)
(154)
where
b
ab
is the angular acceleration.
Issue: Issue 1
Page: 66 of 68
By observing equations 149 and 154, it is possible to note that the acceleration of
P with respect to B in this case is also dependent on the angular speed and on angular
acceleration of b with respect to a:
a
b
P/B
=
b
ab
b
ab
r
b
P/B
+ 2
b
ab
r
b
P/B
+
b
ab
r
b
P/B
+ r
b
P/B
(155)
b
ab

b
ab
r
b
P/B
is the radial acceleration of B due the rotation,
b
ab
r
b
P/B
is the
tangential acceleration of B due the rotation rate and 2
b
ab
r
b
P/B
is the Coriolis eect
due the combination of rotation and relative speed. Note that acceleration in a rotating
frame is dierent from the second derivative of position. If the rotation is constant, then
the radial component of acceleration remains but still accompanied by the Coriolis eect
and relative acceleration.
The attitude of c with respect to a is related to its attitude with respect to b by
T
c
a
= T
c
b
T
b
a
T
b
a
= (T
c
b
)
T
T
c
a
(156)
Considering the attitude of c with respect to b xed, deriving this expression, the angular
rate is
T
b
a
= (T
c
b
)
T

T
c
a
(157)
Using relation 151
T
b
a
a
ba
= (T
c
b
)
T
T
c
a

a
ca
a
ba
=
a
ca
a
ba
=
a
ca
(158)
which shows that the rotation rate is independent of the reference frame origin.
Deriving

a
ba
=
a
ca
a
ba
=
a
ca
(159)
which shows that the same conclusion is valid for the angular acceleration.
B.2. Dynamics
Considering a point with mass m, from the linear momentum denition ([1]):
L = m r (160)
Issue: Issue 1
Page: 67 of 68
the Newtons rst law states that
F =
dL
dt
=
dm
dt
r +m
d r
dt
= m r +ma (161)
where
F is the sum of all actuating forces in the mass point.

If m is constant, then:
F = ma (162)
considering the angular momentum denition:
H = r L = r m r = r ( r)m (163)
the Newtons rst law can be applied for angular motion resulting
M = r
F = r m r +r ma =
dH
dt
=

H (164)
If m is constant, then:
M = r ma (165)
Considering a rigid body composed by a system of points where each point has mass
m
i
, i = 1, 2, .., then the relations above can be regarded as:
F =
(m
i
a
i
) = ma
CM
= F
CM
M =
(r
i
m
i
a
i
) = I
CM
= M
CM
(166)
where F
CM
and M
CM
are respectively the resulting force and momentum applied in the
center of mass (CM) of the body, a
CM
is the corresponding resulting specic force, I
CM
is the bodys matrix of inertia with respect to CM and the bodys angular acceleration.
If H and are both measured in the bodys frame, from the equations 164 and 166
follows
H = I
CM
(167)
from where it is possible to note that the angular momentum and the angular speed
are not necessarily aligned for the general case of I
CM
. However, if the main axes of
inertia are addopted, then the matrix of inertia becomes diagonal, eecting that each
component of the angular momentum is proportional to the respective angular speed
component measured in the body main axes of inertia. Furthermore, if the body has
such a symmetry such that the diagonal elements of its inertia matrix are all identic,
then the angular momentum will be aligned with the rotation vector.
Issue: Issue 1
Page: 68 of 68
It is also worth to dene the relation for forces and momentums applied somewhere
else then CM. Considering a point P in the body with position r with respect to CM
and remembering that is independ on the reference point ([1]):
F
P
= ma
P
M
P
= I
CM
+r F
P
= I
P
(168)
where F
P
and M
P
are respectively the force and momentum applied in P, a
P
is the
corresponding specic force, and the the denition
I
P
= I
CM
+m
_
_
r
2
y
+r
2
z
r
x
r
y
r
x
r
z
r
y
r
x
r
2
x
+r
2
z
r
y
r
z
r
z
r
x
r
z
r
y
r
2
x
+r
2
y
_
_
(169)
represents the bodys matrix of inertia with respect to the point P.
If an external rotation described by is applied to a rotating body, then a momentum
will appear resulting from the try of changing the angular momentum. By remembering
the derivative relationship of a vector with respect to a rotating frame from equation
153, the expression for the resulting momentum is written in function of the bodys
angular momentum ([1]):
M
CM
=

H
CM
+ H
CM
(170)
where the second term represents the angular momentum conservation law, well known
as gyroscopic eect.
If

H
CM
= 0, then the momentum above becomes:
M
CM
= H
CM
(171)
which represents the resulting torque on a body which rotates with constant angular
rate when an external rotation is applied, or the resulting external rotation when
an torque is applied. Analysing this equation it is possible to realize that the resulting
torque is orthogonal to both applied external rotation and bodys angular momentum.

GPS-IMU-EKF Navigation System Documentation

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

GPS-IMU-EKF Navigation System Documentation

Uploaded by

Copyright:

Available Formats

GPS-IMU-EKF Navigation

- Angular rate bias in Platform frame (rad/s)

T - Estimated satellite signal total extra receiver delay (s)

are also propagated using given initial conditions

and observation model h, the measurements can be

, on the current knowledge of

. This is known as the data fusion

and on the linearized measurements dynamics H around the current

Table 1: Summary of Continuous-Discrete EKF equations.

is determined by averaging sensor output.

F is the sum of all actuating forces in the mass point.

You might also like