plotuncertainellip2d.m

来自「移动机器人同时定位与地图创建最前沿技术」· M 代码 · 共 27 行

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SLAM Summer School 2006, Oxford.
% Practical 3. SLAM using Monocular Vision.
% Practical exercise.
% J.M.M. Montiel, Javier Civera, Andrew J. Davison.
% {josemari, jcivera}@unizar.es, [e-mail]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function plotUncertainEllip2D(C,nu,chi2,color)

hold on;

th=0:2*pi/100:2*pi;

x=[cos(th') sin(th')]'*sqrt(chi2);

if(min(eig(C))<0)
    C=eye(2);
    color=[0 0 0];
    fprintf('NPSD matrix, a black false ellipse has been plot\n');
end
K=chol(C)';

nPoints=size(th,2);
y=K*x+[ones(1,nPoints)*nu(1);ones(1,nPoints)*nu(2)];

set(plot(y(1,:),y(2,:)),'Color',color, 'LineWidth', 1.5 );