diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 074ad44a4..39bf7bf08 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -1048,8 +1048,6 @@ Matrix inverse_square_root(const Matrix& A) {
 	return inv;
 }
 
-
-
 /* ************************************************************************* */
 Matrix square_root_positive(const Matrix& A) {
   size_t m = A.size2(), n = A.size1();
@@ -1067,6 +1065,16 @@ Matrix square_root_positive(const Matrix& A) {
   return vector_scale(S, V); // V*S;
 }
 
+/* ************************************************************************* */
+Matrix expm(const Matrix& A, int K) {
+	Matrix E = eye(A.size1()), A_k = eye(4);
+	for (int k=1;k<=K;k++) {
+		A_k = A_k*A/k;
+		E = E + A_k;
+	}
+	return E;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index 491e6ff26..d851ee092 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -365,7 +365,12 @@ Matrix RtR(const Matrix& A);
 /** Return the inverse of a S.P.D. matrix.  Inversion is done via Cholesky decomposition. */
 Matrix cholesky_inverse(const Matrix &A);
 
-
+/**
+ * Numerical exponential map, naive approach, not industrial strength !!!
+ * @param A matrix to exponentiate
+ * @param K number of iterations
+ */
+Matrix expm(const Matrix& A, int K=7);
 
 // macro for unit tests
 #define EQUALITY(expected,actual)\