Electronic – Converting MPU-6050’s quaternion output to YPR angles

imu

I am using the I2Cdev library which configures the MPU-6050 to use it's DMP and generate a quaternion output on it's FIFO. The problem is that when that quaternion number is converted to yaw, pitch and roll angles, the pitch and roll angles are limited to +-90 degrees (this constraint comes from the inverse trigonometric functions used in the equations).

What equations should be used to convert the quaternion number in euler's angles that give full information on the sensor's orientation.

Best Answer

Before I answer, it should be noted that there are some benefits to using quaternions:

  • They avoid gimbal lock (important for devices that may be turned upside down)
  • Quaternion transformations and multiplications generally require fewer mathematical operations than Euler angles

That being said, I would consult the Representing Attitude article from Stanford University.

To summarize, equation 452 states that:

$$ \begin{bmatrix} \psi \\ \theta \\ \phi \end{bmatrix} = \begin{bmatrix} Y \\ P \\ R \end{bmatrix} = \begin{bmatrix} \mathrm{atan2}\left( -2q_1q_2 + 2q_0q_3,\; q_0^2 + q_1^2 - q_3^2 - q_2^2 \right) \\ \mathrm{asin}\left( 2q_1q_3 + 2q_0q_2 \right) \\ \mathrm{atan2}\left( -2q_2q_3 + 2q_0q_1,\; q_3^2 - q_2^2 - q_1^2 + q_0^2 \right) \end{bmatrix} $$

Wikipedia has a good explanation of atan2, if you need it.