imm_kalman_tracking_demo.m

IMM PDAF跟踪滤波的程序

%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;