Particle Filter & Search

1. Particle Filter & Search Unit 3 & 4 Udacity

2. Particle Filter • Show relation to Kalman. • Implementation & examples. • MATLAB Demo

3. Particle Filter • Estimates the state of a system. • Same as Histogram filters and Kalman filters • Used in localization and tracking.

4. Advantages of particle filters compared to KF and HF • Easiest to program • Most flexible • Can easily handle non-linear and non-gaussiansystems. • Multimodal

5. Rememberkalman? Motion/Prediction Measurement update Estimate of position x(t2) Corrected Optimal est x(t3) Measurement z Prediction x’(t3) Prediction x’(t3)

6. Approach (1) – Initialization • Determine robot position • Initialization of multiple guesses

7. Approach(2) – Measurement/Weight - Weights of each particle are determined by the chance of being correct. Laser sensor Measurement noise

8. Approach(3) – Likelihood Calculate weights Normalize factor Mini Quiz 1: Normalized weight Mini Quiz 2:

9. Approach(4) – Resampling • Survival of the fittest • Resampling wheel Resampling

10. Approach(5) – Resampling Measurement update (Kalman) Corrected Optimal est x(t3) Measurement z Prediction x’(t3)

11. Approach (5) – Motion

12. Approach (6) - Prediction/Motion • In the context of localization, the particles are propagated according to the motion model. Motion update D1 (Kalman) Motion Update D2 Posteriori/Estimate of position x(t2) Prediction x’(t3) Each particle is added noise -> gaussian distribution

13. Approach (7)

32. Demo – Findingwally Matlabcode is provide in ParticleFilterUdacity.zip

33. Motion Planning • Find the ”shortest” path to a given goal. • Discrete planning (This lecture) • World divided in grid cells • Continuous planning

34. Motion Planning (Search) • Planning Problem • Given • Map • Starting location • Goal location • Cost • Goal • Find the minimum cost path

35. The Search Problem – Path Planning • Find the shortest path from Start to Goal. • Done with an expand approach. • Openlist: Possible expansions. • G-value: Number of expansions need to reach a given grid cell. • Algorithm continues until goal is reached or openlist is empty.

36. Demo – Search Algorithm • MATLAB: MotionPlanning2DSearchStar

37. Search - A-star • Minimizes the number of expansions • Prioritized search by adding heuristic function.

38. Demo: Search - A start • MATLAB: MotionPlanning2DSearchStar

39. Demo: SearchA-StarQuadrocopter

40. Dynamicprogramming • Given • Map • Goal • Outputs: Best Path from ANYWHERE. • Creates a Policy. • Gives the optimal action for everygridcell.

41. DynamicProgramming Approach • Create a valuegrid

42. Cons and pros • Pro: Gives the optimal path for any location. • Con: Is more computional.

43. Demo: DynamicProgramming • MATLAB: MotionPlanningDynamicProgramming.m

44. Stochastic motion • Avoid robots from getting to close to an obstacle.

45. Stochastic motion • Avoidance from the deterministic model.

46. Example: Forward(1)

47. Example: Falling of the grid (2)

48. Stochastic motion • By updating the valuefunctionwith a stochastic model. The robot willmoveaway from obstacles.