Plot covariance ellipses in MATLAB

examples/matlab/LocalizationExample.m

@@ -45,6 +45,24 @@ result.print(sprintf('\nFinal result:\n  '));
 
 %% Query the marginals
 marginals = graph.marginals(result);
-x1=gtsamSymbol('x',1); marginals.marginalCovariance(x1.key)
+x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key)
-x2=gtsamSymbol('x',2); marginals.marginalCovariance(x2.key)
+x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key)
-x3=gtsamSymbol('x',3); marginals.marginalCovariance(x3.key)
+x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key)
+
+%% Plot Trajectory
+figure(1)
+clf
+X=[];Y=[];
+for i=1:3
+   pose_i = result.pose(i);
+   X=[X;pose_i.x];
+   Y=[Y;pose_i.y];
+end
+plot(X,Y,'b*-');
+
+%% Plot Covariance Ellipses
+hold on
+for i=1:3
+   pose_i = result.pose(i);
+   covarianceEllipse([pose_i.x;pose_i.y],P{i},'g')
+end

examples/matlab/covarianceEllipse.m

@@ -0,0 +1,34 @@
+function covarianceEllipse(x,P,color)
+% covarianceEllipse: plot a Gaussian as an uncertainty ellipse
+% Based on Maybeck Vol 1, page 366
+% k=2.296 corresponds to 1 std, 68.26% of all probability
+% k=11.82 corresponds to 3 std, 99.74% of all probability  
+%
+% covarianceEllipse(x,P,color)
+% it is assumed x and y are the first two components of state x
+
+[e,s] = eig(P(1:2,1:2));
+s1 = s(1,1);
+s2 = s(2,2);
+k = 2.296;
+[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
+line(ex,ey,'color',color);
+
+function [x,y] = ellipse(a,b,c);
+% ellipse: return the x and y coordinates for an ellipse
+% [x,y] = ellipse(a,b,c);
+% a, and b are the axes. c is the center
+
+global ellipse_x ellipse_y
+if ~exist('elipse_x')
+  q =0:2*pi/25:2*pi;
+  ellipse_x = cos(q);
+  ellipse_y = sin(q);
+end
+
+points = a*ellipse_x + b*ellipse_y;
+x = c(1) + points(1,:);
+y = c(2) + points(2,:);
+end
+
+end