1Shandong Province Key Laboratory of Robotics and Intelligent Technology, College of Information and Electrical Engineering, Shandong University of Science and Technology, Qingdao 266590, China2School of Control Science and Engineering, Shandong University, Jinan 250061, China3State Key Laboratory of Management and Control for Complex Systems, Institute of Automation, Chinese Academy of Sciences, Beijing 100090, China

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.

Pseudocode 1: Description of the standard particle filter.

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.

Pseudocode 2: Description of the Rao-Blackwellized particle filter.

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.

Pseudocode 3: The PSO optimized Rao-Blackwellized particle filter.

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.

Table 1: Performance comparison between PF, RBPF, and RBPF-PSO.

Figure 1: The estimations of different particle filters when the regular PF diverges.

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.

Table 2: Performance comparison between PF, RBPF, and RBPF-PSO.

Figure 2: The estimates of different particle filters when the RBPF fails.

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).