You are on page 1of 24

Sigma-Point Kalman Filtering

for Integrated GPS and Inertial Navigation
John L. Crassidis

University at Buffalo, State University of New York, Amherst, NY 14260-4400
A sigma-point Kalman filter is derived for integrating GPS measurements with inertial
measurements from gyros and accelerometers to determine both the position and the atti-
tude of a moving vehicle. Sigma-point filters use a carefully selected set of sample points
to more accurately map the probability distribution than the linearization of the standard
extended Kalman filter, leading to faster convergence from inaccurate initial conditions in
position/attitude estimation problems. The filter formulation is based on standard inertial
navigation equations. The global attitude parameterization is given by a quaternion, while
a generalized three-dimensional attitude representation is used to define the local attitude
error. A multiplicative quaternion-error approach is used to guarantees that quaternion
normalization is maintained in the filter. Simulation results are shown to compare the
performance of the sigma-point filter with a standard extended Kalman filter approach.
I. Introduction
The integration of Global Positioning System (GPS) signals with Inertial Measurement Units (IMUs)
has become a standard approach for position and attitude determination of a moving vehicle. An Inertial
Navigation System (INS) is best described in the Preface section of the excellent book by Chatfield,
1
who
states “Inertial navigation involves a blend of inertial measurements, mathematics, control system design,
and geodesy.” Historically, INS’s were primarily used for military and commercial aircraft applications due
to their high cost. However, with the advent of cheaper sensors, especially micro-mechanical ones,
2
sev-
eral new applications have become mainstream, including uninhabited air vehicles, micro-robots, and even
guided munitions.

Although these cheaper sensors do not perform as well as high-grade sensors in terms of
drift and white-noise measurement errors, they can be used to meet the requirements of several vehicle po-
sition/attitude knowledge specifications when aided with GPS. This allows for an attractive approach since
a completely self-contained system can be used to calibrate IMUs online using GPS-determined position
observations, while also determining vehicle attitude and rates in realtime.
By far the primary mechanism historically used to blend GPS measurements with IMU data has been the
extended Kalman filter (EKF).
3
There are many approaches to mechanize an integrated GPS/INS in an EKF
though. One aspect involves how GPS observations are used in the filter design. The term “loosely-coupled”
is used to signify that position estimates taken from the GPS are used in the EKF as measurements, while
a “tightly-coupled” configuration utilizes the GPS pseudoranges directly. The main advantage of a tightly-
coupled system is that state quantity estimates can still be provided even when the minimum number of four
GPS satellites is not available. However, a tightly-coupled system requires knowledge of variables used to
implement the tracking loops that may not be readily available. Another aspect of an integrated GPS/INS is
the coordinate system used to described the determined position and attitude. The Earth-Centered-Earth-
Fixed (ECEF) frame, which will be described later, is useful since GPS receivers typically calculate positions
in this frame directly. However, the attitude of an air or ground vehicle is not physical intuitive in this
frame which is why a local frame, such as the North-East-Down (NED) frame, is often used. Also, since
a linearization of the equations of motion is required for the EKF, then using one frame over another can
produce different overall performance characteristics. For example, for long duration navigation, the local

Associate Professor, Department of Mechanical & Aerospace Engineering. Email: johnc@eng.buffalo.edu. Associate Fellow
AIAA.

See http://www.airpower.maxwell.af.mil/airchronicles/cc/pinker.html for recent contributions of GPS/INS to Air Force
competencies.
1 of 24
American Institute of Aeronautics and Astronautics
NED frame separates the unstable vertical axis from the more stable horizontal axes which provides more
intuitive schemes for analyzing INS errors than using the ECEF frame.
4
The conversions between various
frames is well known (see Ref. 1 for more details). However, mapping of the covariance of the errors has not
been seen by the present author. Therefore, one of the goals of this paper is to present covariance mappings
between the NED and ECEF frames.
The EKF is widely used in practice, but it has one well-known drawback. If the errors are not within the
“linear region”, then filter divergence may occur. This is especially a problem for an integrated GPS/INS
since, even though position is well known, attitude and IMU calibration parameters may not be well known
a prior. In fact to this day the most researched topic in INS’s has been initial alignment and attitude
determination.
1
Sigma-point Kalman filters essentially provide derivative-free higher-order approximations
by approximating a Gaussian distribution rather than approximating an arbitrary nonlinear function as
the EKF does.
5
They can provide more accurate results than an EKF, especially when accurate initial
condition states are not well known. A sigma-point GPS/INS filter has been presented in Ref. 6, which also
includes a method to fuse latency lagged observations in a theoretically consistent fashion. The attitude
kinematics in that paper are based on the quaternion, which must obey a normalization constraint that
can be violated in the sigma-point filter since the predicted quaternion mean is derived using an averaged
sum of quaternions. In this current paper an unconstrained three-component attitude-error vector is used
to represent the quaternion error vector and the updates are performed using quaternion multiplication,
leading to a natural way of maintaining the normalization constraint.
7
Also, Ref. 6 using an augmented
matrix approach to handle process noise, which means that a decomposition of a matrix that has length
equal to the length of the state vector plus the process-noise vector is required. In this paper a simple
first-order trapezoidal approximation is used so that a decomposition of a matrix equal to the length of the
state vector is only required. This significantly reduces the computational costs.
The organization of this paper proceeds as follows. First, the various coordinate frames used in INS
are summarized. Conversions from the NED frame to the ECEF frame and vice versa are shown. Then,
covariance mappings from the NED frame to the ECEF frame and vice versa are derived. Next, the linearized
equations for the EKF using NED coordinates are shown, which are derived from a multiplicative quaternion
approach. Then, a specific sigma-point filter, called the Unscented filter, is summarized followed by a review
of the equations required to perform GPS/INS operations with this filter. Finally, simulation results are
shown that compare the performance of the EKF with the Unscented filter with respect to initial condition
errors.
II. Reference Frames
In this section the reference frames used to derive the GPS/INS equations are summarized, as shown in
Figure 1:
• Earth-Centered-Inertial (ECI) Frame: denoted by {
ˆ
i
1
,
ˆ
i
2
,
ˆ
i
3
}. The
ˆ
i
1
axis points toward the vernal
equinox direction (also known as the “First Point of Aries” or the “vernal equinox point”), the
ˆ
i
3
axis
points in the direction of the North pole and the
ˆ
i
2
axis completes the right-handed system (note that
the
ˆ
i
1
and
ˆ
i
2
axes are on the equator, which is the fundamental plane). The ECI frame is non-rotating
with respect to the stars (except for precession of equinoxes) and the Earth turns relative to this
frame.
8
Vectors described using ECI coordinates will have a superscript I (e.g., r
I
).
• Earth-Centered-Earth-Fixed (ECEF) Frame: denoted by {ˆ e
1
, ˆ e
2
, ˆ e
3
}. This frame is similar to the ECI
frame with ˆ e
3
=
ˆ
i
3
; however, the ˆ e
1
axis points in the direction of the Earth’s prime meridian, and
the ˆ e
2
axis completes the right-handed system. Unlike the ECI frame, the ECEF frame rotates with
the Earth. The rotation angle is denoted by Θ in Figure 1. Vectors described using ECEF coordinates
will have a superscript E (e.g., r
E
).
• North-East-Down (NED) Frame: denoted by {ˆ n, ˆ e,
ˆ
d}. This frame is used for local navigation pur-
poses. It is formed by fitting a tangent plane to the geodetic reference ellipse at a point of interest.
3
The
ˆ n axis points true North, the ˆ e points East, and the
ˆ
d axis completes the right-handed system, which
points in the direction of the interior of the Earth perpendicular to the reference ellipsoid. Vectors
described using ECI coordinates will have a superscript N (e.g., r
N
).
2 of 24
American Institute of Aeronautics and Astronautics
Prime
Meridian
Plane
Equatorial
Plane
2
ˆ e
3 3
ˆ
ˆ , i e
ˆ n
ˆ e
1
ˆ e
λ
Φ
GPS Satellite
ˆ
d
R
r
ρ
1
ˆ
i
2
ˆ
i
Inertial
Reference
Direction
E
Θ
Figure 1. Definitions of Various Reference Frames
• Body Frame: denoted by {
ˆ
b
1
,
ˆ
b
2
,
ˆ
b
3
}. This frame is fixed onto the vehicle body and rotates with it.
Conventions typically depend on the particular vehicle. Vectors described using body-frame coordinates
will have a superscript B (e.g., r
B
).
We now discuss the transformations between these reference frames. The transformation from the ECI
frame to the ECEF frame follows
_
¸
_
x
y
z
_
¸
_
E
=
_
¸
_
cos Θ sin Θ 0
−sin Θ cos Θ 0
0 0 1
_
¸
_
_
¸
_
x
y
z
_
¸
_
I
(1)
where {x, y, z}
I
are the components of the ECI position vector, and {x, y, z}
E
are the components of the
ECEF position vector. In order to determine the ECEF position vector we must first determine the angle
Θ, which is related to time. A solar day is the length of time that elapses between the Sun reaching its
highest point in the sky two consecutive times. However, the ECI coordinate system is fixed relative to the
stars, not the Sun. A sidereal day is the length of time that passes between a given fixed star in the sky
crossing a given projected meridian. A sidereal day is 4 minutes shorter than a solar day.
8
The Greenwich
Mean Sidereal Time (GMST) is the mean sidereal time at zero longitude, which can be given by the angle
Θ. Several formulas for the conversion from Universal Time (UT) to GMST are given in the open literature
(e.g., see Ref. 9). One of the most widely-used formulas is presented by Meeus.
10
The ECEF position vector is useful since this gives a simple approach to determine the longitude and
latitude of a user. The Earth’s geoid can be approximated by an ellipsoid of revolution about its minor axis.
A common ellipsoid model is given by the World Geodetic System 1984 model (WGS-84), with semimajor
axis a = 6, 378, 137.0 m and semiminor axis b = 6, 356, 752.3142 m. The eccentricity of this ellipsoid is given
by e = 0.0818. The geodetic coordinates are given by the latitude λ, longitude Φ and height h. To determine
the ECEF position vector, the length of the normal to the ellipsoid is first computed, given by
3
N =
a
_
1 −e
2
sin
2
λ
(2)
3 of 24
American Institute of Aeronautics and Astronautics
Then, given the observer geodetic quantities λ, Φ and h, the observer ECEF position coordinates are com-
puted using
x = (N +h) cos λcos Φ (3a)
y = (N +h) cos λsin Φ (3b)
z = [N(1 −e
2
) +h] sin λ (3c)
The conversion from ECEF to geodetic coordinates is not that straightforward. A complicated closed-form
solution is given in Ref. 3, but a good approximation up to low Earth orbit is given by
11
p =
_
x
2
+y
2
, ζ = atan
_
z a
p b
_
, ¯ e
2
=
a
2
−b
2
b
2
(4a)
λ = atan
_
z + ¯ e
2
b sin
3
ζ
p −e
2
a cos
3
ζ
_
(4b)
Φ = atan2(y, x) (4c)
h =
p
cos λ
−N (4d)
where N is given by Eq. (2) and atan2 is a four quadrant inverse tangent function.
The conversion from ECEF coordinates to NED coordinates involves a rotation matrix from the known
latitude and longitude. By the definition of the NED frame, a vehicle is fixed within this frame. This frame
serves to define local directions for the velocity vector determined in a frame in which the vehicle has motion,
such as the ECEF frame.
4
The velocity in NED coordinates is given by
v
N

_
¸
_
v
N
v
E
v
D
_
¸
_ = A
N
E
˙ r
E
(5)
where ˙ r
E
is the vehicle velocity in ECEF coordinates and A
N
E
is the transformation matrix from the ECEF
frame to the NED frame. We should note that v
N
= ˙ r
N
in general since ˙ r
N
= v
N
−ω
N
N/E
×r
N
, where ω
N
N/E
is the angular velocity of the N frame relative to the E frame expressed in N coordinates. This relationship
can be derived by differentiating r
N
= A
N
E
r
E
. The NED frame is generally not used to provide a vehicle’s
positional coordinates, but rather to provide local directions along which the velocities may be indicated.
The positions are determined by relating the velocity v
N
with the derivatives of latitude, longitude and
height, and integrating the resulting equations (see Ref. 4 for more details). The transformation matrix is
given by
3
A
N
E
=
_
¸
_
−sin λcos Φ −sin λsin Φ cos λ
−sin Φ cos Φ 0
−cos λcos Φ −cos λsin Φ −sin λ
_
¸
_ (6)
The attitude matrix which maps the NED frame to the vehicle body frame is given by
A
B
N
=
_
¸
_
cos ψ cos θ sin ψ cos θ −sin θ
−sin ψ cos φ + cos ψ sin θ sin φ cos ψ cos φ + sin ψ sin θ sin φ cos θ sin φ
sin ψ sin φ + cos ψ sin θ cos φ −cos ψ sin φ + sin ψ sin θ cos φ cos θ cos φ
_
¸
_ (7)
where φ, θ and ψ are the roll, pitch and yaw angles, respectively. Note that the transformation from the
ECEF to the body frame is simply given by A
B
E
= A
B
N
A
N
E
.
A. Covariance Mappings
In this section various covariance mappings from the ECEF frame to the the NED frame and vice versa
are shown. These expressions are required since the Kalman filter will be developed in ECEF coordinates.
Denote the covariance associated with the attitude matrix that maps the ECEF frame to the body frame
by P
ECEF
att
. The covariance of the attitude matrix that maps the NED frame to the body frame, denoted by
P
NED
att
, is given by
P
NED
att
= A
N
E
P
ECEF
att
A
E
N
(8)
4 of 24
American Institute of Aeronautics and Astronautics
where A
E
N
is the transpose of A
N
E
. The velocity covariance follows from Eq. (5):
P
NED
vel
= A
N
E
P
ECEF
vel
A
E
N
(9)
The covariance of latitude, longitude and height from the ECEF position covariance is more difficult. To
determine this quantity we require the partials of Eq. (4) with respect to x, y and z. The partials are given
by
∂λ
∂x
=
1
1 +u
2
∂u
∂x
,
∂λ
∂y
=
1
1 +u
2
∂u
∂y
,
∂λ
∂z
=
1
1 +u
2
∂u
∂z
(10)
where u is defined by
u ≡
z + ¯ e
2
b sin
3
ζ
p −e
2
a cos
3
ζ
(11)
The partials of u with respect to x, y and z are given by
∂u
∂x
=−
(z + ¯ e
2
b sin
3
ζ)
(p −e
2
a cos
3
ζ)
2
_
x
p
+e
2
a
∂ζ
∂x
sin ζ cos
2
ζ
_
+
3¯ e
2
b sin
2
ζ cos ζ
p −e
2
a cos
3
ζ
∂ζ
∂x
(12a)
∂u
∂x
=−
(z + ¯ e
2
b sin
3
ζ)
(p −e
2
a cos
3
ζ)
2
_
y
p
+e
2
a
∂ζ
∂y
sin ζ cos
2
ζ
_
+
3¯ e
2
b sin
2
ζ cos ζ
p −e
2
a cos
3
ζ
∂ζ
∂y
(12b)
∂u
∂x
=
_
3¯ e
2
b sin
2
ζ cos ζ
p −e
2
a cos
3
ζ

(z + ¯ e
2
b sin
3
ζ)(e
2
a sin ζ cos
2
ζ)
(p −e
2
a cos
3
ζ)
2
_
∂ζ
∂z
+
1
p −e
2
a cos
3
ζ
(12c)
where
∂ζ
∂x
= −
xz a b
p (p
2
b
2
+z
2
a
2
)
,
∂ζ
∂y
= −
y z a b
p (p
2
b
2
+z
2
a
2
)
,
∂ζ
∂z
=
p a b
p
2
b
2
+z
2
a
2
(13)
The partials of Eq. (4c) with respect to x, y and z are given by
∂Φ
∂x
= −
y
x
2
+y
2
,
∂Φ
∂y
=
x
x
2
+y
2
,
∂Φ
∂z
= 0 (14)
The partials of Eq. (4d) with respect to x, y and z are given by
∂h
∂x
=
x
p cos λ
+
_
p sin λ
cos
2
λ

∂N
∂λ
_
∂λ
∂x
(15a)
∂h
∂y
=
y
p cos λ
+
_
p sin λ
cos
2
λ

∂N
∂λ
_
∂λ
∂y
(15b)
∂h
∂z
=
_
p sin λ
cos
2
λ

∂N
∂λ
_
∂λ
∂z
(15c)
where
∂N
∂λ
=
a e
2
sin λcos λ
(1 −e
2
sin
2
λ)
3/2
(16)
Next, the following sensitivity matrix is formed:
H ≡
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
∂λ
∂x
∂λ
∂y
∂λ
∂z
∂Φ
∂x
∂Φ
∂y
∂Φ
∂z
∂h
∂x
∂h
∂y
∂h
∂z
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(17)
5 of 24
American Institute of Aeronautics and Astronautics
Then, the covariance of the latitude, longitude and height, denoted by P
LLH
pos
, is given by
P
LLH
pos
= H P
ECEF
pos
H
T
(18)
where P
ECEF
pos
denotes the covariance of the ECEF position errors.
The attitude and velocity mappings from the NED frame to the ECEF frame is straightforward from
Eqs. (8) and (9), each given by P
ECEF
att
= A
E
N
P
NED
att
A
N
E
and P
ECEF
vel
= A
E
N
P
NED
vel
A
N
E
. The covariance of ECEF
positions from the latitude, longitude and height covariance is derived by taking the partials of the quantities
in Eq. (3) with respect to λ, Φ and h, which leads to the following partial matrix:
H =
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
∂N
∂λ
cos λcos Φ −(N +h) sin λcos Φ −(N +h) cos λsin Φ cos λcos Φ
∂N
∂λ
cos λsin Φ −(N +h) sin λsin Φ (N +h) cos λcos Φ cos λsin Φ
∂N
∂λ
(1 −e
2
) sin λ + [N(1 −e
2
) +h] cos λ 0 sin λ
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(19)
Then, the covariance of the ECEF position covariance is given by
P
ECEF
pos
= HP
LLH
pos
H
T
(20)
III. Attitude Kinematics
In this section the basic properties of attitude kinematics are summarized. The attitude matrix involves
a total of nine parameters, but they are clearly not independent. Various parameterizations of the attitude
matrix can be used: Euler angles, Euler axis and rotation angle, quaternions, Rodrigues parameters, etc.
12
One of the most useful attitude parameterization is given by the quaternion,
13
which is a four-dimensional
vector, defined as q ≡ [̺
T
q
4
]
T
, with ̺ ≡ [q
1
q
2
q
3
]
T
= ˆ e sin(ϑ/2) and q
4
= cos(ϑ/2), where ˆ e is the
axis of rotation and ϑ is the angle of rotation. Since a four-dimensional vector is used to describe three
dimensions, the quaternion components cannot be independent of each other. The quaternion satisfies a
single constraint given by q
T
q = 1, which is analogous to requiring that ˆ e be a unit vector in the Euler
axis/angle parameterization.
12
The attitude matrix that transform the NED frame to the body frame is
related to the quaternion by
A
B
N
(q) = Ξ
T
(q)Ψ(q) (21)
with
Ξ(q) ≡
_
q
4
I
3×3
+ [̺×]
−̺
T
_
, Ψ(q) ≡
_
q
4
I
3×3
−[̺×]
−̺
T
_
(22)
where [̺×] is the cross product matrix, defined by
[̺×] ≡
_
¸
_
0 −q
3
q
2
q
3
0 q
1
−q
2
q
1
0
_
¸
_ (23)
An advantage to using quaternions is that the attitude matrix is quadratic in the parameters and also does
not involve transcendental functions. For small angles the vector part of the quaternion is approximately
equal to half angles so that ̺ ≈ α/2 and q
4
≈ 1, where α is a vector of the roll, pitch and yaw angles. The
attitude matrix can then be approximated by A
B
N
≈ I
3×3
− [α×] which is valid to within first-order in the
angles.
The attitude kinematics equation is given by
˙
A
B
N
= −[ω
B
B/N
×]A
B
N
(24)
where ω
B
B/N
is angular velocity of the B frame relative to the N frame expressed in B coordinates. Another
form of Eq. (24) is given by
˙
A
N
B
= A
N
B

B
B/N
×] (25)
6 of 24
American Institute of Aeronautics and Astronautics
which will be used in the derivation of the INS equations. The quaternion kinematics equation is given by
˙ q =
1
2
Ξ(q)ω
B
B/N
=
1
2
Ω(ω
B
B/N
)q (26)
where
Ω(ω
B
B/N
) ≡
_
¸
_
−[ω
B
B/N
×] ω
B
B/N
−(ω
B
B/N
)
T
0
_
¸
_ (27)
A major advantage of using quaternions is that the kinematics equation is linear in the quaternion and is also
free of singularities. Another advantage of quaternions is that successive rotations can be accomplished using
quaternion multiplication. Here we adopt the convention of Lefferts, Markley, and Shuster
14
who multiply
the quaternions in the same order as the attitude matrix multiplication (in contrast to the usual convention
established by Hamiliton
13
). Suppose we wish to perform a successive rotation. This can be written using
A(q

)A(q) = A(q

⊗q) (28)
The composition of the quaternions is bilinear, with
q

⊗q =
_
Ψ(q

) q

_
q =
_
Ξ(q) q
_
q

(29)
Also, the inverse quaternion is defined by
q
−1

_
−̺
q
4
_
(30)
Note that q ⊗ q
−1
= [0 0 0 1]
T
, which is the identity quaternion. A computationally efficient algorithm
to extract the quaternion from the attitude matrix is given in Ref. 15. A more thorough review of the
attitude representations shown in this section, as well as others, can be found in the excellent survey paper
by Shuster
12
and in the book by Kuipers.
16
IV. INS Basic Equations
The basic INS equations using NED coordinates with the quaternion parameterization are given by
3, 4
˙ q =
1
2
Ξ(q)ω
B
B/N
(31a)
˙
λ =
v
N
R
λ
+h
(31b)
˙
Φ =
v
E
(R
Φ
+h) cos λ
(31c)
˙
h = −v
D
(31d)
˙ v
N
= −
_
v
E
(R
Φ
+h) cos λ
+ 2ω
e
_
v
E
sin λ +
v
N
v
D
R
λ
+h
+a
N
(31e)
˙ v
E
=
_
v
E
(R
Φ
+h) cos λ
+ 2ω
e
_
v
N
sin λ +
v
E
v
D
R
Φ
+h
+ 2ω
e
v
D
cos λ +a
E
(31f)
˙ v
D
= −
v
2
E
R
Φ
+h

v
2
N
R
λ
+h
−2ω
e
v
E
cos λ +g +a
D
(31g)
where ω
e
is the Earth’s rotation rate given as (from WGS-84) 7.292115 ×10
−5
rad/sec, and
R
λ
=
a(1 −e
2
)
(1 −e
2
sin
2
λ)
3/2
(32a)
R
Φ
=
a
(1 −e
2
sin
2
λ)
1/2
(32b)
7 of 24
American Institute of Aeronautics and Astronautics
The local gravity, g, using WGS-84 parameters is given by
g = 9.780327(1 + 5.3024 ×10
−3
sin
2
λ −5.8 ×10
−6
sin
2
2λ)
−(3.0877 ×10
−6
−4.4 ×10
−9
sin
2
λ)h + 7.2 ×10
−14
h
2
m/s
2
(33)
where h is measured in meters. Note that Eq. (31a) cannot be used directly with the gyro measurement.
However, this problem can be overcome by using the following identity:
ω
B
B/I
= ω
B
B/N

B
N/I
(34)
Solving Eq. (34) for ω
B
B/N
and substituting ω
B
N/I
= A
B
N
(q)ω
N
N/I
yields
ω
B
B/N
= ω
B
B/I
−A
B
N
(q)ω
N
N/I
(35)
where
ω
N
N/I
= w
e
_
¸
_
cos λ
0
−sin λ
_
¸
_ +
_
¸
¸
¸
¸
¸
¸
¸
¸
_
v
E
R
Φ
+h

v
N
R
λ
+h

v
E
tan λ
R
Φ
+h
_
¸
¸
¸
¸
¸
¸
¸
¸
_
(36)
Now Eq. (31a) can be related to the gyro measurements. Also, the acceleration variables are related to the
accelerometer measurements through
a
N

_
¸
_
a
N
a
E
a
D
_
¸
_ = A
N
B
(q)a
B
(37)
where a
B
is the acceleration vector in body coordinates and A
N
B
(q) is the matrix transpose of A
B
N
(q).
The gyro measurement model is given by
˜ ω
B
B/I
= (I
3×3
+K
g

B
B/I
+b
g

gv
(38a)
˙
b
g
= η
gu
(38b)
where b
g
is the gyro “bias”, K
g
is a diagonal matrix of gyro scale factors, and η
gv
and η
gu
are zero-mean
Gaussian white-noise processes with spectral densities given by σ
2
gv
I
3×3
and σ
2
gu
I
3×3
, respectively. The
accelerometer measurement model is given by
˜ a
B
= (I
3×3
+K
g
)a
B
+b
a

av
(39a)
˙
b
a
= η
au
(39b)
where b
a
is the accelerometer “bias”, K
a
is a diagonal matrix of accelerometer scale factors, and η
av
and
η
au
are zero-mean Gaussian white-noise processes with spectral densities given by σ
2
av
I
3×3
and σ
2
au
I
3×3
,
respectively. We should note that most manufacturers give values for σ
gv
and σ
av
, but not σ
gu
and σ
au
.
The scale factors are assumed to be small enough so that the approximation (I + K)
−1
≈ (I − K) is valid
for both the gyros and acclerometers. Simulating gyro and accelerometer using computers is not easy since
continuous measurements cannot be generated using digital computers. A discrete-time simulation is possible
using the spectral densities though, which is shown in the Appendix. The same model can be used for the
accelerometer measurements.
8 of 24
American Institute of Aeronautics and Astronautics
V. Extended Kalman Filter Equations
In this section the implementation equations for the EKF are shown. The estimated quantities are given
by
˙
ˆ q =
1
2
Ξ(ˆ q) ˆ ω
B
B/N
(40a)
ˆ ω
B
B/N
= (I
3×3

ˆ
K
g
)( ˜ ω
B
B/I

ˆ
b
g
) −A
B
N
(ˆ q) ˆ ω
N
N/I
(40b)
˙
ˆ
λ =
ˆ v
N
ˆ
R
λ
+
ˆ
h
(40c)
˙
ˆ
Φ =
ˆ v
E
(
ˆ
R
Φ
+
ˆ
h) cos
ˆ
λ
(40d)
˙
ˆ
h = −ˆ v
D
(40e)
˙
ˆ v
N
= −
_
ˆ v
E
(
ˆ
R
Φ
+
ˆ
h) cos
ˆ
λ
+ 2ω
e
_
ˆ v
E
sin
ˆ
λ +
ˆ v
N
ˆ v
D
ˆ
R
λ
+
ˆ
h
+ ˆ a
N
(40f)
˙
ˆ v
E
=
_
ˆ v
E
(
ˆ
R
Φ
+
ˆ
h) cos
ˆ
λ
+ 2ω
e
_
ˆ v
N
sin
ˆ
λ +
ˆ v
E
ˆ v
D
ˆ
R
Φ
+
ˆ
h
+ 2ω
e
ˆ v
D
cos
ˆ
λ + ˆ a
E
(40g)
˙
ˆ v
D
= −
ˆ v
2
E
ˆ
R
Φ
+
ˆ
h

ˆ v
2
N
ˆ
R
λ
+
ˆ
h
−2ω
e
ˆ v
E
cos
ˆ
λ + ˆ g + ˆ a
D
(40h)
ˆ a
N

_
¸
_
ˆ a
N
ˆ a
E
ˆ a
D
_
¸
_ = A
N
B
(ˆ q)ˆ a
B
(40i)
ˆ a
B
= (I
3×3

ˆ
K
a
)(˜ a
B

ˆ
b
a
) (40j)
˙
ˆ
b
g
= 0 (40k)
˙
ˆ
b
a
= 0 (40l)
˙
ˆ
k
g
= 0 (40m)
˙
ˆ
k
a
= 0 (40n)
where
ˆ
k
g
and
ˆ
k
a
are the elements of the diagonal matrices
ˆ
K
g
and
ˆ
K
a
, respectively. Also, ˆ ω
N
N/I
,
ˆ
R
λ
,
ˆ
R
Φ
and
ˆ g are evaluated at the current estimates. Note that the attitude matrix is coupled into the position now,
which allows us to estimate the attitude from position measurements.
We now derive the error equations, which are used in the EKF covariance propagation. The quaternion
is linearized using a multiplicative approach.
14
First, an error quaternion is defined by
δq = q ⊗ ˆ q
−1
(41)
with δq ≡ [δ̺
T
δq
4
]
T
, where the quaternion multiplication is defined by Eq. (29). The equivalent attitude
error matrix is given by
A
B
N
(δq) = A
B
N
(q)
_
A
B
N
(ˆ q)
¸
T
(42)
If the error quaternion is “small” then to within first order we have δ̺ ≈ δα/2 and δq
4
≈ 1, where δα is a
small error-angle rotation vector. Also, the quaternion inverse is defined by Eq. (30). The linearized model
error-kinematics follow
17
δ ˙ α = −[ ˆ ω
B
B/N
×]δα+δω
B
B/I
−A(ˆ q)δω
N
N/I
(43a)
δ ˙ q
4
= 0 (43b)
where δω
B
B/I
= ω
B
B/I
−ˆ ω
B
B/I
and δω
N
N/I
= ω
N
N/I
−ˆ ω
N
N/I
. Note that the fourth error-quaternion component is
constant. The first-order approximation, which assumes that the true quaternion is “close” to the estimated
quaternion, gives δq
4
≈ 1. This allows us to reduce the order of the system in the EKF by one state.
9 of 24
American Institute of Aeronautics and Astronautics
The linearization using Eq. (41) maintains quaternion normalization to within first-order if the estimated
quaternion is “close” to the true quaternion, which is within the first-order approximation in the EKF. The
error δω
B
B/I
to within first-order can be written as
δω
B
B/I
= −
_
(I
3×3

ˆ
K
g
)∆b
g
+ (
˜

B
B/I

ˆ
B
g
)∆k
g
+ (I
3×3

ˆ
K
g

gv
_
(44)
where ∆b
g
= b
g

ˆ
b
g
, ∆k
g
= k
g

ˆ
k
g
,
˜

B
B/I
is a diagonal matrix of the elements of ˜ ω
B
B/I
and
ˆ
B
g
is a
diagonal matrix of the elements of
ˆ
b
g
. The error δω
N
N/I
can be computed using a first-order Taylor series
expansion. This yields
δ ˙ α = −
_
(I
3×3

ˆ
K
g
)( ˜ ω
B
B/I

ˆ
b
g

_
δα−(I
3×3

ˆ
K
g
)∆b
g
−(
˜

B
B/I

ˆ
B
g
)∆k
g
−(I
3×3

ˆ
K
g

gv
−A
B
N
(q)
∂ω
N
N/I
∂p
¸
¸
¸
¸
¸
ˆ p,ˆ v
N
∆p −A
B
N
(q)
∂ω
N
N/I
∂v
N
¸
¸
¸
¸
¸
ˆ p
∆v
N
(45)
where p ≡ [λ Φ h]
T
, ∆p = p − ˆ p and ∆v
N
= v
N
− ˆ v
N
. The partials are given by
∂ω
N
N/I
∂p
=
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
−ω
e
sin λ −
v
E
(R
Φ
+h)
2
∂R
Φ
∂λ
0 −
v
E
(R
Φ
+h)
2
v
N
(R
λ
+h)
2
∂R
λ
∂λ
0
v
N
(R
λ
+h)
2
−ω
e
cos λ −
v
E
sec
2
λ
R
Φ
+h
+
v
E
tan λ
(R
Φ
+h)
2
∂R
Φ
∂λ
0
v
E
tan λ
(R
Φ
+h)
2
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(46a)
∂ω
N
N/I
∂v
N
=
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
0
1
R
Φ
+h
0

1
R
λ
+h
0 0
0 −
tan λ
R
Φ
+h
0
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(46b)
with
∂R
Φ
∂λ
=
a e
2
sin λcos λ
(1 −e
2
sin
2
λ)
3/2
(47a)
∂R
λ
∂λ
=
3a(1 −e
2
)e
2
sin λcos λ
(1 −e
2
sin
2
λ)
5/2
(47b)
The error equations for the remaining states can be derived using a similar approach to derive the attitude-
error equation.
The state, state-error vector, process noise vector and covariance used in the EKF are defined as
x ≡
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
q
p
v
N
b
g
b
a
k
g
k
a
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
, ∆x ≡
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
δα
∆p
∆v
N
∆b
g
∆b
a
∆k
g
∆k
a
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
, w ≡
_
¸
¸
¸
_
η
gv
η
gu
η
av
η
au
_
¸
¸
¸
_
(48a)
Q =
_
¸
¸
¸
_
σ
2
gv
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
σ
2
gu
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
σ
2
av
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
σ
2
au
I
3×3
_
¸
¸
¸
_
(48b)
10 of 24
American Institute of Aeronautics and Astronautics
The error-dynamics used in the EKF propagation are given by
∆˙ x = F∆x +Gw (49)
where
F ≡
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
F
11
F
12
F
13
F
14
0
3×3
F
16
0
3×3
0
3×3
F
22
F
23
0
3×3
0
3×3
0
3×3
0
3×3
F
31
F
32
F
33
0
3×3
F
35
0
3×3
F
37
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(50a)
G ≡
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
−(I
3×3

ˆ
K
g
) 0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
−A
N
B
(ˆ q)(I
3×3

ˆ
K
a
) 0
3×3
0
3×3
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(50b)
with
F
11
= −
_
(I
3×3

ˆ
K
g
)( ˜ ω
B
B/I

ˆ
b
g

_
, F
12
= −A
B
N
(ˆ q)
∂ω
N
N/I
∂p
¸
¸
¸
¸
¸
ˆ p,ˆ v
N
, F
13
= −A
B
N
(q)
∂ω
N
N/I
∂v
N
¸
¸
¸
¸
¸
ˆ p
(51a)
F
14
= −(I
3×3

ˆ
K
g
), F
16
= −(
˜

B
B/I

ˆ
B
g
) (51b)
F
22
=
∂ ˙ p
∂p
¸
¸
¸
¸
ˆ p,ˆ v
N
, F
23
=
∂ ˙ p
∂v
N
¸
¸
¸
¸
ˆ p
(51c)
F
31
= −A
N
B
(ˆ q)[ˆ a
B
×], F
32
=
∂ ˙ v
N
∂p
¸
¸
¸
¸
ˆ p,ˆ v
N
, F
33
=
∂ ˙ v
N
∂v
N
¸
¸
¸
¸
ˆ p,ˆ v
N
(51d)
F
35
= −A
N
B
(ˆ q)(I
3×3

ˆ
K
a
), F
37
= −A
N
B
(ˆ q)(
˜
A
B

ˆ
B
a
) (51e)
where
˜
A
B
is a diagonal matrix of the elements of ˜ a
B
and
ˆ
B
a
is a diagonal matrix of the elements of
ˆ
b
a
. The
position partials are given by
∂ ˙ p
∂p
=
_
¸
¸
¸
¸
¸
¸
¸
¸
_

v
N
(R
λ
+h)
2
∂R
λ
∂λ
0 −
v
N
(R
λ
+h)
2

v
E
sec λ
(R
Φ
+h)
2
∂R
Φ
∂λ
+
v
E
sec λtan λ
R
Φ
+h
0 −
v
E
sec λ
(R
Φ
+h)
2
0 0 0
_
¸
¸
¸
¸
¸
¸
¸
¸
_
,
∂ ˙ p
∂v
N
=
_
¸
¸
¸
¸
¸
¸
¸
¸
_
1
R
λ
+h
0 0
0
sec λ
R
Φ
+h
0
0 0 −1
_
¸
¸
¸
¸
¸
¸
¸
¸
_
(52)
The velocity partials are given by
∂ ˙ v
N
∂p
=
_
¸
_
Y
11
0 Y
13
Y
21
0 Y
23
Y
31
0 Y
33
_
¸
_,
∂ ˙ v
N
∂v
N
=
_
¸
_
Z
11
Z
12
Z
13
Z
21
Z
22
Z
23
Z
31
Z
32
0
_
¸
_ (53)
11 of 24
American Institute of Aeronautics and Astronautics
where
Y
11
= −
v
2
E
sec
2
λ
R
Φ
+h
+
v
2
E
tan λ
(R
Φ
+h)
2
∂R
Φ
∂λ
−2ω
e
v
E
cos λ −
v
N
v
D
(R
λ
+h)
2
∂R
λ
∂λ
(54a)
Y
13
=
v
2
E
tan λ
(R
Φ
+h)
2

v
N
v
D
(R
λ
+h)
2
(54b)
Y
21
=
v
E
v
N
sec
2
λ
R
Φ
+h

v
E
v
N
tan λ
(R
Φ
+h)
2
∂R
Φ
∂λ
+ 2ω
e
v
N
cos λ −
v
E
v
D
(R
Φ
+h)
2
∂R
Φ
∂λ
−2ω
e
v
D
sin λ (54c)
Y
23
= −v
E
_
v
N
tan λ +v
D
(R
Φ
+h)
2
_
(54d)
Y
31
=
v
2
E
(R
Φ
+h)
2
∂R
Φ
∂λ
+
v
2
N
(R
λ
+h)
2
∂R
λ
∂λ
+ 2ω
e
v
E
sin λ +
∂g
∂λ
(54e)
Y
33
=
v
2
E
(R
Φ
+h)
2
+
v
2
N
(R
λ
+h)
2
+
∂g
∂h
(54f)
and
Z
11
=
v
D
R
λ
+h
, Z
12
= −
2v
E
tan λ
R
Φ
+h
+ 2ω
e
sin λ, Z
12
=
v
N
R
λ
+h
(55a)
Z
21
=
v
E
tan λ
R
Φ
+h
+ 2ω
e
sin λ, Z
22
=
v
D
+v
N
tan λ
R
Φ
+h
, Z
23
=
v
E
R
Φ
+h
+ 2ω
e
cos λ (55b)
Z
31
= −
2v
N
R
λ
+h
, Z
32
= −
2v
E
R
Φ
+h
−2ω
e
cos λ (55c)
with
∂g
∂λ
= 9.780327[1.06048 ×10
−2
sin λcos λ −4.64 ×10
−5
(sin λcos
3
λ −sin
3
λcos λ)] + 8.8 ×10
−9
h sin λcos λ
(56a)
∂g
∂h
= −3.0877 ×10
−6
+ 4.4 ×10
−9
sin
2
λ + 1.44 ×10
−13
h (56b)
The GPS/INS estimation algorithm is summarized in Table 1. The assumed measurements are modelled
by
˜ p
k
= p
k
+v
k
(57)
where v
k
is a zero-mean Gaussian noise process with covariance given by R
k
, which is provided from the
solution of the GPS position determination problem coupled with the transformation given by Eq. (18). The
filter is first initialized with a known state (the bias initial conditions for the gyro and accelerometer are
usually assumed zero) and error-covariance matrix. The first three diagonal elements of the error-covariance
matrix correspond to attitude errors. Then, the Kalman gain is computed using the measurement-error
covariance R
k
and sensitivity matrix. The state error-covariance follows the standard EKF update. The
position, velocity and bias states also follow the standard EKF additive correction while the attitude error-
state update is computed using a multiplicative update.
14
Also, the updated quaternion is re-normalized by
brute force. Finally, the propagation equations follow the standard EKF model. The process noise covariance
is given in Eq. (48), and the matrices F and G are given in Eq. (50).
In order to reduce the computational load a discrete-time propagation of the covariance matrix can be
used, given by
P

k+1
= Φ
k
P
+
k
Φ
T
k
+Q
k
(58)
where Φ
k
is the discrete-time state transition matrix and Q
k
is the covariance matrix. A numerical solution
for these matrices is given by van Loan.
18
First, the following 2n ×2n matrix is formed:
A =
_
¸
_
−F GQG
T
0 F
T
_
¸
_∆t (59)
12 of 24
American Institute of Aeronautics and Astronautics
Table 1. Extended Kalman Filter for (Loose) GPS/INS Estimation
ˆ x(t
0
) = ˆ x
0
Initialize
P(t
0
) = P
0
K
k
= P

k
H
T
k
[H
k
P

k
H
T
k
+R
k
]
−1
Gain
H
k
=
_
0
3×3
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
_
P
+
k
= [I −K
k
H
k
]P

k
∆ˆ x
+
k
= K
k
[˜ p
k
− ˆ p

k
]
ˆ q
+
k
= ˆ q

k
+
1
2
Ξ(ˆ q

k
)δ ˆ α
+
k
, re-normalize quaternion
ˆ p
+
k
= ˆ p

k
+ ∆ˆ p
+
k
ˆ v
N+
k
= ˆ v
N−
k
+ ∆ˆ v
N+
k
Update
ˆ
b
+
g
k
=
ˆ
b

g
k
+ ∆
ˆ
b
+
g
k
ˆ
b
+
a
k
=
ˆ
b

a
k
+ ∆
ˆ
b
+
a
k
ˆ
k
+
g
k
=
ˆ
k

g
k
+ ∆
ˆ
k
+
g
k
ˆ
k
+
a
k
=
ˆ
k

a
k
+ ∆
ˆ
k
+
a
k
ˆ ω
B
B/N
= (I
3×3

ˆ
K
g
)( ˜ ω
B
B/I

ˆ
b
g
) −A
B
N
(ˆ q)ω
N
N/I
˙
ˆ q =
1
2
Ξ(ˆ q) ˆ ω
B
B/N
ˆ a
B
= (I
3×3

ˆ
K
a
)(˜ a
B

ˆ
b
a
)
Propagation
˙
ˆ p = f
p
(ˆ p, ˆ v
N
)
˙
ˆ v
N
= f
v
(ˆ p, ˆ v
N
) + ˆ a
N
˙
P = F P +P F
T
+GQG
T
where ∆t is the constant sampling interval. Then, the matrix exponential of Eq. (59) is computed:
B = e
A

_
¸
_
B
11
B
12
0 B
22
_
¸
_ =
_
¸
_
B
11
Φ
−1
k
Q
k
0 Φ
T
k
_
¸
_ (60)
The state transition matrix is then given by
Φ
k
= B
T
22
(61)
Also, the discrete-time process noise covariance is given by
Q
k
= Φ
k
B
12
(62)
Note, the first-order approximation of Eq. (62) is given by ∆t GQG
T
.
13 of 24
American Institute of Aeronautics and Astronautics
VI. Unscented Filtering
In this section a new approach that has been developed by Julier, Uhlmann, and Durrant-Whyte
19
is
shown as an alternative to the EKF. This approach, which they called the Unscented filter (UF), typically
involves more computations than the EKF, but has several advantages, including: 1) the expected error
is lower than the EKF, 2) the new filter can be applied to non-differentiable functions, 3) the new filter
avoids the derivation of Jacobian matrices, and 4) the new filter is valid to higher-order expansions than the
standard EKF. The UF works on the premise that with a fixed number of parameters it should be easier
to approximate a Gaussian distribution than to approximate an arbitrary nonlinear function. The filter
presented in Ref. 20 is derived for discrete-time nonlinear equations, where the system model is given by
x
k+1
= f (x
k
, k) +w
k
(63a)
˜ y
k
= h(x
k
, k) +v
k
(63b)
Note that a continuous-time model can always be written using eqn. (63a) through an appropriate numerical
integration scheme. It is assumed that w
k
and v
k
are zero-mean Gaussian noise processes with covariances
given by Q
k
and R
k
, respectively. We first rewrite the Kalman filter update equations as
21
ˆ x
+
k
= ˆ x

k
+K
k
υ
k
(64a)
P
+
k
= P

k
−K
k
P
υυ
k
K
T
k
(64b)
where υ
k
is the innovations process, given by
υ
k
≡ ˜ y
k
− ˆ y

k
= ˜ y
k
−h(ˆ x

k
, u
k
, k)
(65)
The covariance of υ
k
is defined by P
υυ
k
. The gain K
k
is computed by
K
k
= P
xy
k
(P
υυ
k
)
−1
(66)
where P
xy
k
is the cross-correlation matrix between ˆ x

k
and ˆ y

k
.
The Unscented filter uses a different propagation than the form given by the standard extended Kalman
filter. Given an n × n covariance matrix P, a set of order n points can be generated from the columns (or
rows) of the matrices ±

nP. The set of points is zero-mean, but if the distribution has mean µ, then simply
adding µ to each of the points yields a symmetric set of 2n points having the desired mean and covariance.
19
Due to the symmetric nature of this set, its odd central moments are zero, so its first three moments are the
same as the original Gaussian distribution. This is the foundation for the UF.
A method for incorporating process noise in the UF is shown in Ref. 22. This approach generates
a set of points in [x
k
, w
k
] space that has the correct mean and covariance, and propagates these points
through the model in Eq. (63a). The predicted mean and covariance are also augmented to included the
process noise, but the basic structure of the their calculations remain unchanged (see Ref. 22 for more
details). Although this approach may more fully utilize the capability of the unscented transformation,
it will be more computationally costly due to the extra required calculations arising from the augmented
system. This significantly increases the computational burden, which may prohibit its use for actual onboard
implementations. Another approach based on a trapezoidal approximation will be shown here.
The general formulation for the propagation equations are given as follows. First, the following set of
sigma points are computed:
σ
k
←2n columns from ±γ
_
P
+
k
+
¯
Q
k
(67a)
χ
k
(0) = ˆ x
+
k
(67b)
χ
k
(i) = σ
k
(i) + ˆ x
+
k
(67c)
where the matrix
¯
Q
k
is related to the process noise covariance, which will be discussed shortly. The parameter
γ is given by
γ =

n +λ (68)
14 of 24
American Institute of Aeronautics and Astronautics
where the composite scaling parameter, λ, is given by
λ = α
2
(n +κ) −n (69)
The constant α determines the spread of the sigma points and is usually set to a small positive value (e.g.,
1×10
−4
≤ α ≤ 1).
20
Also, the significance of the parameter κ will be discussed shortly. One efficient method
to compute the matrix square root is the Cholesky decomposition.
23
Alternatively, the sigma points can be
chosen to lie along the eigenvectors of the covariance matrix. Note that there are a total of 2n values for σ
k
(the positive and negative square roots). The transformed set of sigma points are evaluated for each of the
points by
χ
k+1
(i) = f [χ
k
(i), k] (70)
We now define the following weights:
W
mean
0
=
λ
n +λ
(71a)
W
cov
0
=
λ
n +λ
+ (1 −α
2
+β) (71b)
W
mean
i
= W
cov
i
=
1
2(n +λ)
, i = 1, 2, . . . , 2n (71c)
where β is used to incorporate prior knowledge of the distribution (a good starting guess is β = 2).
The predicted mean for the state estimate is calculated using a weighted sum of the points χ
x
k+1
(i), which
is given by
ˆ x

k+1
=
2n

i=0
W
mean
i
χ
k+1
(i) (72)
The predicted covariance is given by
P

k+1
=
2n

i=0
W
cov
i

k+1
(i) − ˆ x

k+1
] [χ
k+1
(i) − ˆ x

k+1
]
T
+
¯
Q
k
(73)
The mean observation is given by
ˆ y

k+1
=
2n

i=0
W
mean
i
γ
k+1
(i) (74)
where
γ
k+1
(i) = h[χ
k+1
(i), k + 1] (75)
The output covariance is given by
P
yy
k+1
=
2n

i=0
W
cov
i

k+1
(i) − ˆ y

k+1
] [γ
k+1
(i) − ˆ y

k+1
]
T
(76)
Then the innovations covariance is simply given by
P
υυ
k+1
= P
yy
k+1
+R
k+1
(77)
Finally the cross correlation matrix is determined using
P
xy
k+1
=
2n

i=0
W
cov
i

k+1
(i) − ˆ x

k+1
] [γ
k+1
(i) − ˆ y

k+1
]
T
(78)
The filter gain is then computed using eqn. (66), and the state vector can now be updated using eqn. (64).
Even though propagations on the order of 2n are required for the Unscented filter, the computations may
be comparable to the extended Kalman filter (especially if the continuous-time covariance equation needs to
be integrated and a numerical Jacobian matrix is evaluated).
15 of 24
American Institute of Aeronautics and Astronautics
The scalar κ in the previous set of equations is a convenient parameter for exploiting knowledge (if
available) about the higher moments of the given distribution.
21
In scalar systems (i.e., for n = 1), a value
of κ = 2 leads to errors in the mean and variance that are sixth order. For higher-dimensional systems
choosing κ = 3 −n minimizes the mean-squared-error up to the fourth order.
19
However, caution should be
exercised when κ is negative since a possibility exists that the predicted covariance can become non-positive
semi-definite. A modified form has been suggested for this case (see Ref. 19). Also, a square root version of
the Unscented filter is presented in Ref. 20 that avoids the need to re-factorize at each step. Furthermore,
Ref. 20 presents an Unscented Particle filter, which makes no assumptions on the form of the probability
densities, i.e., full nonlinear, non-Gaussian estimation.
Reference 22 states that if the process noise is purely additive in the model, then its covariance can simply
be added using a simple additive procedure. In this paper we expand upon this concept by incorporating an
approximation for the integration over the sampling interval, which more closely follows the actual process.
Any process noise that is added to the state vector in the UF is effectively multiplied by the state transition
matrix, Φ(∆t), which gives Φ(∆t) Q
k
Φ
T
(∆t) at the end of the interval. This mapping is done automatically
by the state propagation, and does not need to be explicitly accounted for in the propagation. However,
adding equal process noise at the beginning and end of the propagation might yield better results. The total
desired process noise follows
Φ(∆t)
¯
Q
k
Φ
T
(∆t) +
¯
Q
k
= Q
k
(79)
where
¯
Q
k
is used in Eq. (67a) and in the calculation of the predicted covariance in Eq. (73). This approach
is similar to a trapezoid rule for integration. Equation (79) is known as the discrete-time Sylvester equation.
A. Unscented GPS/INS Filter
In this section an UF is derived for GPS/INS estimation. This filter is straightforward, except for the
quaternion normalization. Mainly, referring to Eq. (72), since the predicted quaternion mean is derived
using an averaged sum of quaternions, no guarantees can be made that the resulting quaternion will have
unit norm. This makes the straightforward implementation of the UF with quaternions undesirable. A better
way involves using an unconstrained three-component vector to represent an attitude error quaternion. We
begin by defining the following state vector:
χ
k
(0) = ˆ x
+
k

_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
δˆs
+
k
ˆ p
+
k
ˆ v
N+
k
ˆ
b
+
g
k
ˆ
b
+
a
k
ˆ
k
+
g
k
ˆ
k
+
a
k
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
, χ
k
(i) ≡
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
χ
δs
k
(i)
χ
p
k
(i)
χ
V
N
k
(i)
χ
bg
k
(i)
χ
ba
k
(i)
χ
Kg
k
(i)
χ
Ka
k
(i)
_
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
_
(80)
where δˆs
+
k
is a generalized Rodrigues error-vector
24
used to propagate and update a nominal quaternion.
Since this three-dimensional representation is unconstrained, the resulting overall covariance matrix is a
21 × 21 matrix. Therefore, using Eq. (72) poses no difficulties, which makes this an attractive approach.
Now, define the first three components of the vector χ
k
(i) in Eq. (67) as χ
δs
k
(i). To describe χ
δs
k
we first
define a new quaternion generated by multiplying an error quaternion by the current estimate. Using the
notation in Eq. (67) we assume
ˆ q
+
k
(0) = ˆ q
+
k
(81a)
ˆ q
+
k
(i) = δq
+
k
(i) ⊗ ˆ q
+
k
, i = 1, 2, . . . , 42 (81b)
with δq
+
k
(i) ≡
_
δ̺
+T
k
(i) δq
+
4
k
(i)
¸
T
represented by
24
δq
+
4
k
(i) =
−a ||χ
δs
k
(i)||
2
+f
_
f
2
+ (1 −a
2
)||χ
δs
k
(i)||
2
f
2
+||χ
δs
k
(i)||
2
, i = 1, 2 . . . , 42 (82a)
δ̺
+
k
(i) = f
−1
_
a +δq
+
4
k
(i)
¸
χ
δs
k
(i), i = 1, 2, . . . , 42 (82b)
16 of 24
American Institute of Aeronautics and Astronautics
0 60 120 180 240 300 360 420 480
4
5
6
7
8
Time (sec)
A
v
a
i
l
a
b
l
e
N
u
m
b
e
r
Figure 2. Number of Available GPS Satellites
where a is a parameter from 0 to 1, and f is a scale factor. Note when a = 0 and f = 1 then χ
δs
k
gives the
Gibbs vector, and when a = f = 1 then χ
δs
k
gives the standard vector of modified Rodrigues parameters
(MRPs). For small errors the attitude part of the covariance is closely related to the attitude estimation
errors for any rotation sequence, given by a simple factor.
14
For example, the Gibbs vector linearize to half
angles, and the vector of MRPs linearize to quarter angles. We will choose f = 2(a + 1) so that ||χ
δs
k
|| is
equal to the rotational error-angle for small errors. Equation (81a) clearly requires that χ
δs
k
(0) be zero. This
is due to the reset of the attitude error to zero after the previous update, which is used to move information
from one part of the estimate to another part.
25
This reset rotates the reference frame for the covariance, so
we might expect the covariance to be rotated, even though no new information is added. But the covariance
depends on the assumed statistics of the measurements, not on the actual measurements. Therefore, since
the update is zero-mean, the mean rotation caused by the reset is actually zero, so the covariance is in fact
not affected by the reset. Next, the updated quaternions are propagated forward using Eq. (40a), with
˙
ˆ q(i) =
1
2
Ξ[ˆ q(i)] ˆ ω
B
B/N
(i), i = 0, 1, . . . , 42 (83)
with the estimated angular velocities given by
ˆ ω
B
B/N
(i) = [I
3×3
−χ
Kg
(i)]
_
˜ ω
B
B/I
−χ
bg
(i)
_
−A
B
N
[ˆ q(i)]ω
N
N/I
, i = 0, 1, . . . , 42 (84)
where χ
Kg
(i) and χ
bg
(i) are formed from the gyro scale-factor and bias sigma points, respectively. The
propagated error quaternions are computed using
δq

k+1
(i) = ˆ q

k+1
(i) ⊗
_
ˆ q

k+1
(0)
¸
−1
, i = 0, 1, . . . , 42 (85)
Note that δq

k+1
(0) is the identity quaternion. Finally, the propagated sigma points can be computed using
24
χ
δs
k+1
(0) = 0 (86a)
χ
δs
k+1
(i) = f
δ̺

k+1
(i)
a +δq

4
k+1
(i)
, i = 1, 2, . . . , 42 (86b)
with
_
δ̺
−T
k+1
(i) δq

4
k+1
(i)
_
T
= δq

k+1
(i). The predicted mean and covariance can now be computed using
Eqs. (72) and (73).
The procedure in the Unscented GPS/INS filter is as follows. We are given initial estimates for the
attitude, position, velocity, and biases and scale factors, as well as an initial 21 ×21 covariance (P
+
0
), where
17 of 24
American Institute of Aeronautics and Astronautics
the upper 3×3 partition of P
+
0
corresponds to attitude error angles. The first three components of the initial
state vector in the UF is set to zero and the others are set to their initial values for position, velocity, biases
and scale factors. We choose the parameter a and set f = 2(a + 1). Then,
¯
Q
k
is calculated using Eq. (79),
which will be used in Eqs. (67a) and (73), where Φ
k
is given by Eq. (61) and Q
k
is given by Eq. (62). The
sigma points are then calculated using Eq. (67). Next, the corresponding error quaternions are calculated
using Eq. (82), where Eq. (81) is used to compute the sigma-point quaternions from the error quaternions.
The quaternions are subsequently propagated forward in time using Eq. (83). Then, the propagated error
quaternions are determined using Eq. (85), and the propagated sigma points are calculated using Eq. (86).
All other sigma-point quantities, such as position, velocity, biases and scale factors, are propagated using
Eq. (40). The predicted mean and covariance can now be computed using Eqs. (72) and (73). Storing the
propagated quaternions from Eq. (83) we then calculate the mean observations using Eqs. (74) and (75).
The output covariance, innovation covariance and cross-correlation matrix are computed using Eqs. (76),
(77) and (78). Next, the state vector and covariance are updated using Eq. (64). Then, the quaternion is
updated using
ˆ q
+
k+1
= δq
+
k+1
⊗ ˆ q

k+1
(0) (87)
where δq
+
k+1

_
δ̺
+T
k+1
δq
+
4
k+1
_
T
is represented by
24
δq
+
4
k+1
=
−a ||δˆs
+
k+1
||
2
+f
_
f
2
+ (1 −a
2
)||δˆs
+
k+1
||
2
f
2
+||δˆs
+
k+1
||
2
(88a)
δ̺
+
k+1
= f
−1
_
a +δq
+
4
k+1
_
δˆs
+
k+1
(88b)
Finally, δˆs
+
k+1
is reset to zero for the next propagation.
VII. Simulation Results
In this section simulation results are shown that estimate for a moving vehicle’s attitude, position and
velocity, as well as the gyro and accelerometer biases and scale factors. All measurements are assumed to
be sampled every 1 second. The total time of the simulation is 8 minutes. The gyro noise parameters are
given by σ
gv
= 2.9089×10
−7
rad/sec
1/2
and σ
gu
= 9.1989×10
−7
rad/sec
3/2
. The accelerometer parameters
are given by σ
av
= 9.8100 × 10
−5
m/sec
3/2
and σ
au
= 6.0000 × 10
−5
m/sec
5/2
. Initial biases for the gyros
and accelerometers are given by 10 deg/hr and 0.003 m/s
2
, respectively, for each axis. Also, K
g
= 0.01I
3×3
and K
a
= 0.005I
3×3
. The vehicle motion is described in NED coordinates with the origin (point of interest)
location at λ
0
= 38

and Φ
0
= −77

. The initial quaternion is given so that the vehicle body frame is aligned
with the local NED frame. The initial velocity is given by v
N
0
= [200 200 − 10]
T
m/s. The acceleration
inputs are given by a
N
= 0, a
E
= 0 and a
D
= −g
0
, where g
0
is the initial gravity. The rotational rate
profile is given by: 5 deg/min rotation about the x axis for the first 160 seconds and then zero for the final
320 seconds; no rotation about the y axis for the first 160 seconds, then a 5 deg/min rotation for the next
160 seconds and zero for the final 160 seconds; no rotation about the z axis for the first 320 seconds, then 5
deg/min rotation for the final 160 seconds. The GPS constellation is simulated using GPS week 137 and a
time of applicability of 61440.0000 seconds (see Ref. 11 for an explanation of GPS time). Using the position
profile the number of GPS satellites available can be computed using a 15

elevation cutoff.
11
The number
of available GPS satellites over time is shown in Figure 2, which ranges from a minimum of 5 satellites to a
maximum of 6 satellites. The clock-bias drift is modelled using a random walk process: ˙ τ = w
τ
, where the
variance (in seconds) of w
τ
is given by 200. GPS measurements are obtained using a standard deviation of
5 meters for the white-noise errors.
Using all available GPS pseudoranges an ECEF position is determined using nonlinear least squares,
which is then converted into longitude, latitude and height using Eq. (4). These quantities are used as
“measurements” in the filters with covariance computed using Eq. (18), where P
ECEF
pos
is computed from the
nonlinear least square solution. The approach corresponds to a “loose” GPS/INS configuration. In general,
position is very well known but attitude is not. To test the robustness of the EKF an initial attitude error of
15 degrees is given in each axis. This error is not unrealistic for an actual application. The initial covariance
matrix P
0
in the EKF is diagonal. For this case, the three attitude parts of the initial covariance are each
set to a 3σ bound of 15 degrees, i.e., [(15/3) × (π/180)]
2
rad
2
. The initial estimates for position are set
18 of 24
American Institute of Aeronautics and Astronautics
to the true latitude, longitude and height. The initial variances for latitude and longitude are each given
by (1 × 10
−6
)
2
rad
2
. The initial variance for height is given by (20/3)
2
m
2
. To further stress the filters
the initial velocity is set to zero. For this case, the initial variances in the filters for v
N
and v
E
are each
set to (200/3)
2
and the initial variance for v
D
is set to (10/3)
2
. The initial gyro and accelerometer biases
and scale factors are all set to zero. The three gyro-bias parts of the initial covariance are each set to a 3σ
bound of 30 degrees per hour, i.e., [(30/3) × (π/(180 × 3600))]
2
. The three accelerometer-bias parts of the
initial covariance are each set to a 3σ bound of 0.005 meters per second-squared, i.e., (0.005/3)
2
. The three
gyro-scale factor parts of the initial covariance are each set to a 3σ bound of 0.015, i.e., (0.015/3)
2
. Finally,
the three accelerometer-scale factor parts of the initial covariance are each set to a 3σ bound of 0.010, i.e.,
(0.010/3)
2
. The parameters used in the UF are given by α = 0.003, β = 2 and κ = 3 −n, where n = 21.
The resulting EKF attitude errors for a typical case are shown in Figure 3(a). The attitude errors diverge
and significantly drift outside their respective 3σ bounds, which indicates that the EKF is performing in a
subpar fashion. This is due to the large initial errors that are not handled well in the linearization of the
dynamic model in the EKF. However, the UF attitude errors are much closer to their respective 3σ bounds
than the EKF attitude errors, as shown in Figure 3(b). The larger errors in yaw are due to the fact that
this angle is the least observable state for the particular vehicle motion. The biggest concern with the EKF
solutions is the confidence of the results dictated by the 3σ bounds. In fact, if the truth is not known a
prior and we only had the covariance to assess filter performance, this plot would indicate that the EKF
is performing better than the UF. This can certainly provide some misleading results when using the EKF
with large initial condition errors. The position errors for the EKF are given by Figure 3(c). The latitude
and height errors remain within their respective 3σ bounds for the entire simulation run, but longitude drifts
outside the bound for a small period. This is a surprising result for the EKF since position measurements
are directly used in the filter. However, all UF position errors remain inside their respective 3σ bounds.
A comparison of the gyro bias estimates between the EKF and UF is shown by Figures 3(e) and 3(f),
respectively. The errors for the EKF drift outside of their respected 3σ bounds for every axis. However, the
UF bias errors are much closer to their 3σ bounds than the EKF bias errors, as shown in Figure 3(f). These
simulation results clearly indicate that the UF is able to provide more robust characteristics than an EKF
for GPS/INS applications.
VIII. Conclusions
In this paper an Unscented filter formulation was derived for the purpose of GPS/INS applications. The
filter is based on a quaternion parameterization of the attitude. However, straightforward implementation of
the Unscented filter using quaternion kinematics did not produce a unit quaternion estimate. To overcome
this difficulty the quaternion was represented by a three-dimensional vector of generalized Rodrigues param-
eters, which also reduced the size the covariance matrix. Simulation results indicated that the performance
of the Unscented filter exceeds the standard extended Kalman filter for large initialization errors.
Acknowledgements
This work was funded by the DOE NNSA NA-22 Proliferation Detection Program Office, Advanced Radar
Systems project, under the guidance of Theodore Kim and Curtis Webb at Sandia National Laboratories.
The author greatly appreciates this support. Also, the author wishes to thank F. Landis Markley from
NASA Goddard Space Flight Center for numerous discussions on gyro noise modelling throughout the years.
References
1
Chatfield, A. B., Fundamentals of High Accuracy Inertial Navigation, American Institute of Aeronautics and Astronau-
tics, Inc., Reston, VA, 1997.
2
Connelly, J., Kourepenis, A., Marinis, T., and Hanson, D., “Micromechanical Sensors in Tactical GN&C Applications,”
AIAA Guidance, Navigation and Contol Conference, Montreal, QB, Canada, Aug. 2001, AIAA-2001-4407.
3
Farrell, J. and Barth, M., The Global Positioning System & Inertial Navigation, McGraw-Hill, New York, NY, 1998.
4
Jekeli, C., Inertial Navigation Systems with Geodetic Applications, Walter de Gruyter, Berlin, Germany, 2000.
5
Julier, S. J., Uhlmann, J. K., and Durrant-Whyte, H. F., “A New Method for the Nonlinear Transformation of Means and
Covariances in Filters and Estimators,” IEEE Transactions on Automatic Control , Vol. AC-45, No. 3, March 2000, pp. 477–482.
6
van der Merwe, R., Wan, E. A., and Julier, S. I., “Sigma-Point Kalman Filters for Nonlinear Estimation and Sensor-
19 of 24
American Institute of Aeronautics and Astronautics
Fusion: Applications to Integrated Navigation,” AIAA Guidance, Navigation and Contol Conference, Providence, RI, Aug.
2004, AIAA-2004-5120.
7
Crassidis, J. L. and Junkins, J. L., Optimal Estimation of Dynamic Systems, Chapman & Hall/CRC, Boca Raton, FL,
2004.
8
Bate, R. R., Mueller, D. D., and White, J. E., Fundamentals of Astrodynamics, Dover Publications, New York, NY,
1971.
9
Wertz, J. R., “Space-Based Orbit, Attitude and Timing Systems,” Mission Geometry: Orbit and Constellation Design
and Management, chap. 4, Microcosm Press, El Segundo, CA and Kluwer Academic Publishers, The Netherlands, 2001.
10
Meeus, J., Astronomical Algorithms, Willman-Bell, Inc., Richmond, VA, 2nd ed., 1999.
11
Hofmann-Wellenhof, B., Lichtenegger, H., and Collins, J., GPS: Theory and Practice, Springer Wien, New York, NY,
5th ed., 2001.
12
Shuster, M. D., “A Survey of Attitude Representations,” Journal of the Astronautical Sciences, Vol. 41, No. 4, Oct.-Dec.
1993, pp. 439–517.
13
Hamilton, W. R., Elements of Quaternions, Longmans, Green and Co., London, England, 1866.
14
Lefferts, E. J., Markley, F. L., and Shuster, M. D., “Kalman Filtering for Spacecraft Attitude Estimation,” Journal of
Guidance, Control, and Dynamics, Vol. 5, No. 5, Sept.-Oct. 1982, pp. 417–429.
15
Shepperd, S. W., “Quaternion from Rotation Matrix,” Journal of Guidance and Control , Vol. 1, No. 3, May-June 1978,
pp. 223–224.
16
Kuipers, J. B., Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace, and Virtual
Reality, Princeton University Press, Princeton, NJ, 1999.
17
Kim, S.-G., Crassidis, J. L., Cheng, Y., Fosbury, A. M., and Junkins, J. L., “Kalman Filtering for Relative Spacecraft
Attitude and Position Estimation,” AIAA Guidance, Navigation and Contol Conference, San Francisco, CA, Aug. 2005, AIAA-
2005-6087.
18
van Loan, C. F., “Computing Integrals Involving the Matrix Exponential,” IEEE Transactions on Automatic Control ,
Vol. AC-23, No. 3, June 1978, pp. 396–404.
19
Julier, S. J., Uhlmann, J. K., and Durrant-Whyte, H. F., “A New Approach for Filtering Nonlinear Systems,” Proceedings
of the American Control Conference, Seattle, WA, June 1995, pp. 1628–1632.
20
Wan, E. and van der Merwe, R., “The Unscented Kalman Filter,” Kalman Filtering and Neural Networks, edited by
S. Haykin, chap. 7, Wiley, 2001.
21
Bar-Shalom, Y. and Fortmann, T. E., Tracking and Data Association, Academic Press, Boston, MA, 1988, pp. 56–58.
22
Wan, E. and van der Merwe, R., “The Unscented Kalman Filter,” Kalman Filtering and Neural Networks, edited by
S. Haykin, chap. 7, John Wiley & Sons, New York, NY, 2001.
23
Golub, G. H. and Van Loan, C. F., Matrix Computations, The Johns Hopkins University Press, Baltimore, MD, 2nd ed.,
1989, pp. 145–146.
24
Crassidis, J. L. and Markley, F. L., “Unscented Filtering for Spacecraft Attitude Estimation,” Journal of Guidance,
Control, and Dynamics, Vol. 26, No. 4, July-Aug. 2003, pp. 536–542.
25
Markley, F. L., “Attitude Representations for Kalman Filtering,” AAS/AIAA Astrodynamics Specialist Conference,
Quebec City, Quebec, Aug. 2001, AAS 01-309.
26
Grewal, M. S., Weill, L. R., and Andrews, A. P., Global Positioning Systems, Inertial Navigation, and Integration, John
Wiley & Sons, New York, NY, 2001.
27
Farrenkopf, R. L., “Analytic Steady-State Accuracy Solutions for Two Common Spacecraft Attitude Estimators,” Journal
of Guidance and Control , Vol. 1, No. 4, July-Aug. 1978, pp. 282–284.
Appendix
In this appendix the gyro noise model is described in more detail.

The single-axis gyro model with no
scale factor correction is given by
˜ ω(t) = ω(t) +b(t) +η
v
(t) (89a)
˙
b(t) = η
u
(t) (89b)
Dividing Eq. (89a) by ∆t and integrating gives
1
∆t
_
t0+∆t
t0
˜ ω(t) dt =
1
∆t
_
t0+∆t
t0
[ω(t) +b(t) +η
v
(t)] dt (90)
Assuming that the measurement and truth are each constant over the interval (note: we cannot make the
same assumption for the stochastic variables) yields
˜ ω(t
0
+ ∆t) = ω(t
0
+ ∆t) +
1
∆t
_
t0+∆t
t0
[b(t) +η
v
(t)] dt (91)

This model is derived from notes provided by F. Landis Markley of NASA Goddard Space Flight Center.
20 of 24
American Institute of Aeronautics and Astronautics
Integrating Eq. (89b) gives
b(t
0
+ ∆t) = b(t
0
) +
_
t0+∆t
t0
η
u
(t) dt (92)
The variance of the gyro drift bias is given by
E
_
b
2
(t
0
+ ∆t)
_
= E
__
b(t
0
) +
_
t0+∆t
t0
η
u
(t) dt
__
b(t
0
) +
_
t0+∆t
t0
η
u
(τ) dτ
__
(93)
Using E {η
u
(t)η
u
(τ)} = σ
2
u
δ(t −τ) gives
E
_
b
2
(t
0
+ ∆t)
_
= E
_
b
2
(t
0
)
_

2
u
∆t (94)
Therefore, the bias can be simulated using
b
m
(t
0
+ ∆t) = b
m
(t
0
) +σ
u
∆t
1/2
N
u
(95)
where the subscript m denotes a modelled quantity and N
u
is a zero-mean random variable with unit variance.
The bias at time t is given by
b(t) = b(t
0
) +
_
t
t0
η
u
(τ) dτ (96)
Substituting Eq. (96) into Eq. (91) gives
˜ ω(t
0
+ ∆t) = z +
1
∆t
_
t0+∆t
t0
_
t
t0
η
u
(τ) dτ dt +
1
∆t
_
t0+∆t
t0
η
v
(t) dt (97)
where z ≡ ω(t
0
+ ∆t) +b(t
0
). The correlation between the drift and rate measurement is given by
E {b(t
0
+ ∆t) ˜ ω(t
0
+ ∆t)} = E
__
b(t
0
) +
_
t0+∆t
t0
η
u
(τ) dτ
_
×
_
ω(t
0
+ ∆t) +b(t
0
) +
1
∆t
_
t0+∆t
t0
_
t
t0
η
u
(ζ) dζ dt +
1
∆t
_
t0+∆t
t0
η
v
(t) dt
__ (98)
Since η
u
(t) and η
v
(t) are uncorrelated we have
E {b(t
0
+ ∆t) ˜ ω(t
0
+ ∆t)} = E {z b(t
0
)} +
σ
2
u
∆t
_
t0+∆t
t0
_
t0+∆t
t0
_
t
t0
δ(τ −ζ) dζ dτ dt
= E {z b(t
0
)} +
σ
2
u
∆t
_
t0+∆t
t0
(t −t
0
) dt
= E {z b(t
0
)} +
1
2
σ
2
u
∆t
(99)
Equation (99) can be satisfied by modelling the gyro measurement using
˜ ω
m
(t
0
+ ∆t) = ω
m
(t
0
+ ∆t) +b
m
(t
0
) +
1
2
σ
u
∆t
1/2
N
u
+c N
v
(100)
where c is yet to be determined and N
v
is a zero-mean random variable with unit variance. Note that
Eq. (100) can be proven by evaluating E {b
m
(t
0
+ ∆t) ˜ ω
m
(t
0
+ ∆t)}. Solving Eq. (95) for N
u
and substituting
the resultant into Eq. (100) yields
˜ ω
m
(t
0
+ ∆t) = ω
m
(t
0
+ ∆t) +
1
2
[b
m
(t
0
+ ∆t) +b
m
(t
0
)] +c N
v
(101)
Note that
1
2
[b
m
(t
0
+ ∆t) + b
m
(t
0
)] is the “average” of the bias at the two times. This term is present due
to the fact that the trapezoid rule for integration is exact for linear systems. To evaluate c we compute the
21 of 24
American Institute of Aeronautics and Astronautics
variance of the rate measurement:
E
_
˜ ω
2
(t
0
+ ∆t)
_
= E
__
z +
1
∆t
_
t0+∆t
t0
_
τ
t0
η
u
(υ) dυ dτ +
1
∆t
_
t0+∆t
t0
η
v
(τ) dτ
_
×
_
z +
1
∆t
_
t0+∆t
t0
_
t
t0
η
u
(ζ) dζ dt +
1
∆t
_
t0+∆t
t0
η
v
(t) dt
__ (102)
Since η
u
(t) and η
v
(t) are uncorrelated and using E {η
v
(t)η
v
(τ)} = σ
2
v
δ(t −τ), then Eq. (102) simplifies to
E
_
˜ ω
2
(t
0
+ ∆t)
_
= E
_
z
2
_
+
σ
2
u
∆t
2
_
t0+∆t
t0
_
t0+∆t
t0
_
t
t0
_
τ
t0
δ(υ −ζ) dυ dζ dτ dt
+
σ
2
v
∆t
2
_
t0+∆t
t0
_
t0+∆t
t0
δ(t −τ) dτ dt
(103)
The second to last integral can be computed by the following steps:
_
t0+∆t
t0
_
t0+∆t
t0
_
t
t0
_
τ
t0
δ(υ −ζ) dυ dζ dτ dt
=
_
t0+∆t
t0
_
t0+∆t
t0
min(τ −t
0
, t −t
0
) dτ dt
=
_
t0+∆t
t0
_
t0+∆t
t0
min(x, y) dx dy
=
_
∆t
0
_
_
y
0
x dx +
_
∆t
y
y dx
_
dy
=
_
∆t
0
_
1
2
y
2
+y(∆t −y)
_
dy
=
1
3
∆t
3
(104)
Therefore, Eq. (103) reduces down to
E
_
˜ ω
2
(t
0
+ ∆t)
_
= E
_
z
2
_
+
1
3
σ
2
u
∆t +
σ
2
v
∆t
(105)
The variance of the modelled rate measurement in Eq. (100) is given by
E
_
˜ ω
2
m
(t
0
+ ∆t)
_
= E
_
z
2
m
_
+
1
4
σ
2
u
∆t +c
2
(106)
Comparing Eq. (106) to Eq. (105) gives
c
2
=
σ
2
v
∆t
+
1
12
σ
2
u
∆t (107)
Hence, the modelled rate measurement is given by
˜ ω
m
(t
0
+ ∆t) = ω
m
(t
0
+ ∆t) +
1
2
[b
m
(t
0
+ ∆t) +b
m
(t
0
)] +
_
σ
2
v
∆t
+
1
12
σ
2
u
∆t
_
1/2
N
v
(108)
Generalizing Eqs. (95) and (108) for all times and dropping the subscript m gives the following formulas for
the discrete-time rate and bias equations
˜ ω
k+1
= ω
k+1
+
1
2
[b
k+1
+b
k
] +
_
σ
2
v
∆t
+
1
12
σ
2
u
∆t
_
1/2
N
v
(109a)
b
k+1
= b
k

u
∆t
1/2
N
u
(109b)
22 of 24
American Institute of Aeronautics and Astronautics
These equations are valid for both gyro and accelerometer measurements.
We now discuss other aspects of gyro and accelerometer specifications and how they relate to the INS EKF
equations. We first discuss units. Unfortunately, specifications (and units) seem to vary from manufacturer
to manufacturer. There are a number of various definitions for the term “bias” as well. Grewal et al.
26
give
an explanation of these various definitions. There is a fixed bias that only needs to be calibrated once, a
bias stability that varies from turn-on to turn-on due to thermal cycling among other causes, a bias drift
after turn-on, and other biases that are g-dependent and shock dependent. The PSD σ
2
v
is often referred to
as the angle random walk, which is a bit misleading. Also, σ
2
u
is sometimes referred to as the rate random
walk. Many manufacturers give σ
v
in units of deg/sqrt(hr) and a conversion to rad/sqrt(sec) is simple. Some
manufacturers give the “random noise” units as deg/hr/sqrt(Hz). Both values are equivalent; if the second
value is divided by 60 units of deg/sqrt(hr) are obtained:
deg
hr

Hz
=
deg
hr
_
1
sec
×
3600sec
1 hr
=
deg
60hr
_
1
hr
=
deg
60

hr
(110)
The random noise of an accelerometer is often given in units of micro-g/sqrt(Hz). The conversion of these
units into meters/sec
3/2
is given by
µmeters
sec
2

Hz
=
meters ×10
−6
sec
2
_
1
sec
=
meters ×10
−6
sec
3/2
(111)
A small number of manufacturers will give rate changes and integrated output changes over time. The
conversion of these parameters to σ
u
and σ
v
is given in Ref. 27. Suppose we are given a rate change of x
rad/sec (1σ) in t
x
hours. Then σ
u
is given by
σ
u
=
x
60

t
x
rad/sec
3/2
(112)
Next, suppose we are given an integrated output change of y rad (1σ) in t
y
hours. Then σ
v
is given by
σ
v
=
¸
y
2

1
3
σ
2
u
×(3600 t
y
)
3
3600 t
y
rad/sec
1/2
(113)
where σ
2
u
is calculated using Eq. (112).
A more general model is given by
˜ ω(t) = (1 +κ) ω(t) +b(t) +d +η
v
(t) (114a)
˙
b(t) = −αb(t) +η
u
(t) (114b)
˙
d(t) = 0 (114c)
˙ κ(t) = 0 (114d)
where κ is a “scale factor” and and the processes in Eqs. (114c) and (114c) are “random constants”. Often-
times the variance of d and κ are given by manufacturers as the “bias repeatability” and “scale factor error”,
respectively. The bias repeatability can be used to set b(t
0
). The units for the scale factor error are often
given in “parts per million” (ppm). To determine the standard deviation of κ multiply ppm by 1 × 10
−6
.
The process in Eq. (114b) is called a Markov process, where 1/α is the “correlation time”. The variance of
η
u
(t) is written as
4
E {η
u
(t)η
u
(τ)} = 2 σ
2
αδ(t −τ) ≡ σ
2
u
δ(t −τ) (115)
where σ
2
is just another parameter. Note that for gyros the units of σ
2
are often given by deg/hr and for
accelerometers in mgal, where 1 mgal = 10
−5
m/s
2
(1 gal = 10
−2
m/s
2
). Hence, given the “correlated noise”
parameters σ
2
and 1/α, then σ
2
u
= 2 σ
2
α.
23 of 24
American Institute of Aeronautics and Astronautics
0 60 120 180 240 300 360 420 480
−15
−7.5
0
7.5
15
0 60 120 180 240 300 360 420 480
−15
−7.5
0
7.5
15
0 60 120 180 240 300 360 420 480
−30
−15
0
15
30
Time (sec)
R
o
l
l
(
d
e
g
)
P
i
t
c
h
(
d
e
g
)
Y
a
w
(
d
e
g
)
(a) EKF Attitude Errors and 3σ Bounds
0 60 120 180 240 300 360 420 480
−15
−7.5
0
7.5
15
0 60 120 180 240 300 360 420 480
−15
−7.5
0
7.5
15
0 60 120 180 240 300 360 420 480
−30
−15
0
15
30
Time (sec)
R
o
l
l
(
d
e
g
)
P
i
t
c
h
(
d
e
g
)
Y
a
w
(
d
e
g
)
(b) UF Attitude Errors and 3σ Bounds
0 60 120 180 240 300 360 420 480
−2
−1
0
1
2
x 10
−4
0 60 120 180 240 300 360 420 480
−2
−1
0
1
2
x 10
−4
0 60 120 180 240 300 360 420 480
−20
−10
0
10
20
Time (sec)
L
o
n
g
.
(
d
e
g
)
L
a
t
.
(
d
e
g
)
H
e
i
g
h
t
(
m
)
(c) EKF Position Errors and 3σ Bounds
0 60 120 180 240 300 360 420 480
−2
−1
0
1
2
x 10
−4
0 60 120 180 240 300 360 420 480
−2
−1
0
1
2
x 10
−4
0 60 120 180 240 300 360 420 480
−20
−10
0
10
20
Time (sec)
L
o
n
g
.
(
d
e
g
)
L
a
t
.
(
d
e
g
)
H
e
i
g
h
t
(
m
)
(d) UF Position Errors and 3σ Bounds
0 60 120 180 240 300 360 420 480
−50
−25
0
25
50
0 60 120 180 240 300 360 420 480
−50
−25
0
25
50
0 60 120 180 240 300 360 420 480
−70
−35
0
35
70
Time (sec)
x
(
d
e
g
/
h
r
)
y
(
d
e
g
/
h
r
)
z
(
d
e
g
/
h
r
)
(e) EKF Gyro-Bias Errors and 3σ Bounds
0 60 120 180 240 300 360 420 480
−50
−25
0
25
50
0 60 120 180 240 300 360 420 480
−50
−25
0
25
50
0 60 120 180 240 300 360 420 480
−70
−35
0
35
70
Time (sec)
x
(
d
e
g
/
h
r
)
y
(
d
e
g
/
h
r
)
z
(
d
e
g
/
h
r
)
(f) UF Gyro-Bias Errors and 3σ Bounds
Figure 3. EKF and UF Results for Large Initial Errors
24 of 24
American Institute of Aeronautics and Astronautics

NED frame separates the unstable vertical axis from the more stable horizontal axes which provides more intuitive schemes for analyzing INS errors than using the ECEF frame.4 The conversions between various frames is well known (see Ref. 1 for more details). However, mapping of the covariance of the errors has not been seen by the present author. Therefore, one of the goals of this paper is to present covariance mappings between the NED and ECEF frames. The EKF is widely used in practice, but it has one well-known drawback. If the errors are not within the “linear region”, then filter divergence may occur. This is especially a problem for an integrated GPS/INS since, even though position is well known, attitude and IMU calibration parameters may not be well known a prior. In fact to this day the most researched topic in INS’s has been initial alignment and attitude determination.1 Sigma-point Kalman filters essentially provide derivative-free higher-order approximations by approximating a Gaussian distribution rather than approximating an arbitrary nonlinear function as the EKF does.5 They can provide more accurate results than an EKF, especially when accurate initial condition states are not well known. A sigma-point GPS/INS filter has been presented in Ref. 6, which also includes a method to fuse latency lagged observations in a theoretically consistent fashion. The attitude kinematics in that paper are based on the quaternion, which must obey a normalization constraint that can be violated in the sigma-point filter since the predicted quaternion mean is derived using an averaged sum of quaternions. In this current paper an unconstrained three-component attitude-error vector is used to represent the quaternion error vector and the updates are performed using quaternion multiplication, leading to a natural way of maintaining the normalization constraint.7 Also, Ref. 6 using an augmented matrix approach to handle process noise, which means that a decomposition of a matrix that has length equal to the length of the state vector plus the process-noise vector is required. In this paper a simple first-order trapezoidal approximation is used so that a decomposition of a matrix equal to the length of the state vector is only required. This significantly reduces the computational costs. The organization of this paper proceeds as follows. First, the various coordinate frames used in INS are summarized. Conversions from the NED frame to the ECEF frame and vice versa are shown. Then, covariance mappings from the NED frame to the ECEF frame and vice versa are derived. Next, the linearized equations for the EKF using NED coordinates are shown, which are derived from a multiplicative quaternion approach. Then, a specific sigma-point filter, called the Unscented filter, is summarized followed by a review of the equations required to perform GPS/INS operations with this filter. Finally, simulation results are shown that compare the performance of the EKF with the Unscented filter with respect to initial condition errors.

II.

Reference Frames

In this section the reference frames used to derive the GPS/INS equations are summarized, as shown in Figure 1: • Earth-Centered-Inertial (ECI) Frame: denoted by {ˆ1 , ˆ2 , ˆ3 }. The ˆ1 axis points toward the vernal i i i i equinox direction (also known as the “First Point of Aries” or the “vernal equinox point”), the ˆ3 axis i points in the direction of the North pole and the ˆ2 axis completes the right-handed system (note that i the ˆ1 and ˆ2 axes are on the equator, which is the fundamental plane). The ECI frame is non-rotating i i with respect to the stars (except for precession of equinoxes) and the Earth turns relative to this frame.8 Vectors described using ECI coordinates will have a superscript I (e.g., rI ). • Earth-Centered-Earth-Fixed (ECEF) Frame: denoted by {ˆ1 , e2 , e3 }. This frame is similar to the ECI e ˆ ˆ ˆ ˆ frame with e3 = ˆ3 ; however, the e1 axis points in the direction of the Earth’s prime meridian, and i ˆ the e2 axis completes the right-handed system. Unlike the ECI frame, the ECEF frame rotates with the Earth. The rotation angle is denoted by Θ in Figure 1. Vectors described using ECEF coordinates will have a superscript E (e.g., rE ). • North-East-Down (NED) Frame: denoted by {ˆ , e, d}. This frame is used for local navigation purn ˆ ˆ poses. It is formed by fitting a tangent plane to the geodetic reference ellipse at a point of interest.3 The ˆ ˆ ˆ n axis points true North, the e points East, and the d axis completes the right-handed system, which points in the direction of the interior of the Earth perpendicular to the reference ellipsoid. Vectors described using ECI coordinates will have a superscript N (e.g., rN ).

2 of 24 American Institute of Aeronautics and Astronautics

This frame is fixed onto the vehicle body and rotates with it. y. A sidereal day is 4 minutes shorter than a solar day. 378. given by3 N= a 1 − e2 sin2 λ (2) 3 of 24 American Institute of Aeronautics and Astronautics . which is related to time.g. In order to determine the ECEF position vector we must first determine the angle Θ.ˆ . 356. However. The geodetic coordinates are given by the latitude λ. b3 }. A sidereal day is the length of time that passes between a given fixed star in the sky crossing a given projected meridian. not the Sun. A solar day is the length of time that elapses between the Sun reaching its highest point in the sky two consecutive times. A common ellipsoid model is given by the World Geodetic System 1984 model (WGS-84).g. 752.. Several formulas for the conversion from Universal Time (UT) to GMST are given in the open literature (e. 9). longitude Φ and height h. Vectors described using body-frame coordinates will have a superscript B (e.10 The ECEF position vector is useful since this gives a simple approach to determine the longitude and latitude of a user. which can be given by the angle Θ. y. with semimajor axis a = 6.e i3 ˆ 3 GPS Satellite ρ R r ˆ n ˆ e ˆ d Inertial Reference Direction Equatorial Plane λ Φ ˆ e2 ˆ Θ i1 ˆ e1 ˆ i2 Prime Meridian Plane Figure 1. see Ref. We now discuss the transformations between these reference frames. rB ). One of the most widely-used formulas is presented by Meeus. the length of the normal to the ellipsoid is first computed.3142 m. Definitions of Various Reference Frames ˆ ˆ ˆ • Body Frame: denoted by {b1 .0 m and semiminor axis b = 6. 137. The transformation from the ECI frame to the ECEF frame follows   I  E  x cos Θ sin Θ 0 x      (1) y  = − sin Θ cos Θ 0 y  z 0 0 1 z where {x. The Earth’s geoid can be approximated by an ellipsoid of revolution about its minor axis.8 The Greenwich Mean Sidereal Time (GMST) is the mean sidereal time at zero longitude.0818. The eccentricity of this ellipsoid is given by e = 0. z}I are the components of the ECI position vector. and {x. z}E are the components of the ECEF position vector. the ECI coordinate system is fixed relative to the stars. To determine the ECEF position vector. b2 . Conventions typically depend on the particular vehicle..

This frame serves to define local directions for the velocity vector determined in a frame in which the vehicle has motion.4 The velocity in NED coordinates is given by   vN   vN ≡  vE  = AN rE (5) E˙ vD (7) where φ. longitude and height. e2 = ¯ a2 − b2 b2 (4a) (4b) (4c) (4d) λ = atan Φ = atan2(y. and integrating the resulting equations (see Ref. Covariance Mappings In this section various covariance mappings from the ECEF frame to the the NED frame and vice versa are shown. x) p −N h= cos λ z + e2 b sin3 ζ ¯ p − e2 a cos3 ζ ˙ where rE is the vehicle velocity in ECEF coordinates and AN is the transformation matrix from the ECEF E N N ˙ ˙ frame to the NED frame. ζ = atan za pb . is given by NED ECEF E Patt = AN Patt AN (8) E 4 of 24 American Institute of Aeronautics and Astronautics . The transformation matrix is given by3   − sin λ cos Φ − sin λ sin Φ cos λ   (6) AN =  − sin Φ cos Φ 0  E − cos λ cos Φ − cos λ sin Φ − sin λ The attitude matrix which maps the NED frame to the vehicle body frame is given by   cos ψ cos θ sin ψ cos θ − sin θ   AB = − sin ψ cos φ + cos ψ sin θ sin φ cos ψ cos φ + sin ψ sin θ sin φ cos θ sin φ  N sin ψ sin φ + cos ψ sin θ cos φ − cos ψ sin φ + sin ψ sin θ cos φ cos θ cos φ where N is given by Eq. We should note that vN = rN in general since rN = vN − ωN/E × rN . given the observer geodetic quantities λ. Denote the covariance associated with the attitude matrix that maps the ECEF frame to the body frame ECEF by Patt . 3. A complicated closed-form solution is given in Ref. respectively. but a good approximation up to low Earth orbit is given by11 p= x2 + y 2 . 4 for more details). such as the ECEF frame. a vehicle is fixed within this frame. (2) and atan2 is a four quadrant inverse tangent function. where ωN/E is the angular velocity of the N frame relative to the E frame expressed in N coordinates. denoted by NED Patt . pitch and yaw angles. the observer ECEF position coordinates are computed using x = (N + h) cos λ cos Φ y = (N + h) cos λ sin Φ z = [N (1 − e ) + h] sin λ 2 (3a) (3b) (3c) The conversion from ECEF to geodetic coordinates is not that straightforward. The positions are determined by relating the velocity vN with the derivatives of latitude. The covariance of the attitude matrix that maps the NED frame to the body frame. Φ and h.Then. The NED frame is generally not used to provide a vehicle’s E positional coordinates. These expressions are required since the Kalman filter will be developed in ECEF coordinates. The conversion from ECEF coordinates to NED coordinates involves a rotation matrix from the known latitude and longitude. Note that the transformation from the ECEF to the body frame is simply given by AB = AB AN . but rather to provide local directions along which the velocities may be indicated. By the definition of the NED frame. θ and ψ are the roll. This relationship can be derived by differentiating rN = AN rE . E N E A.

y and z. (5): N E NED ECEF Pvel = AN Pvel AE E N (9) The covariance of latitude. 2 b2 + z 2 a2 ) ∂y p (p ∂Φ x . y and z are given by ∂Φ y . (4c) with respect to x. The partials are given by 1 ∂u ∂λ 1 ∂u ∂λ 1 ∂u ∂λ = . The velocity covariance follows from Eq. (4) with respect to x. To determine this quantity we require the partials of Eq. = 2 ∂y x + y2 ∂ζ pab = 2 2 ∂z p b + z 2 a2 3¯2 b sin2 ζ cos ζ ∂ζ e + p − e2 a cos3 ζ ∂x ∂u (z + e2 b sin3 ζ) y ¯ ∂ζ =− + e2 a sin ζ cos2 ζ ∂x (p − e2 a cos3 ζ)2 p ∂y (12a) (12b) (12c) (13) The partials of Eq. (4d) with respect to x. y and z are given by ∂u (z + e2 b sin3 ζ) ¯ =− ∂x (p − e2 a cos3 ζ)2 x ∂ζ + e2 a sin ζ cos2 ζ p ∂x where 3¯2 b sin2 ζ cos ζ ∂ζ e + p − e2 a cos3 ζ ∂y 2 (z + e2 b sin3 ζ)(e2 a sin ζ cos2 ζ) ∂ζ ¯ 3¯ b sin2 ζ cos ζ e ∂u = − 2 a cos3 ζ ∂x p−e (p − e2 a cos3 ζ)2 ∂z 1 + p − e2 a cos3 ζ xzab ∂ζ =− . the following sensitivity matrix is formed:   ∂λ ∂λ ∂λ  ∂x ∂y ∂z         ∂Φ ∂Φ ∂Φ   H≡  ∂x ∂y ∂z         ∂h ∂h ∂h  ∂x ∂y ∂z 5 of 24 American Institute of Aeronautics and Astronautics (16) (17) . = (10) 2 ∂x 2 ∂y ∂x 1+u ∂y 1+u ∂z 1 + u2 ∂z where u is defined by z + e2 b sin3 ζ ¯ (11) u≡ p − e2 a cos3 ζ The partials of u with respect to x.where AE is the transpose of AN . =− 2 ∂x x + y2 ∂Φ =0 ∂z (14) The partials of Eq. = . 2 b2 + z 2 a2 ) ∂x p (p ∂ζ yzab =− . y and z are given by x p sin λ ∂h = + − ∂x p cos λ cos2 λ y ∂h p sin λ = + − ∂y p cos λ cos2 λ ∂h p sin λ ∂N = − ∂z cos2 λ ∂λ where ∂N ∂λ ∂N ∂λ ∂λ ∂z ∂λ ∂x ∂λ ∂y (15a) (15b) (15c) ∂N a e2 sin λ cos λ = ∂λ (1 − e2 sin2 λ)3/2 Next. longitude and height from the ECEF position covariance is more difficult.

Φ and h. The attitude and velocity mappings from the NED frame to the ECEF frame is straightforward from ECEF NED ECEF NED Eqs.12 One of the most useful attitude parameterization is given by the quaternion.13 which is a four-dimensional ˆ ˆ vector. pitch and yaw angles. Attitude Kinematics In this section the basic properties of attitude kinematics are summarized. which is analogous to requiring that e be a unit vector in the Euler axis/angle parameterization. (8) and (9). etc. (24) is given by B ˙ AN = AN [ωB/N ×] (25) B B 6 of 24 American Institute of Aeronautics and Astronautics . The attitude matrix involves a total of nine parameters. Another form of Eq. Since a four-dimensional vector is used to describe three dimensions.12 The attitude matrix that transform the NED frame to the body frame is related to the quaternion by AB (q) = ΞT (q)Ψ(q) (21) N with Ξ(q) ≡ q4 I3×3 + [̺×] . each given by Patt = AE Patt AN and Pvel = AE Pvel AN . quaternions. which leads to the following partial matrix:   ∂N −(N + h) cos λ sin Φ cos λ cos Φ  ∂λ cos λ cos Φ − (N + h) sin λ cos Φ         ∂N (19) H= cos λ sin Φ − (N + h) sin λ sin Φ (N + h) cos λ cos Φ cos λ sin Φ    ∂λ       ∂N 2 2 (1 − e ) sin λ + [N (1 − e ) + h] cos λ 0 sin λ ∂λ Then. The attitude matrix can then be approximated by AB ≈ I3×3 − [α×] which is valid to within first-order in the N angles. Various parameterizations of the attitude matrix can be used: Euler angles. defined by  −q3 0 q1 An advantage to using quaternions is that the attitude matrix is quadratic in the parameters and also does not involve transcendental functions. The covariance of ECEF N E N E positions from the latitude. Euler axis and rotation angle. the covariance of the ECEF position covariance is given by LLH ECEF Ppos = H Ppos HT (20) III. (3) with respect to λ. denoted by Ppos . with ̺ ≡ [q1 q2 q3 ]T = e sin(ϑ/2) and q4 = cos(ϑ/2). Rodrigues parameters. longitude and height covariance is derived by taking the partials of the quantities in Eq. longitude and height. the quaternion components cannot be independent of each other. but they are clearly not independent.LLH Then. where α is a vector of the roll. The quaternion satisfies a ˆ single constraint given by qT q = 1. the covariance of the latitude. −̺T 0  [̺×] ≡  q3 −q2 Ψ(q) ≡ q4 I3×3 − [̺×] −̺T (22) where [̺×] is the cross product matrix. defined as q ≡ [̺T q4 ]T . where e is the axis of rotation and ϑ is the angle of rotation. is given by LLH ECEF Ppos = H Ppos H T (18) ECEF where Ppos denotes the covariance of the ECEF position errors. The attitude kinematics equation is given by B ˙N AB = −[ωB/N ×]AB N  q2  q1  0 (23) (24) B where ωB/N is angular velocity of the B frame relative to the N frame expressed in B coordinates. For small angles the vector part of the quaternion is approximately equal to half angles so that ̺ ≈ α/2 and q4 ≈ 1.

with q′ ⊗ q = Ψ(q′ ) Also. Another advantage of quaternions is that successive rotations can be accomplished using quaternion multiplication. A computationally efficient algorithm to extract the quaternion from the attitude matrix is given in Ref. This can be written using A(q′ )A(q) = A(q′ ⊗ q) The composition of the quaternions is bilinear. INS Basic Equations The basic INS equations using NED coordinates with the quaternion parameterization are given by3. which is the identity quaternion. Markley. the inverse quaternion is defined by q−1 ≡ −̺ q4 (30) q′ q = Ξ(q) ′ q q  B Ω(ωB/N ) ≡  B −(ωB/N )T (27) 0 (28) (29) Note that q ⊗ q−1 = [0 0 0 1]T .which will be used in the derivation of the INS equations. and a(1 − e2 ) (1 − e2 sin2 λ)3/2 a RΦ = 2 sin2 λ)1/2 (1 − e Rλ = 7 of 24 American Institute of Aeronautics and Astronautics (32a) (32b) . The quaternion kinematics equation is given by ˙ q= where 1 1 B B Ξ(q)ωB/N = Ω(ωB/N )q 2 2  B −[ωB/N ×] B ωB/N (26)    A major advantage of using quaternions is that the kinematics equation is linear in the quaternion and is also free of singularities. as well as others. 4 1 B Ξ(q)ωB/N 2 vN ˙ λ= Rλ + h vE ˙ = Φ (RΦ + h) cos λ ˙ h = −vD ˙ q= (31a) (31b) (31c) (31d) (31e) (31f) (31g) vN vD vE + 2ωe vE sin λ + + aN (RΦ + h) cos λ Rλ + h v E vD vE vE = ˙ + 2ωe vN sin λ + + 2ωe vD cos λ + aE (RΦ + h) cos λ RΦ + h 2 2 vN vE − − 2ωe vE cos λ + g + aD vD = − ˙ RΦ + h Rλ + h vN = − ˙ where ωe is the Earth’s rotation rate given as (from WGS-84) 7. Here we adopt the convention of Lefferts. and Shuster14 who multiply the quaternions in the same order as the attitude matrix multiplication (in contrast to the usual convention established by Hamiliton13 ). Suppose we wish to perform a successive rotation. can be found in the excellent survey paper by Shuster12 and in the book by Kuipers.292115 × 10−5 rad/sec.16 IV. 15. A more thorough review of the attitude representations shown in this section.

8 × 10−6 sin2 2λ) (33) − (3. (31a) can be related to the gyro measurements. We should note that most manufacturers give values for σgv and σav . The scale factors are assumed to be small enough so that the approximation (I + K)−1 ≈ (I − K) is valid for both the gyros and acclerometers.2 × 10−14 h2 m/s2 where h is measured in meters.3024 × 10−3 sin2 λ − 5. The accelerometer measurement model is given by ˜ aB = (I3×3 + Kg )aB + ba + ηav ˙ ba = ηau (39a) (39b) where ba is the accelerometer “bias”.780327(1 + 5.0877 × 10−6 − 4. Kg is a diagonal matrix of gyro scale factors. respectively. using WGS-84 parameters is given by g = 9. and ηgv and ηgu are zero-mean 2 2 Gaussian white-noise processes with spectral densities given by σgv I3×3 and σgu I3×3 . but not σgu and σau . (34) for ωB/N and substituting ωN/I = AB (q)ωN/I yields N B B N ωB/N = ωB/I − AB (q)ωN/I N (34) (35) where N ωN/I where aB is the acceleration vector in body coordinates and AN (q) is the matrix transpose of AB (q). Also. Simulating gyro and accelerometer using computers is not easy since continuous measurements cannot be generated using digital computers. respectively. 8 of 24 American Institute of Aeronautics and Astronautics . B N The gyro measurement model is given by B ˜B ωB/I = (I3×3 + Kg )ωB/I + bg + ηgv Now Eq. the acceleration variables are related to the accelerometer measurements through   aN   N a ≡  aE  = AN (q)aB (37) B aD  vE RΦ + h         cos λ vN     = we  0  +  − R + h    λ   − sin λ    v tan λ  E − RΦ + h  (36) (38a) (38b) ˙ bg = ηgu where bg is the gyro “bias”. this problem can be overcome by using the following identity: B B B ωB/I = ωB/N + ωN/I B B N Solving Eq. g.4 × 10−9 sin2 λ)h + 7.The local gravity. (31a) cannot be used directly with the gyro measurement. A discrete-time simulation is possible using the spectral densities though. which is shown in the Appendix. The same model can be used for the accelerometer measurements. Note that Eq. However. and ηav and 2 2 ηau are zero-mean Gaussian white-noise processes with spectral densities given by σav I3×3 and σau I3×3 . Ka is a diagonal matrix of accelerometer scale factors.

where δα is a small error-angle rotation vector. Also. the quaternion inverse is defined by Eq. The first-order approximation. ωN/I . Note that the attitude matrix is coupled into the position now. The estimated quantities are given 1 ˙ ˆ q ˆB q = Ξ(ˆ )ωB/N 2 N ˆ ˆ ˜B = (I3×3 − Kg )(ωB/I − bg ) − AB (ˆ )ωN/I N q ˆ vN ˆ ˆ ˆλ + h R vE ˆ ˙ ˆ Φ= ˆ ˆ ˆ Φ + h) cos λ (R ˙ ˆ λ= ˙ ˆ h = −ˆD v ˙ vN = − ˆ ˙ vE = ˆ vN vD ˆ ˆ vE ˆ ˆ ˆ + 2ωe vE sin λ + + aN ˆ ˆ ˆ ˆ ˆ Φ + h) cos λ ˆλ + h (R R (40a) (40b) (40c) (40d) (40e) (40f) (40g) (40h) ˆB ωB/N vE vD ˆ ˆ vE ˆ ˆ ˆ ˆ + 2ωe vD cos λ + aE ˆ ˆ + 2ωe vN sin λ + ˆ ˆ ˆ ˆ ˆ RΦ + h (RΦ + h) cos λ ˙ vD = − ˆ vE ˆ2 ˆ ˆ RΦ + h ˆ ˆ ˆ − 2ωe vE cos λ + g + aD ˆ ˆ ˆ Rλ + h   aN ˆ   ˆ aN ≡  aE  = AN (ˆ )ˆB ˆ B q a aD ˆ B ˆ ˆ a ˆ a = (I3×3 − Ka )(˜B − ba ) − ˙ ˆ bg ˙ ˆ ba ˙ ˆ kg ˙ ˆ ka =0 =0 =0 =0 vN ˆ2 (40i) (40j) (40k) (40l) (40m) (40n) ˆ ˆ ˆ ˆ ˆ ˆ ˆN where kg and ka are the elements of the diagonal matrices Kg and Ka . RΦ and g are evaluated at the current estimates. The quaternion is linearized using a multiplicative approach. respectively. 9 of 24 American Institute of Aeronautics and Astronautics . where the quaternion multiplication is defined by Eq. (30). Also. an error quaternion is defined by ˆ δq = q ⊗ q−1 (41) with δq ≡ [δ̺T δq4 ]T . ˆ which allows us to estimate the attitude from position measurements. (29). The equivalent attitude error matrix is given by T AB (δq) = AB (q) AB (ˆ ) (42) N N N q If the error quaternion is “small” then to within first order we have δ̺ ≈ δα/2 and δq4 ≈ 1. which are used in the EKF covariance propagation. which assumes that the true quaternion is “close” to the estimated quaternion. by Extended Kalman Filter Equations In this section the implementation equations for the EKF are shown. This allows us to reduce the order of the system in the EKF by one state. Rλ . We now derive the error equations.V. The linearized model error-kinematics follow17 B N ˙ ˆB δ α = −[ωB/N ×]δα + δωB/I − A(ˆ )δωN/I q (43a) (43b) δ q4 = 0 ˙ B B N N ˆB ˆN where δωB/I = ωB/I − ωB/I and δωN/I = ωN/I − ωN/I .14 First. Note that the fourth error-quaternion component is constant. gives δq4 ≈ 1.

The error δωN/I can be computed using a first-order Taylor series expansion.ˆ N ∆p − AB (q) N N ∂ωN/I ∂vN N N ∆vN ˆ p (45) ˆ ˆ where p ≡ [λ Φ h] . The state. which is within the first-order approximation in the EKF.The linearization using Eq. ∆x ≡  ∆bg  . ΩB is a diagonal matrix of the elements of ωB/I and Bg is a B/I N ˆ diagonal matrix of the elements of bg . This yields ˆ ˆ ˜B ˆ ˜ ˆ ˆ ˙ δ α = − (I3×3 − Kg )(ωB/I − bg )× δα − (I3×3 − Kg )∆bg − (ΩB − Bg )∆kg − (I3×3 − Kg )ηgv B/I − AB (q) N N ∂ωN/I ∂p T ˆ v p. ∆kg = kg − kg . state-error vector. w ≡  gu  (48a)     ηav       ba   ∆ba      ηau  kg   ∆kg  ka ∆ka   2 03×3 03×3 03×3 σgv I3×3   2 03×3 03×3  σgu I3×3  0 (48b) Q =  3×3  2  03×3 03×3 σav I3×3 03×3  03×3 03×3 03×3 2 σau I3×3 10 of 24 American Institute of Aeronautics and Astronautics . The B error δωB/I to within first-order can be written as B ˆ ˜ ˆ ˆ δωB/I = − (I3×3 − Kg )∆bg + (ΩB − Bg )∆kg + (I3×3 − Kg )ηgv B/I (44) ˆ ˆ ˜ ˆ ˜B where ∆bg = bg − bg . The partials are given by   ∂RΦ vE vE 0 − −ωe sin λ −  (RΦ + h)2 ∂λ (RΦ + h)2        N ∂ωN/I   vN ∂Rλ vN  0 = 2 ∂λ 2   (Rλ + h) (Rλ + h)  ∂p      2  vE tan λ ∂RΦ vE tan λ  vE sec λ + 0 −ωe cos λ − RΦ + h (RΦ + h)2 ∂λ (RΦ + h)2   1 0 0   RΦ + h     N   ∂ωN/I 1   0 0 = −  Rλ + h  ∂vN       tan λ 0 − 0 RΦ + h with a e2 sin λ cos λ ∂RΦ = ∂λ (1 − e2 sin2 λ)3/2 ∂Rλ 3a(1 − e2 )e2 sin λ cos λ = ∂λ (1 − e2 sin2 λ)5/2 (46a) (46b) (47a) (47b) The error equations for the remaining states can be derived using a similar approach to derive the attitudeerror equation. ∆p = p − p and ∆v = v − vN . (41) maintains quaternion normalization to within first-order if the estimated quaternion is “close” to the true quaternion. process noise vector and covariance used in the EKF are defined as     q δα       p  ∆p   N  N ηgv v  ∆v        η  x ≡  bg  .

The position partials are given by  − vN ∂Rλ (Rλ + h)2 ∂λ 0  vN (Rλ + h)2     vE sec λ  . ˆ v p. ˆ v p. F22 = F31 = −AN (ˆ )[ˆB ×].ˆ N ˆ F35 = −AN (ˆ )(I3×3 − Ka ). B q ˜ ˆ F37 = −AN (ˆ )(AB − Ba ) B q (51e) ˆ ˜ ˆ ˜ where AB is a diagonal matrix of the elements of aB and Ba is a diagonal matrix of the elements of ba . Y33 ∂v ∂vN ˙N (53) 11 of 24 American Institute of Aeronautics and Astronautics .ˆ N ˜B ˆ F16 = −(ΩB/I − Bg ) F23 = ˙ ∂ vN ∂p ˙ ∂p ∂vN .  0 − (RΦ + h)2    − 0  Z11  = Z21 Z31 1  Rλ + h    ˙ ∂p = 0  N ∂v    0  Z12 Z22 Z32  Z13  Z23  0 0 sec λ RΦ + h 0 0     ˙ ∂ p  v sec λ ∂R vE sec λ tan λ E Φ = ∂p − (R + h)2 ∂λ +  RΦ + h Φ   0 The velocity partials are given by  Y11 ∂v  = Y21 ∂p Y31 ˙N 0 0 0 0      0  (52)    −1  Y13  Y23  . ˆ v p.ˆ N ˆ p (51b) (51c) F32 = F33 = ˙ ∂ vN ∂vN (51d) ˆ v p. B q a ˙ ∂p ∂p .ˆ N F13 = −AB (q) N ∂vN (51a) ˆ p ˆ F14 = −(I3×3 − Kg ). F12 = −AB (ˆ ) N q N ∂ωN/I ∂p .The error-dynamics used in the EKF propagation are given by ˙ ∆x = F ∆x + Gw where F11 F12  03×3 F22   F31 F32  F ≡ 03×3 03×3   03×3 03×3  03×3 03×3 03×3 03×3  ˆ −(I3×3 − Kg )  03×3    03×3  G≡ 03×3   03×3    03×3 03×3  F13 F23 F33 03×3 03×3 03×3 03×3 03×3 03×3 03×3 I3×3 03×3 03×3 03×3 F14 03×3 03×3 03×3 03×3 03×3 03×3 03×3 03×3 F35 03×3 03×3 03×3 03×3 F16 03×3 03×3 03×3 03×3 03×3 03×3  03×3  03×3   F37   03×3    03×3   03×3  03×3 (49) (50a) 03×3 03×3 ˆ −AN (ˆ )(I3×3 − Ka ) q B 03×3 03×3 03×3 03×3 with F11  03×3  03×3   03×3   03×3    I3×3   03×3  03×3 N ∂ωN/I (50b) ˆ ˆ ˜B = − (I3×3 − Kg )(ωB/I − bg )× .

Z12 = − + 2ωe sin λ. Then. (50). and the matrices F and G are given in Eq.8 × 10−9 h sin λ cos λ ∂λ (56a) ∂g = −3.44 × 10−13 h (56b) ∂h The GPS/INS estimation algorithm is summarized in Table 1. the updated quaternion is re-normalized by brute force. Z23 = + 2ωe cos λ = RΦ + h RΦ + h RΦ + h 2vN 2vE Z31 = − . which is provided from the solution of the GPS position determination problem coupled with the transformation given by Eq. Z32 = − − 2ωe cos λ Rλ + h RΦ + h Z11 = (55a) (55b) (55c) Z21 with ∂g = 9. The first three diagonal elements of the error-covariance matrix correspond to attitude errors. The position.780327[1. The assumed measurements are modelled by ˜ pk = pk + vk (57) where vk is a zero-mean Gaussian noise process with covariance given by Rk .4 × 10−9 sin2 λ + 1. Finally.where 2 vE sec2 λ v 2 tan λ ∂RΦ vN vD ∂Rλ + E − 2ωe vE cos λ − 2 ∂λ RΦ + h (RΦ + h) (Rλ + h)2 ∂λ 2 vN vD v tan λ − Y13 = E 2 (RΦ + h) (Rλ + h)2 2 vE vN sec λ vE vN tan λ ∂RΦ vE vD ∂RΦ = − + 2ωe vN cos λ − − 2ωe vD sin λ 2 ∂λ RΦ + h (RΦ + h) (RΦ + h)2 ∂λ vN tan λ + vD Y23 = −vE (RΦ + h)2 2 2 ∂RΦ ∂Rλ vN ∂g vE + + 2ωe vE sin λ + Y31 = 2 ∂λ 2 ∂λ (RΦ + h) (Rλ + h) ∂λ 2 2 vE vN ∂g Y33 = + + (RΦ + h)2 (Rλ + h)2 ∂h Y11 = − (54a) (54b) (54c) (54d) (54e) (54f) Y21 and vD 2vE tan λ vN . A numerical solution for these matrices is given by van Loan. The filter is first initialized with a known state (the bias initial conditions for the gyro and accelerometer are usually assumed zero) and error-covariance matrix. The state error-covariance follows the standard EKF update. the Kalman gain is computed using the measurement-error covariance Rk and sensitivity matrix. given by − + Pk+1 = Φk Pk ΦT + Qk (58) k where Φk is the discrete-time state transition matrix and Qk is the covariance matrix. Z22 = . Z12 = Rλ + h RΦ + h Rλ + h vD + vN tan λ vE vE tan λ + 2ωe sin λ. velocity and bias states also follow the standard EKF additive correction while the attitude errorstate update is computed using a multiplicative update. In order to reduce the computational load a discrete-time propagation of the covariance matrix can be used.64 × 10−5 (sin λ cos3 λ − sin3 λ cos λ)] + 8. the following 2n × 2n matrix is formed:   −F G Q GT   (59) A=  ∆t T 0 F 12 of 24 American Institute of Aeronautics and Astronautics . (18). The process noise covariance is given in Eq. the propagation equations follow the standard EKF model.14 Also. (48).18 First.0877 × 10−6 + 4.06048 × 10−2 sin λ cos λ − 4.

(62) is given by ∆t G Q GT . the discrete-time process noise covariance is given by Qk = Φk B12 Note. the first-order approximation of Eq. (59) is computed:     B11 B12 B11 Φ−1 Qk k     B = eA ≡   = T 0 B22 0 Φk T Φk = B22 (60) (61) Also. the matrix exponential of Eq. Then. 2 ˆk ∆ˆ + = Kk [˜ k − p− ] xk p re-normalize quaternion ˆk ˆk pk p+ = p− + ∆ˆ + ˆN ˆN vk + = vk − + ∆ˆ k + vN Update ˆ ˆ ˆ b+ = b− + ∆b+ gk gk gk ˆ ˆ ˆ b+k = b−k + ∆b+k a a a ˆ ˆ ˆ k+ = k− + ∆k+ gk gk gk ˆ ˆ ˆ k+k = k−k + ∆k+k a a a N ˆ ˆ ˜B ˆB ωB/N = (I3×3 − Kg )(ωB/I − bg ) − AB (ˆ )ωN/I N q Propagation 1 ˙ ˆ q ˆB q = Ξ (ˆ ) ωB/N 2 ˆ ˆ a ˆ aB = (I3×3 − Ka )(˜B − ba ) ˙ ˆ p = fp (ˆ . vN ) p ˆ ˙N ˆ ˆ v = fv (ˆ . Extended Kalman Filter for (Loose) GPS/INS Estimation Initialize ˆ ˆ x(t0 ) = x0 P (t0 ) = P0 − T − T Kk = Pk Hk [Hk Pk Hk + Rk ]−1 Gain Hk = 03×3 I3×3 03×3 03×3 03×3 03×3 03×3 + − Pk = [I − Kk Hk ]Pk 1 ˆk ˆk qk ˆ k q+ = q− + Ξ(ˆ − )δ α+ .Table 1. vN ) + aN p ˆ ˙ P = F P + P F T + G Q GT The state transition matrix is then given by where ∆t is the constant sampling interval. (62) 13 of 24 American Institute of Aeronautics and Astronautics .

which they called the Unscented filter (UF). Given an n × n covariance matrix P . where the system model is given by xk+1 = f (xk . then simply adding µ to each of the points yields a symmetric set of 2n points having the desired mean and covariance. Another approach based on a trapezoidal approximation will be shown here. (63a). including: 1) the expected error is lower than the EKF. its odd central moments are zero. The predicted mean and covariance are also augmented to included the process noise. The general formulation for the propagation equations are given as follows. which will be discussed shortly. uk . and propagates these points through the model in Eq. respectively. 22 for more details). (63a) through an appropriate numerical integration scheme. The set of points is zero-mean. given by ˜ ˆ− υk ≡ yk − yk (65) ˜ = yk − h(ˆ − . Uhlmann. a set of order n points can be generated from the columns (or √ rows) of the matrices ± nP . This approach generates a set of points in [xk . but has several advantages.VI. The gain Kk is computed by xy υυ Kk = Pk (Pk )−1 (66) xy ˆk ˆ− where Pk is the cross-correlation matrix between x− and yk . The Unscented filter uses a different propagation than the form given by the standard extended Kalman filter. it will be more computationally costly due to the extra required calculations arising from the augmented system. 22. the following set of sigma points are computed: σk ← 2n columns from ±γ χk (0) = ˆk x+ χk (i) = σk (i) + ˆk x+ + ¯ Pk + Qk (67a) (67b) (67c) ¯ where the matrix Qk is related to the process noise covariance. k) xk υυ The covariance of υk is defined by Pk . 3) the new filter avoids the derivation of Jacobian matrices. The parameter γ is given by √ γ = n+λ (68) 14 of 24 American Institute of Aeronautics and Astronautics . but if the distribution has mean µ. k) + wk ˜ yk = h(xk . First. so its first three moments are the same as the original Gaussian distribution. Unscented Filtering In this section a new approach that has been developed by Julier. Although this approach may more fully utilize the capability of the unscented transformation. and 4) the new filter is valid to higher-order expansions than the standard EKF. wk ] space that has the correct mean and covariance. It is assumed that wk and vk are zero-mean Gaussian noise processes with covariances given by Qk and Rk . A method for incorporating process noise in the UF is shown in Ref.19 Due to the symmetric nature of this set. 20 is derived for discrete-time nonlinear equations. The filter presented in Ref. but the basic structure of the their calculations remain unchanged (see Ref. 2) the new filter can be applied to non-differentiable functions. This approach. The UF works on the premise that with a fixed number of parameters it should be easier to approximate a Gaussian distribution than to approximate an arbitrary nonlinear function. k) + vk (63a) (63b) Note that a continuous-time model can always be written using eqn. typically involves more computations than the EKF. This is the foundation for the UF. and Durrant-Whyte19 is shown as an alternative to the EKF. which may prohibit its use for actual onboard implementations. This significantly increases the computational burden. We first rewrite the Kalman filter update equations as21 ˆk ˆk x+ = x− + Kk υk + Pk (64a) (64b) = − Pk − υυ T Kk P k Kk where υk is the innovations process.

is given by λ = α2 (n + κ) − n (69) The constant α determines the spread of the sigma points and is usually set to a small positive value (e. . the sigma points can be chosen to lie along the eigenvectors of the covariance matrix.where the composite scaling parameter. 2n = 2(n + λ) where β is used to incorporate prior knowledge of the distribution (a good starting guess is β = 2).20 Also. Even though propagations on the order of 2n are required for the Unscented filter. The transformed set of sigma points are evaluated for each of the points by χk+1 (i) = f [χk (i). . k] (70) We now define the following weights: mean W0 = cov W0 = λ n+λ (71a) (71b) (71c) Wimean = Wicov λ + (1 − α2 + β) n+λ 1 . . λ. (66). Note that there are a total of 2n values for σk (the positive and negative square roots). i = 1. 1 × 10−4 ≤ α ≤ 1). 2. One efficient method to compute the matrix square root is the Cholesky decomposition. and the state vector can now be updated using eqn. x The predicted mean for the state estimate is calculated using a weighted sum of the points χk+1 (i).g. 15 of 24 American Institute of Aeronautics and Astronautics . the significance of the parameter κ will be discussed shortly. which is given by 2n ˆ k+1 x− = i=0 Wimean χk+1 (i) (72) The predicted covariance is given by 2n − Pk+1 = i=0 ¯ ˆ k+1 ˆ k+1 Wicov [χk+1 (i) − x− ] [χk+1 (i) − x− ]T + Qk 2n (73) The mean observation is given by ˆ− yk+1 = i=0 Wimean γk+1 (i) (74) where γk+1 (i) = h[χk+1 (i). the computations may be comparable to the extended Kalman filter (especially if the continuous-time covariance equation needs to be integrated and a numerical Jacobian matrix is evaluated). . (64). k + 1] The output covariance is given by 2n yy Pk+1 = i=0 (75) ˆ− ˆ− Wicov [γk+1 (i) − yk+1 ] [γk+1 (i) − yk+1 ]T (76) Then the innovations covariance is simply given by yy υυ Pk+1 = Pk+1 + Rk+1 (77) Finally the cross correlation matrix is determined using 2n xy Pk+1 = i=0 ˆ− ˆ k+1 Wicov [χk+1 (i) − x− ] [γk+1 (i) − yk+1 ]T (78) The filter gain is then computed using eqn..23 Alternatively.

. 42 16 of 24 American Institute of Aeronautics and Astronautics . A. This mapping is done automatically by the state propagation. . . A better way involves using an unconstrained three-component vector to represent an attitude error quaternion. which gives Φ(∆t) Qk ΦT (∆t) at the end of the interval. which more closely follows the actual process. and does not need to be explicitly accounted for in the propagation. Reference 22 states that if the process noise is purely additive in the model. which makes no assumptions on the form of the probability densities. This approach is similar to a trapezoid rule for integration. no guarantees can be made that the resulting quaternion will have unit norm.e.21 In scalar systems (i. caution should be exercised when κ is negative since a possibility exists that the predicted covariance can become non-positive semi-definite. Any process noise that is added to the state vector in the UF is effectively multiplied by the state transition matrix. (72) poses no difficulties. define the first three components of the vector χk (i) in Eq. non-Gaussian estimation. 20 presents an Unscented Particle filter. 20 that avoids the need to re-factorize at each step. using Eq. for n = 1). For higher-dimensional systems choosing κ = 3 − n minimizes the mean-squared-error up to the fourth order. Furthermore. This filter is straightforward. . χk (i) ≡  χbg (i)  ˆ ˆ χk (0) = xk ≡  gk  (80)  k   ˆ+   ba   bak   χk (i)   ˆ+   Kg   kg k   χk (i)  ˆ+ χKa (i) k ak k where δˆ+ is a generalized Rodrigues error-vector24 used to propagate and update a nominal quaternion.. Ref. (67a) and in the calculation of the predicted covariance in Eq. In this paper we expand upon this concept by incorporating an approximation for the integration over the sampling interval. since the predicted quaternion mean is derived using an averaged sum of quaternions. Therefore. This makes the straightforward implementation of the UF with quaternions undesirable. except for the quaternion normalization.¯ where Qk is used in Eq. sk Since this three-dimensional representation is unconstrained. 2. k k f 2 + ||χδs (i)||2 k .19 However. Using the notation in Eq. . However. . . (67) as χδs (i). 42 (82a) (82b) i = 1. full nonlinear. (72). i. Equation (79) is known as the discrete-time Sylvester equation. 19). i = 1. the resulting overall covariance matrix is a 21 × 21 matrix. a value of κ = 2 leads to errors in the mean and variance that are sixth order. Also. 2 . We begin by defining the following state vector:  +   δˆk s χδs (i) k  +  p  ˆ  pk   χk (i)   N +  VN  vk  χk (i) ˆ    +  b+  . A modified form has been suggested for this case (see Ref. . referring to Eq. (67) we assume ˆk ˆk q+ (0) = q+ ˆ+ qk (i) + with δq+ (i) ≡ δ̺+T (i) δq4k (i) k k T (81a) (81b) = δq+ (i) k ⊗ ˆk q+ . Φ(∆t). then its covariance can simply be added using a simple additive procedure. 2.e. Unscented GPS/INS Filter The scalar κ in the previous set of equations is a convenient parameter for exploiting knowledge (if available) about the higher moments of the given distribution. which makes this an attractive approach. . . Mainly. 42 represented by24 f 2 + (1 − a2 )||χδs (i)||2 k + δq4k (i) = −a ||χδs (i)||2 + f k + δ̺+ (i) = f −1 a + δq4k (i) χδs (i). i = 1. To describe χδs we first k k define a new quaternion generated by multiplying an error quaternion by the current estimate. The total desired process noise follows ¯ ¯ Φ(∆t) Qk ΦT (∆t) + Qk = Qk (79) In this section an UF is derived for GPS/INS estimation. Now. . . adding equal process noise at the beginning and end of the propagation might yield better results. (73). a square root version of the Unscented filter is presented in Ref.

. Number of Available GPS Satellites where a is a parameter from 0 to 1. velocity. not on the actual measurements. so we might expect the covariance to be rotated. Therefore. which is used to move information from one part of the estimate to another part. . 1. and f is a scale factor. . The predicted mean and covariance can now be computed using k+1 k+1 Eqs. (72) and (73). since the update is zero-mean. 2 with the estimated angular velocities given by N ˜B ˆB ωB/N (i) = [I3×3 − χKg (i)] ωB/I − χbg (i) − AB [ˆ (i)]ωN/I . We will choose f = 2(a + 1) so that ||χδs || is k δs equal to the rotational error-angle for small errors. . .8 7 Available Number 6 5 4 0 60 120 180 Time (sec) 240 300 360 420 480 Figure 2. respectively. . where 17 of 24 American Institute of Aeronautics and Astronautics . But the covariance depends on the assumed statistics of the measurements. . the mean rotation caused by the reset is actually zero. the updated quaternions are propagated forward using Eq. the Gibbs vector linearize to half angles. . . N q i = 0.25 This reset rotates the reference frame for the covariance. For small errors the attitude part of the covariance is closely related to the attitude estimation errors for any rotation sequence. 42 (86b) a δ̺− (i) k+1 . 1. given by a simple factor. and biases and scale factors. . as well as an initial 21 × 21 covariance (P0 ). . The propagated error quaternions are computed using ˆ k+1 ˆ k+1 δq− (i) = q− (i) ⊗ q− (0) k+1 −1 . . Note when a = 0 and f = 1 then χδs gives the k Gibbs vector. i = 0. and when a = f = 1 then χδs gives the standard vector of modified Rodrigues parameters k (MRPs). . 42 (84) where χKg (i) and χbg (i) are formed from the gyro scale-factor and bias sigma points. . − + δq4k+1 (i) − with δ̺−T (i) δq4k+1 (i) = δq− (i). and the vector of MRPs linearize to quarter angles. Finally. 42 (85) Note that δq− (0) is the identity quaternion. Next. .14 For example. This is due to the reset of the attitude error to zero after the previous update. We are given initial estimates for the + attitude. the propagated sigma points can be computed using24 k+1 χδs (0) = 0 k+1 χδs (i) = f k+1 T (86a) i = 1. The procedure in the Unscented GPS/INS filter is as follows. 2. with 1 ˙ ˆB ˆ q q(i) = Ξ[ˆ (i)]ωB/N (i). position. 42 (83) i = 0. . so the covariance is in fact not affected by the reset. even though no new information is added. 1. (40a). Equation (81a) clearly requires that χk (0) be zero.

for each axis. where Φk is given by Eq. Next. Qk is calculated using Eq. s+ VII. (62). Initial biases for the gyros and accelerometers are given by 10 deg/hr and 0. no rotation about the z axis for the first 320 seconds.8100 × 10−5 m/sec3/2 and σau = 6. the propagated error quaternions are determined using Eq. The output covariance.005I3×3 . (61) and Qk is given by Eq. the corresponding error quaternions are calculated using Eq.0000 seconds (see Ref. no rotation about the y axis for the first 160 seconds. (74) and (75). (77) and (78). The acceleration inputs are given by aN = 0. aE = 0 and aD = −g0 . The gyro noise parameters are given by σgv = 2. (83). In general. (64). (79). are propagated using Eq. which ranges from a minimum of 5 satellites to a ˙ maximum of 6 satellites. The initial estimates for position are set 18 of 24 American Institute of Aeronautics and Astronautics . which will be used in Eqs. The initial quaternion is given so that the vehicle body frame is aligned N with the local NED frame. To test the robustness of the EKF an initial attitude error of 15 degrees is given in each axis. Also. where Eq. as well as the gyro and accelerometer biases and scale factors. (40). The accelerometer parameters are given by σav = 9. 11 for an explanation of GPS time). Using the position profile the number of GPS satellites available can be computed using a 15◦ elevation cutoff. where g0 is the initial gravity. respectively. Then. These quantities are used as ECEF “measurements” in the filters with covariance computed using Eq. The GPS constellation is simulated using GPS week 137 and a time of applicability of 61440. and the propagated sigma points are calculated using Eq. latitude and height using Eq. (86). The first three components of the initial state vector in the UF is set to zero and the others are set to their initial values for position. then a 5 deg/min rotation for the next 160 seconds and zero for the final 160 seconds. Kg = 0.1989 × 10−7 rad/sec3/2 . position and velocity. (85).. which is then converted into longitude. (76). where the variance (in seconds) of wτ is given by 200.003 m/s2 . such as position. The predicted mean and covariance can now be computed using Eqs. Using all available GPS pseudoranges an ECEF position is determined using nonlinear least squares. Next. The vehicle motion is described in NED coordinates with the origin (point of interest) location at λ0 = 38◦ and Φ0 = −77◦ . (82).01I3×3 and Ka = 0. Then. For this case. velocity.0000 × 10−5 m/sec5/2 .11 The number of available GPS satellites over time is shown in Figure 2.+ the upper 3× 3 partition of P0 corresponds to attitude error angles. The total time of the simulation is 8 minutes. All measurements are assumed to be sampled every 1 second. the quaternion is updated using ˆ k+1 ˆ k+1 q+ = δq+ ⊗ q− (0) (87) k+1 + where δq+ ≡ δ̺+T δq4k+1 k+1 k+1 T is represented by24 −a ||δˆ+ ||2 + f sk+1 sk+1 f 2 + (1 − a2 )||δˆ+ ||2 + δq4k+1 = f 2 + ||δˆ+ ||2 sk+1 (88a) (88b) + δ̺+ = f −1 a + δq4k+1 δˆ+ sk+1 k+1 Finally. (81) is used to compute the sigma-point quaternions from the error quaternions. This error is not unrealistic for an actual application. The approach corresponds to a “loose” GPS/INS configuration. (83) we then calculate the mean observations using Eqs.e. The sigma points are then calculated using Eq. where Ppos is computed from the nonlinear least square solution. (67a) and (73). (72) and (73). biases ¯ and scale factors. position is very well known but attitude is not. The quaternions are subsequently propagated forward in time using Eq. the state vector and covariance are updated using Eq. GPS measurements are obtained using a standard deviation of 5 meters for the white-noise errors. The rotational rate profile is given by: 5 deg/min rotation about the x axis for the first 160 seconds and then zero for the final 320 seconds. then 5 deg/min rotation for the final 160 seconds. innovation covariance and cross-correlation matrix are computed using Eqs. All other sigma-point quantities. The initial covariance matrix P0 in the EKF is diagonal. The initial velocity is given by v0 = [200 200 − 10]T m/s. We choose the parameter a and set f = 2(a + 1).9089 × 10−7 rad/sec1/2 and σgu = 9. Simulation Results In this section simulation results are shown that estimate for a moving vehicle’s attitude. Storing the propagated quaternions from Eq. (18). biases and scale factors. i. (67). (4). [(15/3) × (π/180)]2 rad2 . the three attitude parts of the initial covariance are each set to a 3σ bound of 15 degrees. velocity. δˆk+1 is reset to zero for the next propagation. The clock-bias drift is modelled using a random walk process: τ = wτ . Then.

Canada. β = 2 and κ = 3 − n. No. The position errors for the EKF are given by Figure 3(c). March 2000. Finally. However. “A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators. where n = 21. B. References 1 Chatfield. R. AIAA-2001-4407. Acknowledgements This work was funded by the DOE NNSA NA-22 Proliferation Detection Program Office. straightforward implementation of the Unscented filter using quaternion kinematics did not produce a unit quaternion estimate. which also reduced the size the covariance matrix.. and Barth. VIII.to the true latitude. but longitude drifts outside the bound for a small period.. J. i. E.e. C.” AIAA Guidance. The biggest concern with the EKF solutions is the confidence of the results dictated by the 3σ bounds. In fact. 5 Julier. J.005/3)2 . and Hanson. and Julier. McGraw-Hill. The three gyro-scale factor parts of the initial covariance are each set to a 3σ bound of 0. 2000. J. under the guidance of Theodore Kim and Curtis Webb at Sandia National Laboratories... “Sigma-Point Kalman Filters for Nonlinear Estimation and Sensor- 19 of 24 American Institute of Aeronautics and Astronautics . A.. and Durrant-Whyte. Landis Markley from NASA Goddard Space Flight Center for numerous discussions on gyro noise modelling throughout the years. 3. Advanced Radar Systems project.. S. QB. the three accelerometer-scale factor parts of the initial covariance are each set to a 3σ bound of 0. Reston. 1998.. 4 Jekeli. I..005 meters per second-squared. Berlin. (0. as shown in Figure 3(b).015. The parameters used in the UF are given by α = 0. if the truth is not known a prior and we only had the covariance to assess filter performance. The errors for the EKF drift outside of their respected 3σ bounds for every axis. A comparison of the gyro bias estimates between the EKF and UF is shown by Figures 3(e) and 3(f). (0. i. 1997.. The three gyro-bias parts of the initial covariance are each set to a 3σ bound of 30 degrees per hour. The attitude errors diverge and significantly drift outside their respective 3σ bounds. S.003.. 2001. American Institute of Aeronautics and Astronautics. the author wishes to thank F. T. 477–482. This can certainly provide some misleading results when using the EKF with large initial condition errors. VA. Aug. Inc. Navigation and Contol Conference. pp. F.. this plot would indicate that the EKF is performing better than the UF. The initial variances for latitude and longitude are each given by (1 × 10−6 )2 rad2 .. The resulting EKF attitude errors for a typical case are shown in Figure 3(a). This is due to the large initial errors that are not handled well in the linearization of the dynamic model in the EKF. M. the UF bias errors are much closer to their 3σ bounds than the EKF bias errors. Fundamentals of High Accuracy Inertial Navigation. H. 6 van der Merwe. Conclusions In this paper an Unscented filter formulation was derived for the purpose of GPS/INS applications.” IEEE Transactions on Automatic Control. For this case. “Micromechanical Sensors in Tactical GN&C Applications. Vol. 3 Farrell. Walter de Gruyter. The three accelerometer-bias parts of the initial covariance are each set to a 3σ bound of 0. The Global Positioning System & Inertial Navigation. J.. all UF position errors remain inside their respective 3σ bounds.e. longitude and height. The initial variance for height is given by (20/3)2 m2 . which indicates that the EKF is performing in a subpar fashion. NY.. However.010/3)2 . However. 2 Connelly. Kourepenis. Uhlmann. Marinis. New York. [(30/3) × (π/(180 × 3600))]2 . However. The larger errors in yaw are due to the fact that this angle is the least observable state for the particular vehicle motion. i. Simulation results indicated that the performance of the Unscented filter exceeds the standard extended Kalman filter for large initialization errors.. The filter is based on a quaternion parameterization of the attitude. The latitude and height errors remain within their respective 3σ bounds for the entire simulation run. AC-45. To further stress the filters the initial velocity is set to zero. D. The initial gyro and accelerometer biases and scale factors are all set to zero. Inertial Navigation Systems with Geodetic Applications. A.. The author greatly appreciates this support.e.e.. the UF attitude errors are much closer to their respective 3σ bounds than the EKF attitude errors. (0. K.010. respectively. Also. as shown in Figure 3(f). These simulation results clearly indicate that the UF is able to provide more robust characteristics than an EKF for GPS/INS applications. Germany. This is a surprising result for the EKF since position measurements are directly used in the filter.015/3)2 . Montreal. To overcome this difficulty the quaternion was represented by a three-dimensional vector of generalized Rodrigues parameters. A.. the initial variances in the filters for vN and vE are each set to (200/3)2 and the initial variance for vD is set to (10/3)2 . Wan. i.

M. 5th ed. 15 Shepperd. “Analytic Steady-State Accuracy Solutions for Two Common Spacecraft Attitude Estimators. El Segundo.. London. 2nd ed. R. 11 Hofmann-Wellenhof. NY. J. Y. AIAA-2004-5120. Quebec. Control. 2001. Navigation and Contol Conference. 21 Bar-Shalom. 417–429. R. Green and Co. 439–517. Vol. E. 1999.” AIAA Guidance. John Wiley & Sons. F. F.” Mission Geometry: Orbit and Constellation Design and Management. and Dynamics. No. 5. J. 4. “Unscented Filtering for Spacecraft Attitude Estimation. Weill. 1982. chap. Chapman & Hall/CRC. L. W. July-Aug. No.” Proceedings of the American Control Conference. AIAA2005-6087. S.. pp. England. Control. FL. Princeton University Press.. 27 Farrenkopf. 22 Wan. Vol. L.. 16 Kuipers. Inertial Navigation. Quaternions and Rotation Sequences: A Primer with Applications to Orbits. and Andrews. 223–224. 2001. J. A. and van der Merwe.. 536–542. C. 2001. Inc. 12 Shuster. Lichtenegger.. C. and White. D.” Kalman Filtering and Neural Networks. chap. L. edited by S. Haykin. D. 145–146.” Journal of Guidance. H. A. 2001. L.‡ The single-axis gyro model with no scale factor correction is given by ω (t) = ω(t) + b(t) + ηv (t) ˜ ˙ b(t) = ηu (t) Dividing Eq. and Shuster. 17 Kim. Providence. Optimal Estimation of Dynamic Systems. S. “A Survey of Attitude Representations. Springer Wien. 25 Markley. Richmond. J. R.” IEEE Transactions on Automatic Control. No. J.. Markley.. WA. Haykin. and Dynamics.” Journal of the Astronautical Sciences. 282–284. No.-Oct. R. L.. Aerospace. 1628–1632. 1. “Quaternion from Rotation Matrix.. Aug. 1993. GPS: Theory and Practice... Navigation and Contol Conference. New York. and Collins. pp. pp.. 2005. 5. Attitude and Timing Systems. J. CA. MD.. T. Elements of Quaternions. and Integration. “Kalman Filtering for Spacecraft Attitude Estimation. Global Positioning Systems. Vol. 2001. 7.. R. and Van Loan.. AC-23. L. Fosbury. Cheng. Dover Publications. F. L. pp.. NY. M. The Johns Hopkins University Press. Willman-Bell. Oct. “Attitude Representations for Kalman Filtering. pp. July-Aug. edited by S.. L. (89a) by ∆t and integrating gives 1 ∆t t0 +∆t (89a) (89b) ω (t) dt = ˜ t0 1 ∆t t0 +∆t [ω(t) + b(t) + ηv (t)] dt t0 (90) Assuming that the measurement and truth are each constant over the interval (note: we cannot make the same assumption for the stochastic variables) yields ω (t0 + ∆t) = ω(t0 + ∆t) + ˜ ‡ This 1 ∆t t0 +∆t [b(t) + ηv (t)] dt t0 (91) model is derived from notes provided by F. B.. 26 Grewal. 1988. Vol. Longmans. 4. W. AAS 01-309. R. Wiley.” Kalman Filtering and Neural Networks.-G. 20 of 24 American Institute of Aeronautics and Astronautics . “The Unscented Kalman Filter.” Journal of Guidance and Control. R. 1971. Mueller. New York.” AAS/AIAA Astrodynamics Specialist Conference. Princeton. chap. 4.. K. 396–404. Fundamentals of Astrodynamics. 3. Y.. J. M.” Journal of Guidance and Control. 19 Julier. pp. Quebec City. 2003. Appendix In this appendix the gyro noise model is described in more detail. Landis Markley of NASA Goddard Space Flight Center. D. and Junkins. F. H. New York.. and Fortmann. San Francisco... 20 Wan. 1978. 9 Wertz. New York.. 14 Lefferts. M. NJ.” AIAA Guidance. F. May-June 1978. and Markley.. Crassidis.. Baltimore. 7. No. R.Fusion: Applications to Integrated Navigation. B. and Junkins.. 2004. H. Sept. J.-Dec.. J. “Kalman Filtering for Relative Spacecraft Attitude and Position Estimation.. “Computing Integrals Involving the Matrix Exponential. The Netherlands. Academic Press. pp. J. 1999. L. Tracking and Data Association. MA. No. June 1978. 26. Vol. 7 Crassidis. “Space-Based Orbit. P. S. J.. RI. 1989. 8 Bate. J. and Virtual Reality.. “A New Approach for Filtering Nonlinear Systems. E. 23 Golub. pp. D. VA. 3. Matrix Computations. Boca Raton. E. E. 18 van Loan. 2001. 4. “The Unscented Kalman Filter. G. 1. pp.. Uhlmann. and Durrant-Whyte.. June 1995. Aug. 2nd ed. 41. 56–58.” Journal of Guidance. J. Seattle.. E.. Aug.. 13 Hamilton. 2004. 1866. S.. Astronomical Algorithms. L. 24 Crassidis. NY. Microcosm Press. F. 10 Meeus. Boston.. and van der Merwe.. CA and Kluwer Academic Publishers. NY. John Wiley & Sons. Vol.

Integrating Eq. Solving Eq. (100) yields 1 ωm (t0 + ∆t) = ωm (t0 + ∆t) + [bm (t0 + ∆t) + bm (t0 )] + c Nv ˜ 2 (101) Note that 1 [bm (t0 + ∆t) + bm (t0 )] is the “average” of the bias at the two times. Note that Eq. (89b) gives t0 +∆t b(t0 + ∆t) = b(t0 ) + t0 ηu (t) dt (92) The variance of the gyro drift bias is given by t0 +∆t t0 +∆t E b2 (t0 + ∆t) = E b(t0 ) + t0 ηu (t) dt b(t0 ) + t0 ηu (τ ) dτ (93) 2 Using E {ηu (t)ηu (τ )} = σu δ(t − τ ) gives 2 E b2 (t0 + ∆t) = E b2 (t0 ) + σu ∆t (94) Therefore. (100) can be proven by evaluating E {bm (t0 + ∆t) ωm (t0 + ∆t)}. (96) into Eq. (91) gives ω (t0 + ∆t) = z + ˜ 1 ∆t t0 +∆t t0 t ηu (τ ) dτ dt + t0 1 ∆t t0 +∆t ηv (t) dt t0 (97) where z ≡ ω(t0 + ∆t) + b(t0 ). The correlation between the drift and rate measurement is given by t0 +∆t E {b(t0 + ∆t) ω (t0 + ∆t)} = E ˜ 1 × ω(t0 + ∆t) + b(t0 ) + ∆t b(t0 ) + t0 t0 +∆t t t0 ηu (τ ) dτ t0 +∆t (98) ηv (t) dt t0 1 ηu (ζ) dζ dt + ∆t t0 Since ηu (t) and ηv (t) are uncorrelated we have E {b(t0 + ∆t) ω (t0 + ∆t)} = E {z b(t0 )} + ˜ = E {z b(t0 )} + 2 σu ∆t t0 +∆t t0 t0 +∆t t0 +∆t t0 t t0 δ(τ − ζ) dζ dτ dt (99) 2 σu ∆t t0 1 2 = E {z b(t0 )} + σu ∆t 2 (t − t0 ) dt Equation (99) can be satisfied by modelling the gyro measurement using 1 ωm (t0 + ∆t) = ωm (t0 + ∆t) + bm (t0 ) + σu ∆t1/2 Nu + c Nv ˜ 2 (100) where c is yet to be determined and Nv is a zero-mean random variable with unit variance. The bias at time t is given by t b(t) = b(t0 ) + t0 ηu (τ ) dτ (96) Substituting Eq. the bias can be simulated using bm (t0 + ∆t) = bm (t0 ) + σu ∆t1/2 Nu (95) where the subscript m denotes a modelled quantity and Nu is a zero-mean random variable with unit variance. (95) for Nu and substituting ˜ the resultant into Eq. To evaluate c we compute the 21 of 24 American Institute of Aeronautics and Astronautics . This term is present due 2 to the fact that the trapezoid rule for integration is exact for linear systems.

y) dx dy ∆t (104) y dx dy = 0 ∆t 0 x dx + y = 0 1 2 y + y(∆t − y) 2 dy = 1 3 ∆t 3 Therefore. (100) is given by 1 2 2 E ωm (t0 + ∆t) = E zm + σu ∆t + c2 ˜2 4 Comparing Eq. then Eq.variance of the rate measurement: E ω 2 (t0 + ∆t) = E ˜ z+ 1 ∆t t0 +∆t t0 t t0 τ ηu (υ) dυ dτ + t0 1 ∆t t0 +∆t ηv (τ ) dτ t0 1 × z+ ∆t t0 +∆t t0 1 ηu (ζ) dζ dt + ∆t t0 +∆t (102) ηv (t) dt t0 2 Since ηu (t) and ηv (t) are uncorrelated and using E {ηv (t)ηv (τ )} = σv δ(t − τ ). (95) and (108) for all times and dropping the subscript m gives the following formulas for the discrete-time rate and bias equations 2 1 2 σv 1 + σu ∆t ωk+1 = ωk+1 + [bk+1 + bk ] + ˜ 2 ∆t 12 1/2 Nv (109a) (109b) bk+1 = bk + σu ∆t1/2 Nu 22 of 24 American Institute of Aeronautics and Astronautics . Eq. t − t0 ) dτ dt min(x. (105) gives c2 = 2 1 σv + σ 2 ∆t ∆t 12 u (105) (106) (107) Hence. (102) simplifies to E ω 2 (t0 + ∆t) = E z 2 + ˜ σ2 + v2 ∆t 2 σu ∆t2 t0 +∆t t0 t0 +∆t t0 +∆t t0 t t0 τ t0 δ(υ − ζ) dυ dζ dτ dt t0 +∆t t0 (103) t0 δ(t − τ ) dτ dt The second to last integral can be computed by the following steps: t0 +∆t t0 t0 +∆t t τ t0 t0 t0 +∆t t0 t0 +∆t t0 t0 +∆t t0 t0 +∆t t0 y δ(υ − ζ) dυ dζ dτ dt = = t0 ∆t min(τ − t0 . (106) to Eq. the modelled rate measurement is given by 2 1 1 2 σv ωm (t0 + ∆t) = ωm (t0 + ∆t) + [bm (t0 + ∆t) + bm (t0 )] + ˜ + σu ∆t 2 ∆t 12 1/2 Nv (108) Generalizing Eqs. (103) reduces down to σ2 1 2 E ω 2 (t0 + ∆t) = E z 2 + σu ∆t + v ˜ 3 ∆t The variance of the modelled rate measurement in Eq.

26 give an explanation of these various definitions. then σu = 2 σ 2 α. Many manufacturers give σv in units of deg/sqrt(hr) and a conversion to rad/sqrt(sec) is simple. The units for the scale factor error are often given in “parts per million” (ppm). Suppose we are given a rate change of x rad/sec (1σ) in tx hours. We now discuss other aspects of gyro and accelerometer specifications and how they relate to the INS EKF equations. 27. (114c) and (114c) are “random constants”. Oftentimes the variance of d and κ are given by manufacturers as the “bias repeatability” and “scale factor error”. Note that for gyros the units of σ 2 are often given by deg/hr and for accelerometers in mgal. The process in Eq. if the second value is divided by 60 units of deg/sqrt(hr) are obtained: deg √ = hr Hz hr deg 1 sec × 3600sec 1 hr = deg 60hr 1 hr = deg √ 60 hr (110) The random noise of an accelerometer is often given in units of micro-g/sqrt(Hz). specifications (and units) seem to vary from manufacturer to manufacturer. respectively. Some manufacturers give the “random noise” units as deg/hr/sqrt(Hz). a bias stability that varies from turn-on to turn-on due to thermal cycling among other causes. σu is sometimes referred to as the rate random walk. The PSD σv is often referred to 2 as the angle random walk. There are a number of various definitions for the term “bias” as well. Then σu is given by σu = x √ 60 tx rad/sec3/2 (112) Next. a bias drift 2 after turn-on. where 1 mgal = 10−5 m/s2 (1 gal = 10−2 m/s2 ). (112). To determine the standard deviation of κ multiply ppm by 1 × 10−6 . The variance of ηu (t) is written as4 2 E {ηu (t)ηu (τ )} = 2 σ 2 α δ(t − τ ) ≡ σu δ(t − τ ) (115) 23 of 24 American Institute of Aeronautics and Astronautics . The conversion of these units into meters/sec3/2 is given by µmeters meters × 10−6 meters × 10−6 √ = = 2 Hz sec3/2 1 sec sec2 sec (111) A small number of manufacturers will give rate changes and integrated output changes over time. Both values are equivalent. and other biases that are g-dependent and shock dependent. The conversion of these parameters to σu and σv is given in Ref. The bias repeatability can be used to set b(t0 ). Grewal et al. There is a fixed bias that only needs to be calibrated once. Then σv is given by σv = 2 where σu is calculated using Eq. We first discuss units. given the “correlated noise” 2 parameters σ 2 and 1/α. which is a bit misleading. where κ is a “scale factor” and and the processes in Eqs. A more general model is given by 2 y 2 − 1 σu × (3600 ty )3 3 3600 ty rad/sec1/2 (113) ω (t) = (1 + κ) ω(t) + b(t) + d + ηv (t) ˜ ˙ b(t) = −α b(t) + ηu (t) ˙ d(t) = 0 κ(t) = 0 ˙ (114a) (114b) (114c) (114d) where σ 2 is just another parameter. Unfortunately. Also. (114b) is called a Markov process. suppose we are given an integrated output change of y rad (1σ) in ty hours.These equations are valid for both gyro and accelerometer measurements. Hence. where 1/α is the “correlation time”.

5 0 −7.5 −15 0 30 15 0 −15 −30 0 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 Roll (deg) 15 15 7.5 0 −7.5 −15 0 30 15 0 −15 −30 0 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 Yaw (deg) Pitch (deg) Time (sec) Yaw (deg) Pitch (deg) Time (sec) (a) EKF Attitude Errors and 3σ Bounds (b) UF Attitude Errors and 3σ Bounds Lat.5 0 −7.5 −15 0 15 7. (deg) 2 x 10 −4 2 1 0 −1 x 10 −4 Long.5 0 −7. (deg) −2 0 −4 x 10 2 1 0 −1 −2 0 20 10 0 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 Height (m) −10 −20 0 60 120 180 240 300 360 420 480 Height (m) −10 −20 0 60 120 180 240 300 360 420 480 Time (sec) Time (sec) (c) EKF Position Errors and 3σ Bounds (d) UF Position Errors and 3σ Bounds x (deg/hr) 25 0 −25 −50 0 50 25 0 −25 −50 0 70 35 0 −35 −70 0 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 x (deg/hr) 50 50 25 0 −25 −50 0 50 25 0 −25 −50 0 70 35 0 −35 −70 0 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 y (deg/hr) z (deg/hr) Time (sec) z (deg/hr) y (deg/hr) Time (sec) (e) EKF Gyro-Bias Errors and 3σ Bounds (f) UF Gyro-Bias Errors and 3σ Bounds Figure 3.5 −15 0 15 7.Roll (deg) 7. (deg) Long. (deg) 1 0 −1 −2 0 −4 x 10 2 1 0 −1 −2 0 20 10 0 60 120 180 240 300 360 420 480 60 120 180 240 300 360 420 480 Lat. EKF and UF Results for Large Initial Errors 24 of 24 American Institute of Aeronautics and Astronautics .