Converting MPU-6050's quaternion output to YPR angles

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.

• Look up "tangent unwrapping" for a start. The general approach is to find out what quadrant you're in on the Re/Im plane, and adjust accordingly. – Scott Seidman May 25 '17 at 21:14
• When I rotate the sensor from 0 to 180 degrees around the pitch axis, the quaternion's q1 goes from 0 to 1 and q3 goes from 1 to 0, should I divide the quadrant? There has to be a standard way of solving this problem. – Vasil Kalchev May 26 '17 at 9:32

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.

• Thank you, but these equations return the angle constrained in +-90 degrees. – Vasil Kalchev May 26 '17 at 9:03
• @VasilKalchev That's the point of using atan2 instead of atan. By taking a second argument (that defines the quadrant), atan2 returns a result between ±180°. – Caleb Reister May 26 '17 at 18:08
• Yes, but the pitch angle uses asin :(. – Vasil Kalchev May 26 '17 at 19:24
• The pitch doesn't need to go beyond ±90° because yaw and roll are defined over ±180°. This graphic may help visualize it. – Caleb Reister May 27 '17 at 6:22