# Angles from gyroscope readings using quaternions

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.

• Can you provide the model number and/or datasheet for the gyro you're using? Everything I've said is speculative; the datasheet should give an absolute answer. – Chuck Dec 13 '16 at 14:14
• I use MPU 6500 from Invensense. The Sensitivity is 1000 DPS. – Nithin G A Dec 14 '16 at 8:31
• Any inputs from your side?? – Nithin G A Dec 15 '16 at 8:46
• How are you defining the x and y axes relative to the text on the MPU-6500 package? Also, how are you defining your rotations? About the body axes or about global axes? – Chuck Dec 15 '16 at 15:34
• For me the package is the reference, the data sheet mentions the X and Y axis directions with respect to the package. – Nithin G A Dec 16 '16 at 7:47

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.

• Please check the edits in the question. I am not facing a problem in sequence of rotations, I think it is related to the initial position. Please do check the edit and share your thoughts. – Nithin G A Dec 13 '16 at 11:26
• After a few experiments I found that the Initial Position is the key and also based on that we need to select the sequence of rotation. Eg. If the Initial Position is ZXY, Z up and X left and Y towards us(Right Hand Cordinate system), We need to use ZXY rotation sequence. If Initial position is ZYX,(Z up, Y left and X away from us), then we have to use ZYX sequence. – Nithin G A Dec 13 '16 at 13:10
• Your inputs guided me to do some experiments and come up with these observations. Thanks a lot Chuck – Nithin G A Dec 13 '16 at 13:12