Particle Filter & Search Unit 3 & 4 Udacity
Particle Filter • Show relation to Kalman. • Implementation & examples. • MATLAB Demo
Particle Filter • Estimates the state of a system. • Same as Histogram filters and Kalman filters • Used in localization and tracking.
Advantages of particle filters compared to KF and HF • Easiest to program • Most flexible • Can easily handle non-linear and non-gaussiansystems. • Multimodal
Rememberkalman? Motion/Prediction Measurement update Estimate of position x(t2) Corrected Optimal est x(t3) Measurement z Prediction x’(t3) Prediction x’(t3)
Approach (1) – Initialization • Determine robot position • Initialization of multiple guesses
Approach(2) – Measurement/Weight - Weights of each particle are determined by the chance of being correct. Laser sensor Measurement noise
Approach(3) – Likelihood Calculate weights Normalize factor Mini Quiz 1: Normalized weight Mini Quiz 2:
Approach(4) – Resampling • Survival of the fittest • Resampling wheel Resampling
Approach(5) – Resampling Measurement update (Kalman) Corrected Optimal est x(t3) Measurement z Prediction x’(t3)
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
Demo – Findingwally Matlabcode is provide in ParticleFilterUdacity.zip
Motion Planning • Find the ”shortest” path to a given goal. • Discrete planning (This lecture) • World divided in grid cells • Continuous planning
Motion Planning (Search) • Planning Problem • Given • Map • Starting location • Goal location • Cost • Goal • Find the minimum cost path
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.
Demo – Search Algorithm • MATLAB: MotionPlanning2DSearchStar
Search - A-star • Minimizes the number of expansions • Prioritized search by adding heuristic function.
Demo: Search - A start • MATLAB: MotionPlanning2DSearchStar
Dynamicprogramming • Given • Map • Goal • Outputs: Best Path from ANYWHERE. • Creates a Policy. • Gives the optimal action for everygridcell.
DynamicProgramming Approach • Create a valuegrid
Cons and pros • Pro: Gives the optimal path for any location. • Con: Is more computional.
Demo: DynamicProgramming • MATLAB: MotionPlanningDynamicProgramming.m
Stochastic motion • Avoid robots from getting to close to an obstacle.
Stochastic motion • Avoidance from the deterministic model.
Stochastic motion • By updating the valuefunctionwith a stochastic model. The robot willmoveaway from obstacles.