% plot the particles after prediction

% Sets the current figure without raising its window.
if ~exist('Hfigure','var'),
    Hfigure = figure; axis(axislim); %axis equal;
    set(Hfigure,'Position',[100 580 600 400]);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', Hfigure);
catch
    figure(Hfigure);
end

% figure(Hfigure)
hold on;
if exist('Hplot','var'),
    delete(Hplot);
end

Hplot = [];
if N_km1>0,
    hh = plot3(x_k_km1_p(1,:),x_k_km1_p(2,:),x_k_km1_p(3,:),'c*');
    Hplot = [Hplot hh];
end
if N_k_b>0,
    hh = plot3(x_k_km1_b(1,:),x_k_km1_b(2,:),x_k_km1_b(3,:),'b*');
    Hplot = [Hplot hh];
    if m_k>0
        hh = plot3(Z(1,:),Z(2,:),Z(3,:),'ro','markersize',10);
        Hplot = [Hplot hh];
    end
end
hold off;

drawnow
