computer vision 2 — exercise 2 extended kalman filter ...€¦ · kalman filter: single mode...

Computer Vision 2 — Exercise 2  

Extended Kalman Filter & Particle FilterM.Sc. Francis Engelmann, Dr. Jörg Stückler [email protected], [email protected]

RWTH Aachen University, Computer Vision Group http://www.vision.rwth-aachen.de

mailto:[email protected]:[email protected]://www.vision.rwth-aachen.de

Computer Vision 2 - Exercise 2 - EKF & Particle Filter M.Sc. Francis Engelmann, Dr. Jörg Stückler

2

Content Exercise 2

• Question 1: Extended Kalman Filter − Compared to basic KF − Unicycle motion model − Nonlinearities & Jacobians

• Question 2: Particle Filter − Compared to general KF − SIR Algorithm − Implementation Details

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550

-1 0 1 2 3 4 5 6-1

0

1

2

3

4

5Filtered trajectory from noisy measurements

Question 1: Extended Kalman Filter


4

Q1: EKF - Compared to basic KF

• Kalman Filter • Extended Kalman Filter


5

Q1: EKF - Unicycle Motion Model

• The ”unicycle” motion model is an approximation often used for bicycle or car motions.

• State vector:

➢ position x ➢ position y ➢ orientation angle ➢ velocity

-1 0 1 2 3 4 5 6x

-1

0

1

2

3

4

5

y

Example Trajectory

0 0.2 0.4 0.6 0.8 1x

-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

y


6

Q1: EKF

a) Point out, which steps contain nonlinearities and give the definition of the functions g and h.

yt = h(xt) + �t

g : R4 ! R4,

2

664

xt

yt

✓t

vt

3

775 7!

2

664

xt +�tvtcos✓t

yt +�tvtsin✓t

✓t

vt

3

775 h : R4 ! R2,

2

664

xt

yt

✓t

vt

3

775 7!xt

yt

�

xt = g(xt�1) + ✏t

t t

t t

nonlinear functions

Dynamic Model Measurement Model


7

Q1: EKF b)

b) Compute the Jacobians Gt and Ht of g(x) and h(x) respectively.

Ht

=@h(x)

@x=

"@xt@xt

@xt@yt

@xt@✓t

@xt@vt

@yt

@xt

@yt

@yt

@yt

@✓t

@yt

@vt

#=

1 0 0 00 1 0 0

�same as in linear case

G

t

=

@g(x)

@x=

2

6664

@g1

@xt

@g1

@yt

@g1

@✓t

@g1

@vt@g2

@xt

@g2

@yt

@g2

@✓t

@g2

@vt@g3

@xt

@g3

@yt

@g3

@✓t

@g3

@vt@g4

@xt

@g4

@yt

@g4

@✓t

@g4

@vt

3

7775=

2

664

1 0 ��tvt

sin✓ �tcos✓0 1 �tv

t

cos✓ �tsin✓0 0 1 0

0 0 0 1

3

775

t

t t

t

g : R4 ! R4,

2

664

xt

yt

✓t

vt

3

775 7!

2

664

xt +�tvtcos✓t

yt +�tvtsin✓t

✓t

vt

3

775 h : R4 ! R2,

2

664

xt

yt

✓t

vt

3

775 7!xt

yt

�


-1 0 1 2 3 4 5 6x

-1

0

1

2

3

4

5

y

Original trajectory with noisy measurements

8

Q1: EKF c)

c) Implement extended Kalman filter. ▪ Generate measurements by adding Gaussian noise to original state at each

time step. ▪ Use EKF to estimate original trajectory based on noisy measurements.

1.3 1.4 1.5 1.6 1.7 1.8x

-0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

y


-1 0 1 2 3 4 5 6-1

0

1

2

3

4


9

Q1: EKF c)

c) Implement extended Kalman filter.

Original trajectory Measurements Filtered Trajectory


c) Implement extended Kalman filter. ▪ Influence of and

10

Q1: EKF c)

⌃dt ⌃mt

-1 0 1 2 3 4 5 6-1

0

1

2

3

4


sigma_d = eye(4) * 0.01;sigma_m = eye(2) * 0.5;

-1 0 1 2 3 4 5 6-1

0

1

2

3

4


sigma_d = eye(4) * 0.00001;sigma_m = eye(2) * 0.05;

Question 2: Particle Filter


12

Q2: Particle Filter

• What is different from particle filters to Kalman filters? − Kalman Filter: all probability distributions are normal distributions. − Particle Filter: any distribution is possible. − In particular, this allows us to model multiple hypothesis for the state.

-4

-2

0

2

4

-4-3

-2-1

01

23

4

04

0.05

0.1

342

12

0-1 0

-2-2-3

-4 -4

Normal Distribution

-4

-2

0

2

4

-4-3

-2-1

01

23

4

0

0.05

4

0.1

342

12

0-1 0

-2-2-3

-4 -4

Arbitrary Distribution


• What is different from particle filters to Kalman filters? − In particular, this allows us to model multiple hypothesis for the state.

13

Q2: Particle Filter

Kalman Filter: Single Mode Particle Filter: Multiple Modes

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550


14

Q2: Particle Filter

• SIR (Sampling Importance Resampling)

Particle

Weight of particle

Number of particles

Sam

plin

gR

e-sa

mpl

ing

Set of particles (samples from posterior at time t)

State transition distribution, according to dynamic model here: constant position model

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550

Particles x

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550

Detections y

Resampling


15

Q2: Particle Filter

• SIR (Sampling Importance Resampling)

a) generate_particles.m

c) inverse_transform_sampling.m

b) compute_particle_likelihood.m


16

Q2: Particle Filter a)

• Generate particles (samples) − Draw random samples from a 2D Normal distribution. − MATLABs randn returns normally distributed samples.

-4

-2

0

2

4

-4-3

-2-1

01

23

4

04

0.05

Prob

abilit

y D

ensi

ty 0.1

342

12

0-1 0

-2-2-3

-4 -4

2D Normal Distribution Drawn Samples


17

Q2: Particle Filter a)

• Generate particles (samples) − Draw random samples from a 2D Normal distribution. − MATLABs randn returns normally distributed samples.

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550


18

Q2: Particle Filter b)

• Compute particle likelihood (weights) − How likely does a particle correspond to a detection? − Measure: Parzen density estimation with Gaussian kernel

x

it

wit

yjt

w

i

t

=X

j

exp

⇣�12(y

j

x

� xix

�

2x

+y

j

y

� xiy

�

2y

)⌘

�

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550

100 200 300 400 500 600 700

50

100

150

200

250

300

350

400

450

500

550


19

Q2: Particle Filter - Resampling

• Inverse transform sampling − Goal: resample N particles from existing set of N particles, favor particles with

larger weight. 1. From discrete particle distribution compute cumulative distribution. Each bin

corresponds to a particle, the height of the bin corresponds to the weight. 2. Sample u from uniform distribution between 0 and 1 3. Look up bin in cumulative distribution and pick resulting particle x j


20

Q2: Particle Filter - Result

computer vision 2 — exercise 2 extended kalman filter ...€¦ · kalman filter: single mode...

Documents