Extracting correct yaw angle from quaternion with small roll and pitch angle

asked 2019-08-19 15:07:23 -0600

Hi,

I am using tf::getYaw() function to extract yaw angle from quaternion. My robot usually moves in 2D with small (although not zero) roll and pitch angles. When I use getYaw() the output jumps back and forth about 180 degree in certain times.

For example

1) r: 3.14137 p: -3.13828 y: 3.13949 2) r: 0.000697 p: -0.004024 y: 0.001175

These 2 RPY values represent almost the same angle, but R,P,Y values are differed by almost 180 degree.

Is there a way to ensure conversion to yaw angle will always be continuous value? My vehicle moves mostly in flat surface with small roll and pitch angles always.

I tried to zero out x,y values of corresponding quaternion but it didn't work.

edit retag flag offensive close merge delete

Comments

can you confirm the raw quaternion from the source does not exhibit this behavior?

PapaG gravatar imagePapaG ( 2019-08-20 00:55:26 -0600 )edit