function [  ] = plot_trajectory_cov(xtrue, mean, cov, dt )
%PLOT_TRAJECTORY_COV Plot the 2d trajectory of a robot with uncertainty
%                    estimates

% 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.
plot(mean(1,:), mean(2,:))

% Only at some times: Visualize the estimate uncertainty.
ellipsetime = 4/dt; % Once in 4 seconds
em = mean(:,1:ellipsetime:end);
ecov = cov(:,1:ellipsetime:end);

% Draw uncertainty ellipses
for i = 1:size(ecov,2)
    s = reshape(ecov(:,i),3,3);
    
    % Compute and plot uncertainty ellipse for position covariance.
    p_pos = get_ellipse(em(:,i), s);
    plot(p_pos(1,:), p_pos(2,:),'k');
    
    % Compute and plot angle interval showing orientation uncertainty.
    p_th  = get_angle_bracket(em(:,i), s);
    plot(p_th(1,:), p_th(2,:),'k');
end

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


end