% function that draws an ellipse of "one sigma" given covariance matrix
% [ell] = sigmaEllipse(covariance, center)
% adopted code from "Robot Localization and SLAM Using Range from
% Radio" at CMU
function [x_vec, y_vec] = sigmaEllipse(covariance, center)

tmp = inv(covariance);

A = tmp(1,1); 
B = tmp(1,2);
C = tmp(2,2); 

T = sqrt(A*A + C*C - 2*A*C + 4*B*B);
if ( A == C )
   theta = 0;
else
   theta = .5*atan(2*B/(A-C));
end
%a = sqrt(10/(A+C-T));
%b = sqrt(0.5/(A+C+T));
SVD=svd(tmp);
a=sqrt(1/SVD(2));
b=sqrt(1/SVD(1));
t = [0:2*pi*50]/50;

if (covariance(1,1)<covariance(2,2))
   tmp = a;
   a = b;
   b = tmp;
end


ell(1,:) = cos(theta)*a*cos(t) - sin(theta)*b*sin(t) + center(1);
ell(2,:) = sin(theta)*a*cos(t) + cos(theta)*b*sin(t) + center(2);

x_vec = ell(1,1:2:length(t));
y_vec = ell(2,1:2:length(t));

