Particle lter for mobile robot tracking and localisation Tinne De Laet K.U.Leuven, Dept. Werktuigkunde 19 oktober 2005 Particle lter 1
Overview Goal Introduction Particle lter Simulations Particle lter 2
Goal I To make a tutorial about a particle lter for AI I Comparable with the kalman lter tutorial I This tutorial shows the possible application of a Monte Carlo Simulation method (particle lter) for mobile robot tracking and localisation I Basic particle lter Particle lter 3
Overview Goal Introduction General The Bayesian approach Particle lter Simulations Particle lter 4
General I Problem: estimate the state of a system that changes over time using sequence of noisy measurements I Two models required: I System model I Measurement model I If the models are in probablistic form the Bayesian approach is ideally suited Particle lter 5
The Bayesian approach I Approach provides a rigorous general framework for dynamic state estimation problems I Construct the construct the posterior probability density function (pdf) of the state based on all available information including measurements I The "complete"solution to the estimation problem I An optimal (with respect to any criterion) estimate of the state may be obtained from the pdf Particle lter 6
Overview Goal Introduction Particle lter General Model description Prediction Resampling Update Numerical aspects/algorithmic details Estimate Particle lter 7
General I Objective: 'track' a variable of interest I Can typically non-gaussian and multi-model pdf $ Kalman lter I Key idea: represent the required pdf by a set of ramdom samples with associated weights and compute estimates based on these samples and weights I Basis: I construct a sample-based representation of the entire pdf I actions modifying the state of the variable of interest according to the models I observation is made ) information about the variables Particle lter 8
General: Importance sampling I Weigths of the samples are chosen using the principle of /emphimportance sampling I p(x) / (x) x i q(x) with q(x) the importance density I Weighted approximation to p(x): p(x) P N s i=1 w i (x k 0:k x i ) with w i / (x i ) 0:k q(x i ) I The Sequential Importance Sampling algorithm (SIS) supports recursive propagation of weights and support points Particle lter 9
General: Importance sampling I Here the importance sampling function is chosen equal to the prior calculated in the prediction step I ) w i k = w i k 1 P(z kjx k ) {z } measurement model ) Application of the proces model on the samples are samples of the proposal density I Most obvious and most easy to understand choice. Particle lter 10
Model description I State: x k y k k T Particle lter 11
Model description I Proces model I every motion modeled as rst a rotation around axis followed by a translation Particle lter 12
Model description I Rotation: k+1 = k + + N(M rot ; rot ) with = atan y x I Translation: Two sources of error: the actual traveled distance and the changes in orientation during the forward translation (=drift) k+1 = k + + N(M drft ; drft ) x k+1 = x k + ( + N(M trs ; trs )) cos( k+1 ) y k+1 = y k + ( + N(M trs ; trs )) sin( k+1 ) k+1 = k + + N(M drft ; drft ) Particle lter 13
Model description I Measurement model I US-sensor: z k = p 2a a 2 +1 x k + p 2 a 2 +1 atan y k + y k ys x k xs p 2b + a 2 m +1 I RF-sensor: 0 q 1 (x z k = = @ k x s ) 2 + (y k y s ) 2 A k Particle lter 14
Prediction I Uses the proces model (rotation and translation) to simulate the eect of hte action on the set of particles I Add the apropriate noise to simulate the eect of noise on the variables of interest Particle lter 15
Resampling I To prevent the depletion of the population I The eective sample size (ESS) indicates the eectiveness of the particles I If ESS < x:m with M = number of particles ) Resample I Resampling eliminates the particles with smaller weigths and duplicates the ones with higher weights in a probablistic way I Three resampling algorithms I Select with replacement I Linear time resampling I Resampling by Liu et al. Particle lter 16
Update I Reevaluate the weight of the particles based on the latest sensory information available I Thanks the the information obtained from the sensor the pdf can be described more accurately Particle lter 17
Numerical aspects/algorithmic details I Normalize the weights Particle lter 18
I Dierent options I The best particle: x s = xs max I Weighted mean: x s = P M j=1 x j w j I Robust mean: x s = P K Estimate with x max s = x i j w i = max (w j ) j=1 x j w j j jx j xs max j 6 Particle lter 19
Overview Goal Introduction Particle lter Simulations Tracking Localisation Particle lter 20
Tracking I Prediction I Initial: known pose I Uncertainty: traveled distance and angle during translation Example 1 memory I Example 2 I Initial: known pose I Uncertainty: angle during translation I Example 2 I Example 3 I Initial: known pose I Uncertainty: translation distance I Example 3 Particle lter 21
Tracking I US measurement I Initial: known pose I Uncertainty: traveled distance and angle during translation I RF measurement I Initial: known pose I Uncertainty: traveled distance and angle during translation Particle lter 22
Tracking I US and RF measurement I Initial: known pose I Uncertainty: traveled distance and angle during translation Particle lter 23
Localisation I US measurement I Initial: unknown pose I Uncertainty: traveled distance, angle during translation, rotation angle I RF measurement I Initial: unknown position, known orientation I Uncertainty: traveled distance, angle during translation, rotation angle I Example 2 I Initial: unknown pose I Uncertainty: traveled distance, angle during translation, rotation angle I Example 2 Particle lter 24
Localisation I US and RF measurement I Initial: unknown position, known orientation I Uncertainty: traveled distance, angle during translation, rotation angle I Example 2 I Initial: unknown pose I Uncertainty: traveled distance, angle during translation, rotation angle I Example 2 I Example 3 I Initial: unknown position, known orientation I Uncertainty: traveled distance, angle during translation, rotation angle I Example 3 Particle lter 25
Particle lter 26