1. Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal University of Málaga (Spain) Dpt. of System Engineering and Automation May 19-23 Pasadena, CA (USA) An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization

2. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Outline of the talk 1. Introduction 2. The proposed method 3. Experimental results 4. Conclusions

3. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Outline of the talk 1. Introduction 2. The proposed method 3. Experimental results 4. Conclusions

4. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The addressed problem: Bayesian filtering Two choices determine the tools suitable to solve this problem: The representation of the prior/posterior densities: Gaussian vs. samples. Assumptions about the form of the likelihood. p(x) : prior belief p(y|x) : observation likelihood p(x|y) : posterior belief

5. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction In this work: Representation of pdfs? Observation likelihood? Any arbitrary function (need to evaluate it pointwise) Weighted, random samples (particle filter)

6. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters p(x) : prior belief What happens to each particle?

7. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters What happens to each particle? Draw new particles from the proposal distribution:

8. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters What happens to each particle? Weights are updated, depending on: The observation likelihood, and The proposal distribution.

9. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters What happens to each particle? Weights are updated, depending on: The observation likelihood, and The proposal distribution. p(y|x) : observation likelihood

10. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters The goal  To approximate as well as possible the posterior How much does the choice of the proposal distribution matter? p(x|y) : posterior belief

11. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters p(x|y) : posterior belief How much does the choice of the proposal distribution matter? q(·) : proposal distribution

12. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The basic particle filtering algorithm: The role of the proposal distribution in particle filters p(x|y) : posterior belief How much does the choice of the proposal distribution matter? q(·) : proposal distribution For a large mismatch between proposal and posterior, the particles represent the density very poorly:

13. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction The role of the proposal distribution in particle filters The proposal distribution q(·) is the key for the efficiency of a particle filter! It is common to use the transition model as proposal: We refer to this choice as the standard proposal. It is far from optimal. [Doucet et al. 2000] introduced the optimal proposal.

14. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 1. Introduction Relation of our method to other Bayesian filtering approaches: Non-Linear Observation model Optimal proposal Algorithms  Gaussian-Kalman Filter Gaussian-EKF, UKF Arbitrary  SIR, APF, FastSLAM Gaussian FastSLAM 2.0, [Grisetti et al. 2007] Arbitrary This work

15. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Outline of the talk 1. Introduction 2. The proposed method 3. Experimental results 4. Conclusions

16. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Our method: A particle filter based on the optimal proposal [Doucet et al. 2000]. Can deal with non-parameterized observation models, using rejection sampling to approximate the actual densities. Integrates KLD-sampling [Fox 2003] for a dynamic sample size (optional: it’s not fundamental to the approach). The weights of all the samples are always equal.

17. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method The theoretical model for each step of our method is this sequence of operations: Duplication  SIR with optimal proposal  Fixed/Dyn. sample-size resampling

18. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method The theoretical model for each step of our method is this sequence of operations: Duplication  SIR with optimal proposal  Fixed/Dyn. sample-size resampling

19. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method The theoretical model for each step of our method is this sequence of operations: Duplication  SIR with optimal proposal  Fixed/Dyn. sample-size resampling

20. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method The theoretical model for each step of our method is this sequence of operations: Duplication  SIR with optimal proposal  Fixed/Dyn. sample-size resampling

21. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: [1][1] t–1 t [2][2] [3][3] [4][4] Particles at time t-1

22. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Each particle propagates in time probabilistically: this is the reason of the duplication Group [ 1 ] [1][1] [2][2] [3][3] [4][4]

23. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Each particle propagates in time probabilistically: this is the reason of the duplication Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ]

24. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Each particle propagates in time probabilistically: this is the reason of the duplication Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ] Group [ 3 ]

25. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Each particle propagates in time probabilistically: this is the reason of the duplication Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ] Group [ 3 ] Group [4]

26. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t The observation likelihood states what particles are really important… Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ] Group [ 3 ] Group [4] Too distant particles do not contribute to the posterior! Observation likelihood

27. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ] Group [ 3 ] Group [4] We can predict which groups will be more important, before really generating the new samples! Observation likelihood

28. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method The optimal proposal distribution: Importance weights update as: The weight does not depend on the actual value of the particle.

29. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ] Group [ 3 ] Group [4] Group [ 1 ]  55% Group [ 2 ]  0% Group [ 3 ]  45% Group [ 4 ]  0% Observation likelihood

30. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Illustrative example of how our method works: t–1 t Group [ 1 ] [1][1] [2][2] [3][3] [4][4] Group [ 2 ] Group [ 3 ] Group [4] Observation likelihood Given the predictions, we draw particles according to the optimal proposal, only for those groups that really contribute to the posterior. A fixed or dynamic number of samples can be generated in this way.

31. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Comparison to… basic Sequential Importance Resampling (SIR)

32. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Comparison to… basic Sequential Importance Resampling (SIR) t–1 t [1][1] [2][2] [3][3] [4][4] Observation likelihood 1 particle  1 particle Prone to particle depletion!

33. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method Comparison to… Auxiliary Particle Filter (APF) [Pitt & Shephard, 1999]

34. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 2. The proposed method t–1 t [1][1] [2][2] [3][3] [4][4] Observation likelihood 1 particle  variable number of particles Propagation does not use optimal proposal! Comparison to… Auxiliary Particle Filter (APF) [Pitt & Shephard, 1999]

35. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Outline of the talk 1. Introduction 2. The proposed method 3. Experimental results 4. Conclusions 3.1. Numerical simulation 3.2. Robot localization

36. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 3.1. Results Numerical simulations: A Gaussian model for both the filtered density and the observation model. We compare the closed form optimal solution (Kalman filter) to:  PF using the “standard” proposal distribution.  Auxiliary PF method [Pitt & Shephard, 1999].  This work (“optimal” PF). (Fixed sample size for these experiments)

37. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 3.1. Results Results from the numerical simulations, and comparison to 1D Kalman filter: x axis: particles y axis: weights Approximated pdf (histogram) from particles. Actual pdf from Kalman filter. Kullback-Leibler distance (KLD) for increasing number of samples.

38. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 3.1. Results Results from the numerical simulations, and comparison to 1D Kalman filter:

39. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Outline of the talk 1. Introduction 2. The proposed method 3. Experimental results 4. Conclusions 3.1. Numerical simulation 3.2. Robot localization

40. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Robot path during localization Start End 1 m 3.2. Results Localization with real data: Path of the robot: ground truth from a RBPF with a large number of particles.

41. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 3.2. Results Localization with real data: Average errors in tracking (the particles are approximately at the right position from the beginning).

42. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 3.2. Results Ratio of convergence from global localization: Localization with real data: Ratio of convergence success Initial sample size (particles/m 2 ) 10 1 2 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Our method SIR with “standard” proposal

43. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” 3.2. Results

44. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Outline of the talk 1. Introduction 2. The proposed method 3. Experimental results 4. Conclusions

45. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Conclusions A new particle filter algorithm has been introduced. It can cope with non-parameterized observation likelihoods, and a dynamic number of particles. Compared to standard SIR, it provides more robust global localization and pose tracking for similar computation times. It is a generic algorithm: can be applied to other problems in robotics, computer vision, etc.

46. Jose Luis Blanco University of Málaga “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization” Source code (MRPT C++ libs), datasets, slides and instructions to reproduce the experiments available online: http://mrpt.sourceforge.net/ papersICRA 08 Finally…

47. Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal University of Málaga (Spain) Dpt. of System Engineering and Automation Thanks for your attention! An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization