Robot Localization with Particle Filters: Implementation and Algorithms

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

Monte Carlo Localization Repeated Random Sampling

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

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 ( ) ( | ) ( ) Bel x p z x Bel x ( | ) ( ) p z x Bel x = ( | ) w p z x ( ) Bel x

Robot Motion ( ) ( | ) ' x ( ) ' x d ' Bel x p x u Bel x ,

Sensor Information: Importance Sampling ( ) ( | ) ( ) Bel x p z x Bel x ( | ) ( ) p z x Bel x = ( | ) w p z x ( ) Bel x

Robot Motion ( ) ( | ) ' x ( ) ' x d ' Bel x p x u Bel x ,

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"

Particle Filter Algorithm 1. Algorithm particle_filter( St-1, ut-1 zt): 2. 0 , = = = tS Generate new samples 3. For 1 i n 4. Sample index j(i) from the discrete distribution given by wt-1 Sample from using and ) , | ( 1 1 t t t u x x p w = 5. tu ( ) i j i tx tx 1 1 Compute importance weight 6. z i t i t ( | ) p x t = + 7. Update normalization factor i t w w Insert 8. = i t i t { , } S S x t t 9. For = 1 i n Normalize weights 10. / w = i t i t w

Particle Filter Algorithm = ( ) ( | ) ( | , ) ( ) Bel x p z x p x x u Bel x dx 1 1 1 1 t t t t t t t t draw xit 1from Bel(xt 1) draw xitfrom p(xt | xit 1,ut 1) Importance factor for xit: target i t w = distributi on proposal p distributi p x on x ( | ) ( | , Bel ) ( ) z x u Bel x = 1 1 1 t t t t t t ( | , ) ( ) p x x u x 1 1 1 t t t t ( | ) p z x t t

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

Resampling w1 wn w1 wn w2 w2 Wn-1 Wn-1 w3 w3 Stochastic universal sampling Systematic resampling Linear time complexity Easy to implement, low variance Roulette wheel Binary search, n log n

Resampling Algorithm 1. Algorithm systematic_resampling(S,n): = = 2 2. 3. For 4. 5. u 1 ' , S c = w 1 Generate cdf i n w ], = , 0 ] U + 1 i c ~ c 1 n i i = Initialize threshold 1 i 1 = 1 j n 6. For 7. 8. 9. 10. Increment threshold 1 n u u j j Draw samples Skip until next threshold reached u 1 c While ( ) + = j i =i + i 1 = ' i ' , S S x n Insert 1 + 11. ReturnS Also called stochastic universal sampling

Motion Model Reminder Start

Proximity Sensor Model Reminder Sonar sensor Laser sensor

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

Sample-based Localization (sonar) 36

Initial Distribution 37

After Incorporating Ten Ultrasound Scans 38

After Incorporating 65 Ultrasound Scans 39

Estimated Path 40

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)