# Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

Authors: Zeng-Shun Zhao; Xiang Feng; Yan-yan Lin; Fang Wei; Shi-Ku Wang; Tong-Lu Xiao; Mao-Yong Cao; Zeng-Guang Hou; Min Tan
Journal: Journal of Applied Mathematics
 (2013)

---

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


---

## Body

## 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)sk=f(sk-1,ξk-1),zk=h(sk,vk),
where sk represents the state vector at instant k, zk is the observation vector of s at instant k, and f() and h() are the transition function and measurement function, respectively. ξk-1 and vk 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 densityp(sk∣z1:k) by a set of particles {ski,i=1,…,N} with associated weights {wki,i=1,…,N} where N is the number of particles and k denotes the index of current time. In practice, it is inconvenient to sample directly from the posterior density distribution p(sk∣z1:k) the solution is to sample particles ski~q(·), where q(·) is referred to as importance density. Then, a weighted approximation to the posterior p(sk∣z1:k) is given by
(2)p(sk∣z1:k)≈∑i=1Nwkiδ(sk-ski),
where the normalized weights are defined as
(3)wki∝p(ski∣z1:k)q(ski∣z1:k).The more detailed description of particle filter could be referred to [17].A pseudocode description of the standard particle filter is given by Pseudocode1.Pseudocode 1:Description of the standard particle filter.
⌊
{
s
k
i
,
w
k
i
}
i
=
1
N
⌋
=
PF
⌊
{
s
k
-
1
i
,
w
k
-
1
i
}
i
=
1
N
,
z
k
⌋
FOR=1 : N
Drawski~p(sk∣sk-1i)
Calculatewki=p(zk∣ski)
END FOR
Calculate total weight:t=sum⌊{wki}i=1N⌋
For=1 : N
Normalize:wki=t-1wki
END FOR
Resample [17]
⌊
{
s
k
i
,
w
k
i
,
…
}
i
=
1
N
⌋
= RESAMPLE⌊{ski,wki}i=1N⌋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 onlyN 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 sk which can be partitioned according to
(4)sk=[sklskn],
where skl denotes the linear state variables and skn denotes the nonlinear state variables. A relative general model with the properties discussed above is given by
(5)sk+1n=fkn(skn)+Akn(skn)skl+Gkn(skn)ξkn,(6)sk+1l=fkl(skn)+Akl(skn)skl+Gkl(skn)ξkl,(7)zk=hk(skn)+Ck(skn)skl+vk,
where f() and h() are possible nonlinear functions. Furthermore, the noise of the system state is assumed to be white and Gaussian distributed with
(8)ξk=[ξklξkn]~N(0,Qk),Qk=[QklQkln(Qkln)TQkn].
The measurement noise is also assumed to be white and Gaussian distributed according to the following equation:
(9)vk~N(0,Rk).
Furthermore, s0 is divided as follows:
(10)s0l~N(s-0,P-0),(11)s0n~p(s0n),
where the formulas (5), (6), and (7) describe a mixed linear/nonlinear system. If the nonlinear state skn is known, the formula (6) describes a linear, Gaussian subsystem. So the linear state skl can be marginalized out from the posterior density p(sk∣z1:k)(12)p(skl,skn∣z1:k)=p(skl∣skn,z1:k)p(skn∣z1:k),
where the linear state p(skl∣skn,z1:k) is analytically tractable whose solution can be given by the Kalman filter. Meanwhile, the nonlinear state p(skn∣z1:k) can be estimated using the particle filter. Therefore the posterior density of the whole space can be approximated to
(13)p(skl,skn∣z1:k)=p(skl∣skn,z1:k)p(skn∣z1:k)=∑i=1Nwkiδ(skn-sknj)N(skl;sklj,Pkj).
The conditional probability density functions for skl and sk+1l are given by
(14)p(skl∣skn,zk)=N(sk∣kl,Pk∣k),p(sk+1l∣sk+1n,zk)=N(sk+1∣kl,Pk+1∣k),
where
(15)sk∣kl=sk∣k-1k+Kk(zk-hk-Cksk∣k-1),Pk∣k=Pk∣k-1-KkMkKkT,Mk=CkPk∣k-1CkT+Rk,Kk=Pk∣k-1CkTMk-1.The formulation above updates linear state by Kalman filter when the new observationzk is obtained. After getting the prediction of nonlinear state sk+1n, the formulas (5) and (6) can be viewed as measurement equation and prediction equation, and sk+1∣kl and Pk+1∣k can be computed as follows:
(16)sk+1∣kl=A-klsk∣kL+Gkl(Qkln)T(GknQkn)-1ηk+fkl+Lk(ηk-Aknsk∣kl),Pk+1∣k=A-klPk∣k(Akk)T+GklQkl(Gkl)T-LkNkLkT,Nk=AknPk∣k(Akn)T+GknQkn(Gkn)T,Lk=A-klPk∣k(Akn)TNk-1,
where
(17)ηk=sk+1n-fkn,A-kl=Akl-Gkl(Qkln)T(GknQkn)-1Akn,Q-kl=Qkl-(Qkln)T(Qkn)-1Qkln,
where Qkln is covariance matrix of the linear state skl and nonlinear state skn. More references are listed in [14, 15].The measurement equation and the prediction equation of nonlinear state are given by(18)p(zk∣skn,ηk-1)=N(hk+Cksk∣k-1l,CkPk∣k-1CkT+Rk)(19)p(sk+1n∣skn,ηk)=N(fkn+Aknsk∣kl,AknPk∣k(Akn)T+GknQkn(Gkn)T).
A pseudocode description of the Rao-Blackwellized particle filter is supplied in Pseudocode 2.Pseudocode 2:Description of the Rao-Blackwellized particle filter.
(1) Initialization
Fori=1,…,N, initialize the particles, s0n,(i)~p(x0n) and set {s0l,(i),P0(i)}={s-0l,P-0}
(2) FORk>0
(2.1) PF measurement update
Compute the particle weightw~ki using formula (18),
(2.2) normalize the weight
wki=w~ki∑i=1kw~ki
(2.3) Particle filter time update and Kalman filter:
(a) Kalman filter measurement update using formula (15)
(b) Particle filter time update:
Fori=1,…,N predict new particles according to (19)
(c) Kalman filter time update using (16)–(17)
(2.4) SetK:=K+1 and iterate from step (2.1)


## 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 arem particles in an n dimensional space, denoted Xi=(xi1,xi2,…,xin), i=1,…,m, and Vi=(vi1,vi2,…,vin), i=1,…,m, where Xi and Vi are the position and velocity of the ith particle, respectively. At each step of iterations, two main factors, Pi and G, 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, Pi=(pi1,pi2,…,pin) represents the previous best fitness value of ith particle up to current step; G=(g1,g2,…,gn) 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:
(20)Vit+1=w·Vit+c1·r1·(pit-Xit)+c2·r2·(G-Xit),(21)Xit+1=Xit+Vit.Here, we definer1 and r2 as the random numbers derived from the range [0,1], which are generated according to a uniform probability distribution U[0,1]. w 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]. c1 and c2 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(22)fitness=exp[-12Rk(znew-zpred)2].Here,Rk is the observation noise covariance, znew is the newest observation, and zpred 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 velocityVmax 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 ofPi and the global best value of G 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 r1 and r2 are generated according to the Gaussian probability distribution, that is, N (0, 1).Next, each particle’s weight would be updated and normalized by (9) and (10)
(23)wti=wt-1ip(yt∣xti),wti=wti∑i=1Nwti.Then using resampling step to select and replicate the particle with large weights,(24){xti,1N}i=1N={xti,wti}i=1N.A pseudocode description of the PSO optimizes Rao-Blackwellized particle filter which is given by Pseudocode3.Pseudocode 3:The PSO optimized Rao-Blackwellized particle filter.
(1) Initialization
Fori=1,…,N, initialize the particles, x0n,(i)~p(x0n) and set {x0l,(i),P0(i)}={x-0l,P-0}
(2) FORk>0
(2.1) PF measurement update
⌊{xki*}i=1N⌋=PSO⌊{xki}i=1N,zk⌋
Compute the particle weightw~ki using formula (18),
(2.2) normalize the weight
wki=w~ki∑i=1kw~ki
(2.3) Particle filter time update and Kalman filter:
(a) Kalman filter measurement update using formula (15)
(b) Particle filter time update:
Fori=1,…,N predict new particles according to (19)
(c) Kalman filter time update using (16)–(17)
(2.4) SetK:=K+1 and iterate from step (2.1)


## 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(25)sk=Fsk-1+Γξk-1,k=1,2,…,K,
where F=[1100010000100001], Γ=[0.501000.501], the target state vector is sk=[xk,x˙k,yk,y˙k]T, andxk and yk are position of the target in two dimensions plane. x˙k and y˙k are velocities of the target. ξk=[ξkx,ξky]T is assumed to be Gaussian distributed noise, and satisfied ξk*~N(0,δξ2).The measurement equation is given by(26)zk=tan-1(ykxk)+vk.
Inside, the measurement noise satisfies vk~N(0,δv2).The target initialization state iss0=[-0.005,0.001,0.7,-0.005]T, process noise covariance δξ=0.001, measurement noise covariance δv=0.005, Time K=60. We divide target state into nonlinear state skn and linear state skl. Where skn=[xk,yk]T, skl=[x˙k,y˙k]T. Root-mean-square error (RMSE) is used to describe the accuracy estimation, which is given by
(27)RMSEk=1N∑i=1N(x^ki-xk)2+(y^ki-yk)2.The evaluations of different algorithms are displayed in Figure1 and Table 1.Table 1
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
20Figure 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 Figure1, when time k>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 Figure2 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.
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
10Figure 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.



---

*Source: 302170-2013-08-28.xml*