I have an IMU sensor giving me a normalized quaternion for its orientation. However this quaternion does not match the coordinate system that I want to use. For example, the roll and the yaw axis are swapped.
Negating or swapping the x, y and z components of my quaternion is not a suitable solution as I need to compensate for any orientation of my IMU, not only 90° rotations about axis.
How can I do this ? For now I have been using this code:
Quaternion getQuaternionFromEuler(float yaw, float pitch, float roll) {
float yawRad = yaw * DEG_TO_RAD;
float pitchRad = pitch * DEG_TO_RAD;
float rollRad = roll * DEG_TO_RAD;
float cy = cos(yawRad * 0.5);
float sy = sin(yawRad * 0.5);
float cp = cos(pitchRad * 0.5);
float sp = sin(pitchRad * 0.5);
float cr = cos(rollRad * 0.5);
float sr = sin(rollRad * 0.5);
Quaternion q_yaw = {cy, 0, 0, sy}; // Yaw autour de Z
Quaternion q_pitch = {cp, 0, sp, 0}; // Pitch autour de Y
Quaternion q_roll = {cr, sr, 0, 0}; // Roll autour de X
Quaternion q = quaternion_multiply(q_roll, quaternion_multiply(q_pitch, q_yaw));
return q;
}
Quaternion quaternion_multiply(Quaternion q1, Quaternion q2) {
Quaternion result;
result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
result.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
result.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
return result;
}
Quaternion quaternion_conjugate(Quaternion q) {
Quaternion result;
result.w = q.w;
result.x = -q.x;
result.y = -q.y;
result.z = -q.z;
return result;
}
m_initialRotation = getQuaternionFromEuler(0, 90, 0); //Rotating frame 90° around the pitch axis. This should put the yaw axis (Z) upward and the roll axis (X) forward.
Quaternion rotated = quaternion_multiply(quaternion_multiply(quaternion_conjugate(m_initialRotation), imu_quaternion), m_initialRotation);
However this is not giving me the correct results. For example the pitch angle gets affected correctly by the above code (giving me -90° instead of 0° initially), however the yaw (Z) and roll (X) axis are still swapped. I tried everything from changing the order of quaternions multiplication, not mutliplying by the conjugate quaternion... All giving sometimes good results but showing limitations when using combined rotations. I'm starting to think that maybe I cannot solve this problem with quaternion multiplications and that I need first to swap signs of axis to have a good handedness... Which is just a supposition because I'm really confused with this.
Thank you for the help
