function [ p ] = get_ellipse( mean, cov )
%GET_ELLIPSE Compute points on the 95% uncertainty ellipse.

% Compute eigenvalues and eigenvectors of marginalized position covariance 
[V D] = eig(cov(1:2,1:2));
[D order] = sort(diag(D),'descend'); %sort eigenvalues in descending order
V = V(:,order);
evec_max = V(:, 1); % eigenvector with alrgest eigenvalue

% Construct uncertainty ellipse in parallel to eigenvectors
scale = 2.; % in multiples of sigma
a = scale*sqrt(D(1));
b = scale*sqrt(D(2));
theta_grid = linspace(0,2*pi);
ellipse_x_r  = a*cos( theta_grid );
ellipse_y_r  = b*sin( theta_grid );

% Transform to world frame
phi = atan2(evec_max(2), evec_max(1)); % Angle between x and largest EV
R = [cos(phi) -sin(phi); sin(phi) cos(phi)];
p = R*[ellipse_x_r;ellipse_y_r];
p = p + repmat(mean(1:2),1,size(p,2));

end