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

%function IMM_Kalman_Tracking_Demo 
% Make a point move in the 2D plane 
% State = (x y xdot ydot). We only observe (x y). 
% Using Multiple Models 
 
% X(t+1) = F X(t) + noise(Q) 
% Y(t) = H X(t) + noise(R) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% parameters 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Generate trajectories 
Nv = 0.02; 
TrajNum = 7; 
[y,t,dt] = Generate2DTrajectories(TrajNum); 
yn = y+randn(size(y)).*Nv; 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Init IMM parameters  
M = 3; 
ModelDim = 3; 
[Fa,Ha,Qa,Ra,InitXa,InitVa,P] = IMM_Init_Model(dt,M,ModelDim); 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% kalman filtering 
[xfilt, Vfilt, VVfilt, xall] = IMM_Kalman_Filter(yn,Fa,Ha,Qa,Ra,P,InitXa,InitVa); 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% show 
 
yfilt = Ha(:,:,1)*xfilt; 
dfilt = y([1 2],:) - yfilt; 
mse_filt = sqrt(sum(sum(dfilt.^2))) 
 
 
figure(10); 
plot(y(1,:), y(2,:), 'k'); 
hold on; 
plot(yn(1,:), yn(2,:), 'b.'); 
plot(yfilt(1,:), yfilt(2,:), 'r'); 
hold off; 
title('IMM - 2D Trajectory') 
legend('true', 'observed', 'filtered', 0) 
xlabel('X1'),ylabel('X2') 
 
if 1, 
figure(11); 
col = 'rgbmkcy'; 
str = []; 
for i=1:M, 
    xm1 = Ha(:,:,i)*xall(:,:,i); 
    plot(xm1(1,:), xm1(2,:), col(i)); hold on; 
    str = [str,'''',num2str(i),''',']; 
end; 
hold off; 
title('IMM -  Each Filter Trajectory'); 
eval(['legend(',str,'0);']) 
%legend('Model 1', 'Model 2', 'Model 3', 0) 
xlabel('X1'),ylabel('X2') 
end; 
 
if 1, 
figure(12); 
subplot(2,1,1),plot(t,xfilt(2,:),'k');ylabel('V1');title('IMM - Velocities') 
subplot(2,1,2),plot(t,xfilt(4,:),'k');ylabel('V2');xlabel('Time') 
 
if ModelDim == 3, 
    figure(6); 
    subplot(2,1,1),plot(t,xfilt(3,:),'k');ylabel('A1');title('IMM - Acceleration') 
    subplot(2,1,2),plot(t,xfilt(6,:),'k');ylabel('A2');xlabel('Time') 
end; 
end;