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.