I found a formula online to extract yaw from a quaternion like so:
double x = quat; double y = quat; double z = quat; double w = quat; return atan2(2.0f * (w * z + x * y), 1.0f - 2.0f * (y * y + z * z));
I wanted to verify if this is the right way to do it. I created a loop that gets the body's current rotation (i.e
quat variable above), calculates the Yaw from it (as like the formula above) and prints it out. I then started rotating the body along all three axis. I was expecting the printed value to stay the same when I roll or pitch and only change when I yaw. However, when I roll or pitch I see that the value changes, so I assume that the formula above is not correct.
What is the right way to get the rotation around z-axis (i.e Yaw) from a quaternion? I don't care about pitch or roll but only the yaw.
Maybe the test I am doing is non-sense, but ideally what I would like to do is to observe the body's orientation only in terms of the z-axis. If I am given a quaternion of a body, how can I found out the theta value of the body in respect to itself or the world frame. Assuming that I can get the body's x, y position, I would also like to capture the theta value of that body, so I was thinking that I need to extract the Yaw from the quaternion. The result I am looking for is a value that I can use to represent the state of the body (x, y and theta). I already found the x, y and now I am only left with a quaternion that I need to find the theta of the body. I hope this make sense.