diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 2428f9d246..559a8d804d 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -82,6 +82,22 @@ public:
         return (*this);
     }
 
+    void copyTo(Type (&dst)[M*N]) const
+    {
+        memcpy(dst, _data, sizeof(dst));
+    }
+
+    void copyToColumnMajor(Type (&dst)[M*N]) const
+    {
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                dst[i+(j*M)] = self(i, j);
+            }
+        }
+    }
+
     /**
      * Matrix Operations
      */
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index bd8cd9955e..ea24e1fc6e 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -260,20 +260,6 @@ public:
         q = q * scalar;
     }
 
-    /**
-     * Copy quaternion to a float array
-     *
-     * @param dst array of 4 floats
-     */
-    void copyTo(float (&dst)[4])
-    {
-        const Quaternion &q = *this;
-        dst[0] = q(0);
-        dst[1] = q(1);
-        dst[2] = q(2);
-        dst[3] = q(3);
-    }
-
     /**
      * Computes the derivative of q_21 when
      * rotated with angular velocity expressed in frame 1