11/24/2006 Particle Filters for Mobile Robot Localization Instructor: Dr. Shiri Amirkabir University of Technology Aliakbar Gorji Roborics 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: States Dynamic .

x f x, u, t v t White noise with covariance Q y g x, t wt Output Observations White noise with covariance R

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. p( x1:t | y1:t , t ) 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 systems 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

Input and Output measurem ents We want to compute: p( x t | d 1:t ) To convert to a recursive form: p( x t | d 1:t ) p( y t | x t ) p( x t | x t 1 , u t 1 ) p( x t 1 | d 1:t 1 )d x t 1 xt 1 Likelihood

State Model If f and g are linear, the integral is tractable and results in Kalman Filtering. 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. 1 N i p( x t | d 1:t ) ( x )

N i 1 Particle Filter But, sampling from posterior distribution function is intractable. Solution: sampling from a simpler distribution function (proposal 1 ~ (x ) function). p( x | d ) w N N

t i i 1:t i 1 Proposal density

function wi p( y1:t | x 0:t ) p( x 0:t | u1:t 1 ) q( x 0:t | y1:t ) wi i ~ w N wi i 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. q( x t | x t 1 , u1:t 1 ) p( y t | x t ) Good accuracy but hard to implement

q ( x t | x t 1 , u1:t 1 ) p( xt | x t 1 , u1:t 1 ) Suitable accuracy and easy to implement Recursive form for weights Usually, q is chose as: q( x t | x t 1 , u1:t 1 ) p( x t | x t 1 , u1:t 1 ) Recursive Equation: ~ i w

~ i p( y | x ) w t t 1 t t Now we are ready to propose Monte Carlo algorithms. SIS algorithm Draw the samples from prior density function and initialize weights.

~ For t=1:tmax: x 0 p ( x 0 ), w 0 For i=1:N(number of samples): sample x it from p ( x t | x t 1 , u1:t 1 ) Compute importance weight and normalize it: w~ i t w~ i t 1 p( y t |xt ) Check the terminating condition (tmax).

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 eff 1 N (w~ ) i 1 i 2 t Some Modifications

Kernal methods: considering a Gaussian distribution for each sample. KERNAL method and Hybrid SIR i t i t x N ( x , pti )

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. 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 (RaoBlackwellized 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 robots 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: p ( x t | x t 1 , u t 1 ) p ( y | x t ) t Motion 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 GridBased

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: i , j 0 if ith robot do not sense jth robot rt

d the dis tan ce between two robot Multi-Robot Particle Filters Now, the posterior distribution function is determined as: N p ( X t | Dt ) p( X ti | Dti ) i 1 The recursive equation is derived as: j

j j j j i i

i i p( x t | d 1:t ) p ( x t 1 | d 1:t 1 ) p( x t | x t 1 , rti,1 j ) p ( x t 1 | d 1:t 1 )d x t 1 xt 1 The above equation can be implemented by Particle Filter. 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