J. Cent. South Univ. Technol. (2010) 17: 778-785

DOI: 10.1007/s11771-010-0556-7

Federated unscented particle filtering algorithm for SINS/CNS/GPS system

HU Hai-dong(胡海东)¹, HUANG Xian-lin(黄显林)¹, LI Ming-ming(李明明)¹, SONG Zhuo-yue(宋卓越)²

1. Center for Control Theory and Guidance Technology, Harbin Institute of Technology, Harbin 150001, China;

2. School of Electrical and Electronic Engineering, University of Manchester, Manchester M60 IQD, UK

? Central South University Press and Springer-Verlag Berlin Heidelberg 2010

Key words:

navigation system; integrated navigation; unscented Kalman filter; unscented particle filter；

1 Introduction

The strapdown inertial navigation system (SINS)/ celestial navigation system (CNS)/global positioning system (GPS) system is an integrated navigation system which can associate measurement information from the SINS, CNS and GPS systems. The SINS/CNS/GPS system can utilize the advantages of  SINS, CNS and GPS adequately and overcome their shortages respectively. So, the SINS/CNS/GPS association can greatly improve the precision and reliability of navigation system.

The best known algorithm to solve the problem of  the SINS/CNS/GPS system is the Kalman filter [1-3]. The Kalman filter first introduces state space models to solve navigation system estimation problems. But the Kalman filter has the limitation of linearity and Gaussian models. So, for nonlinear/non-Gaussian models of  SINS/CNS/GPS system, a new method called the extended Kalman filter (EKF) was proposed [4-8]. This filter is based upon the principle of linearizing the state and measurement models using Taylor series expansions. The series approximations in EKF can, however, lead to poor representations of nonlinear functions and probability distributions of interest. As a result, this filter can diverge. In the middle of 1990s, motivated by the deficiencies of EKF, the unscented Kalman filter (UKF) was proposed for applying the linear estimation theory to the nonlinear Gaussian system [9-12]. Rather than to approximate the Taylor series to an arbitrary order, a set of samples are used to approximate the first three moments of the prior distribution accurately. This filter predicts the mean and covariance accurately up to the third order. And if the higher order terms in the series are not truncated, it is possible to reduce errors of the mean and covariance in the higher order terms as well.

The above methods of SINS/CNS/GPS system are based on the centralized filter. The centralized filter can result in severe computation loads when processing data from multiple sensors. Worse, when used in two stage (cascaded) filter architectures, the centralized multisensor filter can exhibit poor accuracy and unpredictable behavior under some conditions. Since the 1980s, the development of decentralized filtering methods has received increasing attention. Parallel processing technologies emphasis on fault-tolerant system design. Based on parallel processing technology, CARLSON [13] proposed the federated filter for distributed multisensor systems. In the federated filter, the master filter runs at a selectable reduced rate, fusing local filter outputs via efficient square root algorithms. The federated filter yields estimates that are globally optimal or conservatively suboptimal, depending upon the master filter processing rate. The federated unscented Kalman filter (FUKF) is composed of the federated filter and UKF. FUKF can enhance the fault detection, isolation and recovery capability of the nonlinear Gaussian SINS/CNS/GPS system.

FUKF has the limitation that it can only be utilized in the Gaussian model. Consequently, another popular solution strategy for the non-Gaussian SINS/CNS/GPS system is to use sequential Monte Carlo methods, also known as particle filters [14-20]. The particle filter allows for a complete representation of the posterior distribution of states, so that any statistical estimates, such as the mean, modes, kurtosis and variance, can be easily computed. And the particle filter has the great advantage of not being subject to the assumption of linearity or Gaussianity in the model. The particle filter relies on importance sampling, and however, it is generally impossible to sample from proposal distributions directly. Recently, several methods have been proposed to overcome the problem. For example, the EKF Gaussian estimation and the UKF Gaussian estimation are used as the proposal distribution for the particle filter [21].

In practice, SINS/CNS/GPS system not only needs to deal with the nonlinear, non-Gaussian model problem, but has the requirements of fault detection, isolation and recovery capability. Consequently, in this work, a new algorithm is proposed to use the FUKF approximation as the proposal distribution of the particle filter. We call the new algorithm as the federated unscented particle filter (FUPF). Compared with FUKF, FUPF performs better because the FUPF can deal with non-Gaussian distribution model well. Also, FUPF has fault detection, isolation and recovery capability.

2 FUKF algorithm

In recent years, FUKF has been proposed to solve the nonlinear/Gaussian multisensor integrated navigation problem. FUKF was based on the nonlinear state model and the information fusion theory. FUKF consisted of UKF and the federated filter. First, UKF was employed to estimate local filters and the estimation results of local filters were acquired. Second, local filters’ estimations were fused to acquire the system’s global optimal estimate by the federated filter. The structure of FUKF is shown in Fig.1.

FUKF was composed of the following two parts.

(1) Local filter was estimated by UKF.

(2) All local filter estimation results were fused by the federated filter.

2.1 Local filter estimated by UKF

We considered the following local filter model:

x_t=f(x_t-1, v_t-1)                                 (1)

y_t=h(u_t, x_t, n_t)                                (2)

where x_t denotes the state of the system; y_t denotes the output observations; u_t denotes the input observations; v_t denotes the process noise; and n_t denotes the measurement noise.

Because Eqs.(1) and (2) were nonlinear, the UKF algorithm was employed to estimate local filters. The UKF algorithm was a recursive minimum mean-square- error (RMMSE) estimator for filtering systems with nonlinear process and observation models. The UKF algorithm could be referred to Ref.[9].

Fig.1 Structure of FUKF

The federated filter was an estimator for decentralized, multisensor data fusion. In the federated filter, local filters run in parallel and provided sensor  data. Outputs of local filters were fused via square root algorithms. Estimates yielded by the federated filter were globally optimal or conservatively suboptimal. In this work, the federated filter was used to fuse outputs of all local UKF sensors. The federated algorithm could be referred to Ref.[13].

3 UPF and FUPF

In the SINS/CNS/GPS integrated navigation system, FUKF could solve nonlinear problems and had the capability of fault isolation and recovery. But FUKF had the limitation that it cannot be applied to general non-Gaussian distribution.

3.1 UPF algorithm

The particle filter could solve the non-Gaussian problem. The particle filter relied on the importance sampling and required the design of  proposal distribution that could approximate the posterior distribution reasonably. In general, it was hard to design such proposals. In recent years, the UKF approximation has been used to generate the proposal distribution for the particle filter, which was called the unscented particle filter (UPF) [21]. But the UPF did not satisfy the fault-tolerant requirement of SINS/CNS/ GPS system.

3.2 FUPF algorithm

In this work, a new filter, FUPF, was proposed to overcome FUKF’s Gaussian limitation and satisfy the fault-tolerant requirement of SINS/CNS/GPS system. Fig.2 shows the structure of the FUPF algorithm.

In the FUPF, the UPF estimated the local filters separately and the federated filter fused the outputs of local filters to generate the global estimation. That is to say, the FUPF combined the UPF with the federated filter to solve the problem of information fusion in the SINS/CNS/GPS system which was described by the nonlinear/non-Gaussian/fault-tolerant models. The FUPF could be illustrated as follows.

(1) Initializing with local filter

For k=1, …, M,

For i=1, …, N, sample the ith particle  from the prior p(x₀) in the kth local filter and set

                              (3)

             (4)

               (5)

                       (6)

where M is the number of local filters, and N is the number of sampled particles.

(2) Importance sampling step in local filters

For t{1, …, ∞}

For k=1, …, M,

For i=1, …, N, update the particles with the UKF in the kth local filter in time t.

Fig.2 Structure of FUPF

          (7)

(b) Updating time

                     (8)

                     (9)

   (10)

                   (11)

                     (12)

(c) Updating measurement

   (13)

      (14)

                       (15)

             (16)

               (17)

(d) Sampling

Sample

       (18)

Set

                          (19)

                         (20)

For i=1, …, N, evaluate the importance weights up to a normalising constant:

       (21)

For i=1, …, N, normalize the importance weights:

                           (22)

(3) Resampling of local filters

(a) Evaluating N_{k, eff}

                          (23)

(b) Residual resampling when N_{k, eff}＜N_thres

For i=1, …, N, set

                              (24)

and sample with new weights

                          (25)

                                 (26)

where [ ] denotes the integer part.

(c) Resetting

For i=1, …, N,

                              (27)

                               (28)

                                 (29)

(4) Fusing results of all local filters and generate the final total estimation result

                            (30)

                       (31)

(5) Output

The posterior distribution was approximated as follows.

      (32)

One obtained the following estimate of E(g_t(x_0:t)) as

          (33)

4 SINS/CNS/GPS nonlinear error model

The SINS/CNS/GPS system error model could be described as follows.

(1) State error models

       (34)

         (35)

               (36)

          (37)

                                  (38)

(2) Star sensor error models

     (39)

(3) IMU error models

        (40)

         (41)

and

                    (42)

                      (43)

(4) Measurement error models

                      (44)

                          (45)

                  (46)

where ?α is the attitude rotation vector error; δV is the velocity error; δL is the latitude error; δλ is the longitude error; δh is the height error; [δθ_x, δθ_y, δθ_z] is the star  image drift error angle; [-β_sx, -β_sy, -β_sz] is the reverse autocorrelation time constant of star sensor; [ε_θX, ε_θY, ε_θZ] is the zero-mean Gauss white noise; δf ^b is the accelerometer error;  is the accelerometer zero bias;  is the gyro error; ε is the gyro constant drift; and W_a, W_g, S, M and N are the zero-mean Gauss white noises.

5 Simulation results

To test the performance of the proposed FUPF algorithm, we compared the performance of FUPF with that of FUKF. In the experimental simulation, we designed a trajectory creator to simulate the vehicle’s flight in the “real world” environment. The vehicle trajectory creator was described as the following six phases: climb in 0-90 s, level flight in 90-300 s, left turning in 300-500 s, level flight in 500-800 s, right turning in 800-1 000 s and level flight in 1 000-1 200 s. Consequently, the trajectory creator provided the vehicle’s position, velocity and attitude, as illustrated in Table 1 and Fig.3. The SINS mechanization is shown in Fig.4.

Table 1 Parameters of vehicle’s trajectory

Fig.3 Trajectory of aircraft

The experimental simulation employed the SINS/ CNS/GPS error models that were introduced in section 4. Initial position is [30?, 110?, 0 m], initial velocity is    [0 m/s, 0 m/s, 0 m/s], initial attitude is [0?, 0?, 0?], initial position error is [0.3″, 0.3″, 12 m], initial velocity error is [0.1 m, 0.1 m, 0.1 m], and initial attitude error is [1′, 1′, 1′].

In non-Gaussian noises, one kind of representational noises was the gamma noise. And two important noises, the exponential noise and the χ² noise, belonged to the gamma noise. In simulation, the measurement noises were selected as the exponential noise and the χ² noise separately, and the state noises were drawn from the Gaussian distribution. The FUPF algorithm drew 500 particles and used the residual resampling method. The simulation results are shown in Figs.5-7 (the exponential noise) and Figs.8-10 (the χ² noise).  In these figures,  the real line ‘－’ denotes the FUPF algorithm and the broken line ‘---’ denotes the FUKF algorithm.

Fig.4 SINS mechanization

Fig.5 Estimation of position-error (exponential noise):       (a) Latitude error; (b) Longitude error; (c) Height error

Fig.5 and Fig.8 display the errors of latitude, longitude and height from the FUPF and the FUKF, and the means of the FUPF’s errors are lower compared to the means of the FUKF’s errors. For instance, Fig.5 shows that the mean of FUPF’s position-error is [0.403×10^-6 rad, 0.251×10^-6 rad, 1.36 m] and the mean of FUKF’s position-error is [0.640×10^-6 rad, 0.667× 10^-6 rad,  4.25 m]. Fig.6 and Fig.9 show the velocity- errors of the FUPF and the FUKF. It is observed that the velocity-error of the FUPF is much closer to zero than that of the FUKF. Fig.7 and Fig.10 display the attitude-errors of the FUPF and the FUKF. As expected, the attitude-error of the FUPF is less than that of the FUKF. From Figs.5-10, the FUPF has better performance than the FUKF on an average.

Fig.6 Estimation of velocity-error (exponential noise): (a) East velocity error; (b) North velocity error; (c) Up velocity error

Fig.7 Estimation of attitude-error (exponential noise): (a) Pitch error; (b) Roll error; (c) Course error

These results are not surprising. From Eq.(32)  denotes the proposal distribution produced by FUPF and from Eq.(18) denotes the Gaussian distribution by the UKF. Based on the particle filter theory,  exhibits a larger support overlap with the true posterior non-Gaussian distribution than  So, the FUPF performs better than the FUKF in attitude-error, velocity-error and position-error in the non-Gaussian models.

Fig.8 Estimation of position-error (χ² noise): (a) Latitude error; (b) Longitude error; (c) Height error

Fig.9 Estimation of velocity-error (χ² noise): (a) East velocity error; (b) North velocity error; (c) Up velocity error

Fig.10 Estimation of attitude-error (χ² noise): (a) Pitch error;  (b) Roll error; (c) Course error

6 Conclusions

(1) Simulation results show that the FUPF is feasible for solving the problem of nonlinear/ non-Gaussian system. Also, the FUPF obtains more accurate attitude, velocity and position than the FUKF in the SINS/CNS/GPS integrated navigation system described by nonlinear/non-Gaussian error models.

(2) The FUPF also has the capability of fault detection, isolation and recovery. Furthermore, the FUPF fuses the outputs of local filters in the SINS/CNS/GPS system and estimates the global results.

References

[1] KALMAN R E. A new approach to linear filtering and prediction problems [J]. Journal of Basic Engineering, 1960, 82: 35-46.

[2] FOSSEN T I, PEREZ T. Kalman filtering for positioning and heading control of ships and offshore rigs [J]. IEEE Transactions on Control Systems, 2009, 29(6): 32-46.

[3] SCHNEEBELI M, MATZLER C. A calibration scheme for microwave radiometers using tipping curves and Kalman filtering [J]. IEEE Transactions on Geoscience and Remote Sensing, 2009, 47(12): 4201-4209.

[4] YI J G, WANG H P, ZHANG J J, SONG D Z, JAYASURIYA S, LIU J T. Kinematic modeling and analysis of skid-steered mobile robots with applications to low-cost inertial-measurement-unit-based motion estimation [J]. IEEE Transactions on Robotics, 2009, 25(5): 1087-1097.

[5] SAYADI O, SHAMSOLLAHI M B. ECG denoising and compression using a modified extended Kalman filter structure [J]. IEEE Transactions on Biomedical Engineering, 2008, 55(9): 2240-2248.

[6] BOUSSAK M. Implementation and experimental investigation of sensorless speed control with initial rotor position estimation for interior permanent magnet synchronous motor drive [J]. IEEE Transactions on Power Electronics, 2005, 20(6): 1413-1422.

[7] LIGHTCAP C A, BANKS S A. An extended Kalman filter for real-time estimation and control of a rigid-link flexible-joint manipulator [J]. IEEE Transactions on Control Systems Technology, 2010, 18(1): 91-103.

[8] ALANIS A Y, SANCHEZ E N, LOUKIANOV A G, PEREZ-CISNE- ROS M A. Real-time discrete neural block control using sliding modes for electric induction motors [J]. IEEE Transactions on Control Systems Technology, 2010, 18(1): 11-21.

[9] JULIER S J, UHLMANN J K, DURRANT-WHYTEN H F. A new approach for filtering nonlinear system [C]// Processing of the American Control Conference. Washington D.C., 1995: 1628-1632.

[10] ZHAN Rong-hui, WAN Jian-wei. Neural network-aided adaptive unscented Kalman filter for nonlinear state estimation [J]. Signal Processing Letters, 2006, 13(7): 445-448.

[11] BELLOTTO N, HU H S. Multisensor-based human detection and tracking for mobile service robots [J]. Systems, Man, and Cybernetics, 2009, 39(1): 167-181.

[12] LEVEN W F, LANTERMAN A D. Unscented Kalman filters for multiple target tracking with symmetric measurement equations [J]. IEEE Transactions on Automatic Control, 2009, 54(2): 370-375.

[13] CARLSON N A. Federated square root filter for decentralized parallel processes [J]. IEEE Transactions on Aerospace and Electronic Systems, 1990, 26(3): 517-525.

[14] DOUCET A. On sequential simulation-based methods for Bayesian filtering [J]. Statistics and Computing, 2000, 10(3): 197-208.

[15] LI Y, AI H Z, YAMASHITA T, LAO S H, KAWADE M. Tracking in low frame rate video: A cascade particle filter with discriminative observers of different life spans [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2008, 30(10): 1728-1740.

[16] MARTINEZ-ESPLA J J, MARTINEZ-MARIN T, LOPEZ- SANCHEZ J M. A particle filter approach for INSAR phase filtering and unwrapping [J]. IEEE Transactions on Geoscience and Remote Sensing, 2009, 47(4): 1197-1211.

[17] OZDEMIR O, NIU R, VARSHNEY P K. Tracking in wireless sensor networks using particle filtering: Physical layer considerations [J]. IEEE Transactions on Signal Processing, 2009, 57(5): 1987-1999.

[18] NORDLUND P, GUSTAFSSON F. Marginalized particle filter for accurate and reliable terrain-aided navigation [J]. IEEE Transactions on Aerospace and Electronic Systems, 2009, 45(4): 1385-1399.

[19] HOFFMANN G M, TOMLIN C J. Mobile sensor network control using mutual information methods and particle filters [J]. IEEE Transactions on Automatic Control, 2010, 55(1): 32-47.

[20] MUSTIERE F, BOLIC M, BOUCHARD M. Speech enhancement based on nonlinear models using particle filters [J]. IEEE Transactions on Neural Networks, 2009, 20(12): 1923-1937.

[21] MERWE R V D, DOUCET A, FREITAS N D, ERIC W. The unscented particle filter [R]. Cambridge: Cambridge University, 2000.

Foundation item: Project(60535010) supported by the National Nature Science Foundation of China

Received date: 2009-09-07; Accepted date: 2010-01-29

Corresponding author: HU Hai-dong, Doctoral candidate; Tel: +86-13241459673; E-mail: haidong_2005@163.com

(Edited by YANG You-ping)