Angles from gyroscope readings using quaternions

Question

I am using gyroscope only to get real time angles as I move the IMU using a micro controller.

I am able to get angles at a pretty decent accuracy(2 to 3 degree error). I am using quaternions for obtaining angles.

The angles are with respect to the initial position of the IMU.

I rotate the IMU around individual axis i.e around any one Axis at a time, I get good accuracy. But when I rotate around all axis at once the problem starts as stated below.

Problem:

The problem here is when I give a pitch and then yaw all seems fine. Now at this position of pitch and yaw, I give a roll, the other two angles also change.

The gyroscope gives raw angular rate in DPS, which is converted to radian/sec for processing. The quaternions are calculated from raw angular rate.

The code to convert quaternions to euler angles is as follows,

//Local variables for clarity
fqw = gsangleparam.fquaternion[0];
fqx = gsangleparam.fquaternion[1];
fqy = gsangleparam.fquaternion[2];
fqz = gsangleparam.fquaternion[3];


//Calculate Angles*/
gsangles.yaw = atan2(2 * (fqx*fqy + fqw*fqz), (fqw*fqw + fqx*fqx - fqy*fqy -    fqz*fqz)) * 180/PI;

gsangles.pitch = asin(-2 * (fqx*fqz - fqw*fqy)) * 180/PI;

gsangles.roll = atan2(2 * (fqy*fqz + fqw*fqx), (fqw*fqw - fqx*fqx - fqy*fqy  + fqz*fqz)) * 180/PI;

The Problem of roll is present only when initial position(and also 180 deg counterpart) is as follows:

X - Facing left

Y - Facing towards us

Z - Facing UP

The problem is, When I give a pitch(around X here),yaw(around Z) and then roll(around Y here) or any sequence of roll pitch and yaw, then yaw changes.

In fact whenever I rotate about Y axis(roll here), Yaw(around Z) changes.

There is no problem when the initial position is(and also 180 deg counterpart) as follows,

X - Facing away from us

Y - Facing towards us

Z - Facing UP

There is no problem. i.e When I give a pitch(around Y here),yaw(around Z) and then roll(around X here) or any sequence of roll pitch and yaw, there is no problem.

Why is this happening?? can anyone please explain. Thanks in advance.

Answer 1

I'll link you to some math but try to give a conceptual explanation, too.

Consider a case where you want to start with the IMU upright and pointed forward and want to end with the IMU upside-down (new +z = old -z) but still pointed forward (new +x = old +x).

How do you get from the starting position to the end position?

1. Easiest way is to rotate about +x an angle of 180 degrees. (Roll)
2. You could rotate about +y an angle of 180 degrees and then rotate about the new +z axis an angle of 180 degrees (Pitch then Yaw)
3. You could rotate about +z an angle of 180 degrees and then rotate about the new +y axis an angle of 180 degrees (Yaw then Pitch)

There are other ways you could do it, too - you could rotate about successive axes in 90 degree increments, a bunch of little increments, etc. The point is, you need to define what your order of rotations are to get from one particular starting orientation to get to a particular ending orientation for your rotation angles to make sense.

The problem that you're seeing is that your gyroscope is likely setup to assume a roll first, followed by a pitch, followed by a yaw. That's (again, probably - check your datasheet) the order of rotations the gyro is giving you, but you are giving it a a pitch first, then yaw, which are the last two rotations in the sequence, then giving it the first rotation - roll.

You're giving it the first motion last, which means that it has to go "back" and update the pitch/yaw angles to correct for the fact that you didn't rotate it the way it wants to report the angles to you.

Try doing a roll first, followed by a pitch then a yaw and see if your angles hold steady with that order of rotations. Check your datasheet for specifics, but I think most systems are setup for roll/pitch/yaw (rpy) order.