Particle Filter for INS In-Motion Alignment
Yanling Hao, Zhilan Xiong and Zaigang Hu
College of Automation,
Harbin Engineering University,
Harbin, China 150001.
Email: xiongzhilan@163.com
Abstract
This paper presents a nonlinear dynamical model for the
in-motion alignment of the inertial navigation system
(INS) in the case that the observation variable is the
velocity information. It allows the initial misalignment
uncertainty. Therefore, this model is also suitable for
the transfer alignment based on the velocity matching
algorithm. Then the Gaussian particle filter (GPF) is
analyzed and utilized for the nonlinear filtering. Under
the turn maneuver, this paper analyzes and compares the
misalignment estimation error and convergence rate of
GPF with the unscented Kalman filter (UKF) when the
initial misalignment is uncertain. The results of the
simulation show that GPF is robust for the initial
misalignment, but UKF is influenced badly. When the
misalignment is large, the convergence rate of UKF will
be very slow, but GPF will not. Therefore, GPF is
suitable for INS in-motion alignment.
1 Introduction
The best known algorithm to solve the problem of
nonlinear filtering is the extended Kalman filter (EKF)
[1]. This filter, based upon the principle of linearizing
the process and observation models using Taylor series
expansions, has been successfully implemented in some
nonlinear problems. The series approximations in the
EKF algorithm can, however, lead to poor
representations of the nonlinear functions and
probability distribution of interest. As a result, this filter
can diverge or provide poor approximations. Unlike
EKF, Unscented Kalman filter (UKF) [2][3] uses the
true nonlinear models and approximate a Gaussian
distribution of the state random variable. In UKF, The
state distribution is specified using a minimal set of
deterministically chosen sample points, called Sigma
points. These points completely capture the true mean
and covariance of the Gaussian random variable [4].
Another popular algorithm to deal with the
nonlinearities is sequential Monte Carlo methods, also
known as particle filers (PF). PF relies on importance
sampling and, as a result, requires the design of
proposal distributions that can approximate the posterior
distribution reasonably well. However it can invalid
where the sensors are very accurate or data undergoes
sudden changes [4].
0-7803-9514-X/06/$20.00 ©2006 IEEE
Gaussian particle filter (GPF) [5] updates the
Gaussian approximations by using particles that are
propagated through the process and observation models
without approximations. Unlike PF, the computation-
cost resampling is unnecessary for GPF.
In the in-motion alignment and transfer alignment of
inertial navigation system (INS), many algorithms [7-
14] have been developed and analysed for the large
attitude error based on the nonlinear model, recently. By
defining the sine and cosine of azimuth misalignment as
filter states, an approximated extended psi-angle model
with large azimuth error is presented in [7]. Ref. [8]
introduces the error of sine and cosine of azimuth into
filter states for INS airstart. Ref. [9] presents an INS
error model for the large heading uncertainty and small
tilt misalignment errors by using a perturbation
approach. The heading uncertainty is solved with a
piecewise-Gaussian approximation of a posterior
density function in [9]. Ref. [10] presents a psi-angle
model for the case that the three misalignment angles
are assumed all large, and utilizes EKF to solve the
nonlinear data fusion problem. Ref. [11] and [12] utilize
the H∞ filter as the observer to estimate the attitude. In
addition, PF is used to achieve alignment on a stationary
base in [13]. UKF is utilized to achieve alignment which
allows large initial attitude error uncertainties in [14].
The alignment converged within 50s with RMS values
of 0.388° for heading. This paper utilizes the GPF to
estimate the misalignment when it is uncertainty, and
compares the estimation performance of GPF with UKF
with the turn maneuver.
The outline of the remainder of the paper is as
follows. Section II presents a nonlinear dynamical
model, which is designed for the case that the
measurement variable is the velocity of the vehicle, and
suitable for the initial misalignment uncertainty. The
briefly review of the standard PF and GPF is given in
Section III. Section IV shows the results of simulation
which aims at studying the efficiency of the large
misalignment model based on GPF, and compares it
with UKF under different motions. Section V contains
the main results of the investigation.
2 In-Motion Alignment Model
2.1 Velocity Error Model
ICIEA 2006
The true velocity differential equation in computer The differential equation of the true transformation
frame (c-frame) can be obtained by integrating the matrix C bc can be obtained by
following Coriolis force equation
[ ] [ ]
C& bc = C bc ω ibb − ω icc C bc (12)
V& = C f − 2ω + ω
c c b
( c c
)× V c
+g c
(1)
b ie ec
c
[ ] [ ]
where ω ibb and ω icc are the skew symmetric matrix
where the true velocity in c-frame is denoted by V , the of ω ibb and ω icc , respectively. ω ibb denotes the angular
true specific force in body frame (b-frame) is denoted rate of b-frame with respect to i-frame expressed in b-
by f b , g c denotes the true gravity in c-frame, C bc frame. ω icc denotes the angular rate of c-frame with
denotes the direction cosine matrix from b-frame to c- respect to i-frame expressed in c-frame.
frame. ω iec and ω ecc denote the angular rates of the earth Similarly, the differential equation of the matrix C bp
frame (e-frame) with respect to inertial frame (i-frame) provided by INS can be written as
and the c-frame with respect to e-frame expressed in c-
frame, respectively. [ ] [ ]
C& bp = Cbp ωibb + ε b − ωipp Cbp (13)
The velocity provided by INS in platform (p-frame) where ε b denotes the gyro drift, and can be considered
can be described as as constant with the Gaussian white noise,
V& p = C bp ( f b + ∇ b ) − (2ω iep + ω epp )× V p + g p (2) ε& b = wε (14)
where
ω ipp = ω icc (15)
V = V + δV
p c
(3)
Equation (4) can be written as
C bp = C cp C bc (4)
C cp = C bp C cb (16)
ω =ω p
ie
c
ie
(5) Differentiating both sides of (16), yields
C& cp = C& bp C cb + C bp C& cb (17)
ω epp = ω ecc (6)
Substituting (12) and (13) into (17), results into
g p = gc (7) [ ] [ ] [ ]
C& cp = C cp ω icc − ω icc C cp + C bp ε b C cb (18)
b
∇ is the accelerometer bias, and can be considered as It can be proved that [10]
constant with the Gaussian white noise,
∇& b = w∇ (8)
[ ] [ ] [
C bp ε b C bp = ε p = C bp ε b ] (19)

Substituting (19) into (18), yields

C cp is the direction cosine matrix from c-frame to p-
frame. The misalignment between p-frame and c-frame [ ] [ ] [
C& cp = C cp ω icc − ω icc C cp + C bp ε b C cp ] (20)
is defined as
Consequently, (20) can be simplified as
ψ = [ψ x ψ y ψ z ] [ ] [ ]
T
(9) C& cp = C cp ω icc − ω icc − C bp ε b C cp (21)
Let s i = sin(ψ i ) , c i = cos(ψ i ) , and i = x, y, z .
Therefore, C cp can be written as 2.3 Dynamical Model of In-Motion
c y c z − s y sx s z − cx sz s ycz + cy sx sz  Alignment
 
c
p
C = c y s z + s y s x c y cx c z s y s z − c y s x c z  (10) The dynamical model of INS in-motion alignment can
 − s y cx sx cycx  be given in the form of
 
X k = f ( X k −1 ) + wk (22)
Differentiating both sides of (3), and then
substituting (1) and (2) into it, yields the velocity error
Yk = h ( X k ) + v k (23)
differential equation
δV& = (I − C pc )C bp f b + C bp ∇ b − (2ω iec + ω ecc )× δV (11 where, X k and Yk are the state variable and
measurement variable at time k, respectively. wk and
) v k denote the process noise and measurement noise at
c pT
where C = C p
denotes the direction cosine matrix
c time k, respectively. They are satisfied with the
from p-frame to c-frame. Gaussian distribution, that is w ~ Ν (0, Q ) and
v ~ Ν (0, R ) . Q and R are their covariance,
2.2 Misalignment Model respectively.
The process model (22) is combined by (8), (11),
(14) and (18). The state variable X is defined as
 [
X = δV T ψT ∇b
T
εb
T T
] (24) ~ ( i ) = w ( i )  w ( i ) 
w k k ∑
M

k
(31)
The misalignment ψ can be obtained by (10). The
 i =1 
measurement model is a linear model. The measurement A recursive estimate for the weights can be given by
variable Y is defined as p( yk | xk ) p(xk | x k −1 )
w(x0:k ) ∝ w(x0:k −1 ) (32)
Y = δV% = δV + v (25) q(xk | x0:k −1 , y1:k )
~ ~ ~ In order to avoid the degeneracy of the sequential
where δV = V p − V c , V c is the velocity from the
external sensor. The navigation sensor can be GPS, importance sampling, several resampling methods are
DVL, or another INS. If the information comes from presented, such as sampling importance resampling,
another INS, the alignment is named as the transfer residual resampling and minimum variance sampling [4].
alignment, and the alignment method is the velocity The standard PF updating algorithm is summarized by
matching algorithm. Fig. 1.

3 Particle Filter Sequential importance sampling step

• For i = 1,…, M, sample from the transition
3.1 Standard Particle Filter priors

PF allows to process the nonlinear possibly non-

(
xˆ k(i ) ~ q x k | x0(:ik) −1 , y1:k )
Gaussian problem by estimating the whole posterior (
and set xˆ 0(:ik) = q x k | x 0(:ik) −1 , y1:k )
probability density function of the state given the
M
observation. It draws an set of samples x (i ) i =1 from a { } • For i = 1,…, M, evaluate and normalize the
importance weights
target distribution p (x0:k | y1:k ) , which can be
approximated as (
wk(i ) ∝ p y k | xˆ k(i ) )
M

∑ δ (x )
1 Selection step (resampling)
pˆ (x0:k | y1:k ) = − x0i :k (26)
M i =1
0:k
• Multiply/Suppress samples xˆ 0(:ik) with respect to
high/low importance weights wk(i ) to obtain M
where δ (⋅) denotes the Dirac delta function. particles x 0(:ik)
Consequently, E M ( f ) can converge as follows
1 M Fig. 1 Updating Algorithm of the Standard Particle Filter
EM ( f ) =
M i=1
( )
∑ f x0(:ik) 
N→∞
→
(27) 3.2 Gaussian Particle Filter
E( f ) = ∫ f (x0:k )p(x0:k | y1:k )dx0:k
GPF draws an set of samples x k(i ) i =1 from a target { } M

However, it is usually impossible to sample distribution p(x k | y1:k ) , which is approximated as

efficiently from the posterior density. The idea of
importance sample is to represent the posterior pˆ (xk | y1:k ) = N (xk ; µ k , Σ k ) (33)
distribution by weighted particles. The function can be where
described as
f (x0:k )w(x0:k )q(x0:k | y1:k )dx0:k N (xk ; µ k , Σ k ) =
(
exp − 0.5(xk − µk ) Σ k
T −1
(xk − µ k )) (34)
E( f ) =
∫ (28) (2π ) m
det Σ k
∫ w(x0:k )q(x0:k | y1:k )dx0:k µ k and Σ k denote the mean and covariance of xk at
where w(x 0:k ) denotes the importance weight time k, respectively, m denotes the dimension of x k .
p(x0:k | y1:k ) GPF estimates µ k and Σ k from the samples x k( i ) and
w(x0:k ) ∝ (29) their weights, which are obtained from the proposal
q(x0:k | y1:k )
( )
function q x k | x 0(:ik) −1 , y1:k , and then approximates
q(x0:k | y1:k ) denotes the proposal distribution. p (x k +1 | y1:k ) as a Gaussian, or
Consequently, we can approximate the target p (x k +1 | y1:k ) = N (x k +1 ; µ k +1 , Σ k +1 ) (35)
function by
M where
~ ( i ) f (x (i ) )
EM ( f ) = ∑ w (30) M
k 0:k 1
i =1 µ k +1 =
M
∑x (i )
k +1
(36)
~
where the normalized importance weights w (i )
are
i =1
k
obtained by
 M
4 Simulation
∑ (x )( )
1 T
Σk +1 = (i )
k +1 − µ k +1 xk(i+)1 − µ k +1 (37)
M i =1
The aim of the section is to compare the estimation
GPF approximates p (x k | y1:k ) and p (x k +1 | y1:k ) accuracy and convergence rate of misalignment using
by Gaussian densities using the sequential Monte Carlo GPF and UKF with the differential initial misalignment.
algorithm. There is no need for resampling procedure in The vehicle executes the turn maneuver in this
GPF. The convenient choice for proposal function is [5] simulation.
( )
q x k | x 0(:ik) −1 , y1:k = N (x k ; µ k , Σ k ) (38)
In the turn motion, the vehicle changes its heading
from 45o to 135o when the misalignment is small, and
Several papers use the EKF or UKF Gaussian from 45o to 225 o when large. In this simulation, the
approximation as the proposal distribution for a particle initial position is ϕ 0 = 45.78 o and λ 0 = 126 .67 o , the
filter [4][6]. However, it is not suitable for the INS initial velocity Vx 0 and V y 0 all are 7 m / s , the initial
alignment which usually is required to be achieved attitudes of the vehicle are θ 0 = 0 o , r0 = 5 o and
quickly. GPF updating algorithm is summarized by Fig. ψ 0 = 45 o , where θ0 , r0 and ψ 0 denote the initial
2. pitch, roll and heading, respectively. The parameters of
UKF are α = κ = 0 and β = 2 . The update rates of the
Sequential sampling step two filters are 10 Hz. In the simulation, the major errors
• For i = 1,…, M, sample from the transition of the inertial sensors are gyro constant drift
priors εx =εy =εz = 0.1 deg/ h and accelerometer
bias ∇ x = ∇ y = ∇ z = 10 −4 g . The random noise of
(
x k( i ) ~ q x k | x 0( :ik) −1 , y1:k ) (
gyro is the Gaussian white noise N 0, (0.01 deg/ h )2 , )
(
and set x 0(:ik) = q x k | x 0(:ik) −1 , y1:k ) and the one of accelerometer is N 0, 10− 4 g ( (2
, ))
respectively. The noise of the external velocity is also
• For i = 1,…, M, evaluate and normalize the
importance weights
(
the Gaussian white noise N 0, (0.02 m / s ) . The
2
)
motion path is showed in Fig. 3.
~ (i ) = p(yk | xk )N (x k ; µ k , Σ k )
(i ) (i )
w
q(x k | x0:k −1 , y1:k )
k (i ) ( i)

−1
~ (i )  w ~ ( j) 
M

k ∑ k 
(i )
w k =w
 j =1 
Latitude (degree)

• Estimate the mean and covariance

M
µ k = ∑ wk(i ) xk(i )
i =1

M
(
Σ k = ∑ wk(i ) xk(i ) − µ k x k(i ) − µ k )( )
T

i =1

Updating step
• For i = 1,…, M, sample from
Longitude (degree)
x k(i ) ~ N (x k ; µ k , Σ k ) Fig. 3 The turn-motion path

• For i = 1,…, M, sample from We give the simulation results under the small initial
x (i )
(
~ p x k +1 | x (i )
) misalignment ψ = 1o 1o 1o [ T
]
and the large one
k +1

• Compute the mean and covariance as

k
o
[ o o T
]
ψ = 10 10 60 , respectively. Fig. 4 and Fig. 5
show the estimated results when the misalignment is
1 M small. Fig. 4 is based on GPF, and Fig. 5 based on UKF.
µ k +1 =
M
∑x
i =1
(i )
k +1 From the two figures, the obtained conclusion is that the
convergence rate of the horizontal misalignment based
M on UKF is quicker than based on GPF. But the
∑ (x )( )
1 T
Σk +1 = (i )
k +1 − µ k +1 xk(i+)1 − µ k +1 convergence rate of the azimuth misalignment based on
M i =1 UKF is much slower. There is a constant bias in UKF
estimated azimuth misalignment. For the INS alignment,
Fig. 2 Updating Algorithm of the Gaussian Particle Filter the convergence rate and estimated accuracy of the
azimuth misalignment determines the performance of
the whole alignment process. Therefore, GPF is more
suitable than UKF for the small initial misalignment.
Similarly, Fig. 6 and Fig. 7 show the estimated results
when the misalignment is large. From the two figures,
we can find that the initial convergence rate of the
horizontal misalignment based on UKF is quicker than
based on GPF, but the estimation values of
misalignment based on GPF is smoother. It is not
obvious when the initial misalignment is small. The
estimation values can rapidly converge to the
expectation values over time for GPF. For UKF,
however, it is very slow. So GPF is more effective than
UKF for the INS in-motion alignment.
5 Conclusions
The nonlinear dynamical model for the INS In-Motion
alignment has been presented. It allows large initial
attitude error uncertainty, and uses the velocity as the
observation variable. Then GPF has been utilized for the
nonlinear filtering. The comparisons between GPF and
UKF show that GPF is robust for the initial
misalignment and can quickly converge to the
expectation values. But UKF is easily influenced by it.
Especially, when misalignment is very large, the
convergence rate of UKF will be influenced badly. The
simulation proves that GPF is more effective than UKF
for the INS in-motion alignment.
References
[1] Anderson, B.D. and Moore, J.B., “Optimal
Filtering,” Prentice-Hall, New Jersey, 1979
[2] Julier, S.J. and Uhlmann, J.K., “A General Method
for Approximating Nonlinear Transformations of
Probability Distributions,” Technical report, RRG,
Dept. of Engineering Science, University of Oxford.
[3] Julier, S.J. and Uhlmann, J.K., “A New Extension
of the Kalman Filter to Nonlinear Systems,”
Proceedings - The 11th International Symposium on
Aerospace/Defence Sensing, Simulation and
Controls, Orlando, Florida., Vol. Multi Sensor
Fusion. Tracking and Resource Management II.
[4] Rudolph van der Merwe, Arnaud Doucet, Nando de
Freitas and Eric Wan, “The Unscented Particle
Filter,” Advances in Neural Information Processing
Systems (NIPS13), published by MIT Press, editors
T. K. Leen, T. G. Dietterich and V. Tresp, Dec,
2000
[5] J.H.Kotecha and P.A. Djuric, “Gaussian Particle
Filtering,” IEEE Transactions on Signal Processing,
vol. 51, 2003, pp. 2592-2601.
[6] S.J.Julier, J.K. Uhlmann, and H.F. Durrant-Whyte,
“A New Method for the Nonlinear Transformation
of Means and Covariances in Filters and
Estimators,” IEEE Trans. Automat. Contr., vol. 45,
Mar. 2000, pp. 477-482.
[7] Scherzinger, Bruno M., “Inertial Navigator Error
Models for Large Heading Uncertainty,” Record -
IEEE PLANS, Position Location and Navigation
Symposium, 1996, pp. 477-484.
[8] Pham, Tuan Manh, “Kalman Filter Mechanization
for INS Airstart,” Proceedings IEEE/AIAA 10th
Digital Avionics Systems Conference, 1991, pp.
516-525.
[9] Dmitriyev, S.P., Stepanov, O.A. and Shepel, S.V.,
“Nonlinear Filtering Methods Application in INS
Alignment,” IEEE Transactions on Aerospace and
Electronic Systems, vol. 33, no. 1, Jan, 1997, pp.
260-274.
[10] Kong, Xiaoying, Nebot, Eduardo Mario and
Durrant-Whyte, Hugh, “Development of a Non-
linear Psi-angle Model for Large Misalignment
Errors and Its Application in INS Alignment and
Calibration,” Proceedings - IEEE International
Conference on Robotics and Automation, vol. 2,
1999, pp. 1430-1435.
[11] Yu, Myeong-Jong, Lee, Jang Gyu and Park, Chan
Gook, “Nonlinear Robust Observer Design for
Strapdown INS In-Flight Alignment,” IEEE
Transactions on Aerospace and Electronic Systems,
vol. 40, no. 3, July, 2004, pp. 797-807.
[12] Lim, You-Chol and Lyou, Joon, “Transfer
Alignment Error Compensator Design Using H∞
Filter,” Proceedings of the American Control
Conference, vol. 2, 2002, pp. 1460-1465.
[13] Zhao, Rui and Gu, Qitai, “Nonlinear Filtering
Algorithm with its Application in INS Alignment,”
IEEE Signal Processing Workshop on Statistical
Signal and Array Processing, SSAP, 2000, pp. 510-
513.
[14] Shin, Eun-Hwan and El-Sheimy, Naser, “An
Unscented Kalman Filter for In-Motion Alignment
of Low-Cost IMUs,” Record - IEEE PLANS,
Position Location and Navigation Symposium,
PLANS - 2004 Position Location and Navigation
Symposium, 2004, pp. 273-279.
 Psia x (mrad)

Psia x (mrad)
Psia y (mrad)

Psia y (mrad)
Psia z (mrad)

Psia z (mrad)
Time (sec) Time (sec)
Fig. 4 Estimated misalignments under the turn motion based on GPF Fig. 7 Estimated misalignments under the turn motion based on UKF
when the misalignment is small when the misalignment is large
Psia x (mrad)
Psia y (mrad)
Psia z (mrad)

Time (sec)
Fig. 5 Estimated misalignments under the turn motion based on UKF
when the misalignment is small
Psia x (mrad)
Psia y (mrad)
Psia z (mrad)

Time (sec)
Fig. 6 Estimated misalignments under the turn motion based on GPF
when the misalignment is large