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

%function Simple_PDAF_Tracking_Demo 
% Tracking a moving point in 2D plane 
% State = (x xdot y  ydot). We only observe (x y). 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% parameters 
ProbDim  = 2; 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Generate trajectories 
Nv = 0.01; 
TrajNum = 7; 
[y,t,dt] = Generate2DTrajectories(TrajNum); 
yn = y+randn(size(y)).*Nv; 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Generate clutter (Assuming the trajectory in [0 1] square) 
ClutPointNum = 25; 
yc = rand(size(y,1),ClutPointNum,size(y,2)); 
yc(:,3,:) = yn; 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Init Kalman Filter parameters 
ModelDim = 2; 
[F,H,Q,R,ObservInd] = Kalman_Filter_Init(dt,ModelDim,ProbDim); 
initx = zeros(size(F,1),1); 
initx(ObservInd)=yn([1 2],1); %[.5 .5]'; %%[yn(1,1) 0 yn(2,1) 0]'; 
initV = Q; 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% PDAF filtering 
[xfilt, Vfilt] = Simple_PDAF_Filtering(yc, F, H, Q, R, initx, initV); 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% show results 
dfilt = y([1 2],:) - xfilt(ObservInd,:); 
mse_filt = sqrt(sum(sum(dfilt.^2))) 
 
 
figure(21); 
plot(y(1,:), y(2,:), 'k');hold on; 
plot(yn(1,:), yn(2,:), 'b.'); 
plot(xfilt(ObservInd(1),:), xfilt(ObservInd(2),:), 'r'); 
hold off; 
title('Simple Kalman - 2D Trajectory') 
legend('true', 'observed', 'filtered', 0) 
xlabel('X1'),ylabel('X2') 
 
if 0, 
figure(22); 
plot(t, std(dfilt)); 
title('Kalman Error') 
xlabel('Time'),ylabel('Error') 
end; 
 
if 1, 
figure(23); 
subplot(2,1,1),plot(t,xfilt(2,:),'k');ylabel('V1');title('Simple Kalman - Velocities') 
subplot(2,1,2),plot(t,xfilt(4,:),'k');ylabel('V2');xlabel('Time') 
 
if ModelDim == 3, 
    figure(24); 
    subplot(2,1,1),plot(t,xfilt(3,:),'k');ylabel('A1');title('Simple Kalman - Acceleration') 
    subplot(2,1,2),plot(t,xfilt(6,:),'k');ylabel('A2');xlabel('Time') 
end; 
end; 
 
% running show old 
 
if 0, 
ylen = size(y,2); 
figure(25); 
h = plot(yc(1,:,1), yc(2,:,1), 'b.');  
hold on; 
hf = plot(xfilt(ObservInd(1),1), xfilt(ObservInd(2),1), 'ro'); 
axis([0 1 0 1].*1.1); 
title('Simple Kalman - 2D Trajectory') 
xlabel('X1'),ylabel('X2') 
for i=1:ylen, 
    delete(h);  
    delete(hf); 
    h = plot(yc(1,:,i), yc(2,:,i), 'b.');  
    hf = plot(xfilt(ObservInd(1),i), xfilt(ObservInd(2),i), 'ro'); 
    drawnow; 
    Mov(i) = getframe; 
end; 
hold off; 
%movie(Mov,1); 
end; 
 
 
 
% running show new 
 
if 1, 
ylen = size(y,2); 
yfilt = H*xfilt; %(ObservInd,:) 
     
% init. 
figure(25); 
DataHand = plot(yc(1,:,1), yc(2,:,1), 'b.');  
axis([0 1 0 1].*1.1); 
title('Simple Kalman - 2D Trajectory') 
xlabel('X1'),ylabel('X2') 
hold on; 
 
ElipseHand = Create_Elipse(yfilt(:,1),H,Vfilt(:,:,1),R); 
 
for i=1:ylen, 
    delete(DataHand);  
    delete(ElipseHand); 
    DataHand = plot(yc(1,:,i), yc(2,:,i), 'b.');  
    ElipseHand = Create_Elipse(yfilt(:,i),H,Vfilt(:,:,i),R); 
    drawnow; 
    Mov(i) = getframe; 
end; 
hold off; 
%movie(Mov,1); 
end;