Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

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: 𝑠_𝑘=𝑓𝑠_𝑘−1,𝜉_𝑘−1,𝑧_𝑘=ℎ𝑠_𝑘,𝑣_𝑘, (1) where 𝑠_𝑘 represents the state vector at instant 𝑘, 𝑧_𝑘 is the observation vector of 𝑠 at instant 𝑘, and 𝑓() and ℎ() are the transition function and measurement function, respectively. 𝜉_𝑘−1 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 𝑝(𝑠_𝑘∣𝑧_1∶𝑘) by a set of particles {𝑠^𝑖_𝑘,𝑖=1,…,𝑁} with associated weights {𝑤^𝑖_𝑘,𝑖=1,…,𝑁} 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 𝑝(𝑠_𝑘∣𝑧_1∶𝑘) the solution is to sample particles 𝑠^𝑖_𝑘∼𝑞(⋅), where 𝑞(⋅) is referred to as importance density. Then, a weighted approximation to the posterior 𝑝(𝑠_𝑘∣𝑧_1∶𝑘) is given by 𝑝𝑠_𝑘∣𝑧_1∶𝑘≈^𝑁_𝑖=1𝑤^𝑖_𝑘𝛿𝑠_𝑘−𝑠^𝑖_𝑘, (2) where the normalized weights are defined as 𝑤^𝑖_𝑘∝𝑝𝑠^𝑖_𝑘∣𝑧_1∶𝑘𝑞𝑠^𝑖_𝑘∣𝑧_1∶𝑘. (3)

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.

𝑠𝑖𝑘,𝑤𝑖𝑘𝑁𝑖=1=PF𝑠𝑖𝑘−1,𝑤𝑖𝑘−1𝑁𝑖=1,𝑧𝑘 FOR =1 : N  Draw 𝑠𝑖𝑘∼𝑝𝑠𝑘∣𝑠𝑖𝑘−1  Calculate 𝑤𝑖𝑘=𝑝𝑧𝑘∣𝑠𝑖𝑘 END FOR Calculate total weight: 𝑡=sum𝑤𝑖𝑘𝑁𝑖=1 For =1 : N  Normalize: 𝑤𝑖𝑘=𝑡−1𝑤𝑖𝑘 END FOR Resample [17] 𝑠𝑖𝑘,𝑤𝑖𝑘,…𝑁𝑖=1 = RESAMPLE𝑠𝑖𝑘,𝑤𝑖𝑘𝑁𝑖=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 𝑠_𝑘=𝑠^𝑙_𝑘𝑠^𝑛_𝑘, (4) where 𝑠^𝑙_𝑘 denotes the linear state variables and 𝑠^𝑛_𝑘 denotes the nonlinear state variables. A relative general model with the properties discussed above is given by 𝑠^𝑛_𝑘+1=𝑓^𝑛_𝑘𝑠^𝑛_𝑘+𝐴^𝑛_𝑘𝑠^𝑛_𝑘𝑠^𝑙_𝑘+𝐺^𝑛_𝑘𝑠^𝑛_𝑘𝜉^𝑛_𝑘,𝑠^𝑙_𝑘+1=𝑓^𝑙_𝑘𝑠^𝑛_𝑘+𝐴^𝑙_𝑘𝑠^𝑛_𝑘𝑠^𝑙_𝑘+𝐺^𝑙_𝑘𝑠^𝑛_𝑘𝜉^𝑙_𝑘,𝑧_𝑘=ℎ_𝑘𝑠^𝑛_𝑘+𝐶_𝑘𝑠^𝑛_𝑘𝑠^𝑙_𝑘+𝑣_𝑘, (5)(6)(7) where 𝑓() and ℎ() are possible nonlinear functions. Furthermore, the noise of the system state is assumed to be white and Gaussian distributed with 𝜉_𝑘=𝜉^𝑙_𝑘𝜉^𝑛_𝑘∼𝑁0,𝑄_𝑘,𝑄_𝑘=𝑄^𝑙_𝑘𝑄_𝑘^𝑙𝑛𝑄_𝑘^𝑙𝑛𝑇𝑄^𝑛_𝑘. (8) The measurement noise is also assumed to be white and Gaussian distributed according to the following equation: 𝑣_𝑘∼𝑁0,𝑅_𝑘. (9) Furthermore, 𝑠₀ is divided as follows: 𝑠^𝑙₀∼𝑁𝑠₀,𝑃₀,𝑠^𝑛₀∼𝑝𝑠^𝑛₀, (10)(11) 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 𝑝(𝑠_𝑘∣𝑧_1∶𝑘)𝑝𝑠^𝑙_𝑘,𝑠^𝑛_𝑘∣𝑧_1∶𝑘=𝑝𝑠^𝑙_𝑘∣𝑠^𝑛_𝑘,𝑧_1∶𝑘𝑝𝑠^𝑛_𝑘∣𝑧_1∶𝑘, (12) where the linear state 𝑝(𝑠^𝑙_𝑘∣𝑠^𝑛_𝑘,𝑧_1∶𝑘) is analytically tractable whose solution can be given by the Kalman filter. Meanwhile, the nonlinear state 𝑝(𝑠^𝑛_𝑘∣𝑧_1∶𝑘) can be estimated using the particle filter. Therefore the posterior density of the whole space can be approximated to 𝑝𝑠^𝑙_𝑘,𝑠^𝑛_𝑘∣𝑧_1∶𝑘=𝑝𝑠^𝑙_𝑘∣𝑠^𝑛_𝑘,𝑧_1∶𝑘𝑝𝑠^𝑛_𝑘∣𝑧_1∶𝑘=^𝑁_𝑖=1𝑤^𝑖_𝑘𝛿𝑠^𝑛_𝑘−𝑠_𝑘^𝑛𝑗𝑁𝑠^𝑙_𝑘;𝑠_𝑘^𝑙𝑗,𝑃^𝑗_𝑘. (13) The conditional probability density functions for 𝑠^𝑙_𝑘 and 𝑠^𝑙_𝑘+1 are given by 𝑝𝑠^𝑙_𝑘∣𝑠^𝑛_𝑘,𝑧_𝑘=𝑁𝑠^𝑙_𝑘∣𝑘,𝑃_𝑘∣𝑘,𝑝𝑠^𝑙_𝑘+1∣𝑠^𝑛_𝑘+1,𝑧_𝑘=𝑁𝑠^𝑙_{𝑘+1∣𝑘},𝑃_{𝑘+1∣𝑘}, (14) where 𝑠^𝑙_𝑘∣𝑘=𝑠^𝑘_{𝑘∣𝑘−1}+𝐾_𝑘𝑧_𝑘−ℎ_𝑘−𝐶_𝑘𝑠_{𝑘∣𝑘−1},𝑃_𝑘∣𝑘=𝑃_{𝑘∣𝑘−1}−𝐾_𝑘𝑀_𝑘𝐾^𝑇_𝑘,𝑀_𝑘=𝐶_𝑘𝑃_{𝑘∣𝑘−1}𝐶^𝑇_𝑘+𝑅_𝑘,𝐾_𝑘=𝑃_{𝑘∣𝑘−1}𝐶^𝑇_𝑘𝑀_𝑘⁻¹. (15)

The formulation above updates linear state by Kalman filter when the new observation 𝑧_𝑘 is obtained. After getting the prediction of nonlinear state 𝑠^𝑛_𝑘+1, the formulas (5) and (6) can be viewed as measurement equation and prediction equation, and 𝑠^𝑙_{𝑘+1∣𝑘} and 𝑃_{𝑘+1∣𝑘} can be computed as follows: 𝑠^𝑙_{𝑘+1∣𝑘}=𝐴^𝑙_𝑘𝑠^𝐿_𝑘∣𝑘+𝐺^𝑙_𝑘𝑄_𝑘^ln𝑇𝐺^𝑛_𝑘𝑄^𝑛_𝑘⁻¹𝜂_𝑘+𝑓^𝑙_𝑘+𝐿_𝑘𝜂_𝑘−𝐴^𝑛_𝑘𝑠^𝑙_𝑘∣𝑘,𝑃_{𝑘+1∣𝑘}=𝐴^𝑙_𝑘𝑃_𝑘∣𝑘𝐴^𝑘_𝑘^𝑇+𝐺^𝑙_𝑘𝑄^𝑙_𝑘𝐺^𝑙_𝑘^𝑇−𝐿_𝑘𝑁_𝑘𝐿^𝑇_𝑘,𝑁_𝑘=𝐴^𝑛_𝑘𝑃_𝑘∣𝑘𝐴^𝑛_𝑘^𝑇+𝐺^𝑛_𝑘𝑄^𝑛_𝑘𝐺^𝑛_𝑘^𝑇,𝐿_𝑘=𝐴^𝑙_𝑘𝑃_𝑘∣𝑘𝐴^𝑛_𝑘^𝑇𝑁_𝑘⁻¹, (16) where 𝜂_𝑘=𝑠^𝑛_𝑘+1−𝑓^𝑛_𝑘,𝐴^𝑙_𝑘=𝐴^𝑙_𝑘−𝐺^𝑙_𝑘𝑄_𝑘^𝑙𝑛𝑇𝐺^𝑛_𝑘𝑄^𝑛_𝑘⁻¹𝐴^𝑛_𝑘,𝑄^𝑙_𝑘=𝑄^𝑙_𝑘−𝑄_𝑘^𝑙𝑛𝑇𝑄^𝑛_𝑘⁻¹𝑄_𝑘^𝑙𝑛, (17) 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 𝑝𝑧_𝑘∣𝑠^𝑛_𝑘,𝜂_𝑘−1=𝑁ℎ_𝑘+𝐶_𝑘𝑠^𝑙_{𝑘∣𝑘−1},𝐶_𝑘𝑃_{𝑘∣𝑘−1}𝐶^𝑇_𝑘+𝑅_𝑘𝑝𝑠^𝑛_𝑘+1∣𝑠^𝑛_𝑘,𝜂_𝑘=𝑁𝑓^𝑛_𝑘+𝐴^𝑛_𝑘𝑠^𝑙_𝑘∣𝑘,𝐴^𝑛_𝑘𝑃_𝑘∣𝑘𝐴^𝑛_𝑘^𝑇+𝐺^𝑛_𝑘𝑄^𝑛_𝑘𝐺^𝑛_𝑘^𝑇. (18)(19) A pseudocode description of the Rao-Blackwellized particle filter is supplied in Pseudocode 2.

(1) Initialization   For 𝑖=1,…,𝑁, initialize the particles, 𝑠0𝑛,(𝑖)∼𝑝𝑥𝑛0 and set 𝑠0𝑙,(𝑖),𝑃0(𝑖)=𝑠𝑙0,𝑃0 (2) FOR 𝑘>0   (2.1) PF measurement update    Compute the particle weight 𝑤𝑖𝑘 using formula (18),   (2.2) normalize the weight             𝑤𝑖𝑘=𝑤𝑖𝑘∑𝑘𝑖=1𝑤𝑖𝑘   (2.3) Particle filter time update and Kalman filter:    (a) Kalman filter measurement update using formula (15)    (b) Particle filter time update:     For 𝑖=1,…,𝑁 predict new particles according to (19)    (c) Kalman filter time update using (16)–(17)  (2.4) Set 𝐾∶=𝐾+1 and iterate from step (2.1)

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 𝑋_𝑖=(𝑥_𝑖1,𝑥_𝑖2,…,𝑥_𝑖𝑛), 𝑖=1,…,𝑚, and 𝑉_𝑖=(𝑣_𝑖1,𝑣_𝑖2,…,𝑣_𝑖𝑛), 𝑖=1,…,𝑚, 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, 𝑃_𝑖=(𝑝_𝑖1,𝑝_𝑖2,…,𝑝_𝑖𝑛) 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: 𝑉_𝑖^𝑡+1=𝑤⋅𝑉^𝑡_𝑖+𝑐₁⋅𝑟₁⋅𝑝^𝑡_𝑖−𝑋^𝑡_𝑖+𝑐₂⋅𝑟₂⋅𝐺−𝑋^𝑡_𝑖𝑋_𝑖𝑡+1,=𝑋^𝑡_𝑖+𝑉^𝑡_𝑖. (20)(21)

Here, we define 𝑟₁ and 𝑟₂ as the random numbers derived from the range [0,1], which are generated according to a uniform probability distribution 𝑈[0,1]. 𝑤 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 −1fitness=exp2𝑅_𝑘𝑧_new−𝑧_pred². (22)

Here, 𝑅_𝑘 is the observation noise covariance, 𝑧_new is the newest observation, and 𝑧_pred 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 𝑉_max 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) 𝑤^𝑖_𝑡=𝑤^𝑖_𝑡−1𝑝𝑦_𝑡∣𝑥^𝑖_𝑡,𝑤^𝑖_𝑡=𝑤^𝑖_𝑡∑^𝑁_𝑖=1𝑤^𝑖_𝑡. (23)

Then using resampling step to select and replicate the particle with large weights, 𝑥^𝑖_𝑡,1𝑁^𝑁_𝑖=1=𝑥^𝑖_𝑡,𝑤^𝑖_𝑡^𝑁_𝑖=1. (24)

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

(1) Initialization   For 𝑖=1,…,𝑁, initialize the particles, 𝑥0𝑛,(𝑖)∼𝑝𝑥𝑛0 and set 𝑥0𝑙,(𝑖),𝑃0(𝑖)=𝑥𝑙0,𝑃0 (2) FOR 𝑘>0  (2.1) PF measurement update   𝑥𝑘𝑖∗𝑁𝑖=1=PSO𝑥𝑖𝑘𝑁𝑖=1,𝑧𝑘   Compute the particle weight 𝑤𝑖𝑘 using formula (18),  (2.2) normalize the weight              𝑤𝑖𝑘=𝑤𝑖𝑘∑𝑘𝑖=1𝑤𝑖𝑘  (2.3) Particle filter time update and Kalman filter:   (a) Kalman filter measurement update using formula (15)   (b) Particle filter time update:    For 𝑖=1,…,𝑁 predict new particles according to (19)   (c) Kalman filter time update using (16)–(17)  (2.4) Set 𝐾∶=𝐾+1 and iterate from step (2.1)

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 𝑠_𝑘=𝐹𝑠_𝑘−1+Γ𝜉_𝑘−1,𝑘=1,2,…,𝐾, (25) where 𝐹=^11000100_00100001, Γ=^0.5010_00.501, 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 𝜉^∗_𝑘∼𝑁(0,𝛿²_𝜉).

The measurement equation is given by 𝑧_𝑘=tan⁻¹𝑦_𝑘𝑥_𝑘+𝑣_𝑘. (26) Inside, the measurement noise satisfies 𝑣_𝑘∼𝑁(0,𝛿²_𝑣).

The target initialization state is 𝑠₀=[−0.005,0.001,0.7,−0.005]^𝑇, process noise covariance 𝛿_𝜉=0.001, measurement noise covariance 𝛿_𝑣=0.005, Time 𝐾=60. 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 RMSE_𝑘=1𝑁^𝑁_𝑖=1𝑥^𝑖_𝑘−𝑥_𝑘²+𝑦^𝑖_𝑘−𝑦_𝑘². (27)

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 𝑘>25, 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).