^{1, 2}

^{1}

^{1}

^{1}

^{1}

^{1}

^{1}

^{3}

^{3}

^{1}

^{2}

^{3}

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.

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:

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.

Particle filter, as a suboptimal Bayesian filter, the fundamental idea is to recursively approximate the posterior density

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

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

FOR

Draw

Calculate

END FOR

Calculate total weight:

For

Normalize:

END FOR

Resample [

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

Rao-Blackwellisation is a technique marginalizing out some of the variables from state vector models, which are related to the Rao-Blackwell formula [

The formulation above updates linear state by Kalman filter when the new observation

The measurement equation and the prediction equation of nonlinear state are given by

(1) Initialization

For

(2) FOR

(2.1) PF measurement update

Compute the particle weight

(2.2) normalize the weight

(2.3) Particle filter time update and Kalman filter:

(a) Kalman filter measurement update using formula (

(b) Particle filter time update:

For

(c) Kalman filter time update using (

(2.4) Set

The particle swarm optimization, as well known, is a population-based parallel evolutionary computation technique developed by Kennedy and Eberhart [

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

Here, we define

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

Here,

The PSO moves all the particles towards the region where they have the best fitness values. But sometimes the maximum velocity

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

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

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

(1) Initialization

For

(2) FOR

(2.1) PF measurement update

Compute the particle weight

(2.2) normalize the weight

(2.3) Particle filter time update and Kalman filter:

(a) Kalman filter measurement update using formula (

(b) Particle filter time update:

For

(c) Kalman filter time update using (

(2.4) Set

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

The measurement equation is given by

The target initialization state is

The evaluations of different algorithms are displayed in Figure

Performance comparison between PF, RBPF, and RBPF-PSO.

Algorithm | RMSE | Time (S) | Number of particles |
---|---|---|---|

PF | 0.14298 | 19.625 | 1000 |

RBPF | 0.019902 | 16.078 | 500 |

RBPF-PSO | 0.015079 | 2.297 | 20 |

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

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

From Figure

Performance comparison between PF, RBPF, and RBPF-PSO.

Algorithm | RMSE | Time (S) | Number of particles |
---|---|---|---|

PF | 0.11154 | 20.156 | 1000 |

RBPF | 0.23974 | 16.766 | 200 |

RBPF-PSO | 0.087333 | 9.343 | 10 |

The estimates of different particle filters when the RBPF fails.

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.

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