diff --git a/src/fit_ellipse_3d.m b/src/fit_ellipse_3d.m
new file mode 100644
index 0000000000000000000000000000000000000000..779b4a7a1f7d508d82ac5a7da541c524dcb086e4
--- /dev/null
+++ b/src/fit_ellipse_3d.m
@@ -0,0 +1,31 @@
+function [ f, v ] = fit_ellipse_3d( p, E, method )
+%FIT_ELLIPSE_3D Fit a 2D ellipse on the 3D points.
+%   The fit requires the Euler angles of the plane fitted through the
+%   opints, so that we can project them on this plane. We then make a 2D 
+%   ellipse fit on the projected points. This turns to be much more robust 
+%   than a 3D fit, and also closely match our configuration.
+
+%   Greatly inspired from https://stackoverflow.com/questions/29051168
+%   /data-fitting-an-ellipse-in-3d-space
+
+    if nargin < 3
+        method = 'direct';
+    end
+    
+    c = mean( p );
+    p = p - repmat( c, size( p, 1 ), 1 );
+
+    % Fit a plane to these points.
+    if nargin < 2
+        [ ~, ~, v ] = svd( p );
+    else
+        v = euleurZXZ2rot( E );
+    end
+    
+    % Rotate the points into the principal axes frame.
+    pt = p * v;
+
+    f = fit_ellipse_2d( pt( :, 1:2 ), method );
+    
+end
+
diff --git a/src/plot_ellipse_3d.m b/src/plot_ellipse_3d.m
new file mode 100644
index 0000000000000000000000000000000000000000..dd8e2e1f885bd10a825015f012cd31b24190d789
--- /dev/null
+++ b/src/plot_ellipse_3d.m
@@ -0,0 +1,37 @@
+function h = plot_ellipse_3d( f, v, npoints)
+%PLOT_ELLIPSE_3D Plot a 2D ellipse in 3D.
+
+    if nargin < 3
+        npoints = 20;
+    end
+    
+    x0      = f(1);
+    y0      = f(2);
+    a       = f(3);
+    b       = f(4);
+    theta   = f(5);
+
+    R = [   cos( theta ) sin( theta ) ;
+        -sin( theta ) cos( theta ) ] ;
+    
+    t = linspace( 0 , 2 * pi, npoints )';
+    XY0 = [ a * sin(t), b * cos(t) ];
+    XY1 = XY0 * R;
+    xr = XY1( :, 1 ) + x0;
+    yr = XY1( :, 2 ) + y0;
+    zr = zeros( numel( xr ), 1 );
+    pr = [ xr yr zr ];
+    
+    % Transform back
+    pb = pr * v';
+    xb = pb( :, 1 );
+    yb = pb( :, 2 );
+    zb = pb( :, 3 );
+    
+    xb = [ xb ; xb(1,:) ];
+    yb = [ yb ; yb(1,:) ];
+    zb = [ zb ; zb(1,:) ];
+    h = line( xb, yb, zb );
+
+end
+