Conversion Quaternion to Euler&Conversion Euler to Quaternion J3J3BU5T3R5
Quaternion Quaternions form an interesting algebra where each object contains 4 scalar variables (sometimes known as Euler Parameters not to be confused with Euler angles), these objects can be added and multiplied as a single unit in a similar way to the usual algebra of numbers. However, there is a difference, unlike the algebra of scalar numbers qa * qb is not necessarily equal to qb * qa (where qa and qb are quaternions). In mathematical terms, quaternion multiplication is not commutative. Quaternions have 4 dimensions (each quaternion consists of 4 scalar numbers), one real dimension and 3 imaginary dimensions. Each of these imaginary dimensions has a unit value of the square root of -1, but they are different square roots of -1 all mutually perpendicular to each other, known as i,j and k. So a quaternion can be represented as follows: a + i b + j c + k d
Euler Angles We use the term "Euler Angle" for any representation of 3 dimensional rotations where we decompose the rotation into 3 separate angles. When we first start to think about 3D rotations this seems the natural way to proceed but our intuition can be deceptive and there are a lot of problems that arise when we use Euler angles to do calculations. Notation of angles: View slide
Conversion of Quaternion to Euler Equations: heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)attitude = asin(2*qx*qy + 2*qz*qw) bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2) *except when qx*qy + qz*qw = 0.5 (north pole)which gives: heading = 2 * atan2(x,w)bank = 0 *and when qx*qy + qz*qw = -0.5 (south pole)which gives: heading = -2 * atan2(x,w)bank = 0 View slide
We can derive this by combining the formula derived in the matrix to euler and the quaternion to matrix, let me know if there is a more direct method, so starting with the matrix to euler: heading = atan2(-m20,m00)attitude = asin(m10) bank = atan2(-m12,m11) We can combine this with the quaternion to matrix: So substituting this in above equation gives: heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)attitude = asin(2*qx*qy + 2*qz*qw) bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2) This works at all points except the singularities at attitude = +90 and -90 degrees, at these points we will get atan2(0,0) for heading and bank which will return 0 although this is not a valid result. We therefore need another way to work out heading and bank at the singularities.
Another Method: w = Math.sqrt(1.0 + C1 * C2 + C1*C3 - S1 * S2 * S3 + C2*C3) / 2x = (C2 * S3 + C1 * S3 + S1 * S2 * C3) / (4.0 * w) y = (S1 * C2 + S1 * C3 + C1 * S2 * S3) / (4.0 * w)z = (-S1 * S3 + C1 * S2 * C3 + S2) /(4.0 * w) where: C1 = cos(heading) C2 = cos(attitude) C3 = cos(bank) S1 = sin(heading) S2 = sin(attitude) S3 = sin(bank) note: in the second form the angles are not divided by 2. I don't know which of these forms is most stable? However, as William points out the first is better because it requires the same number of trig operations, no square root, no worry about dividing by zero, uses familiar formulae, and is fairly clearly normalised.
Derivation of the Equation: The quaternion for the rotation by angle a about unit vector (x1,y1,z1) is given by: cos(angle/2) + i ( x1 * sin(angle/2)) + j (y1 * sin(angle/2)) + k ( z1 * sin(angle/2)) where: c1 = cos(h / 2) c2 = cos(a / 2) c3 = cos(b / 2) s1 = sin(h / 2) s2 = sin(a / 2) s3 = sin(b / 2) *The required quaternion can be calculated by multiplying these individual quaternions
From our definitions the order of applying these rotations is heading, attitude then bank (about y,z then x). As we saw on this page the rotation applied first goes on the right hand side of the equation but since we are working in the frame of reference of the moving object the first rotation goes on the left. Applying heading then attitude gives: (Qh * Qa), then applying bank gives: Q = (Qh * Qa)* QbQ = ((c1 + j s1)) * (c2 + k s2))* (c3) + i s3))Q = (c1 c2 + j s1 c2 + k c1 s2 + j k s1 s2)* (c3 + i s3) but jk=i which gives:Q = (c1 c2 + i s1 s2 + j s1 c2 + k c1 s2) * (c3 + i s3)Q = c1 c2 c3 + i s1 s2 c3 + j s1 c2 c3 + k c1 s2 c3 + i s3 c1 c2 + ii s1 s2 s3 + j i s1 c2 s3 + k i c1 s2 s3 but ii=-1 and j i = -k and k i = j which gives:Q = c1 c2 c3 + i s1 s2 c3 + j s1 c2 c3 + k c1 s2 c3 + i c1 c2 s3 + ii s1 s2 s3 + j i s1 c2 s3 + k i c1 s2 s3Q = (c1 c2 c3 - s1 s2 s3) + i (s1 s2 c3 +c1 c2 s3) + j (s1 c2 c3 + c1 s2 s3) + k (c1 s2 c3 - s1 c2 s3) if we define Q = w + i x + j y + k z then w = c1 c2 c3 - s1 s2 s3x = s1 s2 c3 +c1 c2 s3y = s1 c2 c3 + c1 s2 s3z = c1 s2 c3 - s1 c2 s3
“Example” we take the 90 degree rotation from this: heading = 0 degreesbank = 90 degrees attitude = 0 degrees so substituting this in the above formula gives: c1 = cos(heading / 2) = 1 c2 = cos(attitude / 2) = 1 c3 = cos(bank / 2) = 0.7071 s1 = sin(heading / 2) = 0 s2 = sin(attitude / 2) = 0 s3 = sin(bank / 2) = 0.7071 w = c1 c2 c3 - s1 s2 s3 = 0.7071x = s1 s2 c3 +c1 c2 s3 = 0.7071y = s1 c2 c3 + c1 s2 s3 = 0z = c1 s2 c3 - s1 c2 s3 = 0 which gives the quaternion 0.7071 + i 0.7071
Sources: http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm -(Euler Angles) http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm -(Quaternions) http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm -(Quaternion to Euler) http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm -(Euler to Quaternion)