diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
index 0dfd75032b902..ccfcb7bc01d13 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 02e18699a5350..77ed088b8c975 100644
--- a/libraries/AP_Math/matrix3.h
+++ b/libraries/AP_Math/matrix3.h
@@ -219,14 +219,14 @@ class Matrix3 {
     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