1

I am trying to convert quaternions provided by a MPU6050 to heading attitude and bank values but at least one of the three calculations is broken, for example in the third calculation (the uncommented one) the yaw and pitch are in the same axis and the other two almost do not change, do you know which specific formula should apply here, or at least some theory on different ways of quaternions representation that helps me figure out the maths behind this?

float heading;
float attitude;
float bank;
double test = q.x * q.y + q.z * q.w;
if (test > 0.499) { // singularity at north pole
  heading = 2 * atan2(q.x, q.w);
  attitude = M_PI / 2;
  bank = 0;
  return;
}
if (test < -0.499) { // singularity at south pole
  heading = -2 * atan2(q.x, q.w);
  attitude = - M_PI / 2;
  bank = 0;
  return;    
}
double sqw = q.w * q.w;
double sqx = q.x * q.x;
double sqy = q.y * q.y;
double sqz = q.z * q.z;    
/*
 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
heading = atan2(2 * q.y * q.w - 2 * q.x * q.z , 1 - 2 * sqy - 2 * sqz);
attitude = asin(2 * test);
bank = atan2(2 * q.x * q.w - 2 * q.y * q.z , 1 - 2 * sqx - 2 * sqz);
*/
/*
 // https://www.i2cdevlib.com/forums/topic/24-roll-and-pitch-angles-ranges/
heading = atan2(2 * q.x * q.y - 2 * q.w * q.z , 2* sqw + 2 * sqx - 1);
attitude = asin(2 * test);
bank = atan2(2 * q.x * q.w - 2 * q.y * q.z , 1 - 2 * sqx - 2 * sqz);
*/
//https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/indexLocal.htm
heading = atan2(2.0 * (q.x*q.y + q.z*q.w),(sqx - sqy - sqz + sqw));
bank = atan2(2.0 * (q.y*q.z + q.x*q.w),(-sqx - sqy + sqz + sqw));
attitude = asin(-2.0 * (q.x*q.z - q.y*q.w));
Serial.print("conversion\t");    
Serial.print(heading * 180 / M_PI);
Serial.print("\t");
Serial.print(attitude * 180 / M_PI);
Serial.print("\t");
Serial.println(bank);
Raul Lapeira Herrero
  • 111
  • 3
  • When using Euler angles, the order of rotation is important (Euclideanspace says "The following assumes NASA Standard Airplane" which may be the specification for sequence of rotations). So the formulae from two different sources / sites need not match one-to-one. – AJN Jul 17 '20 at 18:29
  • Since this is not specific to electrical engineering, you may try asking in https://aviation.stackexchange.com if you do not receive answers here. May be even space.stackexchange. Read up the policy on posting in multiple sites together. – AJN Jul 17 '20 at 18:31
  • Compare the formulae to this also https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_Angles_Conversion – AJN Jul 17 '20 at 18:41
  • https://electronics.stackexchange.com/a/307494/238590 for comparison. – AJN Jul 17 '20 at 18:47
  • 1
    To me, the variable test doesn't match Wikipedia or the SE answer linked (even after ignoring the sign). However Wiki and SE linked answer matches the attitude variable for the third set (ignoring sign). – AJN Jul 17 '20 at 18:52
  • 1
    Apart from the sequence of rotations being important for Euler angles, you can also note that, for quaternions (or any attitude representation), there are two conventions, frame rotation and vector rotation conventions. The two are exactly opposite. You need to check the convention used by MPU6050. – AJN Jul 17 '20 at 18:56
  • Thanks AJN, I decided to just try and error until the values made sense, I think the final working formulas are a mix of the ones I found online, probably because of the reasons you mention. If you add an answer with my code I can accept it for reference. – Raul Lapeira Herrero Jul 17 '20 at 22:06

1 Answers1

0

This code did work at least for what we needed in our humanoid robot, thanks for comments to everyone specially to AJN.

mpu.dmpGetQuaternion(&q, fifoBuffer);
float heading;
float attitude;
float bank;
double sqw = q.w * q.w;
double sqx = q.x * q.x;
double sqy = q.y * q.y;
double sqz = q.z * q.z;    
//Heading is like yaw, works ok
//attitude should be pitch but is working as roll?? only works correctly in a 180~ degree range
//Bank should be roll but is working as pitch??, works ok 
heading = atan2(2.0 * (q.w*q.z + q.x*q.y),1.0 - 2.0 * (sqy + sqz));
attitude = asin(2 * (q.w * q.y - q.z * q.x));
bank = atan2(2.0 * q.x * q.w -2 * q.y * q.z,1.0 - 2.0 * sqx - 2 * sqz);
Serial.print("conversion\t");    
Serial.print(heading * 180 / M_PI);
Serial.print("\t");
Serial.print(attitude * 180 / M_PI);
Serial.print("\t");
Serial.println(bank * 180 / M_PI);
Raul Lapeira Herrero
  • 111
  • 3