Inertial Navigation Equations

Preliminaries
r =position vector of vehicle wrt center of Earth

rA =position vector of vehicle wrt center of Earth coordinatized in frame A

vA =velocity vector of vehicle as viewed from frame A or as observed from frame A

B
vA =velocity vector of vehicle as viewed from frame A or as observed from frame A
and coordinatized in frame B

[db/dt]A =the time derivative of vector b as viewed from frame A or as observed

from frame A (example: [dr/dt]A = vA )
¯ ¯
db ¯¯ db ¯¯
= +ω×b
dt ¯A dt ¯B

ω B/A = angular velocity of frame B wrt frame A or as observed from frame A

ωC
B/A = angular velocity of frame B wrt frame A or as observed from frame A and
coordinatized in frame C

ω B/A = −ω A/B
ω C/A = ω C/B + ω B/A

For each ω C
B/A , we can form the angular-rate matrix, also know as the angular-rate
cross-product matrix.
 
0 −ω CB/A (3) ωCB/A (2)
ΩCB/A
 ωC B/A (3) 0 −ω CB/A (1)

C C
−ω B/A (2) ω B/A (1) 0

If ω C D D
B/A = TC ω B/A , then it is easy to show that

ΩC D D C
B/A = TC ΩB/A TD

1
Navigation Equations for Inertially Stabilized Gimbaled Plat-
form
• An inertial frame is ”instrumented”

• Gyros are used to keep the platform inertially fixed

• Well suited for space navigation

• When the platform is held fixed and aligned with an inertial frame the, the
navigation equations are
ˆṙI = v̂II , v̇
ˆ II = ãII,non−grav + gI (r̂I )

where ãII,non−grav are the accelerometer measurements.

2
Navigation Equations for a Strapdown System
• Measure anon−grav in the body frame, ãB
non−grav

• Transform acceleration measurements to the inertial frame

ãII,non−grav = TIB ãB

I,non−grav

• The inertial to body (or body to inertial) transformation or direction cosine

matrix is obtained by integrating the following differential equation

ṪIB = TIB Ω̃B

B/I

where Ω̃B B
B/I is the cross-product matrix of ω̃B/I

B
• ω̃B/I are the gyro measurements

• Note: We can also implement these equations with quaternions

1 I
q̇BI = B
qb ⊗ ω̃B/I
2

• The equations that need to be integrated in the flight computer are

ˆṙI = v̂II (1)
ˆ II = T̂IB ãB
v̇ I I
I,non−grav + g (r̂ ) (2)
˙
T̂IB = T̂IB Ω̃B B/I (3)
or (4)
1 I
q̂˙BI = B
q̂b ⊗ ω̃B/I (5)
2

3
Simplified Error Analysis and Effects of Sensor Errors
A simple method that is often used to conduct an error analysis for inertial naviga-
tion systems is Monte Carlo analysis. The basic idea is to systematically introduce
errors into the navigation equations, generate 100’s or 1000’s of trajectories, and then
compare the results to an error free trajectory.
The error free trajectory or error free navigation state estimates are obtained by
integrating the following equations

ṙI = vII
v̇II = TIB aB I I
I,non−grav + g (r )

ṪIB = TIB ΩBB/I

or
1 I
q̇BI = B
q ⊗ ωB/I
2 b
where aB B
I,non−grav and ΩB/I , are the error-free accelerometer and gyro measurements,
respectively. The initial position, velocity, and attitude/orientation are also error-free.
Next, various sources of navigation error are introduced. For inertial navigation
systems, the errors sources are initial conditions, gyro data, and accelerometer data.
The initial position error (δrI ), velocity error (δvII ), and attitude error (δθ) are such
that
r̂I = rI + δrI
v̂II = vII + δvII
µ ¶
δθ B /2
q̂BI = δq ⊗ qBI , where δq ≈
1
The gyro data can have errors due to gyro noise (ν gyro ), bias (bgyro ), and scale factor
(sgyro ),
B B
ω̃B/I = [I + Diag(sgyro )]ωB/I + bgyro + ν gyro (6)
B
= [I + Sgyro ]ωB/I + bgyro + ν gyro (7)

and the accelerometers may have errors due to accelerometer noise (ν accel ), bias
(baccel ), and scale factor (saccel ), misalignment (²), g2 -sensitivity (kg2 ), and g3 -sensitivity
(kg3 ).

a2 a3
ãB B
I,non−grav = [I−(²×)][I+Diag(saccel )]aI,non−grav+ baccel + kg 2 + kg3 2 + ν accel(8)
ge ge
2
a a3
= [I + Eaccel ][I + Saccel ]aB
I,non−grav + b accel + k g 2 + k g 3 + ν accel (9)
ge ge2

where a = |aB 2
I,non−grav | and ge = 9.81 m/s .

4
Linear Covariance Analysis
Instead of doing a Monte Carlo analysis that requires hundreds or thousands of runs,
a linear covariance analysis may be more useful. A linear covariance analysis can be
used to compute the same statistics as a Monte Carlo analysis, but in only one run.
The basic idea is to linearize the navigation equations of motion about the true
error-free trajectory. Once the equations are linearized, the covariance of the naviga-
tion errors is easily determined using well known stochastic linear system theory.
The non-linear navigation equations are obtained by substituting Eq. 10, and Eq.
11 into Eqs. 1-5. If we select the state vector to be
¡ ¢T
x= rI vII θ B sgyro bgyro ² saccel baccel kg2 kg3

the linearized equations of motion are

δ ṙI = δvII
¯
I ∂gI ¯¯ B
δ v̇I = δrI + TIB [aB
I,non−grav ×]δθ +
∂rI ¯nom
¡
TIB −[²×]aB B
I,non−grav + Diag(saccel )aI,non−grav + baccel +
¶
a2 a3
kg2 + kg3 2 + ν accel
ge ge
B B
δ θ̇ = −ω B B
B/I × δθ + Diag(sgyro )ω B/I + bgyro + ν gyro
sgyro = 03×1
bgyro = 03×1
² = 03×1
saccel = 03×1
baccel = 03×1
kg2 = 03×1
kg3 = 03×1

In state-space format, these equations can be written as

µ ¶
ν accel
ẋ = F x + G
ν gyro

where
 
03×3 I3×3 03×3 03×3 03×3 03×3 03×3 03×3 03×3 03×3
 Fv,r 03×3 Fv,θ 03×3 03×3 Fv,² Fv,saccel Fv,baccel Fv,kg2 Fv,kg3 
F =
 03×3 03×3 Fθ,θ Fθ,sgyro Fθ,bgyro 03×3

03×3 03×3 03×3 03×3 
021×3 021×3 021×3 021×3 021×3 021×3 021×3 021×3 021×3 021×3

5
  
03×3 03×3
 I3×3 03×3 
G=
 03×3 I3×3 


021×3 021×3
¯
∂gI ¯¯
Fv,r = I ¯ , Fv,θ = TIB [aB B B
I,non−grav ×], Fv,² = TI [aI,non−grav ×]
∂r nom
a2 a3
Fv,saccel = TIB Diag(aB B B
I,non−grav ), Fv,baccel = TI , Fv,kg2 = TI , Fv,kg3 = TIB 2
ge ge
and
Fθ,θ = −[ω B B
B/I ×], Fθ,sgyro = Diag(ω B/I ), Fθ,bgyro = I3×3

end
·µ ¶ ¸ µ ¶
ν accel ¡ ¢ Qaccel δ(t − τ ) 0
Q=E ν Taccel ν Tgyro =
ν gyro 0 Qgyro δ(t − τ )

To propagate the state covariance matrix,we use

Pi+1 = Φi Pi ΦTi + GQGT ∆t

where
Φi = exp F ∆t ≈ I + F ∆t + F 2 ∆t2 /2

6
Example - Initialization of IMU Systems
• Spacecraft Attitude/Orientation Initialization

– Star-trackers
– Sun sensors
– Horizon sensors
– Magnetometers

• Spacecraft Position Velocity Initialization

– GPS
– Ground uplink
– Celestial Navigation ?

• Aircraft and/or Launch Vehicle Position/Velocity Initialization

– Known runway/launch location

– Ground uplink
– GPS

• Aircraft and/or Launch Vehicle Attitude/Orientation Initialization

– Determine the ”down” vector in body coordinates from the accelerometer

measurements
B ãB
idown = − B
|ã |
– Determine the ”east” vector from the gyro/accel measurements

iB
down × ω̃ gyro
iB
east = B
|idown × ω̃ gyro |

– Determine the ”north” vector

iB B B
north = ieast × idown

– Determine the body to NED coordinate transformation (direction cosine

matrix)  B 
{inorth }T
TBody→N ED =  {iBeast }
T 

{idown }T
B

7
– Using the known position vector, determine the earth-fixed to NED coor-
dinate transformation (direction cosine matrix)

rEF iEF
down × ẑ
iEF
down = − , iEF
east = EF
, iEF EF EF
north = ieast × idown
|rEF | |idown × ẑ|
 EF T 
{inorth }
TEF →N ED =  {iEF east }
T 

{iEF
down }
T

– Using the known time, determine the inertial to Earth-fixed coordinate

transformation (direction cosine matrix)
 
cos(ωt + φ) sin(ωt + φ) 0
TI→EF =  sin(ωt + φ) cos(ωt + φ) 0 
0 0 1

– The initial inertial to body coordinate transformation (direction cosine

matrix) is then given by

TI→B = TN ED→B TEF →N ED TI→EF

8
Applications of Kalman Filters in Inertial Navigation Systems
A typical Kalman filter design approach for an inertial navigation system begins with
a model for the gyro and accelerometer measurements. The gyro data can have errors
due to gyro noise (ν gyro ), bias (bgyro ), and scale factor (sgyro ),

ω̃ B B
B/I = [I + Diag(sgyro )]ω̃ B/I + bgyro + ν gyro (10)

and the accelerometers may have errors due to accelerometer noise (ν accel ), bias
(baccel ), and scale factor (saccel ), and misalignment (²).

ãB B
I,non−grav = [I−(²×)][I+Diag(saccel )]aI,non−grav+ baccel + ν accel (11)

If these are the models for the gyro and accelerometer measurements, then the design
models for the true dynamics are given by

ẋ = f (x) + Gw

where the state vector is given by

¡ ¢
xT = rI vI qBI bgyro sgyro baccel saccel ²

and the state dynamics are given by

ṙI = vI
v̇I = TIB (qIB ){[I + (²×)][I − Diag(saccel )]ãB I
I,non−grav − baccel − ν accel } + g(r )
1 I
q̇BI = qB ⊗ {[I − Diag(sgyro )]ω̃ B
B/I − bgyro − ν gyro }
2
ḃgyro = 0
ṡgyro = 0
ḃaccel = 0
ṡaccel = 0
²̇ = 0

Notice that the true position, velocity, and attitude is recovered by eliminating the
effect of the accelerometer and gyro error sources.

9
 Using the above design model, we can build an extended Kalman filter. The
expected value of the states can be propagated in between non-inertial measurements:

r̂˙ I = v̂I
v̂˙ I = TIB (q̂IB ){[I + (²̂×)][I − Diag(ŝaccel )]ãB I
I,non−grav − b̂accel + g(r̂ )
1 I
q̂˙BI = q̂B ⊗ {[I − Diag(ŝgyro )]ω̃ B
B/I − b̂gyro }
2
˙
b̂gyro = 0
ŝ˙ gyro = 0
˙
b̂accel = 0
ŝ˙ accel = 0
²̂˙ = 0

The design model can be linearized and written in the form

δ ẋ = F δx + Gw

and in between non-inertial measurements the covariance of the state error can be
propagated using
Pi+1 = ΦPi ΦTi + Qd
where
Φi = I + Fi ∆t + Fi2 ∆T 2 /2
¯
∂f ¯¯
Fi =
∂x ¯ x̂i

and
Qd ≈ GQGT ∆t
Now, when a non-inertial measurement is available, for example, z̃ = h from an
altimeter, or z̃ = ṙ from a doppler measurement, or z̃ = rEF from GPS we can
process the measurement in the standard manner

x+ = x− + K(z̃ − ẑ)

P + = (I − KH)P − (I − KH)T + KRK T

where
K = P H T (HP H T + R)−1
and ¯
∂z ¯¯
H=
∂x ¯x̂i
Can we estimate biases, scale factors, etc., from altitude measurements? Doppler
measurements? GPS? Star tracker measurements?

10