% plot the particles after update and resampling

if ~exist('Hfigure2','var'),
    
    Hfigure2 = figure; axis(axislim); %axis equal;
    set(Hfigure2,'Position',[100 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', Hfigure2);
catch
    figure(Hfigure2);
end

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

Hplot2 = [];
if N_k_p>0,
    hh = plot3(x_km1_p(1,:),x_km1_p(2,:),x_km1_p(3,:),'c*');
    Hplot2 = [Hplot2 hh];
end
if m_k>0
    hh = plot3(Z(1,:),Z(2,:),Z(3,:),'ro','markersize',10);
    Hplot2 = [Hplot2 hh];
end

if N_k_b>0,
    hh = plot3(x_km1_b(1,:),x_km1_b(2,:),x_km1_b(3,:),'b*');
    Hplot2 = [Hplot2 hh];
    
end
hold off;
drawnow
