INS • 2018

INS + GNSS Integration

∗

This document presents the derivation of a simple INS solutions obtain by a loosely coupling
of IMU and GNSS. In addition, the terrestrial vehicle constraints will be included to improve the
results.

I. IMU Inertial Solution

I. Coordinate Frame Conversion

In this work the attitude will be measured with the Euler angles: roll (φ), pitch (θ) and yaw (ψ).
The order of rotation chosen from the navigation frame, (n), to the body frame, (b), is:
(n) → ψ → θ → φ → (b)
also referred to as {3,2,1}. The rotation matrix that converts from (n) to (b) is obtained concatenating
rotations. First, a yaw rotation around the (n) Z-axis to transform to the intermediate axis (j) such
that:  
cψ −sψ 0
an =  sψ cψ 0 a j (1)
0 0 1
| {z }
Rψ

Then a pitch rotation around the (j) Y-axis to transform to intermediate frame (e) as:
 
cθ 0 sθ
aj =  0 1 0  ae (2)
−sθ 0 cθ
| {z }
Rθ

Finally a roll rotation around the (e) X-axis to transform to the body frame (b) as:
 
1 0 0
ae = 0 cφ −sφ ab (3)
0 sφ cφ
| {z }
Rφ

Finally, concatenating all three rotations:

an = Rψ Rθ Rφ ab (4)
| {z }
n Rb

where:  
cψcθ cψsφsθ − cφsψ sφsψ + cφcψsθ
n b
R (E) =  cθsψ cφcψ + sφsψsθ cφsψsθ − cψsφ (5)
−sθ cθsφ cφcθ
∗

1
 INS • 2018

and the vector of Euler angles is:

 
φ
E = θ
 (6)
ψ

II. From Body Frame to Euler Angle Rates, i.e. w b → Ė

This section converts from the angular rates measured in the body frame to the Euler angular
rates used to measure the attitude. The robot angular velocity is measured by the gyros in the
body frame, (b), as:
w = wbx b̂1 + wyb b̂2 + wzb b̂3 (7)

where the quantities wbx , wyb and wzb are measured by the gyros. The angular velocity can also be
defined in terms of Euler angle rates using the intermediate frames from Section I as:

w = ψ̇n̂3 + θ̇̂2 + φ̇ê1 (8)

Transforming to (b) each of the unitary vectors:

w = ψ̇b Rn n̂3 + θ̇ b R j ̂2 + φ̇b Re ê1 (9)

where:
 T
b
Re = RφT , b
R j = RφT RθT and b
Rn = RφT RθT RψT = n
Rb (10)

This results in the angular velocity in terms of Euler angles rates:

w = (φ̇ − ψ̇ sin θ ) b̂1 + θ̇ cos φ + ψ̇ sin φ cos θ b̂2 + −θ̇ sin φ + ψ̇ cos φ cos θ b̂3 (11)
| {z } | {z } | {z }
wbx wyb wzb

By identification with equation (8), the components measured by the IMU gyros can be identified
as it is shown underbraced. Using a matrix notation:

wbx
    
1 0 − sin θ φ̇
 wyb  = 0 cos φ sin φ cos θ   θ̇  (12)
wzb 0 − sin φ cos φ cos θ ψ̇

Inverting the matrix, we obtain the conversion from body angle rates to Euler angle rates:

  1 sin φ cos φ tan θ wb 


φ̇ x
Ė =  θ̇  = 0
 cos φ − sin φ   wyb  = w
b
(13)
sin φ cos φ b
ψ̇ 0 wz
| cos θ {z cos θ }
E Qb

or
Ė = E Qb w b (14)

where matrix that transforms from body to Euler rates is defined as E Qb (θ ).

2
 INS • 2018

III. State Evolution Model with IMU Inputs

The states vector is composed of Cartesian position and velocity in the navigation frame and, the
Euler angles (roll, pitch, yaw) from the body frame to the navigation frame. The navigation frame
is defined with North East Down (NED) axis and some known location, e.g. the initial location of
the robot. The system inputs that will be provided by the IMU are the specific forces and angular
rates measured in the body frame.

xn
 
y  n
 n 
z 
 n 
 ẋ 
 
 ẏn   b


 n  n  f u,x
r  ż 

 fb 
n
v   φ 
  b  u,y 
 b 
fu  f u,z 
   
x= E = θ  u= = b  (15)
w bu wu,x 
   
b f   ψ    b 
 wu,y 
b
 
bw
 f ,x 
 
b
wu,z
 b f ,y 
 
 b f ,z 
 
bw,x 
 
 bw,y 
bw,z

If we could perfectly measure the accelerations and angular rates of the system in the body
frame (ab and w b ), the state evolution model would be:

ṙn vn
   

ẋ = v̇n  =  n Rb ab  (16)
Ė E Qb w b

The accelerometers measure the specific forces f which do not include the gravity acceleration, i.e.
without error:
ab = fb + gb (17)

Then:
ṙn vn
   

ẋ = v̇n  = n Rb fb + gn  (18)
Ė E Qb w b

where the gravity vector is gn = 0 0 9.81 in the NED coordinate frame. The sensors are not
 

perfect, they have biases and random noise. Our sensor model is:

b
f̂ = fb + b f + ν f (19)
b b
ŵ = w + bw + ν w (20)

where b f and bw are sensor biases that vary over time, and ν f and ν w are white random noise.
b
Note that f̂ and ŵb are the calibrated measurements where the constant biases and the gravity
scale factor are removed (see the IMU calibration in II). Substituting the sensor model (19), (20)

3
 INS • 2018

into (18):
vn
 
ṙn
 
 b 
n b
ẋ = v̇n  =  R f̂ − b f − ν f + gn 

    (21)
Ė E Qb ŵb − b − ν
w w

The random biases are modeled as first order linear Gaussian processes. For example, for the x
component of the specific force bias:
 
ḃ f ,x = −τ f ,x b f ,x + η f ,x where η f ,x ∼ N 0, s f ,x (22)

and s f ,x is the Power Spectral Density (PSD) of the Gaussian noise specified by the manufacturer.
The time constant τ f ,x is also specified by the manufacturer (τ f ,x ≈ 3600s in the code). The same
model applies for the six biases. This random biases are included in the state vector to finally
obtain the continuous time nonlinear state model:
vn
 
 n
ṙ  b

 v̇n  n Rb f̂ − b f − ν f + gn 
 
     
 Ė  =  E Qb ŵb − bw − ν w  = f (x, û, ν , η )
ẋ =  (23)
  
 ḃ f   
 −τ f b f + η f 
ḃw
−τ w bw + η w

where û is not a random variable, but an observed value and the white random noises are:
   
νf ηf
ν= and η = (24)
νw ηw

Note that the dependencies of n Rb and E Qb on the current attitude E are not made explicit in (23)
The discretization of (23) is:
xk +1 − xk
≈ ẋ = f (xk , ûk , ν k , η k ) (25)
∆T
such that:
xk+1 ≈ xk + ∆T f (xk , ûk , ν k , η k ) (26)
Thus, taking the expected value of this expression, the state is propagated as:

E [xk+1 ] = E [xk ] + E [ f (xk , ûk , ν k , η k )] (27)

Since the expected values of the noises are zeros and the expected values of the state is the
prediction denoted with x̄, then:

x̄k+1 ≈ x̄k + ∆T f (x̄k , ûk , 0, 0) (28)

Equation (28) is used to propagate the state, the following sections, linearize and discretize the
model in (23) to propagate the uncertainty in the system.

IV. Linearization
From the state evolution model in (23), we must obtain a discretized model for the state mean
propagation and a linear discrete model for the error propagation. We have two options: 1)
linearize first and then discretize, or 2) discretize and then linearize.

4
 INS • 2018

Linearize continuous model

Using a first order Taylor expansion on the function f (x, û, ν , η ):
   
∗ ∗ ∗ ∗ ∂ f  ∗ ∂ f  ∗ ∂ f  ∗ ∂ f 
f (x, û, ν , η ) ≈ f (x , û , ν , η ) + (x − x ) + (û − û ) + (ν − ν ) + (η − η ∗ )
∂x ∗ ∂û ∗ ∂νν ∗ ∂ηη ∗
  
∂ f  ∂ f  ∂ f 
≈ f (x∗ , û∗ , ν ∗ , η ∗ ) + ( x − x ∗
) + (ν − ν ∗
) + (η − η ∗ )
∂x ∗ ∂νν ∗ ∂ηη ∗
(29)

where the linearization around û∗ is nonsense if we choose û as the linearization point, which
makes sense since û is known (it is not a random variable). The linearized state evolution in error
terms:
  
∂ f  ∂ f  ∂ f 
f (x, û, ν , η ) − f (x∗ , û, ν ∗ , η ∗ ) ≈ ( x − x ∗
) + (ν − ν ∗
) + (η − η ∗ )
∂x ∗ | {z } ∂νν ∗ | {z } ∂ηη ∗ | {z }
(30)
| {z }
∆ẋ ∆x ∆νν ∆ηη

∆ẋ ≈ F∆x + Gν ∆νν + Gη ∆ηη = F∆x + G∆δδ

where the partial derivative (or Jacobian) matrices are indicated by F, Gν and Gη and

∆νν
 
∆δ =
 
G = Gν Gη and δ (31)
∆ηη

Discretize linearized model

Once the model is linearized, we can apply standard techniques to discretize it. The discrete
model is:
∆xk+1 = Φ k ∆xk + Γ k,ν ∆νν k + Γ k,η ∆ηη k (32)
where:
∆xk = xk − x∗k = xk − x̂k ∼ N (0, Pk )
∆νν k = ν k − ν ∗k = ν k ∼ N (0, Vk ) (33)
∆ηη k = η k − η ∗k = η k ∼ N (0, Nk )
This model is only employed to propagate the covariance matrix of the state which is:

Pk+1 = Φ k Pk Φ kT + Γ k,ν Vk Γ k,ν

T T
+ Γ k,η Nk Γ k,η (34)
| {z } | {z }
V̄k N̄k

For the simplified model in (31):

∆xk+1 = Φ k ∆xk + Γ k ∆δδ k (35)

where:
 
Vk 0
∆δδ k = δ k − δ ∗k = δ k ∼ N (0, Dk ) and Dk = (36)
0 Nk

The uncertainty propagation for this model is:

Pk+1 = Φ k Pk Φ kT + Γ k Vk Γ kT (37)
| {z }
D̄k

5
 INS • 2018

The state evolution matrix is obtained as:

Φ k = eFT (38)

and the matrix D̄k is obtained following the next procedure:

−F GSGT
 
1. Build C = where S is the Power Spectral Density (PSD) matrix of the white
0 FT
 
S 0
noise as S = ν . The PSD of the biases noise is given by the manufacturer and the
0 Sη
PSD of the sensor noise can be obtained from the measure variance.
 
F G2
2. Compute eCT = 2
0 F3

3. Finally D̄k = F3T G2

The PSD deserve a second look. The two PSD matrices are:
   
Sν f 0 Sη f 0
Sν = and Sη = (39)
0 Sνw 0 Sηw

where:
sνx f
 
0 0 sνxw

0 0

y y
Sν f =  0
 sν f 0  Sνw = 0 sνw 0 
0 0 z
sν f 0 0 szνw

sηx f
 
0 0 sηxw

0 0

y y
Sη f =0
 sη f 0  Sηw = 0 s ηw 0  (40)
0 0 szη f 0 0 szηw

The PSD of the ν components are related to the sensor noise. They can be obtained by first
measuring the sensor white noise variances empirically to obtain:
 2 2 2 
y 2
   
y
2
2
V = diagonal σνxf σν f σνzf σνxw σνw σνzw (41)

Then the PSD can be approximated by:

Sν = VTI MU (42)

The PSD of the bias white random noise must be specified by the manufacturer.

II. Calibration
In this section the IMU is calibrated to obtain the static biases and the gravity scale factor to
compensate the gravity value used by the manufacturer not coinciding with the value at our
b
location. The goal of this section is obtaining the f̂ and ŵb values used in the measurement
model (19) (20). The uncalibrated IMU readings are denote with a superindex ˘ and the true
values without any superindex. Note that the IMU accelerations are given in “g” units, where

6
 INS • 2018

Figure 1: Axis definitions.

the standard gravity defined by the manufacturer is g0 = 9.80665m/s2 . The IMU accelerometers
model in standard m/s2 is:
 
f˘x = c x f x + bx f ,0 + bx f + νx f
 
f˘y = cy f y + by f ,0 + by f + νy f (43)
 
f˘z = cz f z + bz f ,0 + bz f + νz f

where the the b frame superindex used in (19) has been ignored. The following tests gather
data such that the scale factors c x , cy and cz , and the constant biases bx f ,0 , by f ,0 and bz f ,0 can be
estimated.

Test 1. Assuming a flat surface, we gather data such that the acceleration components of the
corresponding axis is zero. For example, for the x axis pointing perpendicular to the gravity force:
 
f˘x = c x bx f ,0 + bx f + νx f (44)

Taking the expected value of both sides and assuming that the random moving bias bx f does not
vary during this time (approximately true because it moves very slowly) such that Ebx f = 0, we
have:
E f˘x = Ec x bx f ,0 + Ebx f + Eνx f = c x bx f ,0 (45)

because c x bx f ,0 is a constant and Eνx f = 0. Then, approximating E f˘x by the mean of a lot of data,
and repeating this process for the x and z axis:
 
c x bx f ,0 = mean f˘x , µ x and similarly
 
cy by f ,0 = mean f˘y , µy (46)
 
cz bz f ,0 = mean f˘z , µz

where µ x , µy and µz are the calculated mean when each axis is pointing perpendicular to the
gravitational force. The next test will produce three more equations to solve the 6 calibration
unknowns (c x , cy , cz , bx f ,0 , by f ,0 and bz f ,0 ).

7
 INS • 2018

z y x
z

y z y

x x
Figure 2: The three tests carried out. In each one of them, one axis points in the upward direction.
See Fig 1 to orientate the IMU.

Test 2. In order to calibrate the scale factors, we can align each axis with the gravity direction
pointing “upwards”, such that we should be measuring the gravity acceleration exactly (with a
positive value), which we know is g = 9.80279m/s2 at our location (at IIT). For example, for the x
axis again, the measured value is:
 
f˘x = c x g + bx f ,0 + bx f + νx f (47)

Taking the expected value of both sides and approximating E f˘x by the mean of the data
 
ĝx , mean f˘x = c x g + c x bx f ,0 (48)
| {z }
µx

where a zero-mean moving bias for this period is assumed as in the previous test. Then, using the
estimated value of c x bx f ,0 from (46):

1
cx = ( ĝx − µ x ) and similarly
g
1

cy = ĝy − µy (49)
g
1
cz = ( ĝz − µz )
g

The static biases can then be calculated from (46) as:

µx
bx f ,0 =
cx
µy
by f ,0 = (50)
cy
µz
bz f ,0 =
cz

Finally, from (43):

f˘x
fx = − bx f ,0 −bx f − νx f (51)
c
| x {z }
fˆx

8
 INS • 2018

where the calibrated measurement, fˆx , is identified from (19). The readings obtained from the
IMU will be calibrated as follows:
 ˆ   −1  ˘   
fx cx 0 0 fx bx f ,0
 fˆy  =  0 c− 1 0   f˘y  −  by f ,0  (52)
 
y
ˆf z 0 0 −
cz 1 ˘
fz bz f ,0
| {z } | {z }
C−1 b f ,0

or, in matrix notation:

f̂ = C−1 f̆ − b f ,0 (53)
In the case of the gyros, there is no gravity scale factor and we can simply gather a lot of static
data and correct for the biases in each axis with a single test.

9
 INS • 2018

References

10