how to measure an angle?

asked 2012-01-17 10:48:39 -0600

maruchi
155 ●17 ●21 ●26

updated 2012-01-23 12:52:38 -0600

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 flag offensive close merge delete

add a comment

1

answered 2012-01-17 12:21:44 -0600

rado0x54
191 ●8 ●11 ●18

updated 2012-01-17 21:20:01 -0600

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):