Quaternion to Yaw to Heading

Question

I have quaternion values q available.

These are correct, because they align the object correctly in the ROS RVIZ tool.

I now want to get a heading value from these values.

My idea was to calculate the Yaw value first and then do a Radian to Degree conversion to get the heading. However, this works only conditionally, as soon as the object moves in the direction of NE everything is correct and the correct heading value is output. The same applies for the direction SW. However, when the object moves in the NW direction, I get a heading value of 135 degrees, exactly the opposite direction. Exactly the same applies to the direction SE.

My conversions look as follows:

double heading = atan2(2.0*(q.y*q.z + q.w*q.x), -1.0 +2.0 * (q.w*q.w + q.x*q.x)) * 180 / M_PI;
if(heading < 0){
   heading +=360;
}

I got the formula from: https://math.stackexchange.com/questions/3496329/map-quaternion-to-heading-pitch-roll-using-different-axis

My Question is: What I'm doing wrong.. Why are NW and SE swapped?

Answer 1

My Problem was, that the Yaw value was reflected and rotated by 90 Degrees.

My RVIZ tool does not use N E S W System, so in this case it was displayed correct.

The solution was:

double heading = (atan2(2.0*(q.y*q.z + q.w*q.x), -1.0 +2.0 * (q.w*q.w + q.x*q.x)) * 180 / M_PI) - 90;
if(heading < 0){
   heading +=360;
}
heading = -heading + 360

Thanks, your solutions are also correct, if the value is not rotated.