Introduction to Inertial Navigation and

Kalman Filtering

Tutorial for:
IAIN World Congress,
Stockholm, October 2009
Kenneth Gade, FFI

Outline

Notation
Inertial navigation
Aided inertial navigation system (AINS)
Implementing AINS
Initial alignment (gyrocompassing)
AINS demonstration

Kenneth Gade, FFI

Slide 2

Kinematics

Mathematical model of physical world using

Point, represents a position/particle (affine space)
Vector, represents a direction and magnitude (vector space)

Kenneth Gade, FFI

Slide 3

Coordinate frame

One point (representing position)

Three basis vectors (representing orientation)

6 degrees of freedom
Can represent a rigid body

Kenneth Gade, FFI

Slide 4

Important coordinate frames

Frame
symbol

Description

Inertial

Earth-fixed

Body-fixed

North-EastDown (local
level)

Local level,
wander
azimuth (as N,
but not northaligned =>
nonsingular)

(Figure assumes
spherical earth)

REL longitude,

latitude, wander
azimuth

RNB, RLB roll,

Figure: Gade (2008)

pitch, yaw
Kenneth Gade, FFI

Slide 5

General vector notation

Coordinate free vector (suited for expressions/deductions):
Sum of components along the basis vectors of E (

bE ,i , bE , j , bE ,k ):

x = xi bE ,i + x j bE , j + xk bE ,k
xk

Vector decomposed
in frame E (suited
for computer
implementation):

xi

E
x = xj
xk

E
xi

x
xj

Kenneth Gade, FFI

Slide 6

Notation for position, velocity, acceleration

Symbol

Definition

p AB
C
C

v AB

v AB
C
C

Description

B A

Position vector. A vector whose length and direction is

such that it goes from the origin of A to the origin of B.

d
( p AB )
dt

Generalized velocity. Derivative of p AB , relative to

coordinate frame C.

Standard velocity. The velocity of the origin of

coordinate frame B relative to coordinate frame A. (The

v AB

d2

a AB

( dt )

a AB

p
2 ( AB )

a AB

frame of observation is the same as the origin of the differentiated position vector.)
Note that the underline shows that both orientation and position of A matters
(whereas only the position of B matters)

Generalized acceleration. Double derivative of p AB ,

relative to coordinate frame C.
Standard acceleration. The acceleration of the origin of
coordinate frame B relative to coordinate frame A.

Kenneth Gade, FFI

Slide 7

Notation for orientation and angular

velocity

Symbol

Definition

Description

k AB AB

Angle-axis product. k AB is the axis of rotation and AB is

the angle rotated.

RAB

(to be
published)

Rotation matrix. Mostly used to store orientation and

decompose vectors in different frames,
x A = RAB x B .
Notice the rule of closest frames.

AB

(to be
published)

Angular velocity. The angular velocity of coordinate frame

B, relative to coordinate frame A.

AB

Kenneth Gade, FFI

Slide 8

Outline

Notation
Inertial navigation
Aided inertial navigation system (AINS)
Implementing AINS
Initial alignment (gyrocompassing)
AINS demonstration

Kenneth Gade, FFI

Slide 9

Navigation
Navigation:
Estimate the position, orientation and velocity of a vehicle
Inertial navigation:
Inertial sensors are utilized for the navigation

Kenneth Gade, FFI

Slide 10

Inertial Sensors
Based on inertial principles, acceleration and angular velocity are
measured.

Always relative to inertial space

Most common inertial sensors:
Accelerometers
Gyros

Kenneth Gade, FFI

Slide 11

Accelerometers (1:2)
By attaching a mass to a spring, measuring its deflection, we get a
simple accelerometer.

Figure: Gade (2004)

To improve the dynamical interval and linearity and also

reduce hysteresis, a control loop, keeping the mass close to
its nominal position can be applied.
Kenneth Gade, FFI

Slide 12

Accelerometers (2:2)

Gravitation is also measured (Einstein's principle of equivalence)

= a IB g B = a IB

FB , gravitation

Total measurement called specific force, f IB

Using 3 (or more) accelerometers we can form a 3D specific force

measurement:
B

f IB

This means: Specific force of the body system (B) relative inertial space (I), decomposed in
the body system.

Good commercial accelerometers have an accuracy in the order of 50 g.

Kenneth Gade, FFI

Slide 13

Gyros (1:3)
Gyros measure angular velocity relative inertial space:

IB

Principles:

Maintain angular momentum (mechanical

gyro). A spinning wheel will resist any change in
its angular momentum vector relative to inertial
space. Isolating the wheel from vehicle angular
movements by means of gimbals and then
output the gimbal positions is the idea of a
mechanical gyro.
Figure: Caplex (2000)

Kenneth Gade, FFI

Slide 14

Gyros (2:3)

The Sagnac-effect. The inertial characteristics of light can also be utilized, by

letting two beams of light travel in a loop in opposite directions. If the loop
rotates clockwise, the clockwise beam must travel a longer distance before
finishing the loop. The opposite is true for the counter-clockwise beam.
Combining the two rays in a detector, an interference pattern is formed, which
will depend on the angular velocity.

The loop can be implemented with

3 or 4 mirrors (Ring Laser Gyro), or
with optical fibers (Fiber Optic
Gyro).
Figure: Bose (1998)
Kenneth Gade, FFI

Slide 15

Gyros (3:3)
The Coriolis-effect. Assume a mass that is
vibrating in the radial direction of a rotating
system. Due to the Coriolis force working
perpendicular to the original vibrating
direction, a new vibration will take place in
this direction. The amplitude of this new
vibration is a function of the angular velocity.
MEMS gyros (MicroElectroMechanical
Systems), tuning fork and wineglass gyros
are utilizing this principle.
Coriolis-based gyros are typically cheaper
and less accurate than mechanical, ring laser
or fiber optic gyros.

Tine
radial
vibration
axis

Figure: Titterton & Weston (1997)

Kenneth Gade, FFI

Slide 16

IMU
Several inertial sensors are often assembled to form an Inertial
Measurement Unit (IMU).

Typically the unit has 3 accelerometers and 3 gyros (x, y and z).

In a strapdown IMU, all inertial sensors are rigidly attached to the unit (no
mechanical movement).
In a gimballed IMU, the gyros and accelerometers are isolated from
vehicle angular movements by means of gimbals.

Kenneth Gade, FFI

Slide 17

Example (Strapdown IMU)

Honeywell HG1700 ("medium
quality"):

3 accelerometers, accuracy: 1 mg
3 ring laser gyros, accuracy: 1 deg/h
Rate of all 6 measurements: 100 Hz

Foto: FFI

Kenneth Gade, FFI

Slide 18

Inertial Navigation
B
B
f
An IMU (giving IB and IB) is sufficient to navigate relative to inertial

space (no gravitation present), given initial values of velocity, position

and orientation:
Integrating the sensed acceleration will give velocity.
A second integration gives position.
To integrate in the correct direction, orientation is needed. This is
obtained by integrating the sensed angular velocity.

Kenneth Gade, FFI

Slide 19

Terrestrial Navigation
In terrestrial navigation we want to navigate relative to the Earth (E).
Since the Earth is not an inertial system, and gravity is present, the
inertial navigation becomes somewhat more complex:

Earth angular rate must be compensated for in the gyro

measurements: B = B B
EB

IB

IE

Accelerometer measurement compensations:

Gravitation
Centrifugal force (due to rotating Earth)
Coriolis force (due to movement in a rotating frame)

Kenneth Gade, FFI

Slide 20

Navigation Equations
Gyros

IBB

L
RLB = RLB S (IBB ) S (IEL + EL
) RLB

( )dt

IEL = RLEIEE

Initial attitude

RLB
Accelerometers

f IBB

RLB

L
L
vEB
= RLB f IBB + gBL IEL (IEL pEB
)
L
( 2IEL + EL
) vEBL

( )dt
Assuming:
spherical earth
wander azimuth L
Not included:
vertical direction
gravity calculation

Initial velocity

L
vEB

L
EL
=

1 L
L
nEB vEB
(
)
rEB

L
vEB

L
REL = REL S (EL
)

Initial position

( )dt

REL

EL
Kenneth Gade,
FFI

Slide 21

Inertial Navigation System (INS)

The combination of an IMU and a computer running navigation equations is
called an Inertial Navigation System (INS).

Gyros

Accelerometers

Angular
velocity,

Attitude, RLB or roll/pitch/yaw

B
IB

Velocity,
Specific
force,

IMU

f IBB

Navigation
Equations

L
vEB

Horizontal E or longitude/
position, n
latitude
Depth,

INS

Due to errors in the gyros and accelerometers, an INS will have unlimited drift in
velocity, position and attitude.
The quality of an IMU is often expressed by expected position drift per hour (1).
Examples (classes):
HG1700 is a 10 nautical miles per hour IMU.
HG9900 is a 1 nautical mile per hour IMU.
Kenneth Gade, FFI

Slide 22

Outline

Notation
Inertial navigation
Aided inertial navigation system (AINS)
Implementing AINS
Initial alignment (gyrocompassing)
AINS demonstration

Kenneth Gade, FFI

Slide 23

Aided inertial navigation system

To limit the drift, an INS is usually
aided by other sensors that
provide direct measurements of
the integrated quantities.

Examples of aiding sensors:

Sensor:

Measurement:

Pressure meter

Depth/height

Magnetic compass

Heading

Doppler velocity log

Underwater
transponders

B
v EB

(or

B
, water)
vWB

Range from known

position

GPS

E
pEB

GPS (Doppler shift)

E
v EB

Multi-antenna GPS

Orientation

Kenneth Gade, FFI

Slide 24

Sensor error models

Typical error models for IMU, Doppler velocity log and others:
white noise
colored noise (1st order Markov)
scale factor error (constant)
misalignment error (constant)

Kenneth Gade, FFI

Slide 25

Kalman Filter
A Kalman filter is a recursive algorithm for estimating states in a system.
Examples of states:
Position, velocity etc for a vehicle
pH-value, temperature etc for a chemical process
Two sorts of information are utilized:
Measurements from relevant sensors
A mathematical model of the system (describing how the different
states depend on each other, and how the measurements depend on
the states)
In addition the accuracy of the measurements and the model must be
specified.

Kenneth Gade, FFI

Slide 26

Kalman Filter Algorithm

Description of the recursive Kalman filter algorithm, starting at t0:
1. At t0 the Kalman filter is provided with an initial estimate, including its uncertainty
(covariance matrix).
2. Based on the mathematical model and the initial estimate, a new estimate valid at
t1 is predicted. The uncertainty of the predicted estimate is calculated based on the
initial uncertainty, and the accuracy of the model (process noise).
3. Measurements valid at t1 give new information about the states. Based on the
accuracy of the measurements (measurement noise) and the uncertainty in the
predicted estimate, the two sources of information are weighed and a new updated
estimate valid at t1 is calculated. The uncertainty of this estimate is also calculated.
4. At t2 a new estimate is predicted as in step 2, but now based on the updated
estimate from t1.
...
The prediction and the following update are repeated each time a new measurement
arrives.

If the models/assumptions are correct, the Kalman filter will deliver

optimal estimates.
Kenneth Gade, FFI

Slide 27

Kalman Filter Equations

State space model:

xk = k 1 xk 1 + vk 1 , vk N ( 0 ,Vk )
yk = Dk xk + wk , wk N ( 0, Wk )

Initial estimate (k = 0):

x 0 = E ( x0 ) , P0 = E

State and covariance prediction:

xk = k 1 x k 1
Pk = k 1 Pk 1kT1 + Vk 1

Measurement update (using yk):

Kalman gain matrix:

( x0 x 0 )( x0 x 0 )

x k = xk + K k ( yk Dk xk )
Pk = ( I K k Dk ) Pk
K k = Pk DkT ( Dk Pk DkT + Wk )

Kenneth Gade, FFI

Slide 28

Kalman Filter Design for Navigation

Objective: Find the vehicle position, attitude and velocity with the best
accuracy possible
Possible basis:
Sensor measurements (measurements)
System knowledge (mathematical model)
Control variables (measurements)
We utilize sensor measurements and knowledge of their behavior (error
models).

This information is combined by means of an error-state Kalman filter.

Kenneth Gade, FFI

Slide 29

Example:
HUGIN AUV

DGPS

DGPS: Differential Global

Positioning System

USBL
IMU
Pressure sensor
Compass

USBL: Ultra-Short BaseLine

DVL: Doppler Velocity Log

DVL

Kenneth Gade, FFI

Slide 30

Measurements
Sensor
IMU
DGPS/USBL
Pressure
sensor
DVL

Compass

Measurement
Angular velocity, specific force
Horizontal position
measurement

Symbol

IBB , f IBB
E
pEB

Depth
AUV velocity (relative the
seabed) projected into the body
(B) coordinate system
Heading (relative north)

To make measurements for

the error-state Kalman filter
we form differences of all
redundant information.
This can be done by
running navigation
equations on the IMU-data,
and compare the outputs
with the corresponding
aiding sensors.

B
v EB

north

The INS and the aiding

sensors have
complementary
characteristics.

Kenneth Gade, FFI

Slide 31

Aided Inertial Navigation System

Based on the measurements and sensor error models, the Kalman filter
estimates errors in the navigation equations and all colored sensor
errors.
Kenneth Gade, FFI

Slide 32

Optimal Smoothing
Smoothed estimate: Optimal estimate based on all logged measurements
(from both history and future)
Smoothing gives:
Improved accuracy (number of relevant measurements doubled)
Improved robustness
Improved integrity
Estimate in accordance with process model
First the ordinary Kalman filter is run through the entire time series, saving all estimates and covariance
matrices. The saved data is then processed recursively backwards in time using an optimal smoothing
algorithm adjusting the filtered estimates (Rauch-Tung-Striebel implementation).

Kenneth Gade, FFI

Slide 33

Outline

Notation
Inertial navigation
Aided inertial navigation system (AINS)
Implementing AINS
Initial alignment (gyrocompassing)
AINS demonstration

Kenneth Gade, FFI

Slide 34

Practical navigation processing

Any vehicle with an IMU and some aiding sensors, can use the AINS to
find its position, orientation and velocity.
Typical implementation:
Vehicle:
Sensors

Real-time
navigation
(Kalman
filter)

Pos, orientation,
velocity

Guidance &
control

Control
signals

Hard disk

Real-time navigation
Post-processed navigation

Post mission
download
Post-processed
navigation
(smoothing)

Pos, orientation,
velocity

Geo-referencing
recorded data (e.g.
map making)
Kenneth Gade, FFI

Slide 35

NavLab
NavLab (Navigation Laboratory) is one common tool for solving a variety
of navigation tasks.
Structure:
Development started in
1998
Main focus during
development:
Solid theoretical
foundation
(competitive edge)

IMU Simulator

Navigation
Equations
Error state
Kalman filter

Position
measurement
Simulator
Trajectory
Simulator

Depth
measurement
Simulator

Make Kalman
filter
measurements
(differences)

Filtered
estimates
and
covariance
matrices

Velocity
measurement
Simulator
Optimal
Smoothing
Compass
Simulator
Smoothed
estimates
and
covariance
matrices

Simulator (can be replaced by

real measurements)

Estimator (can interface with

simulated or real measurements)
Kenneth Gade, FFI

Slide 36

Simulator

Trajectory simulator
Can simulate any trajectory
in the vicinity of Earth
No singularities

Sensor simulators
Most common sensors with
their characteristic errors are
simulated
All parameters can change
with time
Rate can change with time

Figure: NavLab
Kenneth Gade, FFI

Slide 37

NavLab Usage
Main usage:
1. Navigation system research and development
2. Analysis of navigation system
3. Decision basis for sensor purchase and mission planning
4. Post-processing of real navigation data
5. Sensor evaluation
6. Tuning of navigation system and sensor calibration
Vehicles navigated with NavLab:
AUVs, ROVs, ships, aircraft, helicopters
Users:
Research groups (e.g. FFI (several groups), NATO Undersea
Research Centre, QinetiQ, Kongsberg Maritime, Norsk Elektro
Optikk)
Universities (e.g. NTNU, UniK)
Commercial companies (e.g. C&C Technologies, Geoconsult,
FUGRO, Thales Geosolutions, Artec Subsea, Century Subsea)
Norwegian Navy
) For more details, see www.navlab.net

Kenneth Gade, FFI

Slide 38

Outline

Notation
Inertial navigation
Aided inertial navigation system (AINS)
Implementing AINS
Initial alignment (gyrocompassing)
AINS demonstration

Kenneth Gade, FFI

Slide 39

Initial alignment (gyrocompassing)

Basic problem:
Find the orientation of a vehicle (B) relative to Earth (E) by means of an
IMU and additional knowledge/measurements
Note: An optimally designed AINS inherently gyrocompasses optimally.
However, a starting point must be within tens of degrees due to
linearizations in the Kalman filter => gyrocompassing/initial alignment
is treated as a separate problem.
Solution: Find Earth-fixed vectors decomposed in B. One vector gives
two degrees of freedom in orientation.
Relevant vectors:
Gravity vector
Angular velocity of Earth relative to inertial space, IE
Kenneth Gade, FFI

Slide 40

Finding the vertical direction (roll and

pitch)
Static condition: Accelerometers measure gravity, thus roll and pitch are
easily found
Dynamic condition: The acceleration component of the specific force
B
B
B
measurement must be found ( f IB = a IB g B )
=> additional knowledge is needed
The following can give acceleration knowledge:
External position measurements
External velocity measurements
Vehicle model

Kenneth Gade, FFI

Slide 41

Finding orientation about the vertical

axis: Gyrocompassing
Gyrocompassing: The concept of finding orientation about the vertical
axis (yaw/heading) by measuring the direction of Earth's axis of
rotation relative to inertial space IE
Earth rotation is measured by means of gyros

Kenneth Gade, FFI

Slide 42

Gyrocompassing under static condition

Static condition ( EB = 0):
A gyro triad fixed to Earth will measure
the 3D direction of Earth's rotation axis
B
B
( IB = IE)
To find the yaw-angle, the down-direction
(vertical axis) found from the
accelerometers is used.
Yaw will be less accurate when getting
closer to the poles, since the horizontal
component of IE decreases
(1/cos(latitude)). At the poles IE is
parallel with the gravity vector and no
gyrocompassing can be done.

Figure assumes x- and y-gyros in

the horizontal plane:
z-gyro axis

z-gyro measurement
Earth's axis of rotation

B
x-gyro measurement

Latitude
y-gyro measurement
y-gyro axis

yaw
North
x-gyro axis (vehicle
heading)
Kenneth Gade, FFI

Slide 43

Gyrocompassing under dynamic

conditions (1:2)
Dynamic condition:
B
B
B
Gyros measure Earth rotation + vehicle rotation, IB = IE + EB
B
B
Challenging to find IE since EB typically is several orders of
magnitude larger

Kenneth Gade, FFI

Slide 44

Gyrocompassing under dynamic

conditions (2:2)
Under dynamic conditions gyrocompassing can be performed if we know
the direction of the gravity vector over time relative to inertial space.
The gravity vector will rotate about Earth's axis of rotation:

Figure assumes zero/low

velocity relative to Earth.
gravity vector at t
= 12 hours

The change in gravity direction due to

own movement over the curved
Earth can be compensated for if
the velocity is known (4 m/s
north/south => 1 error at lat 60)

Earth's axis of rotation

gravity vector at t
= 0 hours

Kenneth Gade, FFI

Slide 45

Outline

Notation
Inertial navigation
Aided inertial navigation system (AINS)
Implementing AINS
Initial alignment (gyrocompassing)
AINS demonstration

Kenneth Gade, FFI

Slide 46

AINS demonstration - simulation

Figures: NavLab
Kenneth Gade, FFI

Slide 47

Position

Posm white (1): 3 m

Posm bias (1): 4 m
Tbias: 60 s
Posm rate: 1/60 Hz
Figure: NavLab
Kenneth Gade, FFI

Slide 48

Position estimation error

Figure: NavLab
Kenneth Gade, FFI

Slide 49

Attitude

Figure: NavLab
Kenneth Gade, FFI

Slide 50

Attitude estimation error

Figure: NavLab
Kenneth Gade, FFI

Slide 51

AINS demonstration - real data

Data from Gulf of Mexico

Recorded with HUGIN 3000

Kenneth Gade, FFI

Slide 52

Position (real data)

Figure: NavLab
Kenneth Gade, FFI

Slide 53

USBL wildpoint (outlier)

Figure: NavLab
Kenneth Gade, FFI

Slide 54

Verification of Estimator Performance

Verified using various
simulations

HUGIN 3000 @
1300 m depth:
Mapped object positions
5

Verified by mapping the

same object repeatedly

Std North = 1.17 m

Std East = 1.71 m

Relative North position [m]

3
2
1
0
-1
-2
-3
-4
-5
-5

-4

-3

-2
-1
0
1
2
Relative East position [m]

5
Kenneth Gade, FFI

Slide 55

Navigating aircraft with NavLab

Cessna 172, 650 m height, much turbulence

Simple GPS and IMU (no IMU spec. available)

Line imager data

Positioned with NavLab (abs. accuracy: ca 1 m verified)

Kenneth Gade, FFI

Slide 56

Conclusions

An aided inertial navigation system gives:

optimal solution based on all available sensors
all the relevant data with high rate
If real-time data not required, smoothing should always be used to
get maximum accuracy, robustness and integrity

Kenneth Gade, FFI

Slide 57