drawprobellipse.m

it is a matlab file foe develop SLAM localization this is a toolbox for develop develop realtime e

%DRAWPROBELLIPSE Draw elliptic probability region of a Gaussian in 2D.
%   DRAWPROBELLIPSE(X,C,ALPHA,COLOR) draws the elliptic iso-probabi-
%   lity contour of a Gaussian distributed bivariate random vector X
%   at the significance level ALPHA. The ellipse is centered at X =
%   [x; y] where C is the associated 2x2 covariance matrix. COLOR is
%   a [r g b]-vector or a color string such as 'r' or 'g'.
%
%   X and C can also be of size 3x1 and 3x3 respectively.
%
%   For proper scaling, the function CHI2INVTABLE is employed to
%   avoid the use of CHI2INV from the Matlab statistics toolbox.
%
%   In case of a negative definite matrix C, the ellipse collapses
%   to a line which is drawn instead.
%
%   H = DRAWPROBELLIPSE(...) returns the graphic handle H.
%
%   See also DRAWELLIPSE, CHI2INVTABLE, CHI2INV.

% v.1.0-v.1.3, 97-Jan.03, Kai Arras, ASL-EPFL
% v.1.4, 03.12.03, Kai Arras, CAS-KTH: toolbox version


function h = drawprobellipse(x,C,alpha,color);

% Calculate unscaled half axes
sxx = C(1,1); syy = C(2,2); sxy = C(1,2);
a = sqrt(0.5*(sxx+syy+sqrt((sxx-syy)^2+4*sxy^2)));   % always greater
b = sqrt(0.5*(sxx+syy-sqrt((sxx-syy)^2+4*sxy^2)));   % always smaller

% Remove imaginary parts in case of neg. definite C
if ~isreal(a), a = real(a); end;
if ~isreal(b), b = real(b); end;

% Scaling in order to reflect specified probability
a = a*sqrt(chi2invtable(alpha,2));
b = b*sqrt(chi2invtable(alpha,2));

% Look where the greater half axis belongs to
if sxx < syy, swap = a; a = b; b = swap; end;

% Calculate inclination (numerically stable)
if sxx ~= syy,
  angle = 0.5*atan(2*sxy/(sxx-syy));	
elseif sxy == 0,
  angle = 0;     % angle doesn't matter 
elseif sxy > 0,
  angle =  pi/4;
elseif sxy < 0,
  angle = -pi/4;
end;
x(3) = angle;

% Draw ellipse
h = drawellipse(x,a,b,color);