Vous êtes sur la page 1sur 5

Particle Filtering for Tracking Robot

Arif R. Hakim, Hariadi Aji, Herminarto Nugroho, Yudha P. Pane


Delft

University of Technology

AbstractTracking problem for mobile robots has been a topic


of interest for many researchers recently. Decades of research has
been fruitful, resulting in numbers of techniques and tools to solve
this problem. One particular framework that is widely used is
so called bayesian filter. This framework incorporates bayesian
rule in estimating posterior belief of robots state. Variants of
bayesian filter are kalman filter and particle filter. In this paper,
we emphasize on studying the principle of particle filter, deriving
particle filter for robots localization problem and discuss the
comparison between kalman filter and particle filter.
Index Termsbayesian filter, particle filter, kalman filter.

I. I NTRODUCTION
Nowadays robots are more and more autonomous; one
interesting problem for them is how to localize themselves
based on their knowledge of the environment. One idea is to
use Particle filter [1].
Particle filterings main objective is to track an object of
interest (can be robots, human, vehicles, or any other objects
of interest) whose state is evolving over time, typically with a
non-gaussian and potentially multi-modal probability density
function (pdf) [3]. Since the variable of interest is evolving
over time, particle filter requires an estimation of the state
of the system that changes over time. The estimation is
formulated using sequence of noisy measurement data made
on the system.
Particle filter uses multiple copies of the variable of interest
(which is called particles). Each of the particle is associated
with a certain weight that signifies the quality of that specific
particle. The estimation of the variable of interest is represented by a pdf obtained by the weighted sum of all particles.
The algorithm is recursive and operates in two main phases:
prediction and update. After each iteration, each particle is
modified according to the existing model (prediction stage),
including the addition of noise in order to simulate the effect
of noise on the variable of interest. Then, each particles
weight is re-evaluated based on the latest sensory information
available (update stage). The particles with (infinitesimally)
small weights will be eliminated through resampling process.
In this paper, we discuss and review particle filter, specifically for robotic tracking application. The paper is organized as
following. In section II, we concern about number of institutes
that has been using particle filter for their research projects.
Their application and the particle filters version is explained.
Section III and IV focuses on the basic theory of particle filter
followed by a discussion about the trade-off introduced in
particle filter, namely degeneracy vs loss of diversity problem.
Section V provides an example of particle filter application
for robotic tracking (localization) problem. We continue with
a potential solution for the problem. In section VI, we try

to explain the reason behind unpopularity of particle filter


compared to kalman filter in control engineering field. Finally,
we conclude the review in section VII.
II. A PPLICATIONS
Several universities have already introduced the use of
particle filter in the robotic application.
A. University of Washington
1) Particle Filters for Mobile Robot Localization [Fox et
al., 2001] [4]:
This project using a family of methods, known as Monte
Carlo Localization (MCL) which is a particle filter combined
with probabilistic model of robot perception and motion. here
the robot pose is unknown, and it has to determine its position
from scratch. in this problem, robots localization error can be
arbitrarily large.

Fig. 1: Global Localization of Mobile Robot using Particle


Filter

B. Carnegie Mellon University


1) Nursebot - robotic assistant in nursing home [Thrun et
al., 1998][7]:
This project introducing Pearl, a mobile robot system being
designed to assist elderly people in navigating their daily
activities and their environment. The type of particle filter used
is called rao-blackwellised particle filter.

cycle by evaluating the coherence to each feature and checking


the coherence between them.

Fig. 4: Localization result of the Humanoid Robot, a) Positioning test, b) Free play test

Fig. 2: Nursebot Pearl at the Longwood Retirement Community

III. BASIC T HEORY


A. Bayesian Filter

C. University of Freiburg
1) Detecting Failure on Mobile Robots [Plagemann et al.,
2007][5]:
The goal of the project is to enable a mobile robot to
determine whether the system is running free of faults or to
identify the cause for faulty behavior. The mixed-abstraction
particle filter is proposed as an efficient way of dealing with
potential failures of mobile robots.

Bayesian filter is a framework for calculating belief of a system state based on bayesian rule. This framework comprises
of two main stages: prediction and update. Prediction is done
using the systems transition function. Afterwards, update is
calculated from the measurement. The bayes filter algorithm
is defined as follows:
Prediction:
Z
p(xk |z1:k1 ) = p(xk |xk1 )p(xk1 |z1:k1 )dxk1
(1)
Update:
p(xk |z1:k ) =

p(zk |xk )p(xk |z1:k1 )


p(zk |z1:k1 )

(2)

B. Particle Filter

Fig. 3: real mobile robot that collides with an undetected glass


door while moving on its planned trajectory to the neighboring
room.
D. Universitat Politecnica de Valencia
1) Particle Filter for Self-Localization in RoboCup Humanoid Robot [Sanchez et al., 2013][6]:
Introducing a complete procedure to implement a reliable
self-localization method ranging from the perception system to
the localization itself. It has been developed a sensor modelling
method and its subsequent adjust to improve the measuring
system and the perception of the environment. on the other
hand it also introduces a new implementation of the particle
filter whose operation lies on finding the best particle in every

Particle filter is a variant of bayesian filter intended for


system with nonlinear transition function, nonlinear noise and
multimodal posterior distribution. The idea of particle filter is
as follows. First, the filter initialize a number of particles, each
particle has the same weight and corresponds to an estimate
about the system state. Through prediction and update stages,
these weights will evolve and concentrate only few particle
which represents the best estimate of the true state.
The general framework for particle filter is sequential importance sampling (SIS) which are further develop into numbers
of algorithms. Examples of SIS are sampling importance
resampling (SIR) filter and auxiliary sampling importance
resampling (ASIR) filter. These two filters output an estimate
of the state as a discrete distribution density. Although particle
filter generally works in discrete domain, it can be extended
into continuous domain using so called regularized particle
filter.

C. Degeneracy vs Loss of diversity


Degeneracy problem is the problem inherent in particle
filter. Initially, every particle is assigned a uniform weight.
As the filter iterates, the weights are updated and after some
time, only few particles are worth paying attention to. The rest
of it are so small thus negligible. This is clearly not desired
because this means we spend computation effort for trivial
particles.
To alleviate the degeneracy problem, resampling technique
is proposed. By resampling the particles for each iteration,
only those who possess valuable information will be likely to
survive. The insignificant particles will die out and disappear
over time. However, this also comes with another problem
known as loss of diversity problem. Due to resampling, points
that have larger weight will be selected more often, hence the
particles will contain the same point more than one. This in
turn will result in a bad representation of posterior density.
This is the trade-off that one must understand in implementing particle filter. There are some other modification to reduce
the effect of loss of diversity such as regularized particle filter
[8]. In this type of particle filter, we consider drawing samples
from a continuous instead of discrete distribution.
IV. VARIOUS V ERSION OF PARTICLE F ILTERS
The Sequential Importance Sampling (SIS) algorithm is
the basic of most version of particle filter developed so far.
Various version of the particle filter in this literature is actually
a special cases for the general SIS algorithm. The various
version of particle filter can be derived by appropriate choice
of importance sampling density and/or modification of the
resampling step.
A. Sampling Importance Resampling (SIR) Filter
The SIR algorithm is derived from SIS algorithm by appropriate choice of importance density and the resampling step. In
the SIR algorithm, q(xk |xik1 , z1:k ) is chosen to be the prior
density p(xk |xik1 ). The importance sampling density for the
SIR filter is independent of measurement zk , so the state space
is explored without any knowledge of the observations. It will
result to the inefficiency of the filter and sensitivity to outliers.
The resampling step of the SIR filter resamples from a discrete approximation. It can be clearly seen from the equation:

p(xk |z1:k )

Ns
X

wki (xk xik )

(3)

i=1

The use of Diracs delta function in the equation will result


in a discrete approximation of posterior density. Diracs delta
function is a generalized function on the real number line
which is zero everywhere except at the origin, and has the
integral over the whole real line equal to 1. It is represented
as infinitely high, infinitely thin at the origin, which the area
under the function is equal to 1 (we can see this 1 value as
the integral of the delta function).
The resampling is applied at every iteration, which can
result in rapid loss of diversity among the particles. This is

due to the fact that SIR resamples from discrete approximation


rather than continuous one. The other version of particle filter
is proposed to handle this problem, which will be discussed
later.
Although the SIR has disadvantages explained before, it
does have advantages. The advantages are the importance
weight are easily evaluated and the importance density can
be sampled easily.
B. Regularized Particle Filter (RPF)
As has been explained before, SIR has disadvantage of rapid
loss of diversity among particles, and there will be another
version of particle filter proposed to handle this problem. Since
the cause of the loss of diversity is the discrete approximation
of the posterior density in resampling stage, we can use a
continuous approximation of the posterior density to cope with
this problem.
A modified particle filter, known as Regularized Particle
Filter (RPF) is proposed as a potential solution of the loss of
diversity problem. RPF is actually identical to SIR, and also
a special case derived from SIS. The only difference between
SIR and RPF is in the resampling stage. Since RPF is proposed
to cope the loss of diversity problem of SIR, it resamples from
a continuous approximation of the posterior density, as shown
in the equation:

p(xk |z1:k )

Ns
X

wki Kh (xk xik )

(4)

x
1
K( )
hnx
h

(5)

i=1

Kh (x) =

Kh (x) is a basis function, a rescaled kernel density K(.).


Kh (x) is actually a kernel density K(.) which is rescaled with
1
hnx . h is kernel bandwidth (scalar), nx is the dimension of
the state vector x. The kernel K(.) and bandwidth are chosen
to minimize the mean integrated square error (MISE) between
the true posterior density and the corresponding regularized
representation in equation 4. The optimal choice of the kernel
is Epanichnikov kernel. The Epanichnikov kernel has the
optimum MISE compared to other kernel such as uniform,
triangular, biweight, triweight, gaussian, cosine and logistic.
To compare the efficiency of all kernels, each of them is
measured in comparison to the Epanichnikov kernel. Below
is the kernel comparison:
Kernel
Epanechnikov
Biweight
Triangular
Normal
Uniform

Efficiency
1.00
0.994
0.986
0.951
0.950

TABLE I: Kernel Comparison


So, we can substitute the kernel that is used to one of those
kernel. Actually the choice of kernel is not as important as the

choice of bandwidth (h). We can analyze deeper how to choose


an optimum bandwidth by reading the paper by Sheather and
Jones (1991) and by Park and Marron (1990).
V. A N A PPLICATION OF PARTICLE F ILTER : SIR F ILTER
FOR ROBOTS S TATE T RACKING
In order to give a more insights on how particle filter
works, we will give an example of particle filter application
for tracking a mobile robot. To be specific, we will apply
SIR particle filter for mobile robots localization. The problem
is defined as follows. First, the robots dynamical equation is
mathematically denoted as follows:
xk+1 = f (xk , uk ) + wk

(6)

zk = h(xk ) + vk

(7)

where x is the robots state, u is the control input given to the


robot, z is the measurement output, f is the non-linear state
transition function and h is the measurement function. Both
the new state xk+1 and measurement output zk are corrupted
by non-gaussian noise w and v respectively.
Secondly, to make our problem definition even clearer,
following assumptions are made:
1) The map is provided for the mobile robots.
2) Without losing generality, we consider the map to be a
feature based map [2]. Concretely, the map is defined as
M = [m1 , m2 , ..., mN ] where each mi represent a feature
(landmark) whose distance to the robot can be measured
using robots sensors. In reality, the map could also be a
position based map.
3) Given a state xk , the robot is able to calculate true
measurement zki by using raytracing algorithm. True
measurement is the correct measurement robot would
have sensed if it was located at xk . For instance, in the
case of feature based map, the measurement might be
the distance of robot to landmarks if the robot is using
ultrasonic/laser range finder. While the true measurement
is that distance calculated for each particle by using the
knowledge of the map.
The SIR particle filter for problem stated above is written
in following pseudo-code.
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:

m2 , ...,
i
hM = [m1 ,
i m3 ]
h
Ns
Ns
xik+1 , di i=1 = SIRLocalization xik , di i=1 , uk
for i = 1:Ns do
xik+1 = f (xik , uk ) + wk
zki = h(xik ) + vk
i
zki = RayCasting(x
k

)
2
wki = N zki ; zki , hit
end P
for
Ns
= i=1
wki
for i = 1:Ns do
wki = 1 wk
end for

13:

{Resampling
usingi algorithm listed
h
hin [1]}
i
Ns
Ns
xik+1 , di , i=1 = Resample xik , di i=1 , uk

In detail, above pseudo-code is explained as following:


1) Line 1 is where the map is defined. Line 2 is the function
declaration which accepts the current particles xik , i =
[1..Ns ], its weights di and the applied control action uk .
2) Line 4 and 5 is the famous predict-update action i.e. the
robot predicts its next state xik+1 based on the transition
function and inquire a new measurement update based on
sensor function. This action is done for every particle.
3) Line 6 and 7 calculates the importance/weight of each
measurement. First, the true measurement is computed.
Then the weight is calculated according to gaussian
2
distribution with mean zki and variance hit
. Line 9-12
is just for the purpose of normalizing the measurement
weight.
4) Line 13 plays an important role in particle filter, namely
the resampling process. The resmapling algorithm is the
same as written in [1].
VI. PARTICLE F ILTER AND K ALMAN F ILTER : A
C OMPARISON
In a linear system whose noises are assumed to be gaussian,
kalman filter is the optimal estimator. But in practice, noises
may come in any other distribution function and many systems
are best described by nonlinear function. Kalman filter will
poorly tackle this problem. Even in Extended Kalman Filter
that is designed to approximate the nonlinearity of system
equation, gaussian assumption for the posterior density still
must hold. Thus, it is quite possible for non-parametric filter
like particle filter to perform better in these cases.
However, in most engineering discipline, Kalman Filter is
more popular and taught more extensively than particle filter.
Arguably, this is because tuning Kalman filter is relatively
easier, with only two parameters needed to specify: initial state
and covariance matrix. In contrast, in particle filter we need
to specify number of samples, importance density function,
resampling algorithm. Furthermore, in high dimensionality of
states, kalman filter is more efficient than particle filter in term
of memory use. While kalman filter is quadratic, particle filter
could be exponential. Another reason is in most of control
engineering application, we often deal with fast-dynamics
system which requires faster convergence. With respect to this
criteria, kalman filter tends to converge faster than particle
filter.
VII. C ONCLUSION
Particle filter has been showing a significant development
over a past decade. It is likely to yield a better estimate than
kalman filter on certain circumstances such as estimating a
highly nonlinear system with multimodal probability distribution. In robotics application, particle filter is used extensively
for tracking and localization problem.

R EFERENCES
[1] S. Arulampalam, S. Maskel, N. Gordon, and T. Clapp A Tutorial on
Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking
IEEE Transaction on Signal Processing, vol. 50, no. 2, pp. 174-188,
February 2002.
[2] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, The MIT
Press,2005.
[3] I. M. Rekleitis,A Particle FIlter Tutorial for Mobile Robot Localization,
Center for Intelligence Machines, McGill University, St., Montreal, Quebec,2004.
[4] D. Fox, S. Thrun, F. Dellaert, W. Burgard, Particle Filters for Mobile
Robot Localization, Sequential Monte Carlo Methods in Practice, pp.
401-428, 2001
[5] C. Plagemann, C. Stachniss, and W. Burgard, Efcient Failure Detection
for Mobile Robots Using Mixed-Abstraction Particle Filters, University
of Freiburg, 2007.
[6] E.M. Sanchez, M.M. Alcobendas, J.F.B. Noguera, G.B. Gilabert, and
J.E.S. Ten, A Reliability-Based Particle Filter for Humanoid Robot SelfLocalization in RoboCup Standard Platform League Sensors (Basel,
Switzerland). Nov 2013; 13(11)14954.
[7] S. Thrun, M. E. Pollack , L. Brown , D. Colbry , C. Orosz , B. Peintner
, S. Ramakrishnan , S. Engberg , J. T. Matthews , J.D. Jacob , C. E.
McCarthy, Pearl: A Mobile Robotic Assistant for the Elderly, 2002
[8] C. Musso, N. Oudjane, and F. LeGland, Improving regularised particle
filters,, in Sequential Monte Carlo Methods in Practice, A. Doucet, J. F.
G. de Freitas, and N. J. Gordon, Eds. New York: Springer-Verlag, 2001
[9] S.J. Sheather, M.C. Jones, A Reliable Data-Based Bandwidth Selection
Method for Kernel Density Estimation, Journal of the Royal Statistical
Society. Series B (Methodolical), Vol. 53, issue 3, pp. 683-390, 1991
[10] B.U. Park, J.S. Marron, Comparison of Data-Driven Bandwidth Selector, Journal of the American Statictical Association, Vol. 85, Issue 409,
March 1990.