Upload
tara
View
32
Download
0
Embed Size (px)
DESCRIPTION
Probability in EECS. Jean Walrand – EECS – UC Berkeley. Kalman Filter. Kalman Filter: Overview. Overview X(n+1) = AX(n) + V(n); Y(n) = CX(n) + W(n); noise ⊥ KF computes L[X(n ) | Y n ] Linear recursive filter, innovation gain K n , error covariance Σ n - PowerPoint PPT Presentation
Citation preview
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