Friday 5 September 2014

Quaternion to Euler Angles(Heading, Pitch, Roll)

// Heading  == Yaw
double computeHeading(double qw, double qx, double qy, double qz, bool isSouthPole)
{
 // For North Pole
 if(isSouthPole == false && qx*qy + qz*qw == 0.5)
 {
  return 2 * atan2(qx,qw);
 }
 // For South Pole
 else if(isSouthPole == true && qx*qy + qz*qw == -0.5)
 {
  return 2 * atan2(qx,qw);
 }
 else
 {
  return atan2(2*qy*qw-2*qx*qz , 1 - 2*qy*qy - 2*qz*qz);
 }
}
// Pitch == Attitude
double computePicth(double qw, double qx, double qy, double qz)
{
 return asin(2*qx*qy + 2*qz*qw);

}
// Bank == Roll
double computeBank(double qw, double qx, double qy, double qz)
{
 return atan2(2*qx*qw-2*qy*qz , 1 - 2*qx*qx - 2*qz*qz);
}

Referred these:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
http://en.wikipedia.org/wiki/Flight_dynamics_(fixed-wing_aircraft)

No comments: