simple_kalman_tracking_demo.m

IMM PDAF跟踪滤波的程序

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