17 views

Uploaded by Miltes Mpasi

save

- Coords Ys
- qa-2000
- Self balancing Robot
- lgo_rn_v7-0_en
- iros10-LowCostAccelerometers
- 218.NCATT_ANS_Standard.pdf
- Higher order systematic effect in Ptolemy’s Geographia_
- Sensors for Autonomous Vehicles
- Apollo Descent Guidance
- A PCR-BIMM filter for maneuvering target tracking
- Animation GUI
- Co Ordinate System Guide
- physica status solidi (a)) Volume 185 issue 1 2001 [doi 10.1002/1521-396x(200105)185:11::aid-pssa13.0.co;2-u] G. Müller; G. Krötz; J. Schalk -- New Sensors for Automotive and Aerospace Applications
- Chapter 2 the Geographical Framework
- $
- Sensor
- 4 Rules of Fractions1640
- Aptitude Exam Tricks
- Straight Lines
- Seminar Kebangsaan Matematik & Masyarakat 2008
- r500.pdf
- guidance and control
- Mate Aplicadas
- Cruise missile Defense
- Dispatches from Pluto: Lost and Found in the Mississippi Delta
- Elon Musk: Tesla, SpaceX, and the Quest for a Fantastic Future
- The Innovators: How a Group of Hackers, Geniuses, and Geeks Created the Digital Revolution
- The Unwinding: An Inner History of the New America
- Yes Please
- Sapiens: A Brief History of Humankind
- The Emperor of All Maladies: A Biography of Cancer
- A Heartbreaking Work Of Staggering Genius: A Memoir Based on a True Story
- Grand Pursuit: The Story of Economic Genius
- This Changes Everything: Capitalism vs. The Climate
- The Prize: The Epic Quest for Oil, Money & Power
- Devil in the Grove: Thurgood Marshall, the Groveland Boys, and the Dawn of a New America
- John Adams
- The World Is Flat 3.0: A Brief History of the Twenty-first Century
- Smart People Should Build Things: How to Restore Our Culture of Achievement, Build a Path for Entrepreneurs, and Create New Jobs in America
- The Hard Thing About Hard Things: Building a Business When There Are No Easy Answers
- Rise of ISIS: A Threat We Can't Ignore
- The New Confessions of an Economic Hit Man
- Team of Rivals: The Political Genius of Abraham Lincoln
- Angela's Ashes: A Memoir
- Steve Jobs
- How To Win Friends and Influence People
- Bad Feminist: Essays
- You Too Can Have a Body Like Mine: A Novel
- The Incarnations: A Novel
- The Light Between Oceans: A Novel
- Extremely Loud and Incredibly Close: A Novel
- The Sympathizer: A Novel (Pulitzer Prize for Fiction)
- The Silver Linings Playbook: A Novel
- Leaving Berlin: A Novel
- Bel Canto
- The Master
- A Man Called Ove: A Novel
- Brooklyn: A Novel
- The Flamethrowers: A Novel
- The Rosie Project: A Novel
- The Blazing World: A Novel
- We Are Not Ourselves: A Novel
- The First Bad Man: A Novel
- The Love Affairs of Nathaniel P.: A Novel
- Life of Pi
- The Perks of Being a Wallflower
- The Bonfire of the Vanities: A Novel
- Lovers at the Chameleon Club, Paris 1932: A Novel
- The Cider House Rules
- A Prayer for Owen Meany: A Novel
- My Sister's Keeper: A Novel
- The Wallcreeper
- The Art of Racing in the Rain: A Novel
- Wolf Hall: A Novel
- Beautiful Ruins: A Novel
- The Kitchen House: A Novel
- Interpreter of Maladies
- De Wolk Boven Het Heiligdom - Karl Von Eckartshausen
- Trying to Improve Communication and Collaboration With IT
- Exercises
- OG21engelsk2010
- Prosthetics in the Developing World-A Review of the Literature
- Untitled
- Word 2010
- Ficha-El-Aparato-Respiratorio-y-sus-Partes-para-Cuarto-de-Primaria.doc
- Exposición enfoques cualitativo y cuantitativo.pptx
- 04
- Alinear Faros Delanteros Del Vehículo
- AI Quick Review
- TechnicalArticle_Solar.pdf
- Complex Numbers
- Histo Ria 7 Profesor
- Barajas Social Housing Blocks _ EMBT
- vulcanologia
- Intra Modernism
- Diseño de Redes Con Gps
- 15_Monologen
- 09pinot Adrien m2
- WHISHWORKS Reviews _ Glassdoor
- Planeación Enero
- Escuelas Inclusivas Digitalizado 11-11-10
- ede 4940 feaps explanation
- POLITICAS PÚBLICA Y SISTEMAS EDUCATIVOS CONTEMPORÁNEOS (Autoguardado)
- 70100425.pdf
- lect02
- 00760124
- ATI1 - S04 - Dimensión de los aprendizajes.pdf

You are on page 1of 24

**for Integrated GPS and Inertial Navigation
**

John L. Crassidis

∗

University at Buﬀalo, State University of New York, Amherst, NY 14260-4400

A sigma-point Kalman ﬁlter 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 ﬁlters use a carefully selected set of sample points

to more accurately map the probability distribution than the linearization of the standard

extended Kalman ﬁlter, leading to faster convergence from inaccurate initial conditions in

position/attitude estimation problems. The ﬁlter 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 deﬁne the local attitude

error. A multiplicative quaternion-error approach is used to guarantees that quaternion

normalization is maintained in the ﬁlter. Simulation results are shown to compare the

performance of the sigma-point ﬁlter with a standard extended Kalman ﬁlter 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 Chatﬁeld,

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 speciﬁcations 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 ﬁlter (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 ﬁlter 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” conﬁguration 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 diﬀerent overall performance characteristics. For example, for long duration navigation, the local

∗

Associate Professor, Department of Mechanical & Aerospace Engineering. Email: johnc@eng.buﬀalo.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 ﬁlter 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 ﬁlters 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 ﬁlter 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 ﬁlter 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

ﬁrst-order trapezoidal approximation is used so that a decomposition of a matrix equal to the length of the

state vector is only required. This signiﬁcantly 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 speciﬁc sigma-point ﬁlter, called the Unscented ﬁlter, is summarized followed by a review

of the equations required to perform GPS/INS operations with this ﬁlter. Finally, simulation results are

shown that compare the performance of the EKF with the Unscented ﬁlter 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 ﬁtting 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. Deﬁnitions of Various Reference Frames

• Body Frame: denoted by {

ˆ

b

1

,

ˆ

b

2

,

ˆ

b

3

}. This frame is ﬁxed 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 ﬁrst 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 ﬁxed relative to the

stars, not the Sun. A sidereal day is the length of time that passes between a given ﬁxed 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 ﬁrst 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 deﬁnition of the NED frame, a vehicle is ﬁxed within this frame. This frame

serves to deﬁne 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 diﬀerentiating 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 ﬁlter 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 diﬃcult. 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 deﬁned 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, deﬁned 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 satisﬁes 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, deﬁned 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 ﬁrst-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 Leﬀerts, 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 deﬁned by

q

−1

≡

_

−̺

q

4

_

(30)

Note that q ⊗ q

−1

= [0 0 0 1]

T

, which is the identity quaternion. A computationally eﬃcient 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 deﬁned by

δq = q ⊗ ˆ q

−1

(41)

with δq ≡ [δ̺

T

δq

4

]

T

, where the quaternion multiplication is deﬁned 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 ﬁrst order we have δ̺ ≈ δα/2 and δq

4

≈ 1, where δα is a

small error-angle rotation vector. Also, the quaternion inverse is deﬁned 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 ﬁrst-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 ﬁrst-order if the estimated

quaternion is “close” to the true quaternion, which is within the ﬁrst-order approximation in the EKF. The

error δω

B

B/I

to within ﬁrst-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 ﬁrst-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 deﬁned 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

ﬁlter is ﬁrst initialized with a known state (the bias initial conditions for the gyro and accelerometer are

usually assumed zero) and error-covariance matrix. The ﬁrst 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 ﬁrst-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 ﬁlter (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 ﬁlter can be applied to non-diﬀerentiable functions, 3) the new ﬁlter

avoids the derivation of Jacobian matrices, and 4) the new ﬁlter is valid to higher-order expansions than the

standard EKF. The UF works on the premise that with a ﬁxed number of parameters it should be easier

to approximate a Gaussian distribution than to approximate an arbitrary nonlinear function. The ﬁlter

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 ﬁrst rewrite the Kalman ﬁlter 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 deﬁned 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 ﬁlter uses a diﬀerent propagation than the form given by the standard extended Kalman

ﬁlter. 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 ﬁrst 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 signiﬁcantly 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 signiﬁcance of the parameter κ will be discussed shortly. One eﬃcient 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 deﬁne 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 ﬁlter 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 ﬁlter, the computations may

be comparable to the extended Kalman ﬁlter (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-deﬁnite. A modiﬁed form has been suggested for this case (see Ref. 19). Also, a square root version of

the Unscented ﬁlter is presented in Ref. 20 that avoids the need to re-factorize at each step. Furthermore,

Ref. 20 presents an Unscented Particle ﬁlter, 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 eﬀectively 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 ﬁlter 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 deﬁning 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 diﬃculties, which makes this an attractive approach.

Now, deﬁne the ﬁrst three components of the vector χ

k

(i) in Eq. (67) as χ

δs

k

(i). To describe χ

δs

k

we ﬁrst

deﬁne 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 modiﬁed 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 aﬀected 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 ﬁlter 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 ﬁrst 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

proﬁle is given by: 5 deg/min rotation about the x axis for the ﬁrst 160 seconds and then zero for the ﬁnal

320 seconds; no rotation about the y axis for the ﬁrst 160 seconds, then a 5 deg/min rotation for the next

160 seconds and zero for the ﬁnal 160 seconds; no rotation about the z axis for the ﬁrst 320 seconds, then 5

deg/min rotation for the ﬁnal 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

proﬁle the number of GPS satellites available can be computed using a 15

◦

elevation cutoﬀ.

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 ﬁlters 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 conﬁguration. 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 ﬁlters

the initial velocity is set to zero. For this case, the initial variances in the ﬁlters 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 signiﬁcantly 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 conﬁdence 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 ﬁlter 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 ﬁlter. 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 ﬁlter formulation was derived for the purpose of GPS/INS applications. The

ﬁlter is based on a quaternion parameterization of the attitude. However, straightforward implementation of

the Unscented ﬁlter using quaternion kinematics did not produce a unit quaternion estimate. To overcome

this diﬃculty 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 ﬁlter exceeds the standard extended Kalman ﬁlter for large initialization errors.

Acknowledgements

This work was funded by the DOE NNSA NA-22 Proliferation Detection Program Oﬃce, 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

Chatﬁeld, 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

Leﬀerts, 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 satisﬁed 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) simpliﬁes 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 speciﬁcations and how they relate to the INS EKF

equations. We ﬁrst discuss units. Unfortunately, speciﬁcations (and units) seem to vary from manufacturer

to manufacturer. There are a number of various deﬁnitions for the term “bias” as well. Grewal et al.

26

give

an explanation of these various deﬁnitions. There is a ﬁxed 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 ﬁlter 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 ﬁlters 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 ﬁlter 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 ﬁlter 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 ﬁrst-order trapezoidal approximation is used so that a decomposition of a matrix equal to the length of the state vector is only required. This signiﬁcantly 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 speciﬁc sigma-point ﬁlter, called the Unscented ﬁlter, is summarized followed by a review of the equations required to perform GPS/INS operations with this ﬁlter. Finally, simulation results are shown that compare the performance of the EKF with the Unscented ﬁlter 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 ﬁtting 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 ﬁxed 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 ﬁrst 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 ﬁxed 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 ﬁrst computed.3142 m. Deﬁnitions 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 ﬁxed relative to the stars. To determine the ECEF position vector. b2 . Conventions typically depend on the particular vehicle..

This frame serves to deﬁne 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 ﬁxed 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 ﬁlter 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 deﬁnition of the NED frame. θ and ψ are the roll. This relationship can be derived by diﬀerentiating 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 deﬁned 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 diﬃcult.

Φ 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 ﬁrst-order in the N angles. Various parameterizations of the attitude matrix can be used: Euler angles. deﬁned 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 satisﬁes 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. deﬁned 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 eﬃcient 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 deﬁned 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 Leﬀerts. 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 deﬁned by Eq. The ﬁrst-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 deﬁned by Eq. (30). Also. an error quaternion is deﬁned 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 ﬁrst 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 ﬁrst-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 ﬁrst-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 ﬁrst-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 ﬁrst-order if the estimated quaternion is “close” to the true quaternion. process noise vector and covariance used in the EKF are deﬁned 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 ﬁrst 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 ﬁlter is ﬁrst 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 ﬁrst-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 ﬁlter (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 ﬁlter uses a diﬀerent propagation than the form given by the standard extended Kalman ﬁlter. 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 deﬁned by Pk . 3) the new ﬁlter 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 ﬁrst 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 ﬁlter 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 ﬁlter presented in Ref. but the basic structure of the their calculations remain unchanged (see Ref. 2) the new ﬁlter can be applied to non-diﬀerentiable functions. This approach. The UF works on the premise that with a ﬁxed 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 signiﬁcantly increases the computational burden. We ﬁrst rewrite the Kalman ﬁlter 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 ﬁlter. The transformed set of sigma points are evaluated for each of the points by χk+1 (i) = f [χk (i). . k] (70) We now deﬁne 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 eﬃcient 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 signiﬁcance 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 ﬁlter (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 ﬁlter 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-deﬁnite. Any process noise that is added to the state vector in the UF is eﬀectively multiplied by the state transition matrix. (72) poses no diﬃculties. deﬁne the ﬁrst three components of the vector χk (i) in Eq. non-Gaussian estimation. 20 presents an Unscented Particle ﬁlter. 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 ﬁlter 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 deﬁning the following state vector: + δˆk s χδs (i) k + p ˆ pk χk (i) N + VN vk χk (i) ˆ + b+ . A modiﬁed 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 ﬁrst k k deﬁne 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 ﬁlter 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 modiﬁed 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 ﬁlter 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 aﬀected 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 ﬁrst 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 ﬁrst 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 proﬁle the number of GPS satellites available can be computed using a 15◦ elevation cutoﬀ. where g0 is the initial gravity. respectively. Then. These quantities are used as ECEF “measurements” in the ﬁlters 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 ﬁrst 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 ﬁnal 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 conﬁguration. (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 proﬁle is given by: 5 deg/min rotation about the x axis for the ﬁrst 160 seconds and then zero for the ﬁnal 320 seconds. then 5 deg/min rotation for the ﬁnal 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 Chatﬁeld. R. AIAA-2001-4407. Acknowledgements This work was funded by the DOE NNSA NA-22 Proliferation Detection Program Oﬃce. straightforward implementation of the Unscented ﬁlter 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 conﬁdence 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 ﬁlter 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 signiﬁcantly 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 ﬁlter 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 ﬁlter exceeds the standard extended Kalman ﬁlter for large initialization errors.. The ﬁlter 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 ﬁlters 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 ﬁlter.015/3)2 . Montreal. To overcome this diﬃculty the quaternion was represented by a three-dimensional vector of generalized Rodrigues parameters. A.. the initial variances in the ﬁlters 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 Leﬀerts. 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 satisﬁed 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) simpliﬁes 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 deﬁnitions. 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 speciﬁcations 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). speciﬁcations (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 deﬁnitions 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 ﬁxed bias that only needs to be calibrated once. Then σv is given by σv = 2 where σu is calculated using Eq. We ﬁrst 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 .

- Coords YsUploaded bygvsbabu63
- qa-2000Uploaded byMohammad Reza Madadi
- Self balancing RobotUploaded byMuhammadHaris
- lgo_rn_v7-0_enUploaded byGulag
- iros10-LowCostAccelerometersUploaded byAbbé Busoni
- 218.NCATT_ANS_Standard.pdfUploaded byMarcos Gomez
- Higher order systematic effect in Ptolemy’s Geographia_Uploaded bybongogenius
- Sensors for Autonomous VehiclesUploaded byChinmaye Purushotham
- Apollo Descent GuidanceUploaded byKiran Kumar
- A PCR-BIMM filter for maneuvering target trackingUploaded byAnonymous 0U9j6BLllB
- Animation GUIUploaded bysolid34
- Co Ordinate System GuideUploaded byAjay Jayabalan
- physica status solidi (a)) Volume 185 issue 1 2001 [doi 10.1002/1521-396x(200105)185:11::aid-pssa13.0.co;2-u] G. Müller; G. Krötz; J. Schalk -- New Sensors for Automotive and Aerospace ApplicationsUploaded byecasayang
- Chapter 2 the Geographical FrameworkUploaded bySrikanth Jutru
- $Uploaded byAdeyemi Olukorede Deborah
- SensorUploaded byAlex Rivkin
- 4 Rules of Fractions1640Uploaded byMelitado Weng
- Aptitude Exam TricksUploaded byshajmalik
- Straight LinesUploaded bytutorciecleteam
- Seminar Kebangsaan Matematik & Masyarakat 2008Uploaded byihwanali
- r500.pdfUploaded byTrandafir Florin
- guidance and controlUploaded byapi-3827338
- Mate AplicadasUploaded byFernando Gomez
- Cruise missile DefenseUploaded byDhimas Afihandarin

- De Wolk Boven Het Heiligdom - Karl Von EckartshausenUploaded byMonique Neal
- Trying to Improve Communication and Collaboration With ITUploaded byAsif Changezi
- ExercisesUploaded byxandercage
- OG21engelsk2010Uploaded byMichael Shelton
- Prosthetics in the Developing World-A Review of the LiteratureUploaded byAnirudhreddy Safal
- UntitledUploaded bymojagmina
- Word 2010Uploaded byAlexandra Ferreira
- Ficha-El-Aparato-Respiratorio-y-sus-Partes-para-Cuarto-de-Primaria.docUploaded byBlanca Canales Reyes
- Exposición enfoques cualitativo y cuantitativo.pptxUploaded bybibiana1006
- 04Uploaded bytegs123
- Alinear Faros Delanteros Del VehículoUploaded byArnold quispe c
- AI Quick ReviewUploaded byNajar Aryal
- TechnicalArticle_Solar.pdfUploaded byjbb_sit
- Complex NumbersUploaded bylhau_8
- Histo Ria 7 ProfesorUploaded bypialorena
- Barajas Social Housing Blocks _ EMBTUploaded byLucas Roberto Terneiro
- vulcanologiaUploaded byisamvgm
- Intra ModernismUploaded byMars Party
- Diseño de Redes Con GpsUploaded byKatherine Gonzalez
- 15_MonologenUploaded byShay Lachman
- 09pinot Adrien m2Uploaded bydmeha
- WHISHWORKS Reviews _ GlassdoorUploaded bypremkumarhyd
- Planeación EneroUploaded bySeran Lopez Reyes
- Escuelas Inclusivas Digitalizado 11-11-10Uploaded byCreaciones Alfil
- ede 4940 feaps explanationUploaded byapi-308679113
- POLITICAS PÚBLICA Y SISTEMAS EDUCATIVOS CONTEMPORÁNEOS (Autoguardado)Uploaded byOliver Mora Juarez
- 70100425.pdfUploaded byJoeMoeller
- lect02Uploaded byapi-3738694
- 00760124Uploaded byAmit Debnath
- ATI1 - S04 - Dimensión de los aprendizajes.pdfUploaded byGaby Zapata