State Estimation: Particle Filter

Transcription

1 State Estimation: Particle Filter Daniel Seliger HAUPT-/ BACHELOR- SEMINAR ADAPTIVE SYSTEME PST PROF. DR. WIRSING 14. JUNI 2009 VORNAME NAME

2 Overview 1. Repitition: Bayesian Filtering 2. Particle Filter 4. Summary 5. Sources Daniel Seliger 2

3 Important Note Note: The terms probability, probability density, probability distribution are mixed up in this presentation and may not be mathematically correct. However, there's not a lot of maths and so the difference shouldn't be important to understand the topic. S. Thrun, W. Burgard, D. Fox also did this in Probabilistic Robotics, The MIT Press Daniel Seliger 3

4 Repitition Bayesian Filters Control Actions u Robot Environment Sensor Measurements z Initial state: Posterior distribution: State transition probability: How does state change over time (e.g. as an effect of a control action)? Measurement probability: x t x 0 p(x t u t, z t ) p(x t x t 1,u t ) p(z t x t ) Assumed state,how probable is measurement? z t Daniel Seliger 4

5 Repitition Bayesian Filters Initialization x 0 Correction p(z t x t ) Prediction p(x t x t 1,u t ) Goal: Extract estimate (e.g. mean of posterior) Update the posterior distribution over time, taking into account the controls and measurements, in a way that the extracted estimate converges against the real state Daniel Seliger 5

6 Parametric vs Non-Parametric Parametric filters: Continous state space Describe probabilities in parametric terms: Advantage: Best possible accuracy Disadvantage: Only simple distributions Example: Kalman filter Abb.1: Dichtefunktion der Standardnormaverteilung Daniel Seliger 6

7 Parametric vs Non-Parametric Non-Parametric filters: Break up continous state space into discrete state space Describe probabilities by a set of samples Advantage: Ability to describe complex probability distributions Disadvantage: Need to trade off accuracy against efficiency Example: Histogram Filter, or... Abb. 2: Histogram representation of a continuous random variable Daniel Seliger 7

8 Particle Filter Daniel Seliger 8

9 Many different names:... Particle Filter Sequential Monte-Carlo-Method CONDENSATION algorithm Interacting particle approximations Bootstrap filtering Survival of the fittest Daniel Seliger 9

10 Particle: Concrete belief of the state, combined with a Weight w that indicates the certainty that the belief is true Daniel Seliger 10

11 Particle: Concrete belief of the state, combined with a Weight w that indicates the certainty that the belief is true Set of Particles: approximation of the posterior distribution from which an estimation of the real state can be extracted Daniel Seliger 11

12 Sampling: draw a number of samples (particles) acchording to a given probability Abb. 3: samples drawn from a Gaussian random variable Daniel Seliger 12

13 Functional Principle of Particle Filter: Initialisation Prediction Resampling Importance Sampling Estimation Daniel Seliger 13

14 Example: Mobile Robot Localisation Abb. 4 x (state space) Daniel Seliger 14

15 Example: Mobile Robot Localisation Abb. 4 x (state space) Robot knows the room, but he doesn't know where he is Robot has a sensor that indicates door / no door Goal: locate Robot! Daniel Seliger 15

16 Functional Principle of Particle Filter: Initialisation Prediction Resampling Importance Sampling Estimation Daniel Seliger 16

17 Initialisation: Robot doesn't have a clue where he is yet, so Randomly draw N particles over the whole state space All particles have the same weight w Abb. 5: Particle Filter Initialisation x Daniel Seliger 17

18 Functional Principle of Particle Filter: Initialisation Prediction Resampling Importance Sampling Estimation Daniel Seliger 18

19 Importance Sampling: After the sensor has made a measurement ( door), weigh the particles according to the measurement probability Particles (=beliefs) with a high probability get a high weight and vice versa Normalize the weights Abb. 6: Importance Sampling p(z t x t ) w x Daniel Seliger 19 x

20 Functional Principle of Particle Filter: Initialisation Prediction Resampling Importance Sampling Estimation Daniel Seliger 20

21 Resampling: The set of weighed particles describes a probability distribution Pick N particles, with replacement, according to that distribution Particles with little weight are eliminated, while others are picked more often Particles concentrate in areas of higher probability There are still N particles Some particles represent the same belief resampling x Abb. 7: Particle Resampling Daniel Seliger 21

22 Functional Principle of Particle Filter: Initialisation Prediction Resampling Importance Sampling Estimation Daniel Seliger 22

23 Prediction: The Robot has moved (control action) Move each particle according to the state transition probability p(x t x t 1,u t ) Abb. 8: Prediction x Increase of granularity! Daniel Seliger 23

24 Resampling and Prediction: w x w Abb. 9: Resampling and Prediction x Daniel Seliger 24

25 Evolution of particles after one loop: Before: w x After: w Abb. 10: Particle evolution after one loop x Particles (beliefs) are concentrated on more probable states Daniel Seliger 25

26 Evolution of particles after two loops: Before: w x After: w Abb. 11: Particle evolution after two loops x The distribution of the particles tends to become a more precise posterior distribution from which we can extract an accurate estimate Daniel Seliger 26

27 Implementation in pseudo-code: Last set of particles Auxilliary set Resulting set Prediction Resampling Importance Sampling Estimation Abb. 12: Particle Filter implementation Daniel Seliger 27

28 Implementation in pseudo-code: Prediction Resampling Importance Sampling Estimation Abb. 12: Particle Filter implementation Daniel Seliger 28

29 Implementation in pseudo-code: Prediction Resampling Importance Sampling Estimation Abb. 12: Particle Filter implementation Daniel Seliger 29

30 Implementation in pseudo-code: Prediction Resampling Importance Sampling Estimation Abb. 12: Particle Filter implementation Daniel Seliger 30

31 Robot Localisation in 3-dimensional state space: Abb. 13: Mobile robot localisation via sonar sensors and Particle Filter Daniel Seliger 31

32 (Dis-)Advantages of PF Particle Filter: Advantages: No Limitation for the complexity of distributions Continous and discrete state spaces Adaptive in the use of computational resources Disadvantages: The most efficient number of particles cannot be calculated Distributions are only approximated which leads to calculation errors Daniel Seliger 32

33 Conclusion What is the best filter for state estimation? There is none! Each filter has advantages over the others The choice of filter depends on the concrete system of which you want to estimate the state In many applications, several filters are used together, e.g. Kalman Filter and Particle Filter Daniel Seliger 33

34 Summary Summary: Bayesian Filters are used to estimate states Real state cannot be retrieved, but approximated by a probability distribution over time Prediction / Correction cycle Parametric and Non-Parametric filters Parametric filters are more accurate but cannot handle complex distributions Non-Parametric filters can deal with very complex distributions but are less accurate because of approximation Daniel Seliger 34

35 Summary Particle Filter: Describes posterior by a set of weighted samples Each sample represents a concrete belief Importance Sampling: weigh each particle according to the measurement distribution Resampling: sample particles new, according to their weight force particles to concentrate in regions of higher interest Prediction: move each particle according to the state transition probability increase granularity Conclusion: Each filter has different advantages and disadvantages Choice of filter depends on field of application Daniel Seliger 35

36 Sources Literature: S. Thrun, W. Burgard, D. Fox: Probabilistic Robotics, The MIT Press 2005 AI-Videos: Robotics-Videos: Drew Bagnell: Statistical Techniques in Robotics Lecture #4, 2008: f11/notes/f08/lecture4/16831_lecture04.mdesnoye.pdf Miodrag Bolic: Theory and Implementation of Particle Filters, University of Ottawa Daniel Seliger 36

37 Sources Pieter Abbeel: Particle Filters, Michael Pfeiffer: A brief introduction to Particle Filters, TU Graz 2004, Wikipedia: Particle Filter, Images: Abb. 1: Dichtefunktion der Standardnormalverteilung, Wikipedia: Normalverteilung, Daniel Seliger 37

38 Sources Abb. 2 - Abb. 6: S. Thrun, W. Burgard, D. Fox: Probabilistic Robotics, The MIT Press 2005 Abb. 7, Abb. 8: Miodrag Bolic: Theory and Implementation of Particle Filters, University of Ottawa 2004 Abb. 9 - Abb. 12: S. Thrun, W. Burgard, D. Fox: Probabilistic Robotics, The MIT Press 2005 Abb. 13: D. Fox, Daniel Seliger 38