diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
index 0dfd75032b..ccfcb7bc01 100644
--- a/libraries/AP_Math/matrix3.cpp
+++ b/libraries/AP_Math/matrix3.cpp
@@ -227,14 +227,6 @@ bool Matrix3<T>::invert()
     return success;
 }
 
-template <typename T>
-void Matrix3<T>::zero(void)
-{
-    a.x = a.y = a.z = 0;
-    b.x = b.y = b.z = 0;
-    c.x = c.y = c.z = 0;
-}
-
 // create rotation matrix for rotation about the vector v by angle theta
 // See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
 template <typename T>
diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h
index 02e18699a5..77ed088b8c 100644
--- a/libraries/AP_Math/matrix3.h
+++ b/libraries/AP_Math/matrix3.h
@@ -219,14 +219,14 @@ public:
     bool invert() WARN_IF_UNUSED;
 
     // zero the matrix
-    void        zero(void);
+    void        zero(void) {
+        memset((void*)this, 0, sizeof(*this));
+    }
 
     // setup the identity matrix
     void        identity(void) {
+        zero();
         a.x = b.y = c.z = 1;
-        a.y = a.z = 0;
-        b.x = b.z = 0;
-        c.x = c.y = 0;
     }
 
     // check if any elements are NAN