diff --git a/gtsam.h b/gtsam.h
index 254ae9607..663f9ce48 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -418,6 +418,7 @@ virtual class Rot3 : gtsam::Value {
   double pitch() const;
   double yaw() const;
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
+  Vector quaternion() const;
 };
 
 virtual class Pose2 : gtsam::Value {
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 1c39d4311..a8e4a6f51 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -345,6 +345,12 @@ namespace gtsam {
      */
     Quaternion toQuaternion() const;
 
+    /**
+     * Converts to a generic matrix to allow for use with matlab
+     * In format: w x y z
+     */
+    Vector quaternion() const;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 1f86ee2f5..4ae1f1788 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -343,6 +343,17 @@ Quaternion Rot3::toQuaternion() const {
   return Quaternion(rot_);
 }
 
+/* ************************************************************************* */
+Vector Rot3::quaternion() const {
+  Quaternion q = toQuaternion();
+  Vector v(4);
+  v(0) = q.w();
+  v(1) = q.x();
+  v(2) = q.y();
+  v(3) = q.z();
+  return v;
+}
+
 /* ************************************************************************* */
 pair<Matrix3, Vector3> RQ(const Matrix3& A) {
 
diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m
index 232df4ca7..c6b0f85aa 100644
--- a/matlab/+gtsam/plot2DTrajectory.m
+++ b/matlab/+gtsam/plot2DTrajectory.m
@@ -42,11 +42,15 @@ for i = 0:keys.size-1
 end
 
 % Draw final covariance ellipse
-if ~isempty(lastIndex) && haveMarginals
+if ~isempty(lastIndex)
     lastKey = keys.at(lastIndex);
     lastPose = values.at(lastKey);
-    P = marginals.marginalCovariance(lastKey);
-    gtsam.plotPose2(lastPose, 'g', P);
+    if haveMarginals
+        P = marginals.marginalCovariance(lastKey);
+        gtsam.plotPose2(lastPose, 'g', P);
+    else
+        gtsam.plotPose2(lastPose, 'g', []);
+    end
 end
 
 if ~holdstate