AIAA Guidance, Navigation, and Control Conference AIAA 2011-6488

08 - 11 August 2011, Portland, Oregon

Tightly-coupled INS/GPS using Quaternion-based

Unscented Kalman filter

Junchuan Zhou1, Yuhong Yang2, Jieying Zhang3, Ezzaldeen Edwan4, Otmar Loffeld5
University of Siegen, Center for Sensorsystems (ZESS), Siegen, D-57076, Germany

Stefan Knedlik6
iMAR GmbH, St. Ingbert, D-66386, Germany

In INS/GPS integration system, Euler angle and quaternion are often used to represent
attitude. In a tightly-coupled system, both the Euler angle-based and quaternion-based
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

approaches have nonlinear system propagation and observation models. In this paper, the
unscented Kalman filter is employed as the nonlinear filtering method in the Euler angle-
based and quaternion-based approaches. Field experiments are conducted based on train
rides. One trajectory involves frequent short GPS outage periods, and the other contains two
long GPS outage periods. Numerical results show that both approaches can present robust
and accurate estimation results. Nevertheless, the quaternion-based approach does not
exhibit singularity problem, and it is more suitable to be used in the INS/GPS system with
very low-cost inertial sensors.

I. Introduction

G PS receiver has dominated the field of positioning and navigation for decades [1]. However, its performance
depends on the signal environments. It provides a continuous navigation solution only when more than four
satellites are in view. In order to solve this problem, other navigation systems, e.g., inertial navigation system, are
often employed to integrate with GPS for having a robust and continuous navigation solution. Historically, due to
the high cost in manufacturing the inertial sensors, the INS/GPS integration system are mostly employed in military
and aerospace industry [2, 3]. Recently, the advent of micro-electromechanical systems (MEMS) technology drives
the discrete, heavy and inflexible inertial sensor system to small, cost-effective, light-weight, portable and low-
power silicon-based inertial devices. Although the cheap MEMS-based inertial sensors do not exhibit high accurate
navigation performance, they can meet the requirements of many land-based navigation applications when aided
with GPS devices. An INS/GPS system combines the advantages of both sides and provides accurate and
uninterrupted navigation results, working in all environments, and constituting a potential and powerful alternative
to the GPS alone navigation devices. Nowadays, the INS and GPS integrated solutions are the back-bones of many
modern navigation systems, which are employed in industrial and military applications. Substantial research effort
has been devoted to extensive algorithmic developments and performance analysis. The objective is mainly at the
promotion of system estimation accuracy with low-cost sensor systems, putting a focus of interest onto powerful
sensor fusion algorithms.
The so-called tightly-coupled integration is one of the approaches to fuse the INS and GPS measurements.
However, when modeling the underlying problem, the system propagation and observation models are nonlinear.
The most common application of the Kalman filter (KF) on nonlinear systems is the extended (or linearized)
Kalman filter (EKF) [4-6], which is based on a first-order linearization of the nonlinear stochastic system models
with the assumption of Gaussian distributed noises. Although the EKF maintains the elegant and computationally
efficient update form of the KF, it suffers from a number of drawbacks. That is, the linearized transformations are
reliable, only if the error propagation can be well approximated by a linear function, because the small error
1
Ph.D. candidate, Email: Zhou@zess.uni-siegen.de.
2
Ph.D. candidate, Email: Yyang@zess.uni-siegen.de.
3
Ph.D. candidate, Email: Czhang@zess.uni-siegen.de.
4
Ph.D. candidate, Email: Edwan@zess.uni-siegen.de.
5
Professor, Email: Loffeld@zess.uni-siegen.de.
6
Product development manager, Email: s.knedlik@imar-navigation.de.
1/14
American Institute of Aeronautics and Astronautics

Copyright © 2011 by Junchuan Zhou. Published by the American Institute of Aeronautics and Astronautics, Inc., with permission.
 tolerances of the EKF can cause inconsistency of the covariance update. Using first order approximations could
result in filter instability in the presence of higher order effects [3, 7]. Besides, the derivation of Jacobian matrix
itself can be a complicated mathematical task. Since 1995, the unscented Kalman filter (UKF) algorithm has been
proposed. It can accurately capture the mean and covariance estimates up to the 3rd order of a Taylor series
expansion for any nonlinearity [8-10] with Gaussian inputs, leading to a faster convergence from inaccurate initial
conditions in position/attitude estimation problems [3]. The analysis of nonlinear problems using EKF and UKF for
Euler angle-based tightly-coupled integration system can be found in [11, 12]. In this contribution, quaternion is
employed for representing attitude using the UKF algorithms. Due to the fact that the unit sphere defined by
quaternion vector is not an Euclidean vector space [13, 14], the usual definitions of vector addition and scaling
should principally not be directly applied on quaternion vectors for representing rotations. Besides, the quaternion
unity magnitude constraint allows three degrees of freedom instead of four. Thus, in this paper, the quaternion
vector is transformed to the rotational space to preserve its nonlinear nature of the unit vector [15-17]. In this way,
the quaternion vector can be updated using the quaternion product chain rule, having a natural way of maintaining
the normalization constraint. That means, the successive rotation can be accomplished using quaternion
multiplication in the same order as the direction cosine matrix multiplication [18]. In this paper, we compare the
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

system performances of the Euler angle-based and quaternion-based approaches using UKF. The objective is to
answer the following two questions:
1) Does quaternion-based approach brings benefits compared with Euler angle-based approaches for land-based
navigation?
2) What is the difference between two approaches if unscented Kalman filter is employed as the nonlinear filtering
methods to cope with the nonlinearity inherited in their nonlinear system state space models?
Although for land-based navigation, other than nonlinearity and GPS outage issues, the multipath effects may
also significantly influence the navigation performance. In order to cope with such non-Gaussian problem, a
quaternion-based unscented particle filter (UPF) was lately proposed by the author. Details can be found in [19].
In the remainder of this paper, in Section 2, the Euler angle-based and quaternion-based UKF algorithms are
introduced. In Section 3, two field experiments are conducted based on train rides. Numerical results are analyzed.
Last but not least, a short summary is made. In appendix A, the relationship between the quaternion vector and
rotation vector is attached.

II. Low-cost INS/GPS using unscented Kalman filter

For a low-cost MEMS-based IMU, its navigation performance deteriorates over time due to the accumulation of
combined contamination from sensor errors, such as noises, sensor biases, scale factor variations, etc. The accuracy
of inertial navigation depends on the level of sensors in use and the modeling and compensation of sensor errors
during run-time. Among other errors, the inaccuracies in attitude determination are the main contributors in the
position and velocity errors in inertial navigation system, which is up to this day the most researched topic related
with inertial navigation [3]. For the integration of measurements coming from a single GPS receiver antenna and an
INS without redundant attitude information (e.g., from magnetometers, or multi-antenna GPS systems), the INS
attitude and inertial sensor errors are weakly observable, which also depend on the dynamic of the platform during
the application [20-22]. It is the nonlinear system propagation model that relates the attitude errors, inertial sensor
errors with the position and velocity errors. As a conventional nonlinear filtering approach, in the EKF algorithm,
the inertial error propagation equations are often used as the INS/GPS system propagation model, where the
navigation errors and inertial sensor errors are chosen as states (i.e., error states) [5, 6, 13]. In this way, the INS
estimated quantities and GPS measurements are compared, while the resulting differences form the new
measurement input to the integration Kalman filter. The filter yields the estimates of inertial sensor errors, which are
used to compensate the incoming inertial sensor measurements. Besides, the compensation of INS navigation errors
is conducted after every Kalman filter measurement update. However, in a UKF algorithm, navigation solutions
(i.e., position, velocity and attitude) and inertial sensor errors are chosen as states (i.e., total states). The nonlinear
system propagation and observation models are directly employed in the filter. In this way, the deviations of
Jacobian matrices are prevented. Moreover, the nonlinearity of system models is more accurately handled.

A. Euler angle-based UKF algorithm

Recently, many researchers have applied the UKF in INS/GPS integrated system and reported the improved
system performance with respect to that of the EKF algorithm. In this paper, among several UKF algorithms, the
approach employing 2n equally weighted sigma points is used, which can be found in Julier and Uhlmann [8] and
Simon [23]. The algorithm is summarized in Table 1.

2/14
American Institute of Aeronautics and Astronautics
 Table 1. Summary of UKF algorithm.
System state space models with additive, xk = f k −1 (xk −1 ) + w k −1
zero mean, Gaussian white noises:
y k = hk (x k ) + v k , with w k ∼ N (0, Q), v k ∼ N (0, R )
(1) Initialization xˆ +0 , P0+
(2) Draw sigma points
( ) ,χ ( ) , i = 1,..., n
T T
χ k ,i = xˆ +k −1 + nPk+−1 k ,i + n = xˆ +k −1 − nPk+−1
i i
(3) Time update 1 2n
xˆ −k = ∑ f k −1 (χ k ,i ),
2n i =1
1 2n
∑  fk −1 (χ k ,i ) − xˆ −k   f k −1 (χ k ,i ) − xˆ −k  + Q
T
Pk− =
2n i =1 
(4) Draw sigma points
( ) ,χ ( ) , i = 1,..., n
T T
χ k ,i = xˆ −k + nPk− k ,i + n = xˆ −k − nPk−
i i
(5) Predict measurements 1 2n
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

yˆ k = ∑ hk (χ k ,i )
2n i =1
(6) Update covariance 1 2n
∑  hk (χ k ,i ) − yˆ k   hk (χ k ,i ) − yˆ k  + R
T
Pky =
2n i =1 
1 2n
∑
T
Pkxy =  χ k ,i − xˆ −k   hk (χ k ,i ) − yˆ k 
2n i =1
(7) Measurement update K k = Pkxy (Pky )−1 , xˆ +k = xˆ −k + K k (y k − yˆ k ), Pk+ = Pk− − K k Pky K Tk
where ‘n’ denotes the dimension of the system state vector; ( nPk+−1 ) is the matrix square root of nP +
k −1 , such as

( ) ( nP ) = nP , which can be obtained from the Cholesky factorization ‘CHOL’ in MATLAB;

T
nPk+−1 +
k −1
+
k −1

( ) is the i-th row of ( nP ) ; ( nP ) is the transpose of ( nP ) ; Q is the covariance matrix of the

T
nPk+−1 +
k −1
+
k −1
+
k −1
i i i

additive stationary system process noise; R is the covariance matrix of the additive stationary measurement noise.
The INS propagation model we use is often seen for the low-cost micro-electromechanical (MEMS) based IMU,
which is also used as the system propagation model in INS/GPS integrated system. In terms of a low-cost IMU, the
effects from the earth rotation cannot be observed. Hence, they are not considered in the model. Moreover, the
transport rate and Coriolis terms have been neglected for simplicity. The simplified propagation model in discrete
time can be expressed in navigation frame as:
p n, k +1 = p n, k + v n, k ⋅ ∆t (1)

v n ,k +1 = v n, k +  R b 2n ,k ⋅ (fɶb, k − fˆberror
,k − w f ) + g n  ⋅ ∆t
ψ k +1 = ψ k + E b 2 n, k ⋅ (ω ɶ b, k − ω ˆ error
b , k − w ω ) ⋅ ∆t

where k is the time instant; fɶb is the measurement vector of the specific force; ω ɶ b represents the measurement
vector of angular rate; g n is the local gravity vector; ψ is the Euler angles; fˆberror
,k and ωˆ error
b, k are the estimated
specific force and angular rate errors; w f and wω are the remaining noise errors; R b 2 n is the frame rotation matrix
from body frame to NED navigation frame; Eb 2 n is the rotation rate transformation matrix between body and
navigation frame.
 Cϕ Cφ Cϕ Sφ Sθ − Sϕ Cθ Cϕ Sφ Cθ + Sϕ Sθ  (2)
 
R b 2 n =  Sϕ Cφ Sϕ Sφ Sθ + Cϕ Cθ Sϕ Sφ Cθ − Cϕ Sθ 
 − Sφ Cφ Sθ Cφ Cθ 
 
 1 Sθ T φ Cθ T φ 
 
Eb 2 n = 0 Cθ − Sθ 
 0 Sθ / Cφ Cθ / Cφ 

3/14
American Institute of Aeronautics and Astronautics
 where CX , SX and TX represent the trigonometry operations of cosine, sine and tangent respectively; θ , φ , ϕ
denote the roll, pitch and yaw angles.
In the tightly-coupled integration, the pseudorange and delta pseudorange (Doppler) measurements are used in
the filter, yielding a nonlinear system observation model. The predicted pseudorange measurement based on current
system position estimates is formulated as:
ρˆ kj = ( xnj,k − xˆn, k ) 2 + ( xej,k − xˆe, k ) 2 + ( xdj , k − xˆd ,k )2 + c∆tˆk (3)

where ρˆ kj is the predicted pseudorange measurement from the j-th satellite; xnj, k , xej, k , xdi , k are the j-th satellite
position coordinates expressed in NED navigation frame; xˆn, k , xˆe , k , xˆd , k are the vehicle position estimates in NED
navigation frame.
The predicted delta pseudorange measurements are related to the velocity estimates as:
ρˆɺ kj = anj, k ( xɺnj, k − xˆɺn , k ) + aej, k ( xɺej,k − xˆɺe, k ) + adj , k ( xɺdj ,k − xˆɺd ,k ) + c∆tˆɺk (4)
x j
− xˆn , k j x − xˆe , k j
j
x − xˆd , k
j
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

n, k e ,k d ,k
where anj, k = , ae , k = , ad , k =
dˆkj
dˆkj
dˆkj
dˆkj = ( xnj, k − xˆn, k ) 2 + ( xej,k − xˆe , k ) 2 + ( xdj , k − xˆd , k )2
where ρɺˆ kj is the predicted delta pseudorange measurement from the j-th satellite; xɺnj, k , xɺej, k , xɺdj , k are the j-th satellite
velocity coordinates expressed in NED navigation frame; xɺˆ , xˆɺ , xˆɺ are the vehicle velocity estimates in NED
n, k e, k d ,k

navigation frame.

B. Quaternion-based UKF algorithm

Instead of using Euler angle, quaternion is also widely employed as the representation of attitude. We denote the
 
quaternion vector as q = q1 + q , where q = [q2 , q3 , q4 ]T . It is used to represent the rotation from navigation frame to
body frame. For the quaternion-based approaches using UKF algorithm, the quaternion product chain is adopted to
update quaternion vectors [15-17]. The system position, velocity, quaternion, inertial sensor errors are updated in
discrete time domain as shown in Equation (5):
p n, k +1 = p n, k + v n ,k ⋅ ∆t (5)
v n ,k +1 = v n, k + R nb (q k )fb , k + g n  ⋅ ∆t
q k +1 = ∆q k ⊗ q k
, k +1 = f b , k
fberror +wf
error

b , k +1 = ωb , k + w ω
ω error error

where ⊗ denotes the quaternion product; ∆q k represents the rotation expressed in quaternion between adjacent
 cos(0.5 θ k ) 
 
time epochs, which is ∆q k =  θ , where θ k represents the integral of IMU body frame angular rate
sin(0.5 θ k ) k 
 θ k 
measurements from time instance k-1 to k; the inertial sensor errors are modeled as constants plus random walks.
For the tightly-coupled integration, the clock drift in distance is modeled as a constant plus a random walk, while
the receiver clock bias is the integral of the clock drift.
 c∆t k +1  1 T  c ∆tk  (6)
 ɺ =  ⋅   + w clk
 c∆t k +1  0 1  c ∆tɺk 
For a quaternion vector, due to the quaternion normalization constraint, its degree of freedom is three instead of
four. Thus, if we use quaternion vector elements as states, the dimension of state vector is 18 × 1 , but the dimension
of state error covariance matrix is 17 ×17 . This dimensional mismatch can be solved by transforming the quaternion
vector error into its corresponding rotation vector in the rotation space. The mutual derivations between rotation
vector and corresponding quaternion vector can be found in Appendix A. We define the state vector and their errors
(i.e., the estimated minus true) as:

4/14
American Institute of Aeronautics and Astronautics
  pˆ +n, k   pˆ +n ,k − p n ,k   δ pˆ +n, k 
 +   +   + 
 vˆ n, k   vˆ n, k − v n, k   δ vˆ n ,k 
 qˆ +k   φˆ +k   φˆ +k 
     
xˆ +k =  fˆb, k  , δ xˆ +k =  fˆb , k − fb, k  =  δ fˆb, k 
error + error + error error +
(7)
 error +   error + error   error + 
ω ˆ b, k  ωˆ b, k − ωb , k  δ ω ˆ b, k 
 ˆ+   ˆ+   ˆ+ 
c∆t k  c∆t k − c∆tk   cδ tk 
c∆tɺˆ+  c∆tˆɺ+ − c∆tɺ   cδ tˆɺ+ 
 k 18×1  k k   k 17×1
where subscript “n” denotes NED navigation frame. The subscript “b” denotes the body frame. “k” is the time
instant. The φˆ +k denotes the rotation vector corresponding to the quaternion error vector δ qˆ +k . The state error
covariance matrix is thus formulated as:
(σ δ2pˆ ) O3×3 O 3×3 O 3×3 O3×3 03×1 03×1 
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

 3×3

 O3×3

( σ δ2vˆ )3×3 O 3×3 O 3×3 O3×3 03×1 03×1 

 O
 3× 3 O3×3 ( σ φˆ2 )3×3 O 3×3 O3×3 03×1 03×1 

+
Pk = 
 O
3× 3 O3×3 O 3×3 (σ δ fˆ )3×3 O3×3
2
03×1 03×1 
 (8)
 O 03×1 
 3× 3 O3×3 O 3×3 O 3×3 (σ δ2ωˆ )3×3 03×1
 T
 03×1 0T3×1 0T3×1 0T3×1 0T3×1 (σ c2δ tˆ )1×1 01×1 
 T 
 03×1 0T3×1 0T3×1 0T3×1 0T3×1 ( )
01×1 σ c2δ tɺˆ 
1×1 17×17

According to the UKF algorithm given in, we firstly generate 2n (n=17) equally weighted sigma points as:
 ∆pˆ +n ,k −1,i   pˆ +n, k −1 + ∆pˆ +n , k −1,i   pˆ +n, k −1 − ∆pˆ n+,k −1,i 
 +   +   + + 
 vˆ n , k −1 − ∆vˆ n, k −1,i 
+
 ∆vˆ n ,k −1,i   vˆ n, k −1 + ∆vˆ n, k −1,i 
 ( k −1,i )
 φˆ +k −1,i  δ q(φˆ +k −1,i ) ⊗ qˆ +k −1  δ q −φˆ + ⊗ qˆ +k −1 
    
( ) ˆ error +  + ˆ error + + ∆fˆ error +  +  error + 
T
+
nPk −1 =  ∆ f b , k −1, i ,χ  f
= b, k −1 b , k −1, i
ˆ error + ˆ
, χ k −1,i + n =  fb, k −1 − ∆fb ,k −1,i  (9)
i  error +  k −1,i  error + error + 
∆ ωˆ
 b ,k −1,i  ˆ
ω
 b, k −1 + ∆ ˆ
ω b , k −1, i  ω +
ˆ b ,k −1 − ∆ω
error
ˆ b, k −1,i 
error +

   ˆ+   
ˆ +
ˆ +
 ∆c∆t k −1,i  c∆t k −1 + ∆c ∆t k −1,i   c ∆tˆk −1 − ∆c ∆tˆk+−1,i 
+

 ˆɺ+   ˆɺ+ ˆɺ+   

 ∆c∆t k −1,i  c∆t k −1 + ∆c ∆t k −1,i   c ∆tˆɺk+−1 − ∆c ∆tˆɺk+−1,i 
where i = 1,..., n, and we denote
 cos(0.5 φˆ + ) 
 k −1,i

δ q(φˆ +k−1,i ) =  φ ˆ +k −1,i  (10)
sin(0.5 φˆ + ) 
 k −1,i
φˆ k −1,i 
+
 
In the time update, we pass sigma points through the nonlinear system propagation model, and compute the
mean as:

5/14
American Institute of Aeronautics and Astronautics
  pˆ −n, k ,i   pˆ −n , k 
 −   − 
 vˆ n, k ,i   vˆ n, k 
 qˆ k ,i 
−
 qˆ −k 
   
ˆ error − 1 2 n
ˆ error − 
χ −k ,i = f k , k −1 (χ +k −1,i ) =  fb, k ,i  , xˆ −k = ∑ k ,i 
χ −
=  f b , k (11)
 error −  2n i =1 error − 
ˆ
 b, k ,i 
ω ˆ
ω b , k 
 ˆ−   ˆ− 
c∆t k ,i  c∆t k 
 ɺˆ−  c∆tˆɺ− 
c∆t k ,i   k 
Due to the fact that the unit quaternion vector is not mathematically close for addition and scalar multiplications,
qˆ −
the renormalization must be conducted, which can be handled as qˆ k− = k− .
qk
We calculate the state error covariance matrix as:
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

1 2n
∑
T
Pk− =  ∆xˆ k−   ∆xˆ k−  + Q (12)
2n i =1
 pˆ −n, k ,i − pˆ −n, k 
 − − 
 vˆ n, k ,i − vˆ n , k 
 φˆ −k ,i 
 
ˆ ˆ
where ∆xˆ k− = χ k−,i − xˆ −k =  fb, k ,i − fb , k  , and φˆ −k ,i is the rotation vector corresponding to qˆ −k ,i ⊗ ( qˆ −k ) .
error − error − −1

 error − −
ωˆ b, k ,i − ω ˆ berror
,k 
 ˆ− ˆ −
 c∆tk , i − c∆tk 
 ɺˆ− ˆɺ − 
 c∆tk , i − c∆tk 
Having the a priori mean and covariance matrix, we generate sigma points for measurement update as:
 ∆pˆ −n, k ,i   pˆ −n, k + ∆pˆ −n, k ,i   pˆ −n, k − ∆pˆ n−,k ,i 
   −   − − 
 vˆ n, k − ∆vˆ n, k ,i 
− −
 ∆vˆ n, k ,i   vˆ n, k + ∆vˆ n ,k ,i 
 ( k ,i )
 φˆ −k ,i   δ q(φˆ −k ,i ) ⊗ qˆ −k   δ q −φˆ − ⊗ qˆ − 
k 
   
( ) ˆ ˆ ˆ  
T error − error − error −
nPk −
=  ∆fb, k ,i  , χ k ,i =  fb ,k + ∆fb ,k ,i  , χ k ,i + n =  fb ,k − ∆fb, k ,i 
− − ˆ error − ˆ error −
(13)
i  error −   error − − 
 ∆ω ˆ b, k ,i  ω ˆ b , k + ∆ω ˆ error
b ,k ,i  ω ˆ berror −
− ∆ωˆ error − 
,k b, k ,i
  
∆ c ∆ t ˆk−,i   ˆ−
c ∆ t + ∆ c ∆ tˆ − 
ˆ
 c∆t k − ∆c ∆tk−,i 
−
ˆ
   k k ,i 
  
ɺˆ −   ˆɺ− ˆɺ− 
 c∆tˆɺk− − ∆c ∆tɺˆk−,i 
 ∆c∆tk ,i   c∆t k + ∆c∆tk ,i 
 cos(0.5 φˆ −k ,i ) 
 
where i = 1,..., n, and δ q(φˆ k−,i ) =  −
φˆ −
k ,i
.
sin(0.5 φˆ k ,i ) − 
 φˆ k ,i 
We pass them through the nonlinear measurement models to compute the predicted measurement and covariance
matrices as:
1 2n
yˆ k =
2n i =1
∑ hk (χ −k ,i )
1 2n
∑
T
Pkyy =  hk (χ −k ,i ) − yˆ k   hk (χ −k ,i ) − yˆ k  + R (14)
2n i =1
1 2n −
∑  χ k ,i − xˆ −k   hk (χ −k ,i ) − yˆ k 
T
Pkxy =
2n i =1 
The Kalman gain and state correction terms are computed as:

6/14
American Institute of Aeronautics and Astronautics
  ∆pˆ +n, k 
 + 
 ∆vˆ n, k 
 φˆ +k 
 
ˆ error +
K k = Pkxy (Pkyy ) −1 , ∆xˆ +k = K k (yɶ k − yˆ k )=  ∆fb, k  (15)
 error + 
ˆ b, k 
 ∆ω
 ˆ+ 
 ∆c∆tk ,i 
 ˆɺ+ 
 ∆c∆tk ,i 
The last step in UKF algorithm is to apply the corrections.
 pˆ −n, k + ∆pˆ n+, k  (16)
 − + 
 vˆ n, k + ∆vˆ n ,k 
 q(φˆ k+ ) ⊗ qˆ k− 
 
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

ˆ error − ˆ error +
xˆ k+ =  fb ,k + ∆fb ,k  , Pk+ = Pk− − K k Pkyy K Tk
 error − +
ωˆ b , k + ∆ω ˆ berror
,k 
 ˆ− ˆ + 
c ∆
 k ,it + ∆c ∆ t k ,i 
 ɺˆ− ˆ + 
c∆t k ,i +∆c∆tk ,i 
ɺ

III. Field Experiments

Two field experiments have been conducted based on train rides. In the following tests, we apply the Kalman
filtering methods mentioned above in field applications, and numerical results are analyzed.

A. Field experiment based on a train ride with frequent GPS outage periods.
We take the train from the city of Betzdorf to Siegen in the north west of Germany. The trajectory lasts about 800
seconds with 4 GPS outage periods, as shown in figure 1. The number of tracked satellites is given in figure 2. One
LandmarkTM20 eXT MEMS-based IMU (100 Hz) is used, which is equipped with a u-blox LEA-5T on-board. The
system can output the synchronized IMU and GPS data. However, the on-board GPS receiver only outputs
navigation solutions (position, velocity and time) instead of the pseudorange and Doppler measurements. Therefore,
one extra u-blox Antaris 4 (1 Hz) is used. In the post processing, using the GPS time output from the landmark on-
board GPS receiver, the IMU raw data can be easily synchronized with the raw data from the external u-blox 4. The
main sensor errors of the LandmarkTM20 eXT IMU are given in Table 2.

Figure 1. Trajectory based on a train ride from Betzdorf to Siegen.

7/14
American Institute of Aeronautics and Astronautics
 10
Number of Satellites
8

0
100 200 300 400 500 600 700 800
Time [s]
Figure 2. Number of satellites in view.

Table 2. LandmarkTM20 eXT MEMS-based IMU performance specification.

Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

Gyroscope Bias in-run stability Noise (ARW) Scale Factor Error

(Angular rates) 20 [°/h] (1 σ ) 0.035[°/s/√Hz] (1 σ ) ≤ 1000 [ppm]
Accelerometer Bias in-run stability Noise (VRW) Scale Factor Error
(Specific forces) 20 [µg] (1 σ ) 40 [µg/√Hz] (1 σ ) ≤ 1000 [ppm]

Along the trajectory, the train went through tunnels 4 times. The durations of GPS outage periods are given in
Table 3, where the period for less than 4 satellites in view is 42 s, and 25 s for complete GPS outage environments.
The residual 17 s represents that although the GPS receiver cannot form the positioning solution by itself, but the
information from the remaining satellites is still available, which can be used in the INS/GPS tightly-coupled
integration. The INS/GPS tightly integrated solution (blue curves) and GPS single point positioning based on least-
squares estimation method (red points) are depicted in Figure 3. As expected, during GPS outage periods, the
INS/GPS system presents continuous and robust positioning results.

Table 3. GPS outage environments.

GPS outages < 4 satellites in view no satellites in view
a) 5 Seconds 2 Seconds
b) 8 Seconds 5 Seconds
c) 13 Seconds 9 Seconds
d) 16 Seconds 9 Seconds
Sum 42 Seconds 25 Seconds

a) GPS outage with 5 s (<4 Sat.) and 2 s (no Sat.)

8/14
American Institute of Aeronautics and Astronautics
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

b) GPS outage with 8 s (<4 Sat.) and 5 s (no Sat.)

c) GPS outage with 13 s (<4 Sat.) and 9 s (no Sat.)

d) GPS outage with 16 s (<4 Sat.) and 9 s (no Sat.)

Figure 3. INS/GPS positioning results during GPS outage periods.

9/14
American Institute of Aeronautics and Astronautics
 For achieving such robust and accurate positioning results in figure 3, the correct estimation of attitude is critical
for INS/GPS systems. In figure 4, we show the attitude estimates using the Euler angle-based UKF algorithm (red
curves). We also plot the attitude estimates from the standalone INS (Gyro. accumulated without GPS aiding) to
show the singularity problem.

180

90
Roll [deg]

−90

−180
100 200 300 400 500 600 700 800
Time [s]
90
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

45
Pitch [deg]

−45

−90
100 200 300 400 500 600 700 800
Time [s]
180

90
Yaw [deg]

0
atan2(vE/vN)
−90 Gyro Accumulated
INS/GPS Integrated
−180
100 200 300 400 500 600 700 800
Time [s]

Figure 4. INS/GPS estimated and gyro accumulated Euler angles.

For the train which moves forwards and backwards on the track, the yaw angle can be derived from the north and
east velocity estimates, i.e., ϕ = atan2(vE vN ) . However, due to the calculation of tangent (i.e.,
sin(ϕ ) vE
tan(ϕ )= = ), when both velocities approach zero, yaw estimates will be wrong, as shown in the third
cos (ϕ ) vN
subplot of figure 4. Thus, the heading information derived from GPS velocity estimates can only be used when the
dynamic of the platform is high.
For the standalone INS without GPS aiding (blue curves), the Euler angle errors are drifting unboundedly over
time due to the integration of raw angular rate measurements from the gyroscopes. With very low-cost inertial
sensors, the attitude errors will drift rapidly. With the elapse of time, when the pitch angle approaches ± 90 degrees,
a singularity problem appears. This means that the roll and yaw angles will be ambiguous at ± 90 degrees pitch
angle if the direction cosine matrix is formed by the rotation sequence of Z-Y-X from the navigation to the body
frame. In this case, using quaternion in INS/GPS algorithm is highly recommended. The quaternion estimates in
gyro accumulated case are presented in figure 5, where no jump errors caused by the singularity problem can be
observed.

10/14
American Institute of Aeronautics and Astronautics
 1.5
Norm of Quat. q1 q2 q3 q4

1
quaternions

0.5

−0.5

−1
0 100 200 300 400 500 600 700 800
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

Time [s]

Figure 5. Gyro accumulated quaternion estimates.

For land-based navigation, the tilt angles are usually small, because the vehicle is assumed to move on the
ground surface. That is, constraints can be introduced to avoid the singularity problem. However, for UAV-based
applications, there are indeed situations, in which the pitch angle can approach ± 90 degrees. In this case, the
quaternion-based approach should be applied.
In the next experiment, we employ a trajectory which contains much longer GPS outage periods to further
explore the system performance from the Euler angle-based and quaternion-based UKF algorithms.

B. Field experiment based on a train ride with long GPS outage environments.
This experiment is conducted on the basis of a train ride from the city of Haiger to Siegen in the North West of
Germany. The trajectory lasts about 1400 s with two tunnels, as shown in figure 6. The number of tracked satellites
is depicted in figure 7, where the first GPS outage environment lasts 93 s (about 3 km), and the second one takes 43
s. As investigated from the former test, the quaternion-based approach does not exhibit singularity problem. And in
this test, we explore its system performance for land-based navigation with long GPS outage periods.

Figure 6. Trajectory based on a train ride with long GPS outage periods.

11/14
American Institute of Aeronautics and Astronautics
 12

10
Number of Satellites 8

0
0 200 400 600 800 1000 1200 1400
Time [s]

Figure 7. Number of satellites in view.

10
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

5
Roll [deg]

0
5
−10
200 400 600 800 1000 1200 1400
5
Pitch [deg]

−5

−10
200 400 600 800 1000 1200 1400
180
Landmark+ublox (euler)
90
Yaw [deg]

Landmark+ublox (Quat.)
0

−90

−180
200 400 600 800 1000 1200 1400
0.2
G (euler) G (Quat.) G (euler) G (Quat.) G (euler) G (Quat.)
0.15 x x y y z z
Gyro bias estmates [deg/s]

0.1

0.05

−0.05

−0.1

−0.15

−0.2
200 400 600 800 1000 1200 1400
Time [s]

Figure 8. Estimation results comparison using Euler-based and quaternion-based UKF algorithms.

As shown in figure 8, the attitude estimation results from both approaches are almost identical. That is, using
UKF as the nonlinear filtering method, the nonlinearity in the Euler angle-based and quaternion-based INS/GPS
system state space models are accurately handled. The same tuning parameters are applied in both approaches.
Besides, with GPS aiding, the gyroscope bias errors are correctly estimated during run-time (4th subplot in figure 8),
which are used to compensate the errors from the incoming new angular rate measurements. In this test, we have not

12/14
American Institute of Aeronautics and Astronautics
 conducted calibration processes for removing the IMU sensor errors before the application starts. However, before
the test, we keep the inertial sensor in stationary for 100 s to roughly detect the gyro biases. They are not used in the
integration, but considered as a coarse reference for comparison (at least for the first few seconds), which are plotted
as black dashed lines in the 4th subplot of Figure 8.
During the test, the train went through two tunnels, and in those time periods, the number of tracked satellites
dropped to zero respectively. Nevertheless, both of the Euler angle-based and quaternion-based UKF approaches
present robust and accurate attitude estimation results. For land-based navigation, a complete GPS outage over 90 s
may be seldom, especially when you take a car ride in the city. However, with such long GPS outage periods, the
system still presents robust and accurate results. This is because that the sensor errors can be correctly estimated
online, and will not change much during short GPS outage periods. Nevertheless, no guarantee can be made on this
issue. It can only be discussed on the basis of applications at hand, e.g., which level of inertial sensor is in use. With
very low-cost inertial sensor and long GPS outage scenario (e.g., over 10 minutes), the quaternion-based approach
will be recommended, because it does not exhibit singularity problem.

IV. Conclusion
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

With nonlinear state space models, the state estimation is often challenging. In this contribution, the system
performances from a tightly-coupled INS/GPS integrated system using Euler angle and quaternion are analyzed.
Unscented Kalman filter is used here as nonlinear filtering method. Two field experiments based on train rides are
made. Numerical results show that both approaches return roughly the same estimation accuracy when the
singularity problem does not appear. However, if the singularity problems appear, the quaternion-based approach is
highly recommended.

Appendix A: Quaternion and rotation vector

A quaternion is often represented in its vector form as:

q  A-1
q =  1 
q

where q = [ q2 , q3 , q4 ] .
T

It is used to compute the rotational transformation from one coordinate system to the other. Based on the Euler’s
theorem, given two right-handed orthogonal coordinates, one coordinate system axes can be aligned onto another by
successive single rotations along fixed axes. The corresponding rotation vector is defined as that one coordinate axes
can be aligned with the second coordinate axes by rotating φ radian (rotation angle magnitude) about an invariant
axis (rotation axis) [24]. This invariant axis is referred to as the Euler axis. Given a unit vector u along the Euler
axis, the quaternion q that represents the same rotational transformation is formulated as:

 cos(0.5 φ )  T A-2
q=  , with u = ux , u y , u z 
sin(0.5 φ )u 

Apparently, q has the normality property (i.e., q = 1 ). That is, although the quaternion vector q has four
parameters, it has only three degrees of freedom.
From Equation A-2, the computation of quaternion vector given a rotation vector φ = [ϕ1 , ϕ2 , ϕ3 ] is handled as
T

follows:
 cos(0.5 φ )  A-3
φ  
φ = ϕ +ϕ +ϕ , u =
2
1
2
2
2
,q = 
3 φ 
φ sin(0.5 φ )
 φ 

And given a quaternion vector q = [ q1 , q2 , q3 , q4 ] , we can also compute its corresponding rotation vector as:
T

13/14
American Institute of Aeronautics and Astronautics
 
cos(0.5 φ ) = q1 , sin(0.5 φ ) = q = q22 + q32 + q42 A-4
    T
φ = 2 ⋅ atan 2( q q1 ), u =  q2 q , q3 q , q4 q 

Thus, we have rotation vector φ = φ ⋅ u .

Acknowledgments
Part of the work reported herein has been funded by the German Research Foundation (DFG), grant number KN
876/1-2, which is gratefully acknowledged.

References
1
El-sheimy, N., and Niu, x. "The Promise of MEMS to the Navigation Community," Inside GNSS. Vol. March/April 2007,
Gibbons Media & Research LLC, 2007.
2
Bar-Itzhack, I. Y., and Mallove, E. F. "Accurate INS Transfer Alignment Using a Monitor Gyro and External Navigation
Downloaded by BEIHANG UNIVERSITY on May 17, 2020 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6488

Measurements," IEEE Trans. on Aerospace and Electronic Systems Vol. AES-16, No. 1, 1980, pp. 53 - 65.
3
Crassidis, J. L. "Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation," AIAA Guidance, Navigation,
and Control Conference. San Francisco, California, 2005.
4
Loffeld, O. Estimationstheorie I + II. München: Oldenbourg Verlag, 1990.
5
Brown, R. G., and Hwang, P. Y. C. Introduction to Random signals and Applied Kalman Filtering. New York: John Wiley
& Sons, 1997.
6
Maybeck, P. S. Stochastic Models, Estimation and Control. New York: Academic Press Inc., 1979.
7
Lerro, D., and Bar-Shalom, Y. "Tracking with debiased consistent converted measurements versus EKF," IEEE Trans. on
Aerospace and Electronic Systems Vol. 29, No. 3, 1993, pp. 1015 - 1022.
8
Julier, S. J., and Uhlmann, J. K. "Unscented filtering and nonlinear estimation," IEEE Review Vol. 92, No. 3, 2004, pp. 401 -
422.
9
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 Trans. on Automatic Control Vol. 45, No. 3, 2000, pp. 477 - 482.
10
Julier, S. J., and Uhlmann, J. K. "A Consistent, Debiased Method for Converting Between Polar and Cartesian Coordinate
Systems," AeroSense: Acquisition, Tracking and Pointing XI. Orlando FL, 1997, pp. 110–121.
11
Li, Y., Wang, J., and Rizos, C. "Comparison of the Extended and Sigma-point Kalman Filters on Inertial Sensor Bias
Estimation Through Tight Integration of GPS and INS," ION GNSS 2006. Fort Worth, TX, 2006, pp. 1625-1634.
12
Wendel, J., Metzger, J., Moenikes, R., Maier, A., and Trommer, G. F. "A performance comparision of tightly coupled
GPS/INS Navigation systems based on extended and Sigma point Kalman filter," NAVIGATION Vol. 53, No. 1, 2006, pp. 21-31.
13
Farrell, J. A. Aided Navigation: GPS with High Rate Sensors. New York: Mcgraw-Hill Professional, 2008.
14
Shin, E.-H., Niu, X., and El-Sheimy, N. "Performance Comparison of the Extended and the Unscented Kalman Filter for
Integrated GPS and MEMS-Based Inertial Systems," ION NTM 2005. San Diego, CA, 2005, pp. 961-969.
15
Shin, E.-H. "A Quaternion-Based Unscented Kalman Filter for the Integration of GPS and MEMS INS," ION GNSS 2004.
Long Beach, CA, 2004, pp. 1060-1068.
16
Shin, E.-H., and El-Sheimy, N. "Unscented Kalman Filter and Attitude Errors of Low-cost Inertial Navigation Systems,"
NAVIGATION Vol. 54, No. 1, 2007, pp. 1-9.
17
Khoder, W., Fassinut-Mombot, B., and Benjelloun, M. "Quaternion Unscented Kalman Filtering for integrated Inertial
Navigation and GPS," 11th International Conference on Information Fusion, 2008 Cologne, Germany, 2008.
18
Lefferts, E. J., Markley, F. L., and Schuster, M. D. "Kalman Filtering for Spacecraft Attitude Estimation," Journal of
Guidance, Control, and Dynamics Vol. 5, No. 5, 1982, pp. 417-429.
19
Zhou, J., Yang, Y., Zhang, J., and Edwan, E., "Applying Quaternion-based Unscented Particle Filter on INS/GPS with Field
Experiments," ION GNSS 2011 (Student paper award). Portland, Oregon, 2011.
20
Hong, S., Lee, M. H., Chun, H.-H., Kwon, S.-H., and Speyer, J. L. "Observability of error States in GPS/INS integration,"
IEEE Trans. on Vehicular Technology Vol. 54, No. 2, 2005, pp. 731 - 743.
21
Ryley, J. T. W., Silson, P. M. G., Tsourdos, A., and Jordan, S. "Observability of a stationary GPS aided INS," AIAA
Guidance, Navigation, and Control Conference. Toronto, Ontario, 2010.
22
Rhee, I., Abdel-Hafez, M. F., and Speyer, J. L. "Observability of an integrated GPS/INS during maneuvers," IEEE Trans.
on Aerospace and Electronic Systems Vol. 40, No. 2, 2004, pp. 526 - 535.
23
Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. New Jersey: John Wiley & Sons,
2006.
24
Grewal, M. S., Weill, L. R., and Andrews, A. P. Global Positioning Systems, Inertial Navigation, and Integration. New
Jersey: Wiley-Interscience, 2007.

14/14
American Institute of Aeronautics and Astronautics