diff --git a/examples/matlab/LocalizationExample.m b/examples/matlab/LocalizationExample.m
index ae66a131c..dacd6d972 100644
--- a/examples/matlab/LocalizationExample.m
+++ b/examples/matlab/LocalizationExample.m
@@ -43,26 +43,13 @@ initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 result = graph.optimize(initialEstimate);
 result.print(sprintf('\nFinal result:\n  '));
 
-%% Query the marginals
-marginals = graph.marginals(result);
-P{1}=marginals.marginalCovariance(1);
-P{2}=marginals.marginalCovariance(2);
-P{3}=marginals.marginalCovariance(3);
-
-%% 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')
+figure(1);clf;
+plot(result.xs(),result.ys(),'k*-'); hold on
+marginals = graph.marginals(result);
+for i=1:result.size()
+    pose_i = result.pose(i);
+    P{i}=marginals.marginalCovariance(i);
+    plotPose2(pose_i,'g',P{i})
 end
+axis equal
diff --git a/examples/matlab/Pose2SLAMExample.m b/examples/matlab/Pose2SLAMExample.m
index 5641d2ac3..a63c2fd80 100644
--- a/examples/matlab/Pose2SLAMExample.m
+++ b/examples/matlab/Pose2SLAMExample.m
@@ -54,3 +54,16 @@ initialEstimate.print(sprintf('\nInitial estimate:\n'));
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate);
 result.print(sprintf('\nFinal result:\n'));
+
+%% Plot Covariance Ellipses
+figure(1);clf;
+plot(result.xs(),result.ys(),'k*-'); hold on
+plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-');
+marginals = graph.marginals(result);
+for i=1:result.size()
+    pose_i = result.pose(i);
+    P{i}=marginals.marginalCovariance(i);
+    fprintf(1,'%.5f %.5f %.5f\n',P{i})
+    plotPose2(pose_i,'g',P{i})
+end
+axis equal
diff --git a/examples/matlab/covarianceEllipse.m b/examples/matlab/covarianceEllipse.m
index 106a10d6e..491e099c3 100644
--- a/examples/matlab/covarianceEllipse.m
+++ b/examples/matlab/covarianceEllipse.m
@@ -2,7 +2,7 @@ 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  
+% 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
@@ -14,21 +14,21 @@ 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
+    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
\ No newline at end of file