diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index 76d6506641..163d716bce 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -87,3 +87,21 @@ void Quaternion::to_euler(float *roll, float *pitch, float *yaw) const
                       1 - 2.0f*(q3*q3 + q4*q4));
     }
 }
+
+float Quaternion::length(void) const
+{
+    return sqrtf(sq(q1) + sq(q2) + sq(q3) + sq(q4));
+}
+
+void Quaternion::normalize(void)
+{
+    float quatMag = length();
+    if (quatMag > 1e-16f)
+    {
+        float quatMagInv = 1.0f/quatMag;
+        q1 *= quatMagInv;
+        q2 *= quatMagInv;
+        q3 *= quatMagInv;
+        q4 *= quatMagInv;
+    }    
+}
diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h
index 87804559d6..5387ad3b0b 100644
--- a/libraries/AP_Math/quaternion.h
+++ b/libraries/AP_Math/quaternion.h
@@ -64,6 +64,9 @@ public:
     // create eulers from a quaternion
     void        to_euler(float *roll, float *pitch, float *yaw) const;
 
+    float length(void) const;
+    void normalize();
+
     // allow a quaternion to be used as an array, 0 indexed
     float & operator[](uint8_t i) {
         float *_v = &q1;