diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 79887eff63..d2eb7b0cb7 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -295,12 +295,9 @@ public:
     {
         const Matrix<Type, M, N> &self = *this;
 
-        // TODO: set this based on Type
-        static constexpr float eps = 1e-4f;
-
         for (size_t i = 0; i < M; i++) {
             for (size_t j = 0; j < N; j++) {
-                if (fabs(self(i, j) - other(i, j)) > eps) {
+                if (!isEqualF(self(i, j), other(i, j))) {
                     return false;
                 }
             }
@@ -560,43 +557,7 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
 template<typename Type, size_t  M, size_t N>
 bool isEqual(const Matrix<Type, M, N> &x,
              const Matrix<Type, M, N> &y, const Type eps=1e-4f) {
-    bool equal = true;
-
-    for (size_t i = 0; i < M; i++) {
-        for (size_t j = 0; j < N; j++) {
-            if (fabs(x(i, j) - y(i, j)) > eps) {
-                equal = false;
-                break;
-            }
-        }
-        if (equal == false) break;
-    }
-
-    if (!equal) {
-        printf("not equal\nx:\n");
-        x.print();
-        printf("y:\n");
-        y.print();
-    }
-
-    return equal;
-}
-
-
-template<typename Type>
-bool isEqualF(Type x,
-              Type y, Type eps=1e-4f) {
-
-    bool equal = true;
-
-    if (fabs(x - y) > eps) {
-        equal = false;
-    }
-
-    if (!equal) {
-        printf("not equal\nx:\n%g\ny:\n%g\n", double(x), double(y));
-    }
-    return equal;
+    return x == y;
 }
 
 #if defined(SUPPORT_STDIOSTREAM)
diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
index 88fc56ab8f..4ddeabf922 100644
--- a/matrix/helper_functions.hpp
+++ b/matrix/helper_functions.hpp
@@ -20,6 +20,23 @@ bool is_finite(Type x) {
 #endif
 }
 
+/**
+ * Compare if two floating point numbers are equal
+ *
+ * Note: Smaller or EQUAL than is important to correctly
+ * handle the comparison to infinite or nan.
+ *
+ * @param x right side of equality check
+ * @param y left side of equality check
+ * @param eps numerical tolerance of the check
+ * @return true if the two values are considered equal, false otherwise
+ */
+template<typename Type>
+bool isEqualF(const Type x, const Type y, const Type eps = 1e-4f)
+{
+    return matrix::fabs(x - y) <= eps;
+}
+
 /**
  * Wrap value to stay in range [low, high)
  *