research article improved rao-blackwellized particle...

Hindawi Publishing CorporationJournal of Applied MathematicsVolume 2013, Article ID 302170, 7 pageshttp://dx.doi.org/10.1155/2013/302170

Research ArticleImproved Rao-Blackwellized Particle Filter byParticle Swarm Optimization

Zeng-Shun Zhao,1,2 Xiang Feng,1 Yan-yan Lin,1 Fang Wei,1 Shi-Ku Wang,1

Tong-Lu Xiao,1 Mao-Yong Cao,1 Zeng-Guang Hou,3 and Min Tan3

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, China3 State Key Laboratory of Management and Control for Complex Systems, Institute of Automation, Chinese Academy of Sciences,Beijing 100090, China

Correspondence should be addressed to Zeng-Shun Zhao; [email protected]

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 AttributionLicense, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properlycited.

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

1. Introduction

The system state estimation is widely used in many fieldsincluding statistics, economics, statistical signal processing,and engineering, such as satellite navigation, communication,radar tracking, sonar ranging, target tracking, and robotlocalization. Many of these problems can be written in theform of the so-called dynamic state space model. A generalformulation of such a model is provided as follows:

𝑠𝑘 = 𝑓 (𝑠𝑘−1, 𝜉𝑘−1) ,

𝑧𝑘 = ℎ (𝑠𝑘, V𝑘) ,(1)

where 𝑠𝑘 represents the state vector at instant 𝑘, 𝑧𝑘 is theobservation vector of 𝑠 at instant 𝑘, and 𝑓() and ℎ() arethe transition function and measurement function, respec-tively. 𝜉𝑘−1 and V𝑘 are the i.i.d. process noise sequence andmeasurement noise sequence. As we know, the most famousalgorithms, such as the Kalman filter (KF) and the hidden

Markov models (HMM), are limited to deal with the finitestate spaces with linear gaussian models. However, in manyapplications, we must handle the time sequence estimationproblems which abide by complex nonlinear, non-Gaussiandistribution. To manage these problems, sequential MonteCarlo methods (SIS) [1], also known variously as bootstrapfiltering [2], the condensation algorithm [3], particles filter[4], interacting particle approximations [5, 6], or survivalof the fittest [7], have been introduced. In 1969, the SISwas used in the control field by Handschin and Mayne[8], but the early algorithm had a shortcoming, which wasdenoted by degeneration. The particle filter (PF) has notcome to the attention of researchers until Gordon et al. firstproposed resampling methods in 1993 [2], which solved thedegeneration inherent in the earlier PF algorithms.One of themajor drawbacks of PF is that sampling in high-dimensionalspaces can be inefficient. If the model contains a substruc-ture with linear equations, subject to Gaussian noise, thedependency structure can be taken advantage of to accelerate

2 Journal of Applied Mathematics

the estimator. This algorithm is known as the marginalizedparticle filter (MPF), which also had a reputation as the Rao-Blackwellized particle filter (RBPF) [9–16]. It is widely usedin the state and parameter estimations of nonlinear dynamicsystems [16], especially in the situations where dependencieswithin the state space can be analytically exploited.

The key idea of RBPF algorithm is to split the state spaceinto two parts one linear state subspace and one nonlinearstate subspace. The linear state subspace can be carried outusing standard algorithms, such as the Kalman filter, theHMM filter, the junction tree algorithm, or other finite-dimensional optimal filters, and the nonlinear state subspaceis implemented by particle filter. Therefore, the workingcondition where we could utilize the RBPF is the state space,which could be able to divide into two parts linear state andnonlinear state. Thus the dimensionality problem associatedwith the standard particle filter is alleviated, since the dimen-sion of the state variables estimated by the particle filter isreduced. In this paper, we propose an improved RBPF thatis optimized by the particle swarm, moving the particles tothe region where has higher likelihood density. Comparedwith standard RBPF, the number of required particles can begreatly reduced.

2. The Standard Particle Filter

Particle filter, as a suboptimal Bayesian filter, the fundamentalidea 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 isinconvenient to sample directly from the posterior densitydistribution 𝑝(𝑠𝑘 | 𝑧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 bereferred to [17].

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

There are several serious problems in the particle filteralgorithm. The first problem is particle degeneracy. Afterseveral updating iterations, only a few particles would havesignificant important weights. The utmost situation is onlyone weight of particles almost approximate one, even othersall almost to zero. The degeneracy problem means that alarge computational effort devoted to updating the parti-cles does not make any sense. The second problem is theparticle impoverishment which comes from resampling. The

introduction of resampling could resolve the particle degen-eracy problem, but at the same time it brings the particleimpoverishment. In resampling step, the particles with littleweights are cast out, so the diversity of particles could bereduced. In some situations, the posterior distribution maybe represented by only 𝑁 of the same particles, resultingin the accuracy of estimation to be very low. The only wayto solve this problem depends on increasing the sample setsize. But, if we provide large enough samples covering wholestate space to ensure a successful estimation, the computationefficiency of particle filter would decrease tremendouslyand even could not meet real-time requirement. The thirdproblem is sampling inefficient in high-dimensional spaces.In some situations, the dimension of the state vector is large;the number of particles required increased exponentiallywith dimension number of the state vector in order toobtain reliable estimates for the states. RBPF, as an importantmodification of the standard particle, is applicable to a certainclass of high-dimensional state space models.

3. Rao-Blackwellized Particle Filter

Rao-Blackwellisation is a technique marginalizing out someof the variables from state vector models, which are relatedto the Rao-Blackwell formula [16, 18]. If some conditionaldependencies relationships between elements of the statevector can be analytically explicit, then it is not necessary todraw samples from the entire state space. Consider a statevector 𝑠𝑘 which can be partitioned according to

𝑠𝑘 =[

[

𝑠𝑙

𝑘

𝑠𝑛

𝑘

]

]

, (4)

where 𝑠𝑙𝑘denotes the linear state variables and 𝑠𝑛

𝑘denotes the

nonlinear state variables. A relative general model with theproperties discussed above is given by

𝑠𝑛

𝑘+1= 𝑓𝑛

𝑘(𝑠𝑛

𝑘) + 𝐴𝑛

𝑘(𝑠𝑛

𝑘) 𝑠𝑙

𝑘+ 𝐺𝑛

𝑘(𝑠𝑛

𝑘) 𝜉𝑛

𝑘, (5)

𝑠𝑙

𝑘+1= 𝑓𝑙

𝑘(𝑠𝑛

𝑘) + 𝐴𝑙

𝑘(𝑠𝑛

𝑘) 𝑠𝑙

𝑘+ 𝐺𝑙

𝑘(𝑠𝑛

𝑘) 𝜉𝑙

𝑘, (6)

𝑧𝑘 = ℎ𝑘 (𝑠𝑛

𝑘) + 𝐶𝑘 (𝑠

𝑛

𝑘) 𝑠𝑙

𝑘+ V𝑘, (7)

where 𝑓() and ℎ() are possible nonlinear functions. Further-more, the noise of the system state is assumed to be white andGaussian distributed with

𝜉𝑘 = [𝜉𝑙

𝑘

𝜉𝑛

𝑘

] ∼ 𝑁 (0, 𝑄𝑘) , 𝑄𝑘 = [

𝑄𝑙

𝑘𝑄𝑙𝑛

𝑘

(𝑄𝑙𝑛

𝑘)

𝑇

𝑄𝑛

𝑘

] . (8)

Themeasurement noise is also assumed to bewhite andGaus-sian distributed according to the following equation:

V𝑘 ∼ 𝑁 (0, 𝑅𝑘) . (9)

Furthermore, 𝑠0 is divided as follows:

𝑠𝑙

0∼ 𝑁(𝑠0, 𝑃0) , (10)

𝑠𝑛

0∼ 𝑝 (𝑠

𝑛

0) , (11)

Journal of Applied Mathematics 3

⌊{𝑠𝑖

𝑘, 𝑤𝑖

𝑘}

𝑁

𝑖=1⌋ = PF ⌊{𝑠𝑖

𝑘−1, 𝑤𝑖

𝑘−1}

𝑁

𝑖=1, 𝑧𝑘⌋

FOR = 1 :NDraw 𝑠𝑖

𝑘∼ 𝑝 (𝑠𝑘 | 𝑠

𝑖

𝑘−1)

Calculate 𝑤𝑖𝑘= 𝑝 (𝑧𝑘 | 𝑠

𝑖

𝑘)

END FOR

Calculate total weight: 𝑡 = sum ⌊{𝑤𝑖𝑘}

𝑁

𝑖=1⌋

For = 1 :NNormalize: 𝑤𝑖

𝑘= 𝑡−1𝑤𝑖

𝑘

END FORResample [17]

⌊{𝑠𝑖

𝑘, 𝑤𝑖

𝑘, . . .}

𝑁

𝑖=1⌋ = RESAMPLE ⌊{𝑠𝑖

𝑘, 𝑤𝑖

𝑘}

𝑁

𝑖=1⌋

Pseudocode 1: Description of the standard particle filter.

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 thelinear 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 wholespace 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𝐶𝑇

𝑘𝑀−1

𝑘.

(15)

The formulation above updates linear state by Kalmanfilter when the new observation 𝑧𝑘 is obtained. After gettingthe prediction of nonlinear state 𝑠𝑛

𝑘+1, the formulas (5) and

(6) can be viewed as measurement equation and predictionequation, and 𝑠𝑙

𝑘+1|𝑘and 𝑃𝑘+1|𝑘 can be computed as follows:

𝑠𝑙

𝑘+1|𝑘= 𝐴

𝑙

𝑘𝑠𝐿

𝑘|𝑘+ 𝐺𝑙

𝑘(𝑄

ln𝑘)

𝑇

(𝐺𝑛

𝑘𝑄𝑛

𝑘)−1𝜂𝑘

+ 𝑓𝑙

𝑘+ 𝐿𝑘 (𝜂𝑘 − 𝐴

𝑛

𝑘𝑠𝑙

𝑘|𝑘) ,

𝑃𝑘+1|𝑘 = 𝐴

𝑙

𝑘𝑃𝑘|𝑘(𝐴

𝑘

𝑘)

𝑇

+ 𝐺𝑙

𝑘𝑄𝑙

𝑘(𝐺𝑙

𝑘)

𝑇

− 𝐿𝑘𝑁𝑘𝐿𝑇

𝑘,

𝑁𝑘 = 𝐴𝑛


𝑛

𝑘)𝑇+ 𝐺𝑛

𝑘𝑄𝑛

𝑘(𝐺𝑛

𝑘)𝑇,

𝐿𝑘 = 𝐴

𝑙


𝑛

𝑘)𝑇𝑁−1

𝑘,

(16)

where

𝜂𝑘 = 𝑠𝑛

𝑘+1− 𝑓𝑛

𝑘,

𝐴

𝑙

𝑘= 𝐴𝑙

𝑘− 𝐺𝑙

𝑘(𝑄𝑙𝑛

𝑘)

𝑇

(𝐺𝑛

𝑘𝑄𝑛

𝑘)−1𝐴𝑛

𝑘,

𝑄

𝑙

𝑘= 𝑄𝑙

𝑘− (𝑄𝑙𝑛

𝑘)

𝑇

(𝑄𝑛

𝑘)−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 equationof nonlinear state are given by

𝑝 (𝑧𝑘 | 𝑠𝑛

𝑘, 𝜂𝑘−1) = 𝑁 (ℎ𝑘 + 𝐶𝑘𝑠

𝑙

𝑘|𝑘−1, 𝐶𝑘𝑃𝑘|𝑘−1𝐶

𝑇

𝑘+ 𝑅𝑘)

(18)

𝑝 (𝑠𝑛

𝑘+1| 𝑠𝑛

𝑘, 𝜂𝑘)

= 𝑁 (𝑓𝑛

𝑘+ 𝐴𝑛

𝑘𝑠𝑙

𝑘|𝑘, 𝐴𝑛


𝑛

𝑘)𝑇+ 𝐺𝑛

𝑘𝑄𝑛

𝑘(𝐺𝑛

𝑘)𝑇) .

(19)

A pseudocode description of the Rao-Blackwellized particlefilter is supplied in Pseudocode 2.

4. Particle Swarm Optimization

The particle swarm optimization, as well known, is a pop-ulation-based parallel evolutionary computation techniquedeveloped by Kennedy and Eberhart [19–23]. The PSO


(1) InitializationFor 𝑖 = 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.

technique has its inspiration rooted in observations andsimulations of social behavior of animal swarms such asbird flocks searching for corn. PSO algorithm is initializedwith a population of random particles which correspond tocandidate solutions. Each simple entity is assigned a randomvelocity according to both its own and the whole flock’sflying experiences. After evaluating the fitness function atits current location, the entities called particles are thencollectively flown through hyperspace, searching for food.Finding the optimum location for the best objective functioncan be viewed as analogues of the evolving trajectory of sucha swarm behavior.

The PSO has been found to be robust and fast in solvingnonlinear, nondifferentiable, and multimodal optimizationproblems.The PSO algorithm can be described inmathemat-ical formulations as below.

For a random particle swarm, there are 𝑚 particles in an𝑛 dimensional space, denoted 𝑋𝑖 = (𝑥𝑖1, 𝑥𝑖2, . . . , 𝑥𝑖𝑛), 𝑖 =1, . . . , 𝑚, and𝑉𝑖 = (V𝑖1, V𝑖2, . . . , V𝑖𝑛), 𝑖 = 1, . . . , 𝑚, where𝑋𝑖 and𝑉𝑖 are the position and velocity of the 𝑖th particle, respectively.At each step of iterations, two main factors, 𝑃𝑖 and 𝐺, havebeen definedwhich could reflect the objective-fitness value ofone given particle or the whole swarm, representing approxi-mation to the expected state; this to say,𝑃𝑖 = (𝑝𝑖1, 𝑝𝑖2, . . . , 𝑝𝑖𝑛)represents the previous best fitness value of 𝑖th particle upto current step; 𝐺 = (𝑔1, 𝑔2, . . . , 𝑔𝑛) denotes the best fitnessvalue of all particles in the whole population. After obtainingthe two best values, each particle updates its position andvelocity according to the following equations:

𝑉𝑡+1

𝑖= 𝑤 ⋅ 𝑉

𝑡

𝑖+ 𝑐1 ⋅ 𝑟1 ⋅ (𝑝

𝑡

𝑖− 𝑋𝑡

𝑖) + 𝑐2 ⋅ 𝑟2 ⋅ (𝐺 − 𝑋

𝑡

𝑖) , (20)

𝑋𝑡+1

𝑖= 𝑋𝑡

𝑖+ 𝑉𝑡

𝑖. (21)

Here, we define 𝑟1 and 𝑟2 as the random numbers derivedfrom the range [0, 1], which are generated according toa uniform probability distribution 𝑈 [0, 1]. 𝑤 denotes theinertial weight (constriction factor), and it has been proventhat using this factor while limiting the maximum velocity tothe dynamic range of the position variable on each dimension

is a better approach [24]. 𝑐1 and 𝑐2 are positive constants calledacceleration coefficients [21–23], which are utilized to controlthe velocity variable in the velocity updating step of PSO.

5. PSO Optimized Rao-BlackwellizedParticle Filter

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

fitness = exp [− 1

2𝑅𝑘

(𝑧new − 𝑧pred)2

] . (22)

Here, 𝑅𝑘 is the observation noise covariance, 𝑧new is thenewest observation, and 𝑧pred is the predicted observation.The reason why we select this formula (22) as the fitnessfunction is that its absolute value can describe how well thenewest observation could “approximate” or “fit” the predictedobservation. The “best value” in Section 4 refers to the max-imun value of the objective function which could correspondto the most satisfied state for our given configuration. Inthis paper, we prefer the value that could well approximatepredicted state with lowest error between the predicted stateand the real state.

The PSO moves all the particles towards the regionwhere they have the best fitness values. But sometimes themaximum velocity 𝑉max of classic PSO is difficult to deter-mine, which impacts the searching step size. Here we adoptan improved PSO-Gaussian swarm, which updates velocitybased on a Gaussian distribution [20].

If all particles are distributed around high likelihoodregion, then the fitness value of each particle would be high.On the other side, if the best previous value of 𝑃𝑖 and theglobal best value of 𝐺 are both very low, it means thatparticles are not distributed around high likelihood area.Hence, via PSOmechanism, each particle updates the velocityand positions of according to (20) and (21). But it must benoticed that, inside the formula (20), the terms 𝑟1 and 𝑟2 aregenerated according to the Gaussian probability distribution,that is,𝑁 (0, 1).


(1) InitializationFor 𝑖 = 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.

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

𝑤𝑖

𝑡= 𝑤𝑖

𝑡−1𝑝 (𝑦𝑡 | 𝑥

𝑖

𝑡) ,

𝑤𝑖

𝑡=

𝑤𝑖

𝑡

∑𝑁

𝑖=1𝑤𝑖𝑡

.

(23)

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

{𝑥𝑖

𝑡,

1

𝑁

}

𝑁

𝑖=1

= {𝑥𝑖

𝑡, 𝑤𝑖

𝑡}

𝑁

𝑖=1. (24)

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 testthe novel algorithm. The basic frequency of the computer is2.6GHZ; EMS memory is 1 G. Hard disk capability is 80G.The simulation software is MATLAB 7.0.

The dynamic system state equation of bearing onlytracking mode is given by

𝑠𝑘 = 𝐹𝑠𝑘−1 + Γ𝜉𝑘−1, 𝑘 = 1, 2, . . . , 𝐾, (25)

where 𝐹 = [1 1 0 00 1 0 00 0 1 00 0 0 1

], Γ = [0.5 01 00 0.50 1

], 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, 𝛿

2

𝜉).

The measurement equation is given by

𝑧𝑘 = tan−1 (𝑦𝑘

𝑥𝑘

) + V𝑘. (26)

Inside, the measurement noise satisfies V𝑘 ∼ 𝑁(0, 𝛿2

V).

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

Algorithm RMSE Time (S) Number of particlesPF 0.14298 19.625 1000RBPF 0.019902 16.078 500RBPF-PSO 0.015079 2.297 20

The target initialization state is 𝑠0 = [−0.005, 0.001, 0.7,−0.005]

𝑇, process noise covariance 𝛿𝜉 = 0.001, measurementnoise covariance 𝛿V = 0.005, Time 𝐾 = 60. We divide targetstate 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

√(𝑥𝑖

𝑘− 𝑥𝑘)2+ (𝑦𝑖

𝑘− 𝑦𝑘)2. (27)

The evaluations of different algorithms are displayed inFigure 1 and Table 1.

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

In addition, we could draw similar conclusions by chang-ing the particle num.

From Figure 2 and Table 2, the RBPF-PSO that uses 10particles can achieve higher accuracy than the PF with 1000


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

Algorithm RMSE Time (S) Number of particlesPF 0.11154 20.156 1000RBPF 0.23974 16.766 200RBPF-PSO 0.087333 9.343 10

−0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8−2.5

−2

−1.5

−1

−0.5

0

0.5

1

X

Y

True xPF estimate

RBPF estimateRBPF-PSO estimate

Filter estimates (posterior means) versus true state

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

particles and RBPF with 200 particles. RBPF-PSO uses a fewparticles 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 thetendency of the real data, beyond all expectations. Even with10 particles, the proposed RBPF-PSO algorithm can still pro-duce smooth and satisfied estimation accuracy. Among thethree algorithms, the worst algorithmwith the poorest RMSEerror; 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-Blackwellizedparticle filter was proposed. We used particle swarm opti-mization to make the particle move to regions where thelikelihood of particle is high, so only a few particles wereable to represent the approximate posterior density. Andthe efficiency of the algorithm were improved. Even whenRBPF algorithm fails to estimate, the proposed RBPF-PSOalgorithm can still achieve satisfied estimation accuracy withmuch less particle number.

Actually, we just take the top maximum value as theunique solution here. However, we think that the interestedreaders can take multiple local maximum as possible can-didate solutions by several subswarms corporations in theimplementation detail for further development.

−0.7 −0.6 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1−3.5

−3

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

X

Y

True xPF estimate

RBPF estimateRBPF-PSO estimate

Filter estimates (posterior means) versus true state

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

Acknowledgments

This work was supported in part by the National Natu-ral Science Foundation of China (Grants nos. 60805028,61175076, and 61225017), Natural Science Foundation ofShandongProvince (ZR2010FM027), China Postdoctoral Sci-ence Foundation (2012M521336), Open Research Projectunder Grant 20120105 from SKLMCCS, SDUST ResearchFund (2010KYTD101), and SDUST Graduate InnovationFoundation (YC130218).

References

[1] M. N. Rosenbluth and A. W. Rosenbluth, “Monte carlo calcula-tion of the average extension of molecular chains,” The Journalof Chemical Physics, vol. 23, no. 2, pp. 356–359, 1955.

[2] N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novelapproach to nonlinear/non-gaussian Bayesian state estimation,”IEE Proceedings F, vol. 140, no. 2, pp. 107–113, 1993.

[3] J. MacCormick and A. Blake, “A probabilistic exclusion princi-ple for tracking multiple objects,” in Proceedings of the 7th IEEEInternational Conference on Computer Vision (ICCV ’99), pp.572–578, September 1999.

[4] J. Carpenter and P. Clifford, “Improved particle filter for non-linear problems,” IEE Proceedings: Radar, Sonar and Navigation,vol. 146, no. 1, pp. 2–7, 1999.

[5] D. Crisan, P. del Moral, and T. Lyons, “Discrete filtering usingbranching and interacting particle systems,” Markov Processesand Related Fields, vol. 5, no. 3, pp. 293–318, 1999.

[6] P. del Moral, “Nonlinear filtering: interacting particle solution,”Markov Processes and Related Fields, vol. 2, no. 4, pp. 555–579,1996.

[7] K. Kanazawa, D. Koller, and S. J. Russell, “Stochastic simulationalgorithms for dynamic probabilistic networks,” in Proceedingsof the 11th Conference on Uncertainty in Artificial Intelligence(UAI ’95), pp. 346–351, 1995.


[8] J. E. Handschin and D. Q. Mayne, “Monte Carlo techniques toestimate the conditional expectation in multi-stage non-linearfiltering,” International Journal of Control, vol. 9, no. 5, pp. 547–559, 1969.

[9] C. Andrieu and A. Doucet, “Particle filtering for partiallyobserved Gaussian state space models,” Journal of the Royal Sta-tistical Society. Series B, vol. 64, no. 4, pp. 827–836, 2002.

[10] A. Doucet, N. de Freitas, andN. Gordon, Eds., SequentialMonteCarlo Methods in Practice, Statistics for Engineering and Infor-mation Science, Springer, New York, NY, USA, 2001.

[11] M. J. Daly, J. P. Reilly, andM. R. Morelande, “Rao-Blackwellisedparticle filtering for blind system identification,” in Proceedingsof the IEEE International Conference on Acoustics, Speech, andSignal Processing (ICASSP ’05), pp. IV321–IV324, Philadelphia,Pa, USA, March 2005.

[12] Z. Khan, T. Batch, and F. Dellaert, “A Rao-Blackwellized particlefilter for eigentracking,” in Proceedings of the IEEE ComputerSociety Conference on Computer Vision and Pattern Recognition(CVPR ’04), vol. 2, pp. II980–II986, July 2004.

[13] G. Grisetti, C. Stachniss, andW. Burgard, “Improved techniquesfor grid mapping with Rao-Blackwellized particle filters,” IEEETransactions on Robotics, vol. 23, no. 1, pp. 34–46, 2007.

[14] A. Giremus, E. Grivel, J. Grolleau, andM. Najim, “A Rao-Black-wellized particle filter for joint channel/symbol estimation inMC-DS-CDMA systems,” IEEE Transactions on Communica-tions, vol. 58, no. 8, pp. 2292–2304, 2010.

[15] S. Sarkka, A. Vehtari, and J. Lampinen, “Rao-Blackwellized par-ticle filter for multiple target tracking,” Information Fusion, vol.8, no. 1, pp. 2–15, 2007.

[16] R. Sajeeb, C. S. Manohar, and D. Roy, “Rao-Blackwellizationwith substructuring for state and parameter estimations of aclass of nonlinear dynamical systems,” International Journal ofEngineering Under Uncertainty: Hazards, Assessment and Miti-gation, vol. 1, no. 1-2, 2009.

[17] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “Atutorial on particle filters for online nonlinear/non-GaussianBayesian tracking,” IEEE Transactions on Signal Processing, vol.50, no. 2, pp. 174–188, 2002.

[18] G. Casella and C. P. Robert, “Rao-blackwellisation of samplingschemes,” Biometrika, vol. 83, no. 1, pp. 81–94, 1996.

[19] J. Kennedy and R. Eberhart, “Particle swarm optimization,” inProceedings of the IEEE International Conference on Neural Net-works, pp. 1942–1948, IEEE Service Center, Perth, Australia,December 1995.

[20] R. A. Krohling, “Gaussian swarm: a novel particle swarm opti-mization algorithm,” in Proceedings of the IEEE Conference onCybernetics and Intelligent Systems (CIS ’04), pp. 372–376, Singa-pore, December 2004.

[21] R. Poli, J. Kennedy, and T. Balckwell, “Particle swarm optimiza-tion,” Swarm Intelligence, vol. 1, pp. 33–57, 2007.

[22] Z.-S. Zhao, J.-Z. Wang, X.-Z. Cheng, and Y.-J. Qi, “Particleswarm optimized particle filter and its application in visualtracking,” in Proceedings of the 6th International Conference onNatural Computation (ICNC ’10), pp. 2673–2676, August 2010.

[23] Z. Zhao, J. Wang, Q. Tian, and M. Cao, “Particle swarm-dif-ferential evolution cooperative optimized particle filter,” in Pro-ceedings of the International Conference on Intelligent Controland Information Processing (ICICIP ’10), pp. 485–490, August2010.

[24] Y. Li, J. Liang, and J. Hu, “A multi-swarm cooperative hybridparticle swarmoptimizer,” inProceedings of the 6th International

Conference onNatural Computation (ICNC ’10), vol. 5, pp. 2535–2539, August 2010.

Submit your manuscripts athttp://www.hindawi.com

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

MathematicsJournal of


Mathematical Problems in Engineering

Hindawi Publishing Corporationhttp://www.hindawi.com

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of


Probability and StatisticsHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

Journal of


Mathematical PhysicsAdvances in

Complex AnalysisJournal of


OptimizationJournal of


CombinatoricsHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

International Journal of


Operations ResearchAdvances in

Journal of


Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

International Journal of Mathematics and Mathematical Sciences


The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Algebra

Discrete Dynamics in Nature and Society



Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com

Volume 2014 Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

Stochastic AnalysisInternational Journal of

research article improved rao-blackwellized particle...

Documents