diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 04bddb111..1ee1a6c4a 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -88,7 +88,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
 
   for(size_t i=0; i<m1; i++)
     for(size_t j=0; j<n1; j++) {
-      if(!fp_isequal(A(i,j), B(i,j), tol)) {
+      if(!fpEqual(A(i,j), B(i,j), tol)) {
         return false;
       }
     }
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index f00f9fddf..8c8b15416 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -35,26 +35,38 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-bool fp_isequal(double a, double b, double epsilon) {
-  double DOUBLE_MIN_NORMAL = std::numeric_limits<double>::min() + 1.0;
+bool fpEqual(double a, double b, double tol) {
+  using std::abs;
+  using std::isnan;
+  using std::isinf;
+
+  double DOUBLE_MIN_NORMAL = numeric_limits<double>::min() + 1.0;
+  double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
 
   // handle NaNs
-  if(std::isnan(a) ^ std::isnan(b)) {
-    return false;
+  if(std::isnan(a) || isnan(b)) {
+    return isnan(a) && isnan(b);
   }
   // handle inf
-  else if(std::isinf(a) ^ std::isinf(b)) {
-    return false;
+  else if(isinf(a) || isinf(b)) {
+    return isinf(a) && isinf(b);
   }
   // If the two values are zero or both are extremely close to it
   // relative error is less meaningful here
-  else if(a == 0 || b == 0 || (std::abs(a) + std::abs(b)) < DOUBLE_MIN_NORMAL) {
-    return std::abs(a-b) < epsilon * DOUBLE_MIN_NORMAL;
+  else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) {
+    return abs(a-b) <= tol * DOUBLE_MIN_NORMAL;
+  }
+  // Check if the numbers are really close
+  // Needed when comparing numbers near zero or tol is in vicinity
+  else if(abs(a-b) <= tol) {
+    return true;
   }
   // Use relative error
-  else {
-    return std::abs(a-b) < epsilon * std::min((std::abs(a) + std::abs(b)), std::numeric_limits<double>::max());
+  else if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
+    return true;
   }
+
+  return false;
 }
 
 /* ************************************************************************* */
@@ -106,7 +118,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
   if (vec1.size()!=vec2.size()) return false;
   size_t m = vec1.size();
   for(size_t i=0; i<m; ++i) {
-    if (!fp_isequal(vec1[i], vec2[i], tol))
+    if (!fpEqual(vec1[i], vec2[i], tol))
       return false;
   }
   return true;
@@ -117,7 +129,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol
   if (vec1.size()!=vec2.size()) return false;
   size_t m = vec1.size();
   for(size_t i=0; i<m; ++i) {
-    if (!fp_isequal(vec1[i], vec2[i], tol))
+    if (!fpEqual(vec1[i], vec2[i], tol))
       return false;
   }
   return true;
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 1b51a1b99..035a6fa87 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -25,11 +25,11 @@
 #define MKL_BLAS MKL_DOMAIN_BLAS
 #endif
 
-#include <cmath>
-#include <limits>
 #include <gtsam/global_includes.h>
 #include <Eigen/Core>
+#include <cmath>
 #include <iosfwd>
+#include <limits>
 #include <list>
 
 namespace gtsam {
@@ -82,10 +82,12 @@ static_assert(
  * Numerically stable function for comparing if floating point values are equal
  * within epsilon tolerance.
  * Used for vector and matrix comparison with C++11 compatible functions.
- * Reference : https://floating-point-gui.de/errors/comparison/
+ * References:
+ * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ * 2. https://floating-point-gui.de/errors/comparison/
  * Return true if two numbers are close wrt epsilon.
  */
-GTSAM_EXPORT bool fp_isequal(double a, double b, double epsilon);
+GTSAM_EXPORT bool fpEqual(double a, double b, double epsilon);
 
 /**
  * print without optional string, must specify cout yourself