四元數轉歐拉角

根據無人機上獲取的四元數轉爲歐拉角（單位弧度）：

C++：

float q0 = flight->getQuaternion().q0;
float q1 = flight->getQuaternion().q1;
float q2 = flight->getQuaternion().q2;
float q3 = flight->getQuaternion().q3;


const double Epsilon = 0.0009765625f;
const double Threshold = 0.5f - Epsilon;
const double PI = 3.1415926;

double TEST = q3*q1 - q0*q2;

if (TEST < -Threshold || TEST > Threshold) // 奇異姿態,俯仰角爲±90°
{
int sign = Sign(TEST);

yaw = -2 * sign * (double)atan2(q0, q3); // yaw

pitch = sign * (PI / 2.0); // pitch

roll = 0; // roll

}
else
{
roll = atan2(2 * (q1*q2 + q3*q1), q3*q3 - q0*q0 - q1*q1 + q2*q2) * 180 / PI;
pitch = asin(-2 * (q0*q2 - q3*q1)) * 180 / PI;
yaw = atan2(2 * (q0*q1 + q3*q2), q3*q3 + q0*q0 - q1*q1 - q2*q2) * 180 / PI;
}

ROS：

geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat)
{
  geometry_msgs::Vector3 ans;

  tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
  R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
  return ans;
}