www.kingston.ac.uk/dirc

Dr Graeme A. Jones

tools from the vision tool box Kalman Tracker -

noise and filter design

www.kingston.ac.uk/dirc

Reviewing the Kalman Equations

Predict state pˆ, and state uncertainty Pˆ

Predict observation zˆ, and observation uncertainty Zˆ

Update state p, P from actual observation z, Z:

QAPAPpAp Ttttt 11

ˆ,ˆ

Ttttt HPHZpHz ˆˆ,ˆˆ

tttt

ttttt

PHKPP

zzKpp

ˆˆ

ˆˆ

1ˆˆ tt

Ttt ZZHPK

www.kingston.ac.uk/dirc

Reducing the uncertainty of the state

Starting from an initial uncertain state, the state uncertainty should steadily reduce with the number of observations.

Observed state and observed state uncertainty

tt zHp ~tt ZHHP

~

-10 -5 0 5 10 15

0

5

10

15

20

x

y

Constant Acceleration

11½

11½

11

11

1

1

, A

y

x

vv

a

a

py

x

y

x

True Observation Noise

10

01*Z

Initial State

1

1

1.0

1.0

01.0

01.0

10

0

0

0

X

x

-10 -5 0 5 10 15

0

5

10

15

20

x

y

true

observed

filtered

www.kingston.ac.uk/dirc

Observation Noise

What is the relationship between the computed state uncertainty Xt and the estimated uncertainty Zt of each observation zt? (Not necessarily the real underlying uncertainty Zt

*)

– the asymptotic value of the state covariance Xt is directly related to the estimated uncertainty Zt rather than the real uncertainty Zt

*

www.kingston.ac.uk/dirc-10 -5 0 5 10 15

0

5

10

15

20

x

y

true

observed

filtered

5.00

05.0*Z

-10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

10

01*Z

-10 -5 0 5 10 15

0

5

10

15

20

x

y

true

observed

filtered

30

03*Z

-10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

20

02*Z

True Observation Noise

10

01*Z

www.kingston.ac.uk/dirc

Observation Noise What is the relationship between the computed state

uncertainty Xt and the estimated uncertainty Zt of each observation zt? (Not necessarily the real underlying uncertainty Zt

*)

– the asymptotic value of the state covariance Xt is directly related to the estimated uncertainty Zt rather than the real uncertainty Zt

*

– when the estimated uncertainty Zt is too small, the necessary data association stage starts to reject even those true observations with modest amounts of noise.

www.kingston.ac.uk/dirc

-10 -5 0 5 10 15

0

5

10

15

20

x

y

true

observed

filtered

-10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

-15 -10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

-15 -10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

True Observation Noise

10

01*Z

2 Threshold = 5.0 (92%)

5.00

05.0*Z

10

01*Z

30

03*Z

20

02*Z

www.kingston.ac.uk/dirc

The role of system noise Q is to enable the filter to adapt to deviations from the assumed trajectory model.– expands the state uncertainty (and hence the uncertainty of the

predicted position)

System Noise

QAPAPpAp Ttttt 11

ˆ,ˆ

www.kingston.ac.uk/dirc

0 5 10 15 20 25 30 35 40

-5

0

5

10

15

20

x

y

trueobserved

Linear Model / Quadratic Trajectory

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

x

y

true

observedfiltered

0 5 10 15 20 25 30 35 40

0

5

10

15

20

25

x

y

true

observedfiltered

1

1

0

0

002.0Q

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

x

y

true

observedfiltered

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

xy

true

observedfiltered

1

1

0

0

01.0Q

0 5 10 15 20 25 30 35 40

-5

0

5

10

15

20

x

y

true

observed

filtered

0 5 10 15 20 25 30 35 40

0

5

10

15

20

25

30

x

y

true

observedfiltered

0

0

0

0

Q

www.kingston.ac.uk/dirc

-10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

-15 -10 -5 0 5 10 150

5

10

15

20

x

ytrue

observed

filtered

-15 -10 -5 0 5 10 150

5

10

15

20

x

y

true

observed

filtered

10

01*ZZNo Data Association

2

2

07.

07.

0

0

0

0

Q

Quadratic Model / Linear Trajectory

www.kingston.ac.uk/dirc

-5 0 5 10 15 20 25 300

5

10

15

20

25

30

x

y

Noise radius at corner point = 0.78

true

observed

filtered

0 5 10 15 20 250

5

10

15

20

25

x

y

Noise radius at corner point = 1.15

true

observed

filtered

0 5 10 15 20 250

5

10

15

20

25

x

y

Noise radius at corner point = 1.26

true

observed

filtered

0 5 10 15 20 250

5

10

15

20

25

x

y

Noise radius at corner point = 1.44

true

observed

filtered

-10 0 10 20 30 400

5

10

15

20

25

30

35

40

45

x

y

Noise radius at corner point = 0.79

true

observed

filtered

-10 -5 0 5 10 15 20 25 30 350

5

10

15

20

25

30

35

x

y

Noise radius at corner point = 1.15

true

observed

filtered

0 5 10 15 20 25 30 35 40 45

0

5

10

15

20

25

30

x

yNoise radius at corner point = 1.26

true

observed

filtered

0 5 10 15 20 250

5

10

15

20

25

x

y

Noise radius at corner point = 1.44

true

observed

filtered

2

2

003.

003.

0

0

0

0

0Q

2

2

003.

003.

0

0

0

0

10Q

2

2

003.

003.

0

0

0

0

50Q

2

2

003.

003.

0

0

0

0

500Q

www.kingston.ac.uk/dirc

Noisy Observation Stream

tZ

tz

tz

tz

Since Gaussian PDF is infinite, the thresholded gate i.e. χthres would miss a predictable number of true observations present in the stream.

21ˆˆˆ ztttt

Ttt zzZZzz

508.0)( 2 threswheremissedp

True observations are typically accompanied by noisy observations (uniformly distributed?)

densityuniformiswhere,,)( μZfp thresttruefalse

www.kingston.ac.uk/dirc

Noisy Observation Stream

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

x

y

true

observedfiltered

Linear Model / Quadratic Trajectory

1

1

0

0

01.0Q

10

01*Z

20

02Z

(10 Uniformly distributed noise samples)

www.kingston.ac.uk/dirc

Noisy Observation Stream

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

x

y

true

observedfiltered

Linear Model / Quadratic Trajectory

10

01*Z

10

01Z

(10 Uniformly distributed noise samples)

www.kingston.ac.uk/dirc

Noisy Observation Stream

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

x

y

true

observedfiltered

Linear Model / Quadratic Trajectory

10

01*Z

30

03Z

(10 Uniformly distributed noise samples)

www.kingston.ac.uk/dirc

Noisy Observation Stream

0 5 10 15 20 25 30 35 40-10

-5

0

5

10

15

20

x

y

true

observedfiltered

Quadratic Model / Quadratic Trajectory

10

01*Z

30

03Z

(10 Uniformly distributed noise samples)

Higher dimensional model particularly vulnerable near initiation where state covariance high.

Recommended solution is to constrain model to linear trajectory using tight initial state covariance and allow added system noise to enable acceleration term.

Higher dimensional model particularly vulnerable near initiation where state covariance high.

Recommended solution is to constrain model to linear trajectory using tight initial state covariance and allow added system noise to enable acceleration term.

www.kingston.ac.uk/dirc

Summary• The estimated observation noise should be at least as large

as the underlying observation noise.• System noise should reflect deviation from trajectory model• Both observation and system noise significantly increase the

size of the predicted position uncertainty, and, hence the size of the data association gate.

• The data association gate is itself a significant source of noise into the system (typically tackled by including appearance matching).

• (No satisfactory practical method of handling update stage when dealing with missing data.)