Probability in EECS

Jean Walrand – EECS – UC Berkeley

Kalman Filter

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 → Σ)

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.

Kalman Filter2. ApplicationsTracking and Positioning:

Apollo Program, GPS, Inertial Navigation, …

Kalman Filter2. ApplicationsCommunications:

MIMO

Kalman Filter2. ApplicationsCommunications:

Video Motion Estimation

Kalman Filter2. ApplicationsBio-Medical:

Kalman Filter3. Big Picture

Kalman Filter3. Big Picture (continued)

Kalman Filter4. Formulas

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

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;

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;

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;

Kalman Filter6. Example 1: Random Walk

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

Kalman Filter7. Example 2: RW with unknown drift

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

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

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

Kalman Filter9. Example 4: Falling body

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

Kalman Filter10. Simulink