% plot the particles after update and resampling

if ~exist('HfigureResults','var'),
    
    HfigureResults = figure; axis(axislim); %axis equal;
    set(HfigureResults,'Position',[800 40 600 380]);grid on;
    xlabel('x (height, m)');ylabel('y (height, m)');zlabel('z (depth, m)');
    view([-37.5,30]);
    cameratoolbar('Show');
    cameratoolbar('SetMode', 'orbit');
    cameratoolbar('SetCoordSys','y');
end


try
    set(0, 'CurrentFigure', HfigureResults);
catch
    figure(HfigureResults);
end

% figure(Hfigure2)
hold on;
% if exist('Hplot2','var'),
%     delete(Hplot2);
% end

% if size(currentHeadpose,2)>0 % at least one estimated target
for i = 1:size(currentHeadpose,2),
    id = currentHeadpose(4,i);
    plot3(currentHeadpose(1,i), currentHeadpose(2,i), currentHeadpose(3,i), '*','color', colours(id,:));
end
%     plot3(currentHeadpose(1,:),currentHeadpose(2,:),currentHeadpose(3,:),'r*');
% end


hold off;
drawnow
