how to measure an angle?

Dear,

I want to measure yaw angle (radian). What command should I use ? In my c++ file, I used like this;

  double q0 = odom->pose.pose.orientation.w;
double q1 = odom->pose.pose.orientation.x;
double q2 = odom->pose.pose.orientation.y;
double q3 = odom->pose.pose.orientation.z;
double omega = odom->twist.twist.angular.z;
double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));


This yaw angle produce only between -pi and pi. But I want to measure directly the yaw angle in radian without the limitation of -pi and +pi, that is, measure the counter-clockwise one and half circling as +3 pi and the clockwise two circling as -4*pi.

One alternative can be coded this way:

  double omega = odom->twist.twist.angular.z;
double delta_psi = omega*dt;
psi += delta_psi;


But the heading angle psi might accumulate some integration error. Do I have to implement more accurate integration method ?

edit retag close merge delete

Sort by » oldest newest most voted

To derive Euler Angles from Quaternions, you need to know (or define) the sequence of rotations around the coordinate axes. You can only get the Yaw in this context. Take a look at this very nice document. Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)

Otherwise I'd recommend to make use of a linear algebra library (e.g Eigen). Conversion can be done in one line:

Eigen::Quaterniond orientation(w,x,y,z);
orientation.toRotationMatrix().eulerAngles(0,1,2)


EDIT: I looked at the conversion formulas for all the angle sequences and could not find anything similar to your calculation. Should rather look similar to this (seq: 0,1,2):

atan2(2xy + 2wz, w*w + x*x - y*y - z*z)


compare to Formula (290) in linked document.

EDIT: Actually your formula is correct, because the norm of the quaternion is always 1:

w*w + x*x - y*y - z*z = 1-2*(y*y+z*z)


Output should be between -PI and PI.

more

1
I edited the original question. Could you check it ?
( 2012-01-23 12:53:16 -0600 )edit

Take a look at the getYaw() function in tf: http://www.ros.org/doc/api/tf/html/c++/namespacetf.html#a33e93f413622e296d0a2a93b252bbb4b

In general, tf has a couple of handy functions for working with quaternions that represent angles in the plane.

more

I edited the original question. Could you check it?
( 2012-01-23 12:53:44 -0600 )edit