From e6a6b4680c84c7796afe0fd8cccef867b8c86cc4 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 16 Aug 2016 01:03:23 -0400
Subject: [PATCH] Use quaternion multiplaction for quaternion derivative.

---
 matrix/Matrix.hpp     |  2 +-
 matrix/Quaternion.hpp | 15 ++-------------
 test/attitude.cpp     |  6 +++++-
 3 files changed, 8 insertions(+), 15 deletions(-)

diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index c9c88b4efe..3188c553d1 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -278,7 +278,7 @@ public:
         const Matrix<Type, M, N> &self = *this;
         for (size_t i = 0; i < M; i++) {
             for (size_t j = 0; j < N; j++) {
-                snprintf(buf + strlen(buf), n - strlen(buf), "\t%.2g", double(self(i, j))); // directly append to the string buffer
+                snprintf(buf + strlen(buf), n - strlen(buf), "\t%g", double(self(i, j))); // directly append to the string buffer
             }
             snprintf(buf + strlen(buf), n - strlen(buf), "\n");
         }
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 1ebb42520f..6917357fbf 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -238,19 +238,8 @@ public:
     Matrix41 derivative(const Matrix31 &w) const
     {
         const Quaternion &q = *this;
-        Type dataQ[] = {
-            q(0), -q(1), -q(2), -q(3),
-            q(1),  q(0), -q(3),  q(2),
-            q(2),  q(3),  q(0), -q(1),
-            q(3), -q(2),  q(1),  q(0)
-        };
-        Matrix<Type, 4, 4> Q(dataQ);
-        Vector<Type, 4> v;
-        v(0) = 0;
-        v(1) = w(0, 0);
-        v(2) = w(1, 0);
-        v(3) = w(2, 0);
-        return Q * v * Type(0.5);
+        Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
+        return v * q  * Type(0.5);
     }
 
     /**
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 764f3143f7..789b7aeb23 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -157,7 +157,11 @@ int main()
     TEST(isEqual(q_from_m, m4));
 
     // quaternion derivate
-    Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
+    Quatf q1(0, 1, 0, 0);
+    Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
+    float data_q_dot_check[] = {-0.5f, 0.0f, -1.5f, 1.0f};
+    Vector<float, 4> q1_dot_check(data_q_dot_check);
+    TEST(isEqual(q1_dot, q1_dot_check));
 
     // quaternion product
     Quatf q_prod_check(