www.kingston.ac.uk/dirc dr graeme a. jones tools from the vision tool box kalman tracker - noise and...
Post on 20-Dec-2015
216 views
TRANSCRIPT
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.)