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