quaternion To Euler algorithm. getPitch() returns Nan?

Question

I am trying to convert the quaternion that I obtained from the sensor and get the pitch by calculating theta. If anyone is interested, have a look in this article- chapter 2

My problem is in the following code:

 private void quaternionToEuler(float[] q, float[] euler)
    {
        euler[0] = (float)Math.Atan2((2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0] * q[0]) + ((2 * q[1] * q[1]) - 1));
        euler[1] = -(float)Math.Asin(((2 * q[1] * q[3]) + (2 * q[0] * q[2]))); // theta
        euler[2] = (float)Math.Atan2((2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + ((2 * q[3] * q[3]) - 1)); // phi

        Console.WriteLine(euler[0] + ","+euler[1]+"," + euler[2]);
    }

euler 1 which gets the pitch always returning Nan (not a number) I am not sure if I implemented the algorithm correctly. For some reason, Asin(d), where d the output is >1 and <-1.

Answer 1

I think that your quaternion is not normalised. Only normalised quaternions represent rotations in 3D, you must have that

q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] == 1

If this is the case, then we always have

d = q[1]*q[3] + q[0]*q[2] <= 0.5

as we have that

q[1]*q[3] <= 0.25 *(q[1] + q[3])^2

where ^ denotes power, by the AM-GM, and similarly for q[0]*q[2]

now we have that

d <= 0.25 * ( (q[1] + q[3])^2 + (q[0] + q[2])^2 ) 
  <= 0.25 * ( q[1]^2 + q[2]^2 + q[3]^2 + q[4]^2 )
  <= 0.25

by the normality assumption.