particle filter & search
DESCRIPTION
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. . - PowerPoint PPT PresentationTRANSCRIPT
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-
gaussian systems.• Multimodal
Remember kalman?
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Motion/Prediction Measurement update
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16 Estimate of position x(t2)
Prediction x’(t3) Prediction x’(t3)
Corrected Optimal est x(t3)
Measurement z
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.
Measurement noise Laser sensor
)|( itt
it xzpw
1
1N
ii
1
N
ii
W w
ii W
1 1( | ) 0.0001w p z x
2 2( | ) 0.01w p z x
3 3( | ) 0.7w p z x
Calculate weights
Normalized weight
2
2
1 1( | ) exp22
i i
ii
zp z x
msp s
æ öæ ö÷-ç ÷ç ÷ç ÷= - × ÷çç ÷÷çç ÷ç ÷è ø÷ç× × è ø
Normalize factor
Approach(3) – Likelihood
Mini Quiz 2:
Mini Quiz 1:
Approach(4) – Resampling
ResamplingN N• Survival of the fittest• Resampling wheel
Approach(5) – Resampling
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Measurement update (Kalman)
Prediction x’(t3)
Corrected Optimal est x(t3)
Measurement z
Approach (5) – Motion
Approach (6) - Prediction/Motion-In the context of localization, the particles are propagated according to the motion model.
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16Posteriori/Estimate of position x(t2)
Prediction x’(t3)
Motion update D1 (Kalman) Motion Update D2
Each particle is added noise -> gaussian distribution
Approach (7)
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
Demo – Finding wally
0 20 40 60 80 1000
10
20
30
40
50
60
70
80
90
100Initial state
RobotLandmarksMeanWally
Matlab code 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
Demo: Search A-Star Quadrocopter
01
23
45
6
01
23
45
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
Dynamic programming
• Given– Map – Goal
• Outputs: Best Path from ANYWHERE.• Creates a Policy.
– Gives the optimal action for every grid cell.
Dynamic Programming Approach
• Create a value grid
Cons and pros
• Pro: Gives the optimal path for any location. • Con: Is more computional.
Demo: Dynamic Programming
• MATLAB: MotionPlanningDynamicProgramming.m
Stochastic motion
• Avoid robots from getting to close to an obstacle.
Stochastic motion
• Avoidance from the deterministic model.
Example: Forward(1)
Example: Falling of the grid (2)
Stochastic motion
• By updating the value function with a stochastic model. The robot will move away from obstacles.