diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp
index 620359236..ef53cd48d 100644
--- a/cpp/Vector.cpp
+++ b/cpp/Vector.cpp
@@ -9,6 +9,7 @@
 #include <iostream>
 #include <sstream>
 #include <iomanip>
+#include <cmath>
 #include <boost/foreach.hpp>
 
 #ifdef WIN32
@@ -117,9 +118,12 @@ namespace gtsam {
     Vector::const_iterator it1 = vec1.begin();
     Vector::const_iterator it2 = vec2.begin();
     if (vec1.size()!=vec2.size()) return false;
-    for(size_t i=0; i<vec1.size(); i++)
-      if(fabs(it1[i] - it2[i]) > tol)
-      return false;
+    for(size_t i=0; i<vec1.size(); i++) {
+    	if(isnan(it1[i]) xor isnan(it2[i]))
+    		return false;
+    	if(fabs(it1[i] - it2[i]) > tol)
+    		return false;
+    }
     return true;
   }
   
diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp
index 706b9bd25..74400172d 100644
--- a/cpp/testVector.cpp
+++ b/cpp/testVector.cpp
@@ -203,6 +203,17 @@ TEST( TestVector, ediv )
   CHECK(assert_equal(c,actual));
 }
 
+/* ************************************************************************* */
+TEST( TestVector, equals )
+{
+	Vector v1 = Vector_(1, 0.0/0.0); //testing nan
+	Vector v2 = Vector_(1, 1.0);
+	double tol = 1.;
+	print(v1, "v1");
+	print(v2, "v2");
+	CHECK(!equal_with_abs_tol(v1, v2, tol));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp
index 6760ab34a..d5076f38f 100644
--- a/cpp/testVectorConfig.cpp
+++ b/cpp/testVectorConfig.cpp
@@ -24,14 +24,38 @@ using namespace std;
 using namespace gtsam;
 	
 /* ************************************************************************* */
-TEST( VectorConfig, equals )
- {	 
+TEST( VectorConfig, equals1 )
+ {
    VectorConfig expected;
    Vector v = Vector_(3, 5.0, 6.0, 7.0);
    expected.insert("a",v);
    VectorConfig actual;
    actual.insert("a",v);
    CHECK(actual.equals(expected));
+}
+
+/* ************************************************************************* */
+TEST( VectorConfig, equals2 )
+ {	 
+   VectorConfig cfg1, cfg2;
+   Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+   Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
+   cfg1.insert("x", v1);
+   cfg2.insert("x", v2);
+   CHECK(!cfg1.equals(cfg2));
+   CHECK(!cfg2.equals(cfg1));
+ }
+
+/* ************************************************************************* */
+TEST( VectorConfig, equals_nan )
+ {
+   VectorConfig cfg1, cfg2;
+   Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+   Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0);
+   cfg1.insert("x", v1);
+   cfg2.insert("x", v2);
+   CHECK(!cfg1.equals(cfg2));
+   CHECK(!cfg2.equals(cfg1));
  }
 
 /* ************************************************************************* */