Particle Filters for Mobile Robot Localization

11/24/2006

Aliakbar Gorji

Roborics

Instructor: Dr. ShiriAmirkabir University of Technology

Preface• State Space models• Bayesian Filters for State

estimation• Particle Filters• Mobile Robot Localization• Particle Filters for real time

localization• Conclusion

Nonlinear State Space Systems

• A General Model:

t

t

wtxgy

vtuxfx

,

,,.

White noise with

covariance R

White noise with

covariance Q

State’s Dynamic

Output Observations

Nonlinear State Space Systems

• Ultimate Goal in modeling:– Inference (State Estimation)– Learning (Parameter Estimation)

• Inference designates to the estimation of states with regard to output observations and known parameters. ),|(

:1:1 ttt yxp Parameters of f and g

Various inference approaches

Online System Identification

• First Case: f and g are known.

• Second Case: There is not any information about the system’s dynamic:– Proposing parametric structures for f

and g.

Classic (Linear or Nonlinear)

Intelligent (Neural, RBF

or Fuzzy)

What do we seek?• We consider case 1,that is f and g are

known.• There is not any parametric structure ,

therefore, parameter estimation is eliminated.

• We are seeking the estimation of states (Latent Variables) based on observations (Sensor Measurements)

Bayesian Filters

• We want to compute:• To convert to a recursive form:

• If f and g are linear, the integral is tractable and results in Kalman Filtering.

)|( :1 tt dxp

Input and Output

measurements

11:1111:1

1

)|(),|()|()|(

t

x

ttttttttt xddxpuxxpxypdxpt

State ModelLikelihood

Bayesian Filters

• If f and g are nonlinear, the density distributions are not in Gaussian form.

• Extended Kalman filter: by linearization about nominal point, f and g convert to linear forms.

• EKF is not applicable in many real applications such as Target Tracking.

• Particle filters prove a strong tool to model the Non-Gaussian distributions.

What is Particle Filter?• It is the online version of Monte

Carlo algorithms.• Its idea is to estimate a distribution

function by sampling.

N

i

itt x

Ndxp

1:1 )(

1)|(

Particle Filter

• But, sampling from posterior distribution function is intractable.

• Solution: sampling from a simpler distribution function (proposal function).

Proposal density function

N

i

i

ii

tt

tttti

N

i

iitt

w

ww

yxq

uxpxypw

xwN

dxp

1

:1:0

1:1:0:0:1

1:1

~

)|(

)|()|(

)(~1)|(

What did change?• Sampling is conducted via proposal

function rather than posterior density function.

• Question: How can one determine proposal density function.

• There are two choices.

)|(),|( 1:11 ttttt xypuxxq ),|(),|( 1:111:11 tttttt uxxpuxxq

Good accuracy but

hard to implement

Suitable accuracy and easy to implement

Recursive form for weights

• Usually, q is chose as:

• Recursive Equation:

• Now we are ready to propose Monte Carlo algorithms.

),|(),|( 1:111:11 tttttt uxxpuxxq

)|(~~1 ttt

iti xypww

SIS algorithm

• Draw the samples from prior density function and initialize weights.

• For t=1:tmax:• For i=1:N(number of samples):

– sample– Compute importance weight and

normalize it:

• Check the terminating condition (tmax).

000~),( wxpx

),|( 1:11 tttit uxxpfromx

)|(~~1 ttt

iti xypww

Degeneracy Problem and SIR algorithm

• During the implementation of SIS algorithm the weight of all samples approach zero and only one sample has the weight 1.

• Solution: in each iteration, the weights with higher value are multiplied.

SIR algorithm

SIR algorithm

N

i

it

eff

wN

1

2)~(

1

Some Modifications

• Kernal methods: considering a Gaussian distribution for each sample.

KERNAL method and Hybrid SIR

• To adjust the parameters of the above distribution, KALMAN Filter method is combined with SIR algorithm.

• The stages of Hybrid SIR algorithm:– KALMAN Filter measurement update.

– SIS algorithm to choose the new samples and computing importance weights.

– Resampling stage to avoid degeneracy problem.

),ˆ( it

it

it pxNx

KALMAN Filter measurement update

SIS and Resampling stage

The general Particle Filter

The other Particle Filter Algorithms

• Sequential Monte Carlo : mixing Particle Filters with common Monte Carlo methods [ De.Freits PhD thesis, University of Cambridge, 1999].

• Marginalized Particle Filters (Rao-Blackwellized Particle Filters): dividing states to linear and nonlinear ones. For linear states KALMAN Filter and for nonlinear ones Particle Filter is applied.

Applications

• Navigation and Positioning.

• Multiple Target Tracking and Data Fusion.

• Financial Forecasting.

• Computer Vision.

• Wireless Communication and Blind Equalization problems.

Mobile Robots Localization

• Predicting robot’s position relative to its environment map.

• There are three types of positioning:– Position Tracking: the initial position of robot is

known.– Global Positioning: the initial conditions are not

given (initial values of states are not determined).

– Multiple Robot Positioning.• Particle Filters provide satisfactory results

for all of above issues.

Particle Filters for Mobile Robot Localization

• The following points should be considered:– As the point of State Space Models, f is

motion dynamic and g is Sensor characteristic and both are supposed to be known.

– The following distribution are designated as: ),|( 11 ttt uxxp )|( tt

xypMotion Model

Perceptual Likelihood

How can we determine each distribution?

• Motion model is determined by the behaviour of values measured by odometry.

• Perceptual Likelihood model is dependent to the sensor used for measurement, such as Sonar, Camera or Laser.

• Usually, one sensor is used as target (the one with highest accuracy) and the others’ data are modified by the mentioned sensor.

• After determining the structure of each distribution, general Particle Filter is applied for tracking.

Simulation

• A XR400 robot is tested to be tracked in the following map.

Comparison With Grid-Based Markov Model

Grid-Based

Particle Filter

Comparison With Grid-Based Markov Model

• A Particle Filter with 1000 to 5000 samples had a similar error compared with a Grid-Based method with resolution 4cm.

• The mentioned Grid-Based is not possible to apply in real-time mode but a Particle Filter with 5000 samples is easily implemented in real-time condition.

Multi-Robot Particle Filters

• A team of robots want to localize each other.

• A difficult problem: the states of each robot are dependent to the other robots’ states.

• Solution: the following dependency factor is defined:

•

robottwobetweencedisthed

robotjthsensenotdorobotithifr jit tan

0,

Multi-Robot Particle Filters

• Now, the posterior distribution function is determined as:

• The recursive equation is derived as:

• The above equation can be implemented by Particle Filter.

)|()|(1

it

N

i

ittt DXpDXp

i

x

iijiijjjjj

t

t

tttttttttxddxprxxpdxpdxp

1

1

1:11111:11:1)|(),|()|()|( ,

Conclusion

• Particle Filters can estimate the wide variety of Non-Gaussian distribution functions.

• In comparison with KALMAN Filters, Particle Filters have a more accurate result relative to KALMAN Filters.

• Particle Filters are easily implemented and in comparison with Grid-Based methods can provide better results for mobile robot localization.

Some References • Dieter Fox, Particle Filters for Mobile Robot

Localization.• Jo.ao F. G. de Freitas, Bayesian Methods for Neural

Networks, PhD thesis, University of Cambridge.• Website of Dr. Arnaud Doucet,

www.cs.ubc.ca/~arnaud/ .• Pierre Del Moral, Arnaud Doucet, ‘Sequential Monte

Carlo Samplers’, J. R. Statist. Soc. B (2006).• Huosheng Hu and John Q. Gan, ‘Sensors and Data

Fusion Algorithms in Mobile Robotics’, Technical Report: CSM-422, University of Essex.

Best Wishes

The End