SF 9DOF Razor AHRS ) DCM To Euler angles, Euler To Rotation matrix problem

Posted by Hari on August 23, 2010 at 12:35am in Sensors & IMUs View Discussions

Dear all, I have the Sparkfun 9DOF Razor board. It is working fine with Python GUI provided at http://www.sparkfun.com/commerce/product_info.php?products_id=9623 which is coded by Jose Julio. I'm pretty good at rotation matrices and their order of rotation, but not familiar with DCM to Euler angle conversions. The brief description in the Arduino code base is as follows // Axis definition: // X axis pointing forward (to the FTDI connector) // Y axis pointing to the right // and Z axis pointing down. // Positive pitch : nose up // Positive roll : right wing down // Positive yaw : clockwise DCM to Euler conversion pitch = -asin(DCM_Matrix[2][0]); roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); ( I'm unable to trace the mathematics involved in Vpython GUI in implementing rotations of 3D object) I tried converting the above Euler angles to Rotation matrix as below .

Rotation matrix(R) = AxAyAz c = cos , s = sin , r = roll, p =pitch , y = yaw

Multiplying the previous rotation matrix with the present , perfect rotation can be achieved. I got perfect rotations with Wii motion plus gyro data, using the above order of rotation. I would like to make the Razor 9DOF board work in Visual C++ environment using the rotation matrix . I have the following doubts . 1. what order i should follow to make it work . 2. Do i have to follow different DCM to Euler angles conversion other than the above method. where is my mathematics going wrong. DCMDraft2.pdf in page 10 ,he said relation between the DCM and euler angles

is this can be obtained with AzAyAx order ,I tried this also, but no use . I tried experimenting few other order of rotations other than AxAyAz. no use. Hope i will hear especially from Doug Weibel , Jose Julio , Jordi Munoz and William Premerlani .

Maths - Conversion Euler to Axis-Angle

Euler to Axis-Angle Calculator.

Prerequisites
Definition of terms:

Euler Angles Axis Angle

Equations
angle = 2 * acos(c1c2c3 - s1s2s3) x = s1 s2 c3 +c1 c2 s3 y = s1 c2 c3 + c1 s2 s3 z = c1 s2 c3 - s1 c2 s3

to normalise divide x,y and z by: sqrt(x2 + y2 + z2) = sqrt((s1 s2 c3 +c1 c2 s3)2+(s1 c2 c3 + c1 s2 s3)2+(c1 s2 c3 - s1 c2 s3)2) where:

c1 = cos(heading / 2) c2 = cos(attitude / 2) c3 = cos(bank / 2) s1 = sin(heading / 2) s2 = sin(attitude / 2) s3 = sin(bank / 2)

Code
public final void rotate(double heading, double attitude, double bank) { // Assuming the angles are in radians. double c1 = Math.cos(heading/2); double s1 = Math.sin(heading/2); double c2 = Math.cos(attitude/2); double s2 = Math.sin(attitude/2); double c3 = Math.cos(bank/2); double s3 = Math.sin(bank/2); double c1c2 = c1*c2; double s1s2 = s1*s2; w =c1c2*c3 - s1s2*s3; x =c1c2*s3 + s1s2*c3; y =s1*c2*c3 + c1*s2*s3; z =c1*s2*c3 - s1*c2*s3; angle = 2 * Math.acos(w); double norm = x*x+y*y+z*z; if (norm < 0.001) { // when all euler angles are zero angle =0 so // we can set axis to anything to avoid divide by zero x=1; y=z=0; } else { norm = Math.sqrt(norm); x /= norm; y /= norm; z /= norm; } }

Derivation of Equations
Start from quaternion to axis angle as shown here: angle = 2 * acos(qw) x = qx / sqrt(1-qw*qw) y = qy / sqrt(1-qw*qw) z = qz / sqrt(1-qw*qw)

Substitute from Euler to Quaternion as shown here: qw = c1 c2 c3 - s1 s2 s3 qx = s1 s2 c3 +c1 c2 s3 qy = s1 c2 c3 + c1 s2 s3 qz = c1 s2 c3 - s1 c2 s3 where:

c1 = cos(heading / 2) c2 = cos(attitude / 2) c3 = cos(bank / 2) s1 = sin(heading / 2) s2 = sin(attitude / 2) s3

Euler Angles

According to Euler's rotation theorem, any rotation may be described using three angles. If the rotations are written in terms of rotation matrices , , and , then a general rotation can be written as
(1)

The three angles giving the three rotation matrices are called Euler angles. There are several conventions for Euler angles, depending on the axes about which the rotations are carried out. Write the matrix as
(2)

The so-called " -convention," illustrated above, is the most common definition. In this convention, the rotation given by Euler angles , where

1. the first rotation is by an angle about the z-axis using , 2. the second rotation is by an angle about the former x-axis (now ) using , and

3. the third rotation is by an angle about the former z-axis (now ) using . Note, however, that several notational conventions for the angles are in common use. Goldstein (1980, pp. 145-148) and Landau and Lifschitz (1976) use , Tuma (1974) says is used in aeronautical engineering in the analysis of space vehicles (but claims that is used in the analysis of gyroscopic motion), while Bate et al. (1971) use . Goldstein remarks that continental authors usually use , and warns that left-handed coordinate systems are also in occasional use (Osgood 1937, Margenau and Murphy 1956-64). Varshalovich (1988, pp. 21-23) uses the notation or to denote the Euler angles, and gives three different angle conventions, none of which corresponds to the -convention. Here, the notation is used, a convention that could be used in versions of Mathematica prior to 6 as RotationMatrix3D[phi, theta, psi] (which could be run after loading Geometry`Rotations`) and RotateShape[g, phi, theta, psi] (which could be run after loading Geometry`Shapes`). In the -convention, the component rotations are then given by

(3)

(4)

(5)

so
(6) (7) (8) (9) (10) (11)

(12) (13) (14)

To obtain the components of the angular velocity in the body axes, note that for a matrix
(15)

it is true that

(16)

(17)

Now,

corresponds to rotation about the axis, so look at the

component of

,
(18)

(19)

The line of nodes corresponds to a rotation by about the -axis, so look at the ,

component of

(20) (21)

(22)

Similarly, to find rotation by about the remaining axis, look at the

component of

,
(23)

(24)

(25)

Combining the pieces gives

(26)

For more details, see Goldstein (1980, p. 176) and Landau and Lifschitz (1976, p. 111). The -convention Euler angles are given in terms of the Cayley-Klein parameters by
(27)

(28) (29)

In the " -convention,"

(30) (31)

Therefore,
(32) (33) (34) (35)

giving rotation matrices

(36)

(37)

(38)

and is given by
(39) (40) (41) (42) (43) (44) (45) (46) (47)

In the "

(pitch-roll-yaw) convention," is pitch, is roll, and is yaw.

(48)

(49)

(50)

and is given by
(51) (52) (53) (54) (55) (56) (57) (58) (59)

A set of parameters sometimes used instead of angles are the Euler parameters defined by

and ,

(60)

(61)

Using Euler parameters (which are quaternions), an arbitrary rotation matrix can be described by
(62) (63) (64) (65) (66)

(67) (68) (69) (70)

(Goldstein 1980, p. 153). If the coordinates of two sets of points and are known, one rotated with respect to the other, then the Euler rotation matrix can be obtained in a straightforward manner using least squares fitting. Write the points as arrays of vectors, so
(71)

Writing the arrays of vectors as matrices gives

(72) (73)

and solving for gives

(74)

However, we want the angles , , and , not their combinations contained in the matrix . Therefore, write the matrix
(75)

as a

vector

(76)

Now set up the matrices

(77)

Using nonlinear least squares fitting then gives solutions which converge to