function [  ] = plot_trajectory_particles( xtrue, x, w, dt )
%PLOT_TRAJECTORY_PARTICLES Summary of this function goes here
%   Detailed explanation goes here

% Create a new figure for this plot
figure

% Plot ground truth trajectory:
plot(xtrue(1,:), xtrue(2,:), 'r');
hold on % keep drawing into the same figure

% Draw the estimate of the same trajectory.
% Normalize weights
s = sum(w,3);
w = w ./ repmat(s,1,1,size(w,3));
mux = sum(x.*repmat(w,3,1,1),3);
plot(mux(1,:), mux(2,:), 'b')

% Only at some times: Draw all particles
ptime = 4/dt; % Once in 4 seconds

xdraw = x(:,1:ptime:end,:);

% Draw particles
nSamples = size(x,3);
for i = 1:size(xdraw,2)
    currx = xdraw(:,i,:);
    quiver(reshape(currx(1,:,:),nSamples,1,1), ...
        reshape(currx(2,:,:),nSamples,1,1), ...
        cos(reshape(currx(3,:,:),nSamples,1,1)), ...
        sin(reshape(currx(3,:,:),nSamples,1,1)));        
end

% Description & legend
axis equal
xlabel('x in m')
ylabel('y in m')
title('robot trajectory with uncertainty estimates')
legend('true trajectory', ...
       'estimate (mean)', ...
       'location', 'SouthEast');


end