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

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 . 
 \begin{array}{lcl} <br /
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 WeibelJose Julio , Jordi Munoz and William Premerlani .
Thanks for reading ....

Views: 8074

Reply to This

Replies to This Discussion

You might want to read this file written by William Premerlani. I used it to extract euler angles from the DCM in VC# and it works flawlessly ;)
I have gone through the document and tried what he said to do when working with UAV Dev board also, no use.
could you post, your extraction of euler angles from DCM , euler to rotation matrix.Are you directly passing the euler angles to your rotation matrix or is there any function involved before rotation takes place.
I would like to hear from you ..
Thanks
Hi Harinath,

If you are using the UDB you have to realize that the axis system on the UDB is different from the 'normal' convention. On the Razor board i'm not sure....
I'm using a board I designed myself, though based on the UDB. This board does use the 'normal' axis system.
To extract the euler angles I use the following C# code:

public double Roll
{
get
{
// atan2(zy, zz)
return Math.Atan2(this._matRotation.GetValue(2, 1), this._matRotation.GetValue(2, 2));
}
}

public double Pitch
{
get
{
// -asin(zx)
return -Math.Asin(this._matRotation.GetValue(2, 0));
}
}

public double Yaw
{
get
{
// atan2(yx, xx)
return Math.Atan2(this._matRotation.GetValue(1, 0), this._matRotation.GetValue(0, 0));
}
}

_matRotation is the rotation matrix, or rmat in the UDB firmware. For the GetValue function the first parameter is the row(y coordinate) the second the column(x coordinate) and the indices are zero based.

Could you explain why you want to convert euler angles to a rotation matrix? I've never needed to that...

Hope this helps you any.

Greets,
Wouter
Hi Harinath,

In going from DCM to Euler angles, the axis conventions is the biggest stumbling block.

I am not familiar with Jose's code for the Razor so I am not able to help you much there, but it is my understanding that it is correct. The equations look right to me.

Regarding the conversion of DCM to Euler for the UAV DevBoard, I am positive my document that Wouter references is correct. We are using those equations, those are the conversion equations that Peter Hollands is using in his flight analysis tool. Peter plots an icon of a plane on Google Earth by converting the DCM values to Euler angles and passing them to the Goole Earth application interface. The plotted orientations for our flights look correct.

As I said, the most likely source of the problem probably is a misunderstanding of the axis conventions. Euler angles are defined in relation to specific coordinate system. I am using a non-standard coordinate system in the UAV DevBoard.

It looks to me that Jose is using the standard coordinate system, and his equations look correct for that, so I am as puzzled as you are.

Sorry that I could not help you more than that.

Best regards,
Bill
I'm using OpenGL in VC++ , ModelView_matrix is 4x4

first 3 columns from 3 rows, is rotation matrix, this can be

( order is important, in this case xyz)
This is the reason why I'm converting euler angles to rotation matrix.
(Ofcourse there is OpenGL function glRotatef( angle, x,y,z) to rotate an object, for this also we need to convert euler angles to Angle, Axis.)
see here http://www.sparkfun.com/commerce/product_info.php?products_id=9623 for axis convention.
would you show your graphical object rotation code method.
Thanks for the quick response.
Hi Harinath,

For my rotating cube I use managed directX. This is the code I use to get the cube to rotate:
m_Device.Transform.World =
Matrix.RotationZ((float)this._roll) *
Matrix.RotationX((float)this._pitch) *
Matrix.RotationY((float)this._yaw);

As you can see I use the euler angles I extracted from the DCM using the code I gave you earlier and just construct a rotation matrix by using 3 seperate rotations. Also note that DirectX does not use an standard coordinate system. I used the order Bill presents in his papers, roll -> pitch -> yaw, but I changed the axes. X = Z, Y = X, Z = Y. Maybe this is what is causing trouble for you aswell? I have no experience with OpenGL, but I'm pretty sure it doesn't use a standard coordinate system...
If you want the full rotating cube code, just ask and I'll tidy it up and upload it for you ;)

Just for my understanding: The razor board outputs roll, pitch and yaw data and you want to show this using a rotating cube in VC++ using OpenGL?

Greets,
Wouter
Hi Harinath,

I just took a quick look at OpenGL and its coordinate system is a lot like DirectX's.
You could probably use the following code (I haven't tested it, 'cause im not familiar with OpenGL, so it's just a guess...), atleast until we can figure out your problems:
glRotatef(ROLL, 0.0f, 0.0f, 1.0f);
glRotatef(PITCH, 1.0f, 0.0f, 0.0f);
glRotatef(YAW, 0.0f, 1.0f, 0.0f);
Again I'm not familiar with OpenGL, but this might work.

Alternatively, euler angles to axis-angle is described, for example here:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/...

Greets, Wouter
Hi sir,
Jose must be correct since his code released public and working fine with Python GUI.
I'm unable to trace math he used in Python GUI code. He uses 2 vectors , axis and up . I don't know the math behind those functions in VPython, this is where i'm bugging.
have a look at these two vectors below.
axis=(cos(pitch)*cos(yaw) , -cos(pitch)*sin(yaw) , sin(pitch))
up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw), sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw), -cos(roll)*cos(pitch) )

When I follow the relation between DCM and Euler in the DCMDraft2 to convert the Euler angles back to DCM. It is not working. See the procedure in the attached file.

However, i made it work using the Opengl rotation functions, using in the following way.

glRotatef(yaw, 0.0f, 0.0f, 1.0f); // z- axis last
glRotatef(pitch, 0.0f, 1.0f, 0.0f); // y-axis next
glRotatef(roll, 1.0f, 0.0f, 0.0f); // x-axis rotation first

What makes converting the euler angles to DCM does not work ?

Thanks
Attachments:
glRotatef(yaw, 0.0f, 0.0f, 1.0f); // z- axis last
glRotatef(pitch, 0.0f, 1.0f, 0.0f); // y-axis next
glRotatef(roll, 1.0f, 0.0f, 0.0f); // x-axis rotation first

This order is just working fine. But , please read the reply for William Premerlani.

Thanks
Hi Harinath,

I just took a look at your code. If my understanding of your code is correct, then what you're trying to accomplish here:

Multiplication (R, R, A1); // multiply

I think what you are trying to do here is eqn. 17 on page 15 of dcmdraft2? And I also believe the Razor board outputs the euler angles, not the change? If this is the case, I think this might be where the error is. Matrix A1 should be the change of roll, pitch and yaw, rather than the values of roll, pitch and yaw.

However, since you already have the angles I think you can just skip this line.

Greets,
Wouter
Hi Harinath,

I just ran some tests of my own. This is what I did: I let my DCM algorithm run like usual, but I also wrote the following function:

public static Matrix RMatrixFromEulerAngles(double Roll, double Pitch, double Yaw)
{
Matrix Result = new Matrix(3, 3);

double sr = Math.Sin(Roll), cr = Math.Cos(Roll);
double sp = Math.Sin(Pitch), cp = Math.Cos(Pitch);
double sy = Math.Sin(Yaw), cy = Math.Cos(Yaw);

Result.SetValue(0, 0, cp * cy);
Result.SetValue(1, 0, cp * sy);
Result.SetValue(2, 0, -sp);

Result.SetValue(0, 1, sr * sp * cy - cr * sp);
Result.SetValue(1, 1, sr * sp * sy + cr * cy);
Result.SetValue(2, 1, sr * cp);

Result.SetValue(0, 2, cr * sp * cy + sr * sy);
Result.SetValue(1, 2, cr * sp * sy - sr * cy);
Result.SetValue(2, 2, cr * cp);

return Result;
}

It's not very optimized etc. etc. but it was just a quick test. This code works just fine for me.
As its arguments I pass in the euler angles I get from my DCM algorithm. Both the matrix I extracted the euler angles from as the matrix I created with the euler angles are exactly the same.

I hope you can port this to VC++ for you own app.
Thanks for your valuable time to pointing the place to think of it. I really appreciate. I removed Multiplication (R, R, A1); // multiply
I didn't mean to accomplish the eqn. 17 on page 15 of dcmdraft2 . I was using this function call when i was working with wii motion plus, so i just copied.
I understood that, i should not multiply the previous rotation matrix by the present since we are already doing glMultMatrixd(m); .
It is working fine.
Thanks a lot.
Regards
Harinath

Reply to Discussion

RSS

Groups

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2017   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service