diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 2910ce74c..fa70e5b00 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -17,6 +17,7 @@
  * @author  Frank Dellaert
  * @author  Alex Cunningham
  * @author  Alex Hagiopol
+ * @author  Varun Agrawal
  */
 
 // \callgraph
@@ -88,10 +89,9 @@ 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(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
-        return false;
-      else if(std::abs(A(i,j) - B(i,j)) > tol)
+      if(!fpEqual(A(i,j), B(i,j), tol)) {
         return false;
+      }
     }
   return true;
 }
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index ac03c0f53..beec030ba 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -14,6 +14,7 @@
  * @brief   typedef and functions to augment Eigen's Vectors
  * @author  Kai Ni
  * @author  Frank Dellaert
+ * @author  Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -33,6 +34,45 @@ using namespace std;
 
 namespace gtsam {
 
+/* *************************************************************************
+ * References:
+ * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ * 2. https://floating-point-gui.de/errors/comparison/
+ * ************************************************************************* */
+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) || isnan(b)) {
+    return isnan(a) && isnan(b);
+  }
+  // handle inf
+  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 || (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 if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
+    return true;
+  }
+
+  return false;
+}
+
 /* ************************************************************************* */
 //3 argument call
 void print(const Vector& v, const string& s, ostream& stream) {
@@ -82,9 +122,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(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
-      return false;
-    if(std::abs(vec1[i] - vec2[i]) > tol)
+    if (!fpEqual(vec1[i], vec2[i], tol))
       return false;
   }
   return true;
@@ -95,9 +133,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(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
-      return false;
-    if(std::abs(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 99c5a4d42..81be36c0a 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -15,6 +15,7 @@
  * @author  Kai Ni
  * @author  Frank Dellaert
  * @author  Alex Hagiopol
+ * @author  Varun Agrawal
  */
 
 // \callgraph
@@ -75,6 +76,19 @@ static_assert(
   "Error: GTSAM was built against a different version of Eigen");
 #endif
 
+/**
+ * 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.
+ *
+ * If either value is NaN or Inf, we check for both values to be NaN or Inf
+ * respectively for the comparison to be true.
+ * If one is NaN/Inf and the other is not, returns false.
+ *
+ * Return true if two numbers are close wrt tol.
+ */
+GTSAM_EXPORT bool fpEqual(double a, double b, double tol);
+
 /**
  * print without optional string, must specify cout yourself
  */