Why FusionAhrsGetQuaternion() gets conjugated quaternion? #9
Description
/**
* @brief Returns the quaternion describing the sensor relative to the Earth.
* @param ahrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth.
*/
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) {
return FusionQuaternionConjugate(ahrs->quaternion);
}
I tried to compair Fusion with original MadgewickAHRS, and found that Fusion gives reversed quaternion result. It appears that quaternion get by FusionAhrsGetQuaternion() always be conjugated, thus result in reversed quaternion orientation. But if i change it to return ahrs->quaternion; then FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); returns reversed euler angle.
According to https://quaternions.online/, convert quaternion wxyz{0.96,-0.009,-0.28,-0.024} to euler angle should be xyz{-2.085,-32.467,-3.471}. But FusionQuaternionToEuler((FusionQuaternion){0.96,-0.009,-0.28,-0.024}); returns {2.084,32.491,3.469}, which every axis are reversed. So I think something goes wrong in conversion between quaternion to euler angle.
The problem can be fixed by both applying the method from Wikipedia or just simply reverse signs in the original formula.
Original
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {
#define Q quaternion.element
const float qwqwMinusHalf = Q.w * Q.w - 0.5f; // calculate common terms to avoid repeated operations
FusionEuler euler;
euler.angle.roll = FusionRadiansToDegrees(atan2f(Q.y * Q.z - Q.w * Q.x, qwqwMinusHalf + Q.z * Q.z));
euler.angle.pitch = FusionRadiansToDegrees(-1.0f * asinf(2.0f * (Q.x * Q.z + Q.w * Q.y)));
euler.angle.yaw = FusionRadiansToDegrees(atan2f(Q.x * Q.y - Q.w * Q.z, qwqwMinusHalf + Q.x * Q.x));
return euler;
#undef Q
}
Sign reversed
// Sign reversed
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {
#define Q quaternion.element
const float qwqwMinusHalf = Q.w * Q.w - 0.5f; // calculate common terms to avoid repeated operations
FusionEuler euler;
euler.angle.roll = FusionRadiansToDegrees(-1.0f * atan2f(Q.y * Q.z - Q.w * Q.x, qwqwMinusHalf + Q.z * Q.z));
euler.angle.pitch = FusionRadiansToDegrees(asinf(2.0f * (Q.x * Q.z + Q.w * Q.y)));
euler.angle.yaw = FusionRadiansToDegrees(-1.0f * atan2f(Q.x * Q.y - Q.w * Q.z, qwqwMinusHalf + Q.x * Q.x));
return euler;
#undef Q
}
Method from Wikipedia
// Source:https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {
#define Q quaternion.element
FusionEuler euler;
// Source:https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// roll (x-axis rotation)
float sinr_cosp = 2 * (Q.w * Q.x + Q.y * Q.z);
float cosr_cosp = 1 - 2 * (Q.x * Q.x + Q.y * Q.y);
euler.angle.roll = FusionRadiansToDegrees(atan2f(sinr_cosp, cosr_cosp));
// pitch (y-axis rotation)
float sinp = 2 * (Q.w * Q.y - Q.z * Q.x);
if (fabsf(sinp) >= 1)
euler.angle.pitch = FusionRadiansToDegrees(copysignf(M_PI / 2, sinp)); // use 90 degrees if out of range
else
euler.angle.pitch = FusionRadiansToDegrees(asinf(sinp));
// yaw (z-axis rotation)
float siny_cosp = 2.0f * (Q.w * Q.z + Q.x * Q.y);
float cosy_cosp = 1.0f - 2.0f * (Q.y * Q.y + Q.z * Q.z);
euler.angle.yaw = FusionRadiansToDegrees(atan2f(siny_cosp, cosy_cosp));
return euler;
#undef Q
}
I currently use the 'Sign reversed' version, and a pull request will be sent, tell me if I miss something.