Download - Probability in EECS

Transcript
Page 1: Probability in EECS

Probability in EECS

Jean Walrand – EECS – UC Berkeley

Kalman Filter

Page 2: Probability in EECS

Kalman Filter: OverviewOverview

X(n+1) = AX(n) + V(n); Y(n) = CX(n) + W(n); noise ⊥ KF computes L[X(n) | Yn] Linear recursive filter, innovation gain Kn, error covariance Σn

If (A, C) observable and Σ0 = 0, then Σn → Σ finite, Kn → K If, in addition, (A, Q = (ΣV)1/2) reachable,

then filter with Kn = K is asymptotically optimal (i.e., Σn → Σ)

Page 3: Probability in EECS

Kalman Filter1. Kalman

Rudolf (Rudy) Emil Kálmán

b. 5/19/1930 in BudapestResearch Institute for Advanced Studies Baltimore, Maryland from 1958 until 1964.Stanford 64-71Gainesville, Florida 71-92

He is most noted for his co-invention and development of the Kalman Filter that is widely used in control systems, avionics, and outer space manned and unmanned vehicles.For this work, President Obama awarded him with the National Medal of Science on October 7, 2009.

Page 4: Probability in EECS

Kalman Filter2. ApplicationsTracking and Positioning:

Apollo Program, GPS, Inertial Navigation, …

Page 5: Probability in EECS

Kalman Filter2. ApplicationsCommunications:

MIMO

Page 6: Probability in EECS

Kalman Filter2. ApplicationsCommunications:

Video Motion Estimation

Page 7: Probability in EECS

Kalman Filter2. ApplicationsBio-Medical:

Page 8: Probability in EECS

Kalman Filter3. Big Picture

Page 9: Probability in EECS

Kalman Filter3. Big Picture (continued)

Page 10: Probability in EECS

Kalman Filter4. Formulas

Page 11: Probability in EECS

Kalman Filter5. MATLAB

for n = 1:N-1

V = normrnd(0,Q); W = normrnd(0,R);

X(n+1,:) = A*X(n,:) + V; Y = C*X(n+1,:) + W; S = A*H*A' + Q*Q’; K = S*C'*(C*S*C'+R*R’)^(-1); H = (1 - K*C)*S; Xh(n+1,:) = (A - K*C*A)*Xh(n,:) + K*Y; end

System

KF

Page 12: Probability in EECS

Kalman Filter5. MATLAB (complete)

%FILTER S = A*H*A' + SV; %Gain calculation K = S*C'*(C*S*C'+SW)^(-1); H = (1 - K*C)*S; %Estimate update Xh(:,n+1) = (A - K*C*A)*Xh(:,n) + K*Y; end %PLOT P = [X(2,:); Xh(2,:)]'; plot(P)

% KF: System X+ = AX + V, Y = CX + W (by X+ we mean X(n+1))% \Sigma_V = Q^2, \Sigma_W = R^2% we construct a gaussian noise V = normrnd(0, Q), W = normrnd(0, R)% where Z is iid N(0, 1)% The filter is Xh+ = (A - KCA)Xh + KY (Here Xh is the estimate)% K+ = SC'[CSC' + R^2)% S = AHA' + Q^2 (Here H = \Sigma = est. error cov.)% H+ = (1 - KC)S % CONSTANTSA = [1, 1; 0, 1];C = [1, 0]; Q = [1; 0];R = 0.5;SV = Q*Q';SW = R*R'; N = 20; %time steps M = length(A);

%SYSTEMX = zeros(M, N);Xh = X;H = zeros(M, M);K = H; X(:,1) = [0; 3]; %initial state for n = 1:N-1% These are the system equations V = normrnd(0,Q) W = normrnd(0,R;); X(:,n+1) = A*X(:,n) + V; Y = C*X(:,n+1) + W;

Page 13: Probability in EECS

Kalman Filter6. Example 1: Random Walk

A = 1 ;C = 1 ; Q = 0.2;R = 0.3;

A = 1 ;C = 1 ; Q = 0.2;R = 1;

Page 14: Probability in EECS

Kalman Filter6. Example 1: Random Walk

A = 1 ;C = 1 ; Q = 0.2;R = 0.3;

A = 1 ;C = 1 ; Q = 0.2;R = 1;

Page 15: Probability in EECS

Kalman Filter6. Example 1: Random Walk

A = 1 ;C = 1 ; Q = 10;R = 10;

Page 16: Probability in EECS

Kalman Filter7. Example 2: RW with unknown drift

A = [1, 1; 0, 1];C = [1, 0]; Q = [1; 0];R = 0.5;

Page 17: Probability in EECS

Kalman Filter8. Example 3: 1-D tracking, changing drift

A = [1, 1; 0, 1];C = [1, 0]; Q = [1; 0.1];R = 0.5;

Page 18: Probability in EECS

Kalman Filter9. Example 4: Falling body

A = [1, 1; 0, 1];C = [1, 0]; Q = [10; 0];R = 40;

Page 19: Probability in EECS

Kalman Filter10. Simulink