diff --git a/libraries/AP_Math/vectorN.h b/libraries/AP_Math/vectorN.h
index 7272bbe8a5..b939c243de 100644
--- a/libraries/AP_Math/vectorN.h
+++ b/libraries/AP_Math/vectorN.h
@@ -46,6 +46,14 @@ public:
         return _v[i];
     }
 
+    // test for equality
+    bool operator ==(const VectorN<T,N> &v) const {
+        for (uint8_t i=0; i<N; i++) {
+            if (_v[i] != v[i]) return false;
+        }
+        return true;
+    }
+
     // zero the vector
     inline void zero()
     {