Switch to Hamilton quaternions and add Cholesky decomposition.

matrix/Quaternion.hpp

@@ -4,11 +4,20 @@
  * All rotations and axis systems follow the right-hand rule.
  * The Hamilton quaternion product definition is used.
  *
- * In order to rotate a vector v by a righthand rotation defined by the quaternion q
+ * In order to rotate a vector in frame b (v_b) to frame n by a righthand
+ * rotation defined by the quaternion q_nb (from frame b to n)
  * one can use the following operation:
- * v_rotated = q^(-1) * [0;v] * q
- * where q^(-1) represents the inverse of the quaternion q.
- * The product z of two quaternions z = q1 * q2 represents an intrinsic rotation
+ * v_n = q_nb * [0;v_b] * q_nb^-1
+ *
+ * Just like DCM's: v_n = C_nb * v_b (vector rotation)
+ * M_n = C_nb * M_b * C_nb^(-1) (matrix rotation)
+ *
+ * or similarly
+ * v_b = q_nb^1 * [0;v_n] * q_nb
+ *
+ * where q_nb^(-1) represents the inverse of the quaternion q_nb =  q_bn
+ *
+ * The product z of two quaternions z = q2 * q1 represents an intrinsic rotation
  * in the order of first q1 followed by q2.
  * The first element of the quaternion
  * represents the real part, thus, a quaternion representing a zero-rotation
@@ -211,9 +220,9 @@ class Quaternion : public Vector<Type, 4>
         const Quaternion &p = *this;
         Quaternion r;
         r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3);
-        r(1) = p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2);
-        r(2) = p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1);
-        r(3) = p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
+        r(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2);
+        r(2) = p(0) * q(2) - p(1) * q(3) + p(2) * q(0) + p(3) * q(1);
+        r(3) = p(0) * q(3) + p(1) * q(2) - p(2) * q(1) + p(3) * q(0);
         return r;
     }
 

test/attitude.cpp

@@ -170,13 +170,13 @@ int main()
     // quaternion derivative in frame 1
     Quatf q1(0, 1, 0, 0);
     Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
-    float data_q_dot1_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
+    float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
     Vector<float, 4> q1_dot1_check(data_q_dot1_check);
     TEST(isEqual(q1_dot1, q1_dot1_check));
 
     // quaternion derivative in frame 2
     Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
-    float data_q_dot2_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
+    float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
     Vector<float, 4> q1_dot2_check(data_q_dot2_check);
     TEST(isEqual(q1_dot2, q1_dot2_check));
 
@@ -296,8 +296,8 @@ int main()
 
     // conjugate
     Vector3f v1(1.5f, 2.2f, 3.2f);
-    TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q)*v1));
-    TEST(isEqual(q.conjugate(v1), Dcmf(q).T()*v1));
+    TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
+    TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
 
     AxisAnglef aa_q_init(q);
     TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
@@ -319,7 +319,7 @@ int main()
     Dcmf dcm3(Eulerf(1, 2, 3));
     Dcmf dcm4(Eulerf(4, 5, 6));
     Dcmf dcm34 = dcm3 * dcm4;
-    TEST(isEqual(Eulerf(Quatf(dcm4)*Quatf(dcm3)), Eulerf(dcm34)));
+    TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
 
     // check corner cases of matrix to quaternion conversion
     q = Quatf(0,1,0,0); // 180 degree rotation around the x axis