INS/GPS Integration Using Gaussian Sum Particle Filter - CiteSeerX

INS/GPS Integration Using Gaussian Sum Particle Filter Yukihiro Kubo*,** and Jinling Wang** * Department of Electrical and Electronic Engineering, Ritsumeikan University, Japan ** School of Surveying and Spatial Information Systems, University of New South Wales, Australia

BIOGRAPHY Yukihiro Kubo received the B.S. (1997), M.S. (1999) and Ph.D. (2002) degrees in electrical and electronic engineering from Ritsumeikan University. From 2002 to 2004, he worked in the production section of GPS carnavigation systems at Mitsubishi Electric Corp. He is presently an Associate Professor, Department of Electrical and Electronic Engineering at Ritsumeikan University, Shiga and Kyoto, Japan. He was a Senior Visiting Fellow at the School of Surveying & Spatial Information Systems, UNSW from Sep. 2007 to Sep. 2008. His research interests include GPS/GNSS signal processing and INS/GNSS integration systems. Dr Jinling Wang is a Senior Lecturer in the School of Surveying and Spatial Information Systems, UNSW. He is a Fellow of the Royal Institute of Navigation, UK, and a Fellow of the International Association (IAG) of Geodesy. Jinling is a member of the Editorial Board for the international journal GPS Solutions, and was Chairman of the study group (2003-2007) on pseudolite applications in positioning and navigation within the IAG’s Commission. He was 2004 President of the International Association of Chinese Professionals in Global Positioning Systems (CPGPS). Jinling holds a PhD in GPS/Geodesy from the Curtin University of Technology, Australia.

for updating each distribution similarly to the EKF. So there exists the possibility of degrading the filtering performance under high nonlinearity shown in the classic EKF. In this paper we apply a nonlinear filter combining the GSF and PF, which is referred to as Gaussian Sum Particle Filter (GSPF). The GSPF is based on the similar concept of the GSF, but the GSPF updates its Gaussian sum expressions by using the particles instead of the linear approximations. The performance of the GSPF based loosely coupled INS/GPS integration is compared with other filters in numerical simulations. From the simulation results, it is found that the GSPF has an ability to improve the navigation performance when there exist large uncertainties in the initial estimates. INTRODUCTION In INS/GPS integration there are nonlinear models that should be properly handled, for example, a) nonlinear state equations describing the INS error propagation; b) nonlinear measurement equations that are related to pseudoranges, carrier phases and Doppler shifts measured in the receiver (Grewal et al., 2007). Therefore the nonlinear filtering methods have been commonly applied in the INS/GPS integration to estimate the state vector. The most popular and commonly used method is the Extended Kalman Filter (EKF) which approximates the nonlinear state and measurement equations using the first order Taylor series expansion.

ABSTRACT In INS/GPS integration, the data fusion algorithm involves properly handling of nonlinear models. Therefore the nonlinear filtering methods have been commonly applied in the INS/GPS integration to estimate the state vector. The most popular and commonly used method is the Extended Kalman Filter (EKF) which approximates the nonlinear state and measurement equations using the first order Taylor series expansion. On the other hand, recently other nonlinear filters such as Particle Filter (PF), Unscented Kalman Filter (UKF) and Gaussian Sum Filter (GSF) are also considered for use in the INS/GPS integration. The GSF is a nonlinear filter where its predictive priori density is assumed to be the sum of several normal distributions. However the first order Taylor series approximation is applied in the GSF

One of the problems with the EKF is the divergence caused by the nonlinearity errors in some applications (Gelb, 1974). In recent years, to overcome the problems with the nonlinearity, other nonlinear filters are also considered for use in the INS/GPS integration, for example: 1) Particle Filter (PF), 2) Unscented Kalman Filter (UKF) and 3) Gaussian Sum Filter (GSF) (Yi et al., 2005; Aggarwal et al., 2006; Nishiyama et al., 2006; Wendel et al., 2006; Kubo et al., 2007). And it is reported (Nishiyama et al., 2006; Kubo et al., 2007) that the integrated systems with these nonlinear filters show the similar performances, i.e., producing almost the same accuracies in horizontal position and velocity while the accuracy of the heading angle can be improved. Such improvements are expected due to the fact that there are severe nonlinearities in the attitude and heading error

equations. Especially in the so-called in-motion alignment procedure of the integrated system, the GSF and the modified GSF show better performances among all the nonlinear filters mentioned above. The GSF is a nonlinear filter where its predictive priori density is assumed to be the sum of several normal distributions. Therefore, with the GSF, it works well in the applications where there exist large uncertainties in the initial values such as in-motion alignment or navigation, because we can assume some different initial distributions (initial state estimates and their variances) simultaneously. However the first order Taylor series approximation is applied in the GSF for updating each distribution similarly to the EKF (Alspach & Sorenson, 1972). So there exists the possibility of degrading the filtering performance under high nonlinearity shown in the classic EKF. On the other hand, the Gaussian Sum Particle Filter (GSPF) (Kotecha & Djuric, 2003) has been derived based on the concept of the GSF i.e. the predictive priori density is assumed to be the sum of normal distributions but the GSPF updates each distribution by using the particles propagated through the state and measurement equations without linear approximations. Therefore it is expected that the GSPF works well in the highly nonlinear integrated navigation systems such as integrated INS/GPS. However, such investigations have not been reported in literature. In this paper, we apply the GSPF to the INS/GPS integrated navigation and investigate the filter performance. In the following sections, after reviewing the integration formulae and designing the associated algorithms for the GSPF, the filter performance of the GSPF based loosely coupled integrated system is compared with the EKF, GSF and PF in numerical simulations and the performance of those nonlinear filters are discussed.

“E”CEF frame in the L frame coordinate system, ωEC / C is also the rotation rate vector similarly defined for the “C”omputer frame. The overbar such as ωEC / C means INS computed value, and (ωEL / L ×) denotes the skew-symmetric matrix defined by the vector ωEL / L . The matrix TLC is the coordinate transformation matrix from the local level to the computer frame. The velocity error δ vC is modeled by

δ vC = bC + fC × δθC + vC × (δρC + 2δ ΩC ) − ( ρC + 2ΩC ) × δ vC − (δρC + 2δ ΩC ) × δ vC + δ gC

where bC is the accelerometer bias, f C is the nongravitational specific force vector, θ C is the attitude, ρC is the relative rate vector, ΩC is the earth rate vector, gC is the gravity vector and the symbols δ in front of several components mean their errors. The attitude error model is

δθ C = δωEC / C + δω IC/ E + δθ C × ω IC/ C + dC

(3)

where dC is the gyro bias. The accelerometer and gyro bias errors are modeled as the first order Markov processes respectively. From the INS mechanization equations (1)-(3), the state vector is defined as follows. x = ⎡⎣δ rC , x , δ rC , y , δ vC , x , δ vC , y , δθC , x , δθC , y , γ , β , δ hC ,

δ vC , z , bB , x , bB , y , bB , z , d B , x , d B , y , d B , z ⎤⎦

T

In this paper, the INS and GPS are combined as a data fusion system using nonlinear filters. The system works as a GPS aided INS system, where the GPS position and velocity information is used to estimate and correct the INS position, velocity, attitude and IMU sensor errors. The system dynamics model is given by the differential equations of the INS mechanization. The INS mechanization applied in this paper is detailed in Rogers (2001, 2003) and Tanikawara et al. (2004). The position error model is formulated by R = R (ωEL / L ×) − (ωEC / C ×)(TLC + R ) + (ωEC / C ×)TLC

(1)

where R is the position error matrix which consists of functions of the INS position errors and the error of wander azimuth angle. The vector ωEL / L is the rotation rate of the “L”ocal level frame with respect to the

(4)

where the subscript C and B indicate the computer frame and the body frame, respectively. The descriptions of the Table 1: List of states

INTEGRATION ALGORITHM

(2)

No. 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15

16

Symbol δ rC , x δ rC , y δ vC , x δ vC , y δθC , x δθC , y γ

β δ hC δ vC , z bB , x bB , y bB , z d B, x d B, y d B, z

Error state X C -axis position error YC -axis position error X C -axis velocity error YC -axis velocity error X C -axis tilt error YC -axis tilt error sin δα cos δα − 1 Z C -axis altitude error Z C -axis velocity error X B -axis accelerometer bias YB -axis accelerometer bias Z B -axis accelerometer bias X B -axis gyro bias YB -axis gyro bias Z B -axis gyro bias,

state vector components are listed in Table 1. Then the discrete time state equation can be formulated by x(k + 1) = f ( x(k ), k ) + w(k )

G

p ( x(k ) | Yk −1 ) = ∑ γ j (k | k − 1) × N ⎣⎡ μ j (k | k − 1), Pj (k | k − 1) ⎦⎤

(5)

where w(k ) is the Gaussian white noise with zero mean and covariance matrix Q. The measurement equation which describes the relations between the GPS position/velocity measurements and the system states can be established by subtracting the GPS position and velocity from the INS position and velocity respectively. For example, the INS position is formulated by the sum of the true position plus the INS position error, and the GPS position is modeled as the true position corrupted by the measurement noise. By subtracting the GPS position from the INS position, the true position is canceled out and the INS position error is observed. Similarly, the INS velocity error can be observed by subtracting the GPS velocity from the INS velocity. Then the measurement equation for the navigation filters can be expressed as y (k ) = Hx(k ) + ε g (k )

(9)

j =1

where G is the number of normal distributions, and γ j is the weight for the j-th distribution such that G

∑ γ (k | k − 1) = 1, i

function with mean θ and covariance matrix Σ . Then, by the Bayesian recursion relations, a posteriori density can be formulated by G

p( x(k ) | Yk ) = ∑ γ j (k | k ) N ⎡⎣ μ j (k | k ), Pj (k | k ) ⎤⎦

(11)

j =1

where

μ j (k | k ) = μ j (k | k − 1) + K j (k ) ( y (k ) − h( μ j (k | k − 1), k ) )

Pj (k | k ) = Pj (k | k − 1) − K j (k ) H j (k ) Pj (k | k − 1) K j (k ) = Pj (k | k − 1) H j (k )

and covariance matrix R.

T

⎡ H j (k ) Pj (k | k − 1) H j (k ) T + R (k ) ⎤ ⎣ ⎦

NONLINEAR FILTERING

(10)

And N [θ , Σ ] denotes the normal probability density

(6)

where y (k ) is the measurement vector (INS minus GPS) and ε g (k ) is the Gaussian white noise with zero mean

γ i (k | k − 1) ≥ 0

j =1

−1

and

Nonlinear filtering techniques are applied to the integrated INS/GPS system to estimate the state vector (the INS errors described above). In this section, firstly we briefly review the filter algorithms of the GSF and the PF for a general nonlinear system. Then the Gaussian sum particle filter (GSPF) applied in this paper is introduced. Let us consider the following general system

x(k + 1) = f ( x(k ), k ) + w(k )

(7)

y (k ) = h( x(k ), k ) + v(k )

(8)

where f and h are the system and measurement nonlinear models, w and v are the system and measurement noises with appropriate dimensions respectively. The set of observations up to time k is denoted by Yk , i.e. Yk ≡ { y (0), y (1),… , y (k )} .

⎡ ∂h( x, k ) ⎤ H j (k ) = ⎢ ⎥ ⎣ ∂x ⎦ x = μ j ( k |k −1)

(12)

The weight γ j (k | k ) is given by

γ j (k | k ) =

γ j (k | k − 1) β j (k )

∑

{γ l (k | k − 1) β l (k )} l =1 G

(13)

where

γ j (k | k − 1) = γ j (k − 1 | k − 1) β j (k ) = N ⎡⎣⎢ν j (k | k − 1), Pν , j ⎤⎦⎥ ν j (k | k − 1) ≡ y (k ) − h( μ j (k | k − 1), k ) Pν , j ≡ H j (k ) Pj (k | k − 1) H j T (k ) + R(k )

Gaussian sum filtering

In the GSF (Alspach & Sorenson, 1972; Ristic et al., 2004), a posteriori probability density p( x(k ) | Yk ) is formed by the weighted combination of the outputs of several Kalman filters processed in parallel. The a priori density p( x(k ) | Yk −1 ) is assumed that it is formulated by the weighted sum of normal distributions as follows:

Therefore, we have the filtered estimator G

xˆ(k | k ) = ∑ γ j (k | k ) μ j (k | k )

(14)

j =1

With p( x(k ) | Yk ) expressed by the sum of Gaussian distributions as shown in Eq. (11), the a priori density p( x(k + 1) | Yk ) can be also expressed by the Gaussian sum with the same algorithm as the EKF as follows:

G

p( x(k + 1) | Yk ) = ∑ γ j (k + 1 | k )

(15)

j =1

× N ⎡⎣ μ j (k + 1 | k ), Pj (k + 1 | k ) ⎤⎦

Then a resampling procedure is implemented to eliminate the particles with less weight and to multiple the particles N with higher weights. The particles { xi (k )}i =1 are resampled and the new particles { x i (k )}i =1 with probabilities N

where

μ j (k + 1 | k ) = f ( μ j (k | k ), k )

p( xi (k ) = x j (k ) | Yk ) =

Pj (k + 1 | k ) = F j (k ) Pj (k | k ) FjT (k ) + Q(k )

α j (k )

∑

N i =1

(22)

α i (k )

are obtained. By the resampling, each weight of x1 (k ), x 2 (k ),… x N (k ) is 1 N . The filtering algorithm applied in this paper is summarized as follows (Tanikawara et al., 2004).

γ j (k + 1 | k ) = γ j (k | k )

and ⎡ ∂f ( x, k ) ⎤ Fj (k ) = ⎢ ⎥ ⎣ ∂x ⎦ x = μ j ( k |k )

(16)

1.

Set k=0 and generate initial N particles { xi (0)}i =1 N

from the initial distribution p( x(0)) . 2.

After receiving y (k ) , for i=1,…,N, compute the likelihood of the particle xi (k ) by Eq. (18).

3.

Compute the filtered estimate xˆ(k | k ) by Eq. (21).

4.

Resample the particles { xi (k )}i =1 according to the

Particle filtering

In the PF, the probability density function (pdf) p( x(k ) | Yk ) is evaluated by the set of samples of x as the numerical approximation based on the Monte Carlo method (Kitagawa, 1996; Doucet et al., 2000; Ristic et al., 2004). The samples are called particles. The filter recursively updates the particles and corresponding weights of the particles. The pdf is approximated by the collection of these particles.

a) For i=1,…,N and ν ∈ (0,1] , compute thresholds by u i = (i −ν ) N .

∑

(17)

α i (k ) = p ( y (k ) | xi (k )) = r (G ( y (k ), xi (k )))

∂G ∂y

5.

(18)

=

α i (k )

∑

α i (k ) i =1 N

(20)

Therefore the filtered estimate xˆ(k | k ) is obtained by N

i

i

i =1

N

i =1

i

i

l

(k ) < u i ≤

l =1

j

1

∑

N i =1

∑α α (k ) i

l

(k )

l =1

For i=1,…,N, predict one step ahead particles N { xi (k + 1)}i =1 according to the state equation (7) and

Repeat the steps 2-5.

(19)

and r is the probability density function of the observation noise v. For the given y (k ) , the posterior probability is calculated by

∑ α (k ) x (k ) xˆ(k | k ) = ∑ α (k )

i =1

∑α α (k )

samples of the system noise generated from the noise distribution, and compute the one step ahead estimate xˆ(k + 1| k ) by taking sample means. 6.

G ( y (k ), xi (k )) = y (k ) − h( xi (k ), k )

N

then set xi (k ) = x j (k ) .

where G is the inverse function of h such that

α i (k ) N1 i =1

j −1

1

After receiving the observation at time k, y (k ) , we can obtain the likelihood of each particle by

∑

N

i =1

b) For i=1,…,N, find index j satisfying

{ x1 (k ), x 2 (k ),…, x N (k )} ∼ p( x(k ) | Yk −1 )

N

i

with equal weight 1 N .

N

α i (k ) N1

{ x (k )}

resampling algorithm below and obtain

Now, let { xi (k )}i =1 be N particles such that

p( x(k ) = x i (k ) | Yk ) =

N

(21)

Gaussian sum particle filtering

In the GSF, we can see from Eqs. (12) and (16) that the linearization of the nonlinear functions in the state and measurement equations is applied similarly to the EKF. In order to improve the handling of the nonlinear functions, in this paper, we apply the measurement and time updating algorithms of the particle filter described above to the GSF. The combination of these two filters is referred to as Gaussian sum particle filter (Kotecha et al., 2003). Instead of banks of the EKF as used in the GSF, banks of the PF are utilized in the GSPF. Therefore the a priori density p( x(k ) | Yk −1 ) is formulated by the sum of several normal distributions as shown in Eq. (9), but the

weight γ j and mean μ j are updated by using the particles generated for each distribution. The algorithm of the GSPF applied in this paper can be summarized as follows. 1.

accelerations and decelerations. The vehicle speed at the start point was 0 [km/h] and the azimuth angle was 30 [deg]. The test trajectory in the local level horizontal plane, height and horizontal speed profiles are shown in Fig. 1.

Set k=0. For j-th (j=1,…,G) distribution, generate N initial N particles { xij (0)}i =1 from the initial

200 End

distribution p( x j (0)) .

μj 4.

∑ (k | k ) =

N i =1

For j=1,…,G, update the weights as N i =1

α ij (k )

∑ ∑ G

N

j =1

i =1

α ij (k )

then normalize the weights by

γ j (k | k ) =

γ j (k | k )

∑

G j =1

-100 0 East [m]

44

α ij (k )

∑

0

-100 -200

α i (k ) xij (k ) i =1 j

∑

100

Start

N

γ j (k | k ) = γ j (k | k − 1)

5.

North [m]

according to α ij (k ) = p( y (k ) | xij (k )) similarly to Eq. (18). For j=1,…,G, compute μ j (k | k ) by

Height [m]

3.

After receiving y (k ) , for j=1,…,G and i=1,…,M, compute the likelihood of the particle xij (k )

γ j (k | k )

Compute the filtered estimate by

Hor. speed [km/h]

2.

100

43 42 0 20

50

100

150

200

250

300

350

50

100

150 200 Time [sec]

250

300

350

10 0 0

Fig. 1: Test trajectory, height and speed profiles Table 2: Sensor error specifications

G

xˆ(k | k ) = ∑ γ j (k | k ) μ j (k | k ) j =1

6.

For j=1,…,G and i=1,…,M, resample the particles N { xij (k )}i =1 by the resampling algorithm similarly to the PF and obtain { xij (k )}i =1 .

Accelerometer Bias Scale factor Random error

Specification 80 [ μ G] 150 [ppm] 0.0003 [m/s]2

N

7.

For j=1,…,G and i=1,…,M, predict one step ahead N particles { xij (k + 1)}i =1 according to the state equation (7) and samples of the system noise generated from the noise distribution, and compute the one step ahead estimate μ j (k + 1| k ) and xˆ(k + 1| k ) by taking the sample means.

8.

For j=1,…,G, update the weights as

γ j (k + 1| k ) = γ j (k | k ) 9.

Repeat the steps 2-8.

SIMULATION RESULTS

In order to evaluate the filter performance in the INS/GPS navigation, simulation tests were carried out. In the simulations, we assumed a vehicle run at a speed of around 15 [km/h] for 6 minutes with several turns,

Gyroscope Bias Scale factor Random error

Specification 20 [deg/h] 500 [ppm] 0.06 [deg/ h ]

For the generation of the IMU data, the tactical grade sensor characteristics shown in Table 2 were assumed. For the GPS data generation, RTK (Real Time Kinematic) GPS and its typical accuracies, 6 [cm](2drms) for position and 0.03 [m/s](rms) for velocity, were assumed. The IMU and GPS data were generated at 50 [Hz] and 1 [Hz] rate, respectively. Four types of filters, EKF, GSF, PF, and GSPF were used in the simulations and were compared. Throughout the simulations, for the INS computations, the following initial errors were assumed: initial position errors 2 [m], initial velocity errors 0.04 [m/s], initial roll and pitch angle errors 0 [deg]. The nonlinearity of the INS system model usually occurs when there exist large attitude and azimuth errors. So in the simulations, the relations

value of the EKF and PF ( P (0 | −1) ). The initial estimates μ j (0 | −1) for j=1, 2, 3 were also set to 0 except for the 7th and 8th components of the state vector that represent the azimuth error. They were assumed to have the initial azimuth error estimates such that δα = −60, 0, + 60 [deg]. In the PF and GSPF, 5000 particles were utilized, i.e. M=5000.

U. pos. error [m]

N. pos. error [m]

E. pos. error [m]

Firstly, numerical simulations with a small initial azimuth error were performed. The initial azimuth error was set to 3 [deg]. The east, north and up position errors, velocity errors and azimuth angle errors are shown in Fig. 2, Fig. 3 and Fig. 4 respectively. It can be seen from these figures that, for position errors, all the filters have produced nearly identical results. Except for the very first phase (transient responses of the filters), the Up velocity error results of all the filters are almost same, and the East and North velocity errors of the EKF, GSF and GSPF are almost same up to about 250 [s]. However after 250 [s], the East velocity errors of the EKF, GSF and PF seem to have shown a better convergence to 0 than GSPF. 2

EKF

1.5

GSF

PF

GSPF

1 0.5 0 0 2

50

100

150 EKF

1.5

200

250

GSF

300 PF

350 GSPF

1 0.5 0 0 2

50

100

150 EKF

1.5

200

250

GSF

300 PF

350 GSPF

1 0.5 0 0

50

100

150 200 Time [s]

250

300

350

Fig. 2: East, North and Up position errors (initial azimuth error: 3 [deg])

E. vel. error [m/s]

N. vel. error [m/s] U. vel. error [m/s]

In the EKF and PF, the initial state estimate (normal distributed) xˆ(0 | −1) was set to 0, and covariance matrices P (0 | −1) , Q and R were configured from the nominal equipment specifications described above. In the GSF and GSPF, three normal distributions were utilized, i.e. G=3, and Pj (0 | −1) for j=1, 2, 3 were set to the same

0.4

EKF

0.2

GSF

PF

GSPF

0 -0.2 -0.4 0 0.4

50

100

150 EKF

0.2

200

250

GSF

300 PF

350 GSPF

0 -0.2 -0.4 0 0.4

50

100

150 EKF

0.2

200

250

GSF

300 PF

350 GSPF

0 -0.2 -0.4 0

50

100

150 200 Time [s]

250

300

350

Fig. 3: East, North and Up velocity errors (initial azimuth error: 3 [deg]) 4 Azimuth error [deg]

between filter performances and the initial azimuth error were mainly focused on in this paper. Therefore two different values of the initial azimuth errors: (1) small azimuth error 3 [deg] and (2) large azimuth error 30 [deg] were investigated. The filter configurations were as follows.

EKF

GSF

PF

GSPF

2 0 -2 -4 -6 0

50

100

150 200 Time [sec]

250

300

350

Fig. 4: Azimuth errors (initial azimuth error: 3 [deg])

From Fig. 4, the EKF and GSF produce the nearly identical azimuth errors and there exist large errors in the transient responses. Also from 190 to 230 [s], there exist bias errors of maximum 1.3 [deg] for the EKF and GSF. In contrast, the PF and GSPF show faster convergence, whereas the errors of GSPF vary after 230 [s]. The PF shows the best performance for the azimuth error. Next, numerical simulations with large initial azimuth error were performed. The initial azimuth error was set to 30 [deg]. The east, north and up position errors, velocity errors and azimuth angle errors are shown in Fig. 5, Fig. 6 and Fig. 7 respectively. It can be seen from these figures that the results of position and velocity errors show similar tendencies in the case of a small initial azimuth error. However there exist large errors in the transient responses of the filters, and the PF shows large east velocity errors around 180 [s]. From Fig. 7, it can be seen that the PF and GSPF quickly converge to 0, but the EKF and GSF take almost 150 [s] to converge. After 150 [s], all the filters except for the PF show nearly identical results, and the performance of the PF degrades from 180 to 220 [s].

E. pos. error [m]

N. pos. error [m] U. pos. error [m]

2

EKF

1.5

GSF

PF

GSPF

1 0.5 0 0 2

50

100

150 EKF

1.5

200

250

GSF

300 PF

350 GSPF

1 0.5 0 0 2

50

100

150 EKF

1.5

200

250

GSF

300 PF

350 GSPF

1 0.5 0 0

50

100

150 200 Time [s]

250

300

350

U. vel. error [m/s]

N. vel. error [m/s]

E. vel. error [m/s]

Fig. 5: East, North and Up position errors (initial azimuth error: 30 [deg]) 0.5

EKF

GSF

PF

GSPF

In this paper, the GSPF has been applied to the loosely coupled INS/GPS integrated system, and the filter performance has been investigated by comparing with the EKF, GSF and PF using the simulated data. From the simulation results, we can observe that the performances of all the filters are very similar after the filters converge. However in the situation when there exists large initial azimuth error, the GSPF shows faster convergence and better performance than other filters. Therefore we can consider that the GSPF has an ability to improve the navigation performance when there exist large uncertainties in the initial states as in the case of inmotion alignment applications, etc. In this paper, the numbers of normal distributions in the GSF and GSPF were fixed to 3 in the simulations, and the initial azimuth error was mainly focused on for analyzing the results. However it is possible to apply more than three distributions to the GSF and GSPF and there are more error states to be investigated in real situations. Therefore the relations between the number of distributions and the filter performances as well as the filter performances with real INS and GPS data will be investigated in the future work.

0

REFERENCES -0.5 0 0.5

50

100

150 EKF

200

250

GSF

300 PF

350 GSPF

0

-0.5 0 0.5

50

100

150 EKF

200

250

GSF

300 PF

350 GSPF

Aggarwal P., Gu D. and El-Sheimy N. (2006) Adaptive Particle Filter for INS/GPS Integration, Proc. of the Institute of Navigation, ION GNSS 2006, pp. 1606-1613, Fort Worth, TX. Alspach D. L. and Sorenson H. W. (1972) Nonlinear Bayesian Estimation Using Gaussian Sum Approximations, IEEE Trans. on Automatic Control, Vol. AC-17, No. 4, pp. 439-448.

0

-0.5 0

50

100

150 200 Time [s]

250

300

350

Fig. 6: East, North and Up velocity errors (initial azimuth error: 30 [deg])

Azimuth error [deg]

CONCLUSIONS

10 0 -10 -20 -30 -40 -50 0

EKF

GSF

PF

GSPF

Doucet A., Godsill S. J. and Andrieu C. (2000) On sequential Monte Carlo sampling methods for Bayesian filtering, Statistics and Computing, Vol. 3, pp. 197-208. Gelb A. (1974) Applied Optimal Estimation, MIT Press, Massachusetts. Grewal M. S., Weill L. R. and Andrews A. P. (2007) Global Positioning Systems, Inertial Navigation, and Integration, Second Edition, John Wiley & Sons, Hoboken. Kitagawa G. (1996) Monte Carlo Filter and Smoother for Non-Gaussian Nonlinear State Space Models, Journal of Computational and Graphical Statistics, Vol. 5, pp. 1-25.

50

100

150 200 Time [sec]

250

300

350

Fig. 7: Azimuth errors (initial azimuth error: 30 [deg])

Kotecha J. H. and Djuric M. (2003) Gaussian Sum Particle Filtering, IEEE Trans. On Signal Processing, Vol. 51, No. 10, pp. 2602-2612. Kubo Y., Sato T. ans Sugimoto S. (2007) Modified Gaussian Sum Filtering Methods for INS/GPS Integration,

Journal of Global Positioning Systems, Vol. 6, No. 1, pp. 65-73. Nishiyama M., Fujioka S., Kubo Y., Sato T. and Sugimoto S. (2006) Performance Studies of Nonlinear Filtering Methods in INS/GPS In-Motion Alignment, Proc. of the Institute of Navigation, ION GNSS 2006, pp. 2733-2742, Fort Worth, TX. Ristic B., Arulampalam S. and Gordon N. (2004) Beyond the Kalman Filter, Artech House, Boston. Rogers R. M. (2001) Large Azimuth INS Error Models for In-Motion Alignment Land-Vehicle Positioning, Proc. of the Institute of Navigation National Technical Meeting 2001, pp. 1104-1114, Long Beach, CA. Rogers R. M. (2003) Applied Mathematics in Integrated Navigation Systems, 2nd edition, AIAA, Virginia. Tanikawara M., Asaoka N., Oiwa M., Kubo Y. and Sugimoto S. (2004) Real-Time Nonlinear Filtering Methods for INS/DGPS In-Motion Alignment, Proc. Of the Institute of Navigation ION GPS/GNSS 2004, pp. 1104-1114, Long Beach, CA, USA. Wendel J., Metzger J., Moenikes R., Maier A. and Trommer G. F. (2006) A Performance Comparison of Tightly Coupled GPS/INS Navigation Systems based on Extended and Sigma Point Kalman Filters, Navigation: Journal of The Institute of Navigation, Vol. 53, No. 1, Spring. Yi Y. and Grejner-Brzezinska D. A. (2006) Nonlinear Bayesian Filter: Alternative to the Extended Kalman Filter in the GPS/INS Fusion Systems, Proc. of the Institute of Navigation, ION GNSS 2005, pp. 1391-1400, Long Beach, CA.