Changing Gyro Output Range Throws of Positional Reading

[user550587]

I am using the MPU6050 in conjunction with an Arduino, and I would like for the gyro rate outputs be more precise than the default setting, which is 1/16.4 of a degree (+/-2000 deg/sec range). To get the more precise values, I called the method mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500) to get a range of +/-500 deg/sec, and a precision of 1/131 of a degree.

In this project, I also need the YPR position, which I obtain through mpu.dmpGetQuaternion(&q, fifoBuffer) and mpu.dmpGetGravity(&gravity, &q) and mpu.dmpGetYawPitchRoll(ypr, &q, &gravity).

That's all easy, but the problem is that when I change the gyro output range, it messes up the YPR output. What happens is the YPR position changes dramatically when the MPU is being rotated, and once the MPU is held still again the YPR catches up slowly and levels off to the actual value. I think there is an error in the filter that combines the GYRO and ACCEL data to get the rotational position. Maybe the DMP is dividing the GYRO rate data by the default sensitivity factor (16.4) when it should be dividing by the new one (131)? It just looks to me like the gyro data is too sensitive when position is being determined.

How can I get accurate YPR readings without delay?

I have attached a screenshot of a program I wrote. The blue line is the gyro rate data, and the pink line is the Roll Position of the MPU. The graph shows two rotations of the MPU.