R&B - Navigation Technology

A Tutorial to Inertial Navigation

Yuanxin Wu
yuanxin.wu@sjtu.edu.cn

Shanghai Jiao Tong University

Spring 2018
 About Myself

Research Interests:

Inertial-based navigation Inertial-visual fusion

State estimation

More info:
http://english.seiee.sjtu.edu.cn/english/detail/2127_1925.htm
 Lecture Outline

Lecture Contents Homework

1 Introduction and kinematic basics Homework #1

2 Inertial navigation computation Homeworks #2-3

Attitude alignment/Initialization
3
INS Error equation

Inertial and magnetic calibration Extensive Reading

 Category of Navigation Technology

A. Dead reckoning

B. Position fixing (5)

(After Dr. Groves’ book)

Introduction of Inertial Navigation
 Introduction of Inertial Navigation

Examples of inertial navigation systems (INS)

iMAR iNAV-RQH IXSEA PHINS Xsens MTI-G Mobile Phone

~$100k ~$10k $1-3k < $500
 Introduction of Inertial Navigation

Definition
dead reckoning forward from current
Inertial Navigation = position using inertial measurements

Principles of dead reckoning

(After Dr. Groves’ book)
 Introduction of Inertial Navigation
1. How to obtain acceleration?
Definition 2. How to obtain v(0) and p(0)?
3. What’s the reference of acc., vel & pos?
Given acceleration:

1. Calculate velocity: 0  

2. Calculate position: v

0  

0 0    

0 0    

Principles of dead reckoning

 1. How to obtain acceleration?
Introduction
2. How of Inertial
to obtain Navigation
v(0) and p(0)?
3. What’s the reference of acc., vel & pos?
Newton’s laws of motion
First law:
In an inertial frame of reference, an object either remains at
rest or continues to move at a constant velocity, unless
acted upon by a force.

Second law:
In an inertial reference frame, the vector sum of
the forces F (net force) on an object is equal to the mass m of
that object multiplied by the acceleration a of the
object: F = ma. (assumed that the mass m is constant.)

Third law:
When one body exerts a force on a second body, the
second body simultaneously exerts a force equal in
magnitude and opposite in direction on the first body.
 Introduction of Inertial Navigation

What makes an inertial frame?

An inertial frame of reference is one in which the motion of a
particle not subject to forces is in a straight line at constant
speed, in which the laws of mechanics take their simplest
form.
In an inertial frame of reference (seen by an observer at rest in the frame)

In a rotating frame of reference (seen by an observer at rest in the frame)

(Coriolis force) (Centrifugal force) (Euler force)

the velocity of the body and the position of the body
 Introduction of Inertial Navigation

What makes an inertial frame?

An inertial frame of reference is one in which the motion of a
particle not subject to forces is in a straight line at constant
speed, in which the laws of mechanics take their simplest
form.
In Newton's time, the fixed stars were invoked as a
reference frame
 Introduction of Inertial Navigation

Separate non-inertial from inertial frames?

In an inertial frame of reference, the laws of mechanics should
take their simplest form.

Newton examined this

problem himself using
rotating spheres:

The presence of fictitious

forces indicates the physical
laws are not the simplest
laws available so a frame
where fictitious forces are
present is not an inertial
frame.
 Introduction of Inertial Navigation

Choose an inertial frame for practical navigation?

The Sun

Inertial frame does not exist in our real world.

 Introduction of Inertial Navigation

Choose an inertial frame for practical navigation?

An Earth-centered inertial frame (ECI)

is nominally centered at the Earth’s
center of mass and oriented with
respect to the Earth’s spin axis and the
stars.

This is not strictly an inertial frame as the Earth experiences acceleration

in its orbit around the Sun, its spin axis slowly moves, and the galaxy
rotates.

But ECI frame may be treated as an inertial frame for practical purposes
near the Earth.
 Introduction of Inertial Navigation

Newton’s laws of motion

Dynamics the study of motion and its relation to forces

Inertial sensors (gyroscope and accelerometer)

Kinematics dealing with the implications of observed

motions without regard for circumstances
causing them (mass or forces)
 Inertial Sensors
Introduction of Inertial Navigation
1. How to obtain acceleration?
Inertial sensors 2. How to obtain v(0) and p(0)?
3. What’s the reference of acc., vel & pos?
(Einstein’s Relativity) Only can non-gravitational
acceleration and angular velocity w.r.t. inertial
frame be directly sensed, and nothing else.

accelerometer gyroscope
 Introduction
Inertial Sensors
of Inertial Navigation

Accelerometer – 2 major types

Pendulous Acc. Vibrating-Beam Acc.

(MEMS)
Accurate Less-Accurate

(After Dr. Groves’ book)

 Introduction
Inertial Sensors
of Inertial Navigation

Gyroscope – 3 major types

Spinning-Mass Optical Gyro Vibratory Gyro

Gyro (RLG/FOG) (MEMS)
Accurate Accurate Less-Accurate

(After Dr. Groves’ book)

 Introduction of Inertial Navigation
Inertial Sensors

Promising cold-atom sensors (~meter/h !)

iMAR iNAV-RQH
~$100k

~1km/h
 Introduction
Inertial Sensors
of Inertial Navigation

Developing trends of gyro/acc.

1 Earth rot. rate
nm/h ~15.04 deg/h
Cold-
atom
Price
Per
Unit RLG/FOG

MEMS

Accuracy/Bias Stability
1nm = 1852m
 Introduction
Inertial Sensors
of Inertial Navigation

Sensor errors (acc. or gyro)

Ideally, output = input (angular velocity/acceleration)

(After Prof. El-Sheimy)

 IMU
Introduction
and Calibration
of Inertial Navigation

Inertial Measurement Unit

IMU =
a triad of gyro
&
a triad of acc.
 Introduction of Inertial Navigation

Tasks involved in inertial navigation

• IMU calibration
1. How to obtain acceleration?
• INS initialization 2. How to obtain v(0) and p(0)?
3. What’s the reference of acc., vel & pos?
• INS computation
• INS Error characteristics
Basic Kinematics
 Principle ofBasic
Inertial
Kinematics
Navigation
- acceleration
Accelerometers measure acceleration?

: inclination angle

Spring force
cos
gravitation force

: all that the accelerometer can know by itself (specific force)

cos : unknown gravitation force along the sensitive axis

 Principle ofBasic
Inertial
Kinematics
Navigation
- acceleration
Accelerometers measure acceleration?
1. How to obtain acceleration?
: inclination angle 2. How to obtain v(0) and p(0)?
3. What’s the reference of acc., vel & pos?

Spring force
cos
gravitation force

Equivalent statements about accelerometers:

1. It measures the force acted on unit mass, i.e., specific force
2. It measures the superposition of acceleration and gravitation
3. It cannot measure gravitation or the total acceleration
 Principle ofBasic
Inertial
Kinematics
Navigation
- acceleration
Accelerometers measure acceleration?
1. How to obtain acceleration?
: inclination angle 2. How to obtain v(0) and p(0)?
3. What’s the reference of acc., vel & pos?

Gravitation is not sensed because it

Spring force acts equally on all points, causing
them to move together. Other forces
gravitation force such as specific force are sensed as
they are transmitted from point to
point. (after Grove’s Book)

Undetectability of straight-line accelerations held in common (by Newton):

If bodies, any how moved among themselves, are urged in the direction of
parallel lines by equal accelerative forces, they will continue to move
among themselves, after the same manner as if they had been urged by
no such forces.
 Basic Navigation
Principle of Inertial Kinematics - orientation

Orientation between any two Cartesian frames

(e.g. n and b)

Consider a physical vector r expressed in either frame

r  r i +r j +r k
x
n n
y
n n
z
n n

or
r  r i +r j +r k
x
b b b
y
b
z
b b
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Orientation between any two Cartesian frames

r i +r j +r k  r i +r j +r k
x
n n
y
n n
z
n n
x
b b b
y
b
z
b b

Project both sides in turn onto i , j or k (n-frame axes)

n n n

r  r i i + r j i + r k i
x
n
x
b b n b
y
b n
z
b b n

ryn  rxb i b  jn + ryb jb  jn + rzb k b  jn

rzn  rxb i b  k n + ryb jb  k n + rzb k b  k n
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Orientation between any two Cartesian frames

rxn  rxb i b  i n + ryb jb  i n + rzb k b  i n

ryn  rxb i b  jn + ryb jb  jn + rzb k b  jn
rz  rx i  k + ry j  k + rz k  k
n b b n b b n b b n

In matrix form,
 rxn   i b  i n j i
b n
k i
b n
  rx 
b

 n  b n n  b
 ry    i  j jb  jn k  j   ry 
b

 rzn   i b  k n jb  k n k b  k n   rzb 
  
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Orientation between any two Cartesian frames

r
x
n
  i i
b n
j i
b n
k  i  r 
b n
x
b

   b n n  
  i j jb  jn k  j  r 
n b b
r
y y
rn  i b  k n j k
b n
k  k   r 
b n b
z   z

r = n
C n
b r b

Rotation/attitude/orientation/DCM matrix,
which transforms the coordinate vector in b-
frame to the coordinate vector in n-frame.
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Orientation between any two Cartesian frames

Similarly,  r x
b
 i i
n b
j i
n b
k  i  r
n b
x
n

   n b b  
  i j jn  jb k  j  r
b n n
r
y y 
rb  i n  k b j k
n b b
k  k  r
n n 
z   z 
r = b
C b
n r n

C  C
n
b n
b T Rotation/attitude/orientation/DCM matrix,
which transforms the coordinate vector in n-
frame to the coordinate vector in b-frame.
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Orientation between any two Cartesian frames

r =C r
n n b
b and r =C r
b b n
n
So, we get
r =C r =C C r
n n b
b
n
b
b n
n

 I  C C  C  Cb 
n
b
b
n
n
b
n T

This means that the orientation between any two Cartesian frames is
mathematically an orthogonal matrix.
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Other properties of rotation matrix

 Transitive C C C p
m
p
n
n
m
 1-determinant (for right-hand frame) C  1
 Single-axis equivalence

C  I  C  I CT  I  CT  I  C   C  I  CI  0

which means there exists a rotation-invariant unit vector

n such that
Cn  n
Several rotations in order equal a fixed-axis rotation.
 Basic Navigation
Principle of Inertial Kinematics - orientation

Other properties of rotation matrix

Rotation matrix C is related to the unit vector n (from
start-frame to end-frame) and the rotation angle θ

C  I  sin   n    1  cos   n  
2

where (·˟) means the formed anti-symmetric matrix

0
, 0 ,
0 n

For small θ, the first-order appr. C  I    n  

 Principle of Inertial
Basic Navigation
Kinematics - orientation

Rotation vector and quaternion

From the unit vector n (from start-frame to end-frame)
and the rotation angle θ, other orientation parameters
can be constructed, e.g.

Rotation vector: σ  n
 
Quaternion: q  cos  sin n
2 2

p.s., Rodrigues vector and Euler angle….

g  2n tan  2    
 Principle of Inertial
Basic Navigation
Kinematics - orientation

Orientation Kinematics – rotation vector

Non-commutativity term
Consider time interval [0 t],
1 1   sin  
σ  ω  σ  ω  2 1   σ  σ  ω 
2   2(1  cos  ) 
 0 1 1
 ω  σ  ω  σ  σ  ω   σ
2 12

1 t
for small  , σ  ω  α  ω, α   ω d
2 0
 Principle of Inertial
Basic Kinematics
Navigation - orientation
Orientation Kinematics – DCM (method 1)
r =C r
n n b
b
Assume n-frame is stationary : Infinitesimal rotation during ∆
∆t

∆
∆ ∆ ∆ ≜

∆
lim
∆ → ∆

lim lim ≜ ≜
∆ → ∆ ∆ → ∆
where denotes the angular rate vector from n-frame to b-
frame, expressed in b-frame
Homework-1: Is it correct when n-frame is non-stationary?
 Principle of Inertial Navigation - orientation
Basic Kinematics

Orientation Kinematics – DCM (method 2)

Important: to discriminate 2 kinds of rotations

Vector rotation r  ω  r
r
ωr r
r  r sin  ω t  , r  lim  ωr
ωr t  0 t
ω
vector rotation
Frame rotation r  ω  r
(=opposite vector rotation)
 Principle of Inertial
Basic Kinematics
Navigation - orientation

Orientation Kinematics – DCM (method 2, cont’d)

r =C r
n n b
b

Consider vector r fixed w.r.t n-frame, then the derivative

0 = Cb r  Cb r  Cb r  Cb  ω nb  r 
 n b

n b  n b n b b

 Cb  Cb  ω nb    Cb Ω nb
 n n b n b

where ω denotes the angular rate vector from n-frame

b
nb
to b-frame, expressed in b-frame
 Principle of Inertial Navigation

D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology,

2nd ed. 2007.
 Principle of
Principle
Inertial of
Navigation
Inertial Navigation

Relevant frames

Inertial frame (I):

space-fixed
Body frame (B):
IMU frame
Earth frame (E):
Earth-fixed, e.g., WGS-84
Local-level frame (L):
local-levelled & north-directed
Navigation frame (N): computation reference
 Principle of
Principle
Inertial of
Navigation
Inertial Navigation
Navigation equation relative to the Earth
As inertial sensors measure motion w.r.t the i-frame,
consider the position vector r of a moving object
relative to the Earth center as
r =C r
i i e
e velocity w.r.t
Take time derivative of both sides: Earth

r = Ce r  Ce r  Ce  Ωie r  r 
i  i e i e i e e e

r i = Cie Ωiee  Ωiee r e  r e   Cie  Ωiee r e  

 re 
 Cie  Ωiee Ωiee r e  2Ωiee r e  
re 
 Principle of
Principle
Inertial of
Navigation
Inertial Navigation

Navigation equation relative to the Earth

r i  Cie  Ωiee Ωiee r e  2Ωiee r e  

f i  g i  ai   re 
: specific force; : gravitation acceleration


Denote the velocity w.r.t Earth by v  r
e e
e

f  g  C  Ω Ω r  2Ω v  v
i i i
e
e
ie
e e
ie
e
ie
e
e
e
e 
v ee = Cen v en  v ee = Cen  Ω en
n
v en  v en 
 Principle of
Principle
Inertial of
Navigation
Inertial Navigation

Navigation equation relative to the Earth

i i i
e 
f  g  C Ω Ω r  2Ω C v  C  Ω v  v e 
e
ie
e e
ie
 n e
ie
e
n
n
e
e
n
n
en
n
e 
f  g  Ω Ω r  2Ω C v  C  Ω v  v e 
e e e
ie
e e
ie
 n e
ie
e
n
n
e
e
n
n
en
n
e

f  g  Ω Ω r  2Ω C v  C  Ω v  v
e e e
ie
e e
ie
e
ie
e
n
n
e
e
n
n
en
n
e
n
e 
f n  g ln  2Ωien v en   Ω en
n
v en  v en 
v en  Cbn f b   2Ωien  Ω enn  v en  g ln

Cbn  x n  y n    xb  y b  g le  g e  Ωiee Ωiee r e

 Principle of Inertial of
Principle Navigation
Inertial Navigation

Gravity vs. Gravitation

Ze

Xe

g le  g e  Ωiee Ωiee r e : gravity (fictitious force)

: gravitation (real force)
 Principle of
Principle
Inertial of
Navigation
Inertial Navigation

Navigation equation relative to the Earth

n b

Velocity: v e  Cb f  2Ωie  Ω en v e  g l
n n n
 n n

Specific force Coriolis acceleration gravity acceleration

    v en,E sec L  RE  h  
Curvilinear    
 L    v e , N  RN  h  
n
Position: to be explained…

 h   v n 
   e ,U 

Attitude: 
Cb  Cb Ω nb
n n b
Earth and Gravity Model
 Earth and Gravity Model

Earth Shape

The Earth’s surface is an irregular oblate spheroid.

Commonly, it is approximated to a regular shape, which is
then fitted to the true surface of the Earth at mean sea level.
(after Grove’s book)
 Earth and Gravity Model

Reference ellipsoid of Earth

r: semi-minor axis Earth rotation rate

ie  7.292115  10 5 rad s
15.041067  h 

R2  r 2
Eccentricity: e
R
Rr
Flattening: f 
R: semi-major axis R
R r

World Geodetic System 1984

 Earth and Gravity Model

ECEF Position vs. Curvilinear Position

ECEF Position: [x y z]
parallel plane

Curvilinear Position: [λ L h]

λ: longitude
L: geodetic latitude
h: height/altitude

Φ: geocentric latitude

Parallel plane
Meridian plane
 Earth and Gravity Model

Two Radii of Curvature

Meridian radius of curvature for north-south motion

R 1  e 2 
RN 
1  e sin L 
2 2 32

L  v nN  RN  h 

Transverse radius of curvature for east-west motion

R
RE    v nE  RE  h  cos L 
1  e 2 sin 2 L
 Earth and Gravity Model

ECEF/Curvilinear Position Conversion

1. [λ L h] → [x y z]

x   RE  h  cos L cos 
y   RE  h  cos L sin 
z  1  e 2  RE  h  sin L

2. [x y z] → [λ L h]

Iterative methods RE  PQ
e.g., (Wu, TAES’2003)
 Earth and Gravity Model
Gravity

gravity

 g g g
T
Gal = 1cm/s2, mGal = 1e-5 m/s2
gravity anomaly vertical deflections
(up to 10-50”)
g  0   9.780318 1  5.3024  103 sin 2 L  5.9  106 sin 2 2 L  m s 2
g  0
g h  , R0  RE RN
1  h R0 
2
Inertial Navigation Computation
(NED-geodetic frame as an example)

(Wu, TAES’2013)
 Principle Inertial
of Inertial
Navigation
Navigation
Computation

Navigation equation relative to the Earth

n b

Velocity: v e  Cb f  2Ωie  Ω en v e  g l
n n n
 n n

Specific force Coriolis acceleration gravity acceleration

    v en,E sec L  RE  h  
Curvilinear    
 L    v e , N  RN  h  
n
Position:
 h   v n 
   e ,U 

Attitude: 
Cb  Cb Ω nb
n n b
 Navigation Computation
Inertial Navigation Computation

Attitude computation

Cb  Cb Ω nb
n n b
ω b
nb  ω C ω
b
ib
b
n
n
in
Dilemma: the right side contains the solution to be computed

C t   C t  C t 
n
b
n
i
i
b

 i  Ci Ω b
C  i  Ci Ω n
C
b b ib n n in

ω    cos L 0  sin L 
T
ω  ω ω
n n n n
in ie en ie
T
   T  vE vN v E tan L 
ω en   cos L  L  sin L   
n
  
 RE  h RN  h RE  h 
  v E  RE  h  cos L  ，L  v N  RN  h 
 Navigation
Inertial
Computation
Navigation Computation

Attitude computation
Recall its relation to rotation vector  σ  n 
C  I  sin   n    1  cos   n  
2

So we turn to compute the incremental rotation vector in

the update interval t 1 t
σ   ω dt   α  ω dt
k 1 k 1

1 t 2 t
σ  ω  α  ω
k k

2 2
Two samples:  θ  θ  θ  θ 1 2 1 2
3
One sample:  θ

Homework-2: how comes the two-sample algorithm?

Hints: to fit angular velocity using incremental angular measurement
 Navigation
Inertial
Computation
Navigation Computation

Attitude computation

C  k  1  C C k C
n n  k 1 n b k 
Then b n k  b b  k 1

in which Cb  k 1  I  sin  b  σ    1  cos  b   σ  2

b k 
bb 2
b
b

sin  n 1  cos  n 
Cnn  kk 1 σ n    σ n 
2
I
n n2

where σ b and σ n are respectively computed using ωbib and ωinn

 Inertial Navigation Computation

Attitude computation (flowchart)

Cbn  k  Cnn kk 1 Cbn  k  Cbb  kk 1 Cbn  k  1

Cnn kk 1 Cbb  kk 1

2
ωinn σ n  θ n
σ b  θ1b  θb2  θ1b  θb2 ωbib
3

previous vel/pos gyroscope meas.

 Navigation
Inertial
Computation
Navigation Computation

Velocity computation

v en  Cbn f b   2Ωien  Ω en
n
 e l
v n
 g n

Substituting Cbn  t   C nn tk Cbn  k  Cbb tk

Cnn  tk v en  Cbn  k  Cbb  tk f b  Cnn  tk  2Ωien  Ω en

n
 e nt  g l
v n
 C n k  n

Integrating over interval tk tk 1 

v en  k  1 
n  k 1  v n  k   Cn  k  tk 1 Cb  k f b dt  tk 1 Cn  k Ω n v n dt  tk 1 Cn  k g n dt 
b  k  t tk nt  ie e tk nt  
C nk   e k
b t 
 Navigation
Inertial
Computation
Navigation Computation

Velocity computation

The last two integrals (both integrands are slow-changing)

Substituting C n k 
n t 
 I  σn  I  
tk
t

ω inn d   I   t  tk  Ωinn

 T2 n  n n
Cn t Ωien v en dt
tk 1
   TI  Ωin  Ωie v e
n k
tk  2 

tk 1 nk  n  T2 n  n
tk
C nt 
g dt   TI 
 2
Ωin  g
 T  tk 1  tk
 Navigation
Inertial
Computation
Navigation Computation

Velocity computation
The first integral (integrand is fast-changing)
Substituting C
bk 
b t 
 I  σb   I  tk
t

ω bib d 

tk
tk 1
C bk  b
f dt  t
b t 
tk 1

k
 
I
tk
t

ωbib d  f b dt

1 2
 v1  v 2   θ1  θ 2    v1  v 2    θ1  v 2  v1  θ 2 
2 3

v1 , v 2 : Acc. samples θ1 , θ2 : Gyro samples

Homework-3: how comes the above discrete integration?

Hints: to fit angular velocity/specific force using incremental measurements
 Navigation
Inertial
Computation
Navigation Computation

Velocity computation
v en  k  1 
n  k 1  v n  k   Cn  k  tk 1 Cb  k f b dt  tk 1 Cn  k Ω n v n dt  tk 1 Cn  k g n dt 
b  k  t tk nt  ie e tk nt  
C nk   e k
bt 

 n  T2 n  n n  T 2 n  n 
v  k  1  C  v e  k   u  k  1   TI  Ωin  Ωie v e  k    TI  Ωin  g 
n n k 1
e n k 
  2   2  

where u  k  1  C  k  v n
b 1  v 2 
1
 θ1  θ2    v1  v 2    θ1  v 2  v1  θ2 
2
 2 3 
 Navigation
Inertial
Computation
Navigation Computation

Position computation
    v en,E sec L  RE  h    0 sec L  RE  h  0   v en, N 
      
p   L    v en, N  RN  h    1  RN  h  0 0   v en,E   R c v en
 h   v n   0  1 0   v n 
   e ,U     e ,D 

Integrating over interval tk tk 1 

p  k  1  p  k    R c v dt  p  k   R c  v en dt  p  k   R c r n  k  1
tk 1 tk 1
n
tk e tk

r  k  1  C
n  k 1 Tv n  k   tk 1 Cn  k Ω n r n dt
tk nt  in
n
n k   e

C  k   tk n  g d dt 

tk 1 t bk  tk 1 t nk  tk 1 t n k  n
 C f d dt    C Ω v d dt   C
n b n n
b tk tk b   tk tk n   ie e tk
 Navigation
Inertial
Computation
Navigation Computation

Position computation

Using similar technique to calculate those integrals,

p  k  1  p  k   R cr n  k  1

 n T2 T3 n  n n T2 T3 n  n
r  k  1  C
n n  k 1
nk  Tv e  k   I u  k  1   I  Ωin  Ωie v e  k    I  Ωin  g 
  2 6   2 6  

T n
where I u  k  1  Cb  k  25v1  5v 2  12θ1  v1  8θ1  v 2  2v1  θ 2  2θ 2  v 2 
30
 Navigation
Inertial
Computation
Navigation Computation

Summary & Comparison

 Navigation Computation
Inertial Navigation Computation

A Level-flight example
v  0 a sin  wt  0  , a = 10 and w = 0.02π
n T
e