Professional Documents
Culture Documents
• Example
• General Setup
• Derivation
• Numerical examples
– Estimating the voltage
– 1D tracking
– 2D tracking
2
• Initial position X0 = 0
• Observations
Yk = Xk + Zk
where Zk is a noise term.
General Setup
Estimation of a stochastic dynamic system
• Dynamics
Xk = Fk−1 Xk−1 + Bk−1 uk−1 + Wk−1
– Xk : state of the system at time k
– uk−1 : control-input
– Wk−1 : 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
The objective is to find, for each time k, the minimum MSE filter based on
Y0 , Y1 , . . . , Yk−1
k
(k−1)
X
X̂k = hj Yk−j
j=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) (k−1)
hj+1 = (ak − h1 )hj , j = 1, . . . , k.
(k)
Given the filter h(k−1) , we only need to find h1 to get the filter at the next
time step.
6
(k)
How to find h1 ?
Observe that the next prediction is equal to
k
(k) (k) (k−1)
X
X̂k+1 = h1 Yk + (ak − h1 )hj Yk−j
j=1
(k)
= ak X̂k + h1 (Yk − X̂k )
Interpretation
(k)
X̂k+1 = ak X̂k + h1 Ik
Error of Prediction
(k)
To find h1 , we look at the error of prediction
k = Xk − X̂k
• 0 = Z0
• E(k ) = 0
(k) (k)
• E(2k+1 ) = [ak − h1 ]2 E(2k ) + E(Wk2 ) + [h1 ]2 E(Zk2 )
8
(k)
To minimize the MSE k+1 , we adjust h1 so that
(k) (k)
∂h(k) E(2k+1 ) = 0 = −2(ak − h1 )E(2k ) + 2h1 E(Zk2 )
1
which is given by
(k) ak E(2k )
h1 =
E(2k ) + E(Zk2 )
• Loop: for k = 0, 1, . . .
(k) ak E(2k )
h1 =
E(2k ) + E(Zk2 )
(k)
X̂k+1 = ak X̂k + h1 (Yk − X̂k )
(k)
E(2k+1 ) = ak (ak − h1 )E(2k ) + E(Wk2 )
10
Benefits
• Requires no knowledge about the structure of Wk and Zk (only variances)
• Easy implementation
• Many applications
– Inertial guidance system
– Autopilot
– Satellite navigation system
– Many others
11
General Formulation
Prediction
X̂k+1|k = Fk X̂k|k−1
Ek+1|k = Fk Ek|k−1 FkT + Qk
Update
Ik = Yk − Hk X̂k+1|k Innovation
Sk = Hk Ek+1|k HkT + Rk Innovation covariance
Kk = Ek+1|k HkT Sk−1 Kalman Gain
X̂k+1|k+1 = X̂k+1|k + Kk Ik Updated state estimate
Ek+1|k+1 = (Id − Kk Hk )Ek+1|k Updated error covariance
13
X k = X 0 + Wk , k = 1, 2, . . .
Zk = Xk + Vk , k = 1, 2, . . .
0.7
0.6
0.5
Voltage
0.4
0.3
0.2
0.1 estimate
exact process
measurements
0
0 5 10 15 20 25 30 35 40 45 50
Iteration
15
0.7
0.6
0.5
Voltage
0.4
0.3
0.2
0.1 estimate
exact process
measurements
0
0 5 10 15 20 25 30 35 40 45 50
Iteration
16
0.7
0.6
0.5
Voltage
0.4
0.3
0.2
0.1 estimate
exact process
measurements
0
0 5 10 15 20 25 30 35 40 45 50
Iteration
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
Xn+1 = AXn + Wn
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
4
Position
!2
!4
0 5 10 15 20 25
Time
19
2D Example
General setup
x1 (t) 1 0 1 0 x1 (t − 1) w1(t − 1)
x (t)
2
0 1 0 1 x2 (t − 1) w2 (t − 1)
= +
dx1 (t)
0
0 1 0 dx1 (t − 1) dw1 (t − 1)
dx2 (t) 0 0 0 1 dx2 (t − 1) dw2 (t − 1)
20
Observations
We only observe the position of the particle.
x1 (t)
y (t) 1 0 0 0 x2 (t) v1 (t)
1 =
+
y2 (t) 0 1 0 0 dx1 (t)
v2 (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).
ss = 4; % state size
os = 2; % observation size
F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
H = [1 0 0 0; 0 1 0 0];
Q = 1*eye(ss);
R = 10*eye(os);
initx = [10 10 1 0]’;
initV = 10*eye(ss);
22
figure;
plot(x(1,:), x(2,:), ’ks-’);
hold on
plot(y(1,:), y(2,:), ’g*’);
plot(xfilt(1,:), xfilt(2,:), ’rx:’);
hold off
legend(’true’, ’observed’, ’filtered’, 0)
xlabel(’X1’), ylabel(’X2’)
24
70
true
observed
filtered
60
50
X2
40
30
20
10
!40 !30 !20 !10 0 10 20
X1
25
140
120
100
80
X2
60
40
20
true
observed
filtered
0
0 20 40 60 80 100 120 140
X1
26
figure;
hold on
plot(x(1,:), x(2,:), ’ks-’);
plot(y(1,:), y(2,:), ’g*’);
plot(xsmooth(1,:), xsmooth(2,:), ’rx:’);
hold off
legend(’true’, ’observed’, ’smoothed’, 0)
xlabel(’X1’), ylabel(’X2’)
27
(0
*+,-
./0-+1-2
03..*4-2
'0
50
)2
!0
#0
20
10
!!0 !#0 !20 !10 0 10 20
)1
28
140
120
100
80
X2
60
40
20
true
observed
smoothed
0
0 20 40 60 80 100 120 140
X1