From 2c614c65a12ab1510bb7495c4072ccea992bf29d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 3 Feb 2017 17:57:58 -0500 Subject: [PATCH] Change to hamilton quaternion convention. --- matrix/Quaternion.hpp | 23 ++++++++++++++++------- test/attitude.cpp | 10 +++++----- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index a06165f..6c14b02 100644 --- a/matrix/Quaternion.hpp +++ b/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 @@ -213,9 +222,9 @@ class Quaternion : public Vector 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; } diff --git a/test/attitude.cpp b/test/attitude.cpp index bb5baa4..8dc5c01 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -182,13 +182,13 @@ int main() // quaternion derivative in frame 1 Quatf q1(0, 1, 0, 0); Vector 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 q1_dot1_check(data_q_dot1_check); TEST(isEqual(q1_dot1, q1_dot1_check)); // quaternion derivative in frame 2 Vector 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 q1_dot2_check(data_q_dot2_check); TEST(isEqual(q1_dot2, q1_dot2_check)); @@ -308,8 +308,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))); @@ -331,7 +331,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