Journal of Applied Mathematics

Volume 2013 (2013), Article ID 302170, 7 pages

http://dx.doi.org/10.1155/2013/302170

## Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

^{1}Shandong Province Key Laboratory of Robotics and Intelligent Technology, College of Information and Electrical Engineering, Shandong University of Science and Technology, Qingdao 266590, China^{2}School of Control Science and Engineering, Shandong University, Jinan 250061, China^{3}State Key Laboratory of Management and Control for Complex Systems, Institute of Automation, Chinese Academy of Sciences, Beijing 100090, China

Received 12 April 2013; Revised 22 July 2013; Accepted 30 July 2013

Academic Editor: Debasish Roy

Copyright © 2013 Zeng-Shun Zhao et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

The Rao-Blackwellized particle filter (RBPF) algorithm usually has better performance than the traditional particle filter (PF) by utilizing conditional dependency relationships between parts of the state variables to estimate. By doing so, RBPF could not only improve the estimation precision but also reduce the overall computational complexity. However, the computational burden is still too high for many real-time applications. To improve the efficiency of RBPF, the particle swarm optimization (PSO) is applied to drive all the particles to the regions where their likelihoods are high in the nonlinear area. So only a small number of particles are needed to participate in the required computation. The experimental results demonstrate that this novel algorithm is more efficient than the standard RBPF.

#### 1. Introduction

The system state estimation is widely used in many fields including statistics, economics, statistical signal processing, and engineering, such as satellite navigation, communication, radar tracking, sonar ranging, target tracking, and robot localization. Many of these problems can be written in the form of the so-called dynamic state space model. A general formulation of such a model is provided as follows: where represents the state vector at instant , is the observation vector of at instant , and and are the transition function and measurement function, respectively. and are the i.i.d. process noise sequence and measurement noise sequence. As we know, the most famous algorithms, such as the Kalman filter (KF) and the hidden Markov models (HMM), are limited to deal with the finite state spaces with linear gaussian models. However, in many applications, we must handle the time sequence estimation problems which abide by complex nonlinear, non-Gaussian distribution. To manage these problems, sequential Monte Carlo methods (SIS) [1], also known variously as bootstrap filtering [2], the condensation algorithm [3], particles filter [4], interacting particle approximations [5, 6], or survival of the fittest [7], have been introduced. In 1969, the SIS was used in the control field by Handschin and Mayne [8], but the early algorithm had a shortcoming, which was denoted by degeneration. The particle filter (PF) has not come to the attention of researchers until Gordon et al. first proposed resampling methods in 1993 [2], which solved the degeneration inherent in the earlier PF algorithms. One of the major drawbacks of PF is that sampling in high-dimensional spaces can be inefficient. If the model contains a substructure with linear equations, subject to Gaussian noise, the dependency structure can be taken advantage of to accelerate the estimator. This algorithm is known as the marginalized particle filter (MPF), which also had a reputation as the Rao-Blackwellized particle filter (RBPF) [9–16]. It is widely used in the state and parameter estimations of nonlinear dynamic systems [16], especially in the situations where dependencies within the state space can be analytically exploited.

The key idea of RBPF algorithm is to split the state space into two parts one linear state subspace and one nonlinear state subspace. The linear state subspace can be carried out using standard algorithms, such as the Kalman filter, the HMM filter, the junction tree algorithm, or other finite-dimensional optimal filters, and the nonlinear state subspace is implemented by particle filter. Therefore, the working condition where we could utilize the RBPF is the state space, which could be able to divide into two parts linear state and nonlinear state. Thus the dimensionality problem associated with the standard particle filter is alleviated, since the dimension of the state variables estimated by the particle filter is reduced. In this paper, we propose an improved RBPF that is optimized by the particle swarm, moving the particles to the region where has higher likelihood density. Compared with standard RBPF, the number of required particles can be greatly reduced.

#### 2. The Standard Particle Filter

Particle filter, as a suboptimal Bayesian filter, the fundamental idea is to recursively approximate the posterior density by a set of particles with associated weights where is the number of particles and denotes the index of current time. In practice, it is inconvenient to sample directly from the posterior density distribution the solution is to sample particles , where is referred to as importance density. Then, a weighted approximation to the posterior is given by where the normalized weights are defined as

The more detailed description of particle filter could be referred to [17].

A pseudocode description of the standard particle filter is given by Pseudocode 1.

There are several serious problems in the particle filter algorithm. The first problem is particle degeneracy. After several updating iterations, only a few particles would have significant important weights. The utmost situation is only one weight of particles almost approximate one, even others all almost to zero. The degeneracy problem means that a large computational effort devoted to updating the particles does not make any sense. The second problem is the particle impoverishment which comes from resampling. The introduction of resampling could resolve the particle degeneracy problem, but at the same time it brings the particle impoverishment. In resampling step, the particles with little weights are cast out, so the diversity of particles could be reduced. In some situations, the posterior distribution may be represented by only of the same particles, resulting in the accuracy of estimation to be very low. The only way to solve this problem depends on increasing the sample set size. But, if we provide large enough samples covering whole state space to ensure a successful estimation, the computation efficiency of particle filter would decrease tremendously and even could not meet real-time requirement. The third problem is sampling inefficient in high-dimensional spaces. In some situations, the dimension of the state vector is large; the number of particles required increased exponentially with dimension number of the state vector in order to obtain reliable estimates for the states. RBPF, as an important modification of the standard particle, is applicable to a certain class of high-dimensional state space models.

#### 3. Rao-Blackwellized Particle Filter

Rao-Blackwellisation is a technique marginalizing out some of the variables from state vector models, which are related to the Rao-Blackwell formula [16, 18]. If some conditional dependencies relationships between elements of the state vector can be analytically explicit, then it is not necessary to draw samples from the entire state space. Consider a state vector which can be partitioned according to where denotes the linear state variables and denotes the nonlinear state variables. A relative general model with the properties discussed above is given by where and are possible nonlinear functions. Furthermore, the noise of the system state is assumed to be white and Gaussian distributed with The measurement noise is also assumed to be white and Gaussian distributed according to the following equation: Furthermore, is divided as follows: where the formulas (5), (6), and (7) describe a mixed linear/nonlinear system. If the nonlinear state is known, the formula (6) describes a linear, Gaussian subsystem. So the linear state can be marginalized out from the posterior density where the linear state is analytically tractable whose solution can be given by the Kalman filter. Meanwhile, the nonlinear state can be estimated using the particle filter. Therefore the posterior density of the whole space can be approximated to The conditional probability density functions for and are given by where

The formulation above updates linear state by Kalman filter when the new observation is obtained. After getting the prediction of nonlinear state , the formulas (5) and (6) can be viewed as measurement equation and prediction equation, and and can be computed as follows: where where is covariance matrix of the linear state and nonlinear state . More references are listed in [14, 15].

The measurement equation and the prediction equation of nonlinear state are given by A pseudocode description of the Rao-Blackwellized particle filter is supplied in Pseudocode 2.

#### 4. Particle Swarm Optimization

The particle swarm optimization, as well known, is a population-based parallel evolutionary computation technique developed by Kennedy and Eberhart [19–23]. The PSO technique has its inspiration rooted in observations and simulations of social behavior of animal swarms such as bird flocks searching for corn. PSO algorithm is initialized with a population of random particles which correspond to candidate solutions. Each simple entity is assigned a random velocity according to both its own and the whole flock’s flying experiences. After evaluating the fitness function at its current location, the entities called particles are then collectively flown through hyperspace, searching for food. Finding the optimum location for the best objective function can be viewed as analogues of the evolving trajectory of such a swarm behavior.

The PSO has been found to be robust and fast in solving nonlinear, nondifferentiable, and multimodal optimization problems. The PSO algorithm can be described in mathematical formulations as below.

For a random particle swarm, there are particles in an dimensional space, denoted , , and , , where and are the position and velocity of the th particle, respectively. At each step of iterations, two main factors, and , have been defined which could reflect the objective-fitness value of one given particle or the whole swarm, representing approximation to the expected state; this to say, represents the previous best fitness value of th particle up to current step; denotes the best fitness value of all particles in the whole population. After obtaining the two best values, each particle updates its position and velocity according to the following equations:

Here, we define and as the random numbers derived from the range , which are generated according to a uniform probability distribution . denotes the inertial weight (constriction factor), and it has been proven that using this factor while limiting the maximum velocity to the dynamic range of the position variable on each dimension is a better approach [24]. and are positive constants called acceleration coefficients [21–23], which are utilized to control the velocity variable in the velocity updating step of PSO.

#### 5. PSO Optimized Rao-Blackwellized Particle Filter

First, the newest observation is taken into account by defining a PSO fitness function

Here, is the observation noise covariance, is the newest observation, and is the predicted observation. The reason why we select this formula (22) as the fitness function is that its absolute value can describe how well the newest observation could “approximate” or “fit” the predicted observation. The “best value” in Section 4 refers to the maximun value of the objective function which could correspond to the most satisfied state for our given configuration. In this paper, we prefer the value that could well approximate predicted state with lowest error between the predicted state and the real state.

The PSO moves all the particles towards the region where they have the best fitness values. But sometimes the maximum velocity of classic PSO is difficult to determine, which impacts the searching step size. Here we adopt an improved PSO-Gaussian swarm, which updates velocity based on a Gaussian distribution [20].

If all particles are distributed around high likelihood region, then the fitness value of each particle would be high. On the other side, if the best previous value of and the global best value of are both very low, it means that particles are not distributed around high likelihood area. Hence, via PSO mechanism, each particle updates the velocity and positions of according to (20) and (21). But it must be noticed that, inside the formula (20), the terms and are generated according to the Gaussian probability distribution, that is, (0, 1).

Next, each particle’s weight would be updated and normalized by (9) and (10)

Then using resampling step to select and replicate the particle with large weights,

A pseudocode description of the PSO optimizes Rao-Blackwellized particle filter which is given by Pseudocode 3.

#### 6. Experimental Results

We choose the classical bearing only tracking mode to test the novel algorithm. The basic frequency of the computer is 2.6 GHZ; EMS memory is 1 G. Hard disk capability is 80 G. The simulation software is MATLAB 7.0.

The dynamic system state equation of bearing only tracking mode is given by where , , the target state vector is , and and are position of the target in two dimensions plane. and are velocities of the target. is assumed to be Gaussian distributed noise, and satisfied .

The measurement equation is given by Inside, the measurement noise satisfies .

The target initialization state is , process noise covariance , measurement noise covariance , Time . We divide target state into nonlinear state and linear state . Where , . Root-mean-square error (RMSE) is used to describe the accuracy estimation, which is given by

The evaluations of different algorithms are displayed in Figure 1 and Table 1.

From the experimental results, we can see that RBPF-PSO uses a few particles to get the required estimation accuracy and consume the least computing time; RBPF-PSO with 20 particles could achieve higher accuracy than PF with 1000 particles and RBPF with 500 particles. The RMSE of RBPF-PSO shows the best prevision. By this simulation, we could see that it is nearly one order of magnitude promotion between RMSE of the proposed algorithm and the RMSE of standard PF. Meanwhile, the compution time spent by the proposed algorithm is much less than that the others. What is more, in Figure 1, when time , PF and RBPF both start to diverge from the real tracking trajecory.

In addition, we could draw similar conclusions by changing the particle num.

From Figure 2 and Table 2, the RBPF-PSO that uses 10 particles can achieve higher accuracy than the PF with 1000 particles and RBPF with 200 particles. RBPF-PSO uses a few particles to get the required estimation accuracy. However, the difference between Figures 1 and 2 is that, in Figure 2, the RBPF algorithm with 200 particles failed to track the tendency of the real data, beyond all expectations. Even with 10 particles, the proposed RBPF-PSO algorithm can still produce smooth and satisfied estimation accuracy. Among the three algorithms, the worst algorithm with the poorest RMSE error; and the best algorithm is still the proposed RBPF-PSO, with the smallest particle number and the best accuracy.

#### 7. Conclusion and Discussions

In this paper, particle swarm optimization Rao-Blackwellized particle filter was proposed. We used particle swarm optimization to make the particle move to regions where the likelihood of particle is high, so only a few particles were able to represent the approximate posterior density. And the efficiency of the algorithm were improved. Even when RBPF algorithm fails to estimate, the proposed RBPF-PSO algorithm can still achieve satisfied estimation accuracy with much less particle number.

Actually, we just take the top maximum value as the unique solution here. However, we think that the interested readers can take multiple local maximum as possible candidate solutions by several subswarms corporations in the implementation detail for further development.

#### Acknowledgments

This work was supported in part by the National Natural Science Foundation of China (Grants nos. 60805028, 61175076, and 61225017), Natural Science Foundation of Shandong Province (ZR2010FM027), China Postdoctoral Science Foundation (2012M521336), Open Research Project under Grant 20120105 from SKLMCCS, SDUST Research Fund (2010KYTD101), and SDUST Graduate Innovation Foundation (YC130218).

#### References

- M. N. Rosenbluth and A. W. Rosenbluth, “Monte carlo calculation of the average extension of molecular chains,”
*The Journal of Chemical Physics*, vol. 23, no. 2, pp. 356–359, 1955. View at Google Scholar · View at Scopus - N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novel approach to nonlinear/non-gaussian Bayesian state estimation,”
*IEE Proceedings F*, vol. 140, no. 2, pp. 107–113, 1993. View at Google Scholar · View at Scopus - J. MacCormick and A. Blake, “A probabilistic exclusion principle for tracking multiple objects,” in
*Proceedings of the 7th IEEE International Conference on Computer Vision (ICCV '99)*, pp. 572–578, September 1999. View at Scopus - J. Carpenter and P. Clifford, “Improved particle filter for nonlinear problems,”
*IEE Proceedings: Radar, Sonar and Navigation*, vol. 146, no. 1, pp. 2–7, 1999. View at Publisher · View at Google Scholar · View at Scopus - D. Crisan, P. del Moral, and T. Lyons, “Discrete filtering using branching and interacting particle systems,”
*Markov Processes and Related Fields*, vol. 5, no. 3, pp. 293–318, 1999. View at Google Scholar · View at Zentralblatt MATH · View at MathSciNet - P. del Moral, “Nonlinear filtering: interacting particle solution,”
*Markov Processes and Related Fields*, vol. 2, no. 4, pp. 555–579, 1996. View at Google Scholar · View at Zentralblatt MATH · View at MathSciNet - K. Kanazawa, D. Koller, and S. J. Russell, “Stochastic simulation algorithms for dynamic probabilistic networks,” in
*Proceedings of the 11th Conference on Uncertainty in Artificial Intelligence (UAI '95)*, pp. 346–351, 1995. - J. E. Handschin and D. Q. Mayne, “Monte Carlo techniques to estimate the conditional expectation in multi-stage non-linear filtering,”
*International Journal of Control*, vol. 9, no. 5, pp. 547–559, 1969. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at MathSciNet - C. Andrieu and A. Doucet, “Particle filtering for partially observed Gaussian state space models,”
*Journal of the Royal Statistical Society. Series B*, vol. 64, no. 4, pp. 827–836, 2002. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at MathSciNet · View at Scopus - A. Doucet, N. de Freitas, and N. Gordon, Eds.,
*Sequential Monte Carlo Methods in Practice*, Statistics for Engineering and Information Science, Springer, New York, NY, USA, 2001. View at MathSciNet - M. J. Daly, J. P. Reilly, and M. R. Morelande, “Rao-Blackwellised particle filtering for blind system identification,” in
*Proceedings of the IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP '05)*, pp. IV321–IV324, Philadelphia, Pa, USA, March 2005. View at Publisher · View at Google Scholar · View at Scopus - Z. Khan, T. Batch, and F. Dellaert, “A Rao-Blackwellized particle filter for eigentracking,” in
*Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR '04)*, vol. 2, pp. II980–II986, July 2004. View at Scopus - G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid mapping with Rao-Blackwellized particle filters,”
*IEEE Transactions on Robotics*, vol. 23, no. 1, pp. 34–46, 2007. View at Publisher · View at Google Scholar · View at Scopus - A. Giremus, E. Grivel, J. Grolleau, and M. Najim, “A Rao-Blackwellized particle filter for joint channel/symbol estimation in MC-DS-CDMA systems,”
*IEEE Transactions on Communications*, vol. 58, no. 8, pp. 2292–2304, 2010. View at Publisher · View at Google Scholar · View at Scopus - S. Särkkä, A. Vehtari, and J. Lampinen, “Rao-Blackwellized particle filter for multiple target tracking,”
*Information Fusion*, vol. 8, no. 1, pp. 2–15, 2007. View at Publisher · View at Google Scholar · View at Scopus - R. Sajeeb, C. S. Manohar, and D. Roy, “Rao-Blackwellization with substructuring for state and parameter estimations of a class of nonlinear dynamical systems,”
*International Journal of Engineering Under Uncertainty: Hazards, Assessment and Mitigation*, vol. 1, no. 1-2, 2009. View at Google Scholar - M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,”
*IEEE Transactions on Signal Processing*, vol. 50, no. 2, pp. 174–188, 2002. View at Publisher · View at Google Scholar · View at Scopus - G. Casella and C. P. Robert, “Rao-blackwellisation of sampling schemes,”
*Biometrika*, vol. 83, no. 1, pp. 81–94, 1996. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at MathSciNet · View at Scopus - J. Kennedy and R. Eberhart, “Particle swarm optimization,” in
*Proceedings of the IEEE International Conference on Neural Networks*, pp. 1942–1948, IEEE Service Center, Perth, Australia, December 1995. View at Scopus - R. A. Krohling, “Gaussian swarm: a novel particle swarm optimization algorithm,” in
*Proceedings of the IEEE Conference on Cybernetics and Intelligent Systems (CIS '04)*, pp. 372–376, Singapore, December 2004. View at Scopus - R. Poli, J. Kennedy, and T. Balckwell, “Particle swarm optimization,”
*Swarm Intelligence*, vol. 1, pp. 33–57, 2007. View at Google Scholar - Z.-S. Zhao, J.-Z. Wang, X.-Z. Cheng, and Y.-J. Qi, “Particle swarm optimized particle filter and its application in visual tracking,” in
*Proceedings of the 6th International Conference on Natural Computation (ICNC '10)*, pp. 2673–2676, August 2010. View at Publisher · View at Google Scholar · View at Scopus - Z. Zhao, J. Wang, Q. Tian, and M. Cao, “Particle swarm-differential evolution cooperative optimized particle filter,” in
*Proceedings of the International Conference on Intelligent Control and Information Processing (ICICIP '10)*, pp. 485–490, August 2010. View at Publisher · View at Google Scholar · View at Scopus - Y. Li, J. Liang, and J. Hu, “A multi-swarm cooperative hybrid particle swarm optimizer,” in
*Proceedings of the 6th International Conference on Natural Computation (ICNC '10)*, vol. 5, pp. 2535–2539, August 2010. View at Publisher · View at Google Scholar · View at Scopus