Particle filters for Robot Localization An implementation of Bayes Filtering Markov Localization

Monte Carlo Localization Repeated Random Sampling

 Idea: Represent belief by random samples  Estimation of non-Gaussian, nonlinear processes  Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter Particle Filters

MC Localization Start with lots of random samples (aka particles) Weight each with probability based on sensor reading.

MC Localization Given an action a, Use importance weighting to select from the old particles Probabilistically create new particle given action and motion model

Adaptive Monte Carlo Localization often abbreviated AMCL

Particle Filters

Sensor Information: Importance Sampling

Robot Motion

Sensor Information: Importance Sampling

Robot Motion

Particle Filter Algorithm Sample the next generation for particles using the proposal distribution Compute the importance weights : weight = target distribution / proposal distribution Resampling: "Replace unlikely samples by more likely ones"

1. Algorithm particle_filter( S t-1, u t-1 z t ): For Generate new samples 4. Sample index j(i) from the discrete distribution given by w t-1 5. Sample from using and 6. Compute importance weight 7. Update normalization factor 8. Insert 9. For 10. Normalize weights Particle Filter Algorithm

draw x i t  1 from Bel (x t  1 ) draw x i t from p ( x t | x i t  1,u t  1 ) Importance factor for x i t : Particle Filter Algorithm

Resampling Given: Set S of weighted samples. Wanted : Random sample, where the probability of drawing x i is given by w i. Typically done n times with replacement to generate new sample set S’.

w2w2 w3w3 w1w1 wnwn W n-1 Resampling w2w2 w3w3 w1w1 wnwn W n-1 Roulette wheel Binary search, n log n Stochastic universal sampling Systematic resampling Linear time complexity Easy to implement, low variance

1. Algorithm systematic_resampling(S,n): ForGenerate cdf Initialize threshold 6. ForDraw samples … 7. While ( )Skip until next threshold reached Insert 10. Increment threshold 11. Return S’ Resampling Algorithm Also called stochastic universal sampling

Start Motion Model Reminder

Proximity Sensor Model Reminder Laser sensor Sonar sensor

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36 Sample-based Localization (sonar)

37 Initial Distribution

38 After Incorporating Ten Ultrasound Scans

39 After Incorporating 65 Ultrasound Scans

40 Estimated Path

Mobile Robot Localization Each particle is a potential pose of the robot Proposal distribution is the motion model of the robot (prediction step) The observation model is used to compute the importance weight (correction step)