Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

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


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. −1 and V  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][10][11][12][13][14][15][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 finitedimensional 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.

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

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,  0 is divided as follows: 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 the linear state    can be marginalized out from the posterior density ( 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 The conditional probability density functions for    and   +1 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   +1 , the formulas ( 5) and ( 6) can be viewed as measurement equation and prediction equation, and   +1| and  +1| 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.

Particle Swarm Optimization
The 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   = (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 , 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;  = ( 1 ,  2 , . . .,   ) 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  1 and  2 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]. 1 and  2 are positive constants called acceleration coefficients [21][22][23], which are utilized to control the velocity variable in the velocity updating step of PSO.

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,  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  1 and  2 are generated according to the Gaussian probability distribution, that is,  (0, 1). (

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 Inside, the measurement noise satisfies V  ∼ (0,  2 V ).
The evaluations of different algorithms are displayed in Figure 1 and Table 1.
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.

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.

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

Figure 2 :
Figure 2: The estimates of different particle filters when the RBPF fails.
ẋ  ,   , ẏ  ]  , and   and   are position of the target in two dimensions plane.ẋ