title: Math behind some quaternion code and gimbal lock
body: <p>I have the following matlab code that gives a Rotation matrix from Quaternion. The person who did this said he used some library and it is originally from this Rotation Matrix 
  <a href="https://i.stack.imgur.com/hC6Bs.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/hC6Bs.jpg" alt="Rotation Matrix from Quaternion"></a></p>
  
  <p>The Rotation matrix according to the code looks like</p>
  
  <p><a href="https://i.stack.imgur.com/schnW.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/schnW.jpg" alt="Rot Matrix according to code"></a></p>
  
  <p>The diagonal elements and the signs are changed. Can someonw explain to me the math or the conditions for the disparity between the two.</p>
  
  <pre><code>Q0 = Quat(1);
  Q1 = Quat(2);
  Q2 = Quat(3);
  Q3 = Quat(4);
  
  %% set f2q to 2*q0 and calculate products
  f2q = 2 * Q0;
  f2q0q0 = f2q * Q0;
  f2q0q1 = f2q * Q1;
  f2q0q2 = f2q * Q2;
  f2q0q3 = f2q * Q3;
  
   %% set f2q to 2*q1 and calculate products
   f2q = 2 * Q1;
   f2q1q1 = f2q * Q1;
   f2q1q2 = f2q * Q2;
   f2q1q3 = f2q * Q3;
  
   %% set f2q to 2*q2 and calculate products
   f2q = 2 * Q2;
   f2q2q2 = f2q * Q2;
   f2q2q3 = f2q * Q3;
   f2q3q3 = 2 * Q3 * Q3;
  
   %% calculate the rotation matrix assuming the quaternion is normalized
   R(1, 1) = f2q0q0 + f2q1q1 - 1;
   R(1, 2) = f2q1q2 + f2q0q3;
   R(1, 3) = f2q1q3 - f2q0q2;
   R(2, 1) = f2q1q2 - f2q0q3;
   R(2, 2) = f2q0q0 + f2q2q2 - 1;
   R(2, 3) = f2q2q3 + f2q0q1;
   R(3, 1) = f2q1q3 + f2q0q2;
   R(3, 2) = f2q2q3 - f2q0q1;
   R(3, 3) = f2q0q0 + f2q3q3 - 1;
  </code></pre>
  
  <p>Also...</p>
  
  <p>The Rotation Matrix is used to find Euler Angles that also deals with Gimbal lock</p>
  
  <pre><code> %%calculate the pitch angle -90.0 &lt;= Theta &lt;= 90.0 deg
          pitchdeg = asin(-R(1, 3)) * (180 / pi);
  
          %% calculate the roll angle range -180.0 &lt;= Phi &lt; 180.0 deg
          rolldeg = atan2(R(2, 3), R(3, 3)) * (180 / pi);
  
          %% map +180 roll onto the functionally equivalent -180 deg roll
          if (rolldeg == 180)
  
              rolldeg = -180;
          end
  
          %% calculate the yaw (compass) angle 0.0 &lt;= Psi &lt; 360.0 deg
          if (pitchdeg == 90)
  
              %% vertical upwards gimbal lock case
              yawdeg = (atan2(R(3, 2), R(2, 2)) * (180 / pi)) + rolldeg;
          elseif (pitchdeg == -90)
  
              %% vertical downwards gimbal lock case
              yawdeg = (Math.Atan2(-R(3, 2), R(2, 2)) * (180 / pi)) - rolldeg;
  
          else
  
              %% general case
              yawdeg = atan2(R(1, 2), R(1, 1)) * (180 / pi);
          end
  
          %% map yaw angle Psi onto range 0.0 &lt;= Psi &lt; 360.0 deg
          if (yawdeg &lt; 0)
  
              yawdeg = yawdeg + 360;
          end
  
          %% check for rounding errors mapping small negative angle to 360 deg
          if (yawdeg &gt;= 360)
  
              yawdeg = 0;
          end
  
          Euler.Roll = rolldeg;
          Euler.Pitch = pitchdeg;
          Euler.Yaw = yawdeg;
  </code></pre>
  
  <p>Googling the formula to convert from Quaternion to Euler angles gave me a formula which i could not attach because i dont have enough reputation but you can find it in wiki Conversion_between_quaternions_and_Euler_angles</p>
  
  <p>The terms are different comparing with the diagonal of Rotation matrix. Also, is there a math behind the compensation for gimbal lock?</p>
  
  <p>The IMU uses North-East-Down system</p>
  
  <p>I am using euler angles to visualize the gyro. Any kind of guidance is appreciable</p>