Beruflich Dokumente
Kultur Dokumente
1D tracking
2D tracking
General Setup
Estimation of a stochastic dynamic system
Dynamics
Xk = Fk1 Xk1 + Bk1 uk1 + Wk1
uk1 : control-input
Wk1 : noise
Observations
Yk = Hk Xk + Zk
Yk is observed
Zk is noise
Derivation
Derivation in the simpler model where the dynamics is of the form
Xk = ak1 Xk1 + Wk1
and the observations
Yk = Xk + Zk
The objective is to find, for each time k, the minimum MSE filter based on
Y0 , Y1 , . . . , Yk1
k
X
(k1)
k =
X
hj
Ykj
j=1
k
X
j=1
(k1)
hj
Ykj )Y` ) = 0,
` = 0, 1, . . . , k 1.
Recursion
The beautiful thing about the Kalman filter is that one can almost deduce the
optimal filter to predict Xk+1 from that predicting Xk .
(k)
(k)
(k1)
j = 1, . . . , k.
(k)
(k)
How to find h1 ?
Observe that the next prediction is equal to
k+1 = h1(k) Yk +
X
k
X
(k)
(k1)
(ak h1 )hj
Ykj
j=1
k + h1(k) (Yk X
k)
= ak X
Interpretation
k+1 = ak X
k + h1(k) Ik
X
k is the prediction based on the estimate at time k
ak X
(k)
h1
Error of Prediction
(k)
(k)
(k)
(k)
so that
(k)
(k)
which is given by
(k)
h1
ak E(2k )
E(2k ) + E(Zk2 )
ak E(2k )
E(2k ) + E(Zk2 )
k+1 = ak X
k + h1(k) (Yk X
k)
X
(k)
10
Benefits
Requires no knowledge about the structure of Wk and Zk (only variances)
Easy implementation
Many applications
Autopilot
Many others
11
General Formulation
Xk = Fk1 Xk1 + Wk1
Yk = Hk Xk + Zk
The covariance of Wk is Qk and that of Zk is Rk .
Two variables:
k|k estimate of the state at time k based upon Y0 , . . . , Yk1
X
k|k )
Ek|k error covariance matrix, Ek|k = Cov(Xk X
12
Prediction
k+1|k = Fk X
k|k1
X
Ek+1|k = Fk Ek|k1 FkT + Qk
Update
k+1|k
Ik = Yk Hk X
Sk = Hk Ek+1|k HkT + Rk
Kk = Ek+1|k HkT Sk1
k+1|k+1 = X
k+1|k + Kk Ik
X
Ek+1|k+1 = (Id Kk Hk )Ek+1|k
Innovation
Innovation covariance
Kalman Gain
Updated state estimate
Updated error covariance
13
k = 1, 2, . . .
k = 1, 2, . . .
14
0.7
0.6
Voltage
0.5
0.4
0.3
0.2
0.1
estimate
exact process
measurements
0
10
15
20
25
Iteration
30
35
40
45
50
15
0.7
0.6
Voltage
0.5
0.4
0.3
0.2
0.1
estimate
exact process
measurements
0
10
15
20
25
Iteration
30
35
40
45
50
16
0.7
0.6
Voltage
0.5
0.4
0.3
0.2
0.1
estimate
exact process
measurements
0
10
15
20
25
Iteration
30
35
40
45
50
17
1D Tracking
Estimation of the position of a vehicle.
Let X be a state variable (position and speed), and A is a transition matrix
1 t
.
A=
0 1
The process is governed by:
Xn+1 = AXn + Wn
where Wn is a zero-mean Gaussian white noise process. The observation is
Yn = CXn + Zn
where the matrix C only picks up the position and Zn is another zero-mean
Gaussian white noise process independent of Wn .
18
Position
!2
!4
10
15
Time
20
25
19
2D Example
General setup
X(t + 1) = F X(t) + W (t),
Y (t) = HX(t) + V (t),
W N (0, Q),
V N (0, R)
x1 (t)
x (t)
0
2
=
dx1 (t)
0
dx2 (t)
0
0
x1 (t 1)
w1(t 1)
1 x2 (t 1) w2 (t 1)
0 dx1 (t 1) dw1 (t 1)
1
dx2 (t 1)
dw2 (t 1)
20
Observations
We only observe the position of the particle.
y (t)
1
1 =
y2 (t)
0
x1 (t)
0
x2 (t)
v1 (t)
0
v2 (t)
dx1 (t)
dx2 (t)
Source: http://www.cs.ubc.ca/murphyk/Software/Kalman/kalman.html
21
Implementation
% Make a point move in the 2D plane
% State = (x y xdot ydot). We only observe (x y).
22
23
24
70
true
observed
filtered
60
X2
50
40
30
20
10
!40
!30
!20
!10
X1
10
20
25
140
120
100
X2
80
60
40
20
true
observed
filtered
0
20
40
60
80
X1
100
120
140
26
27
(0
*+,./0-+1-2
03..*4-2
'0
)2
50
!0
#0
20
10
!!0
!#0
!20
!10
)1
10
20
28
140
120
100
X2
80
60
40
20
true
observed
smoothed
0
20
40
60
80
X1
100
120
140