www.pudn.com > un_IMM_PDAF.zip > Simple_Kalman_Tracking_Demo.m

%function Simple_Kalman_Tracking_Demo 
% Tracking a moving point in the 2D plane 
% State = (x xdot y  ydot). We only observe (x y). 
 
% X(t+1) = F X(t) + noise(Q) 
% Y(t) = H X(t) + noise(R) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% parameters 
ProbDim     = 2;    % problem dimensionality 
ModelDim    = 2;    % kalman filter model dimension 
TrajNum     = 6;    % trajectory test number 1-circ.,2-log,5-exp,6-triang,7-rise 
 
Nv          = 0.0;  % noise 
StateVar    = 5; 
ObserVar    = .05; 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Generate trajectories 
[y,t,dt]    = Generate2DTrajectories(TrajNum); 
yn          = y+randn(size(y)).*Nv; 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Init Kalman Filter parameters 
[F,H,Q,R,ObservInd,initx,initP] = Kalman_Filter_Init(dt,ModelDim,ProbDim,StateVar,ObserVar); 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% kalman filtering 
[x, P, K, LL]   = Simple_Kalman_Filter(yn, F, H, Q, R, initx, initP); 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% show results 
yest    = H*x; 
dfilt   = y - yest; 
fprintf('   Total MSE :%2.4f\n', sqrt(sum(sum(dfilt.^2)))) 
 
 
figure(1); 
plot(y(1,:), y(2,:), 'k');hold on; 
plot(yn(1,:), yn(2,:), 'b.'); 
plot(yest(1,:), yest(2,:), 'r'); 
hold off; 
title('Simple Kalman - 2D Trajectory') 
legend('true', 'observed', 'filtered', 0) 
xlabel('X1'),ylabel('X2') 
 
if 1, 
figure(2); 
plot(t, std(dfilt)); 
title('Kalman Error') 
xlabel('Time'),ylabel('Error') 
end; 
 
if 1, 
figure(3); 
semilogy(t, LL); 
title('Log liklihood') 
xlabel('Time'),ylabel('Probability') 
end; 
 
if 1, 
figure(4); 
%plot(t, squeeze(P(ObservInd(1),ObservInd(1),:)),'b',t, squeeze(P(ObservInd(2),ObservInd(2),:)),'r'); 
%plot(t, squeeze(P(1,1,:)),'r',t, squeeze(P(3,3,:)),'b'); 
%title('Variance') 
 
subplot(2,1,1),semilogy(t,squeeze(P(1,1,:)),'r',t, squeeze(P(2,2,:)),'b');ylabel('X'); 
title('Variances Loc (Red), Velocity (Blue)') 
subplot(2,1,2),semilogy(t,squeeze(P(3,3,:)),'r',t, squeeze(P(4,4,:)),'b');ylabel('Y');xlabel('Time') 
xlabel('Time') 
end; 
 
if 1, 
figure(5); 
subplot(2,1,1),plot(t,x(2,:),'k');ylabel('V1');title('Simple Kalman - Velocities') 
subplot(2,1,2),plot(t,x(4,:),'k');ylabel('V2');xlabel('Time') 
end; 
 
if 1, 
figure(6); 
subplot(2,1,1),plot(t,squeeze(K(:,1,:))');ylabel('X');title('Kalman Gains') 
subplot(2,1,2),plot(t,squeeze(K(:,2,:))');ylabel('Y');xlabel('Time') 
end; 
 
 
if ModelDim == 3, 
    figure(7); 
    subplot(2,1,1),plot(t,x(3,:),'k');ylabel('A1');title('Simple Kalman - Acceleration') 
    subplot(2,1,2),plot(t,x(6,:),'k');ylabel('A2');xlabel('Time') 
end; 
end;