Compass Alignment with Roll and Pitch Axes (DMP)

Subject of the issue

I'm finding that the Compass (yaw) is 90 deg off from what I'm expecting with respect to the roll and pitch directions.

I'm using SparkFun Example11_DMP_Bias_Save_Restore_ESP32 that I have modified slightly for the ESP8266. This example takes the quaternion data and calculates roll, pitch and yaw. The angles are all good, except for the zero reference for the yaw angle. That seems to be rotated 90 deg.

If I set the board flat with 0 yaw, the "nose" of the board will point north. If I lift the nose up I would expect the pitch angle to change, but the roll changes.

issue #114 @texasfunambule may have made a comment about this > The quat9 example seems to work fine (except maybe for compass alignement)

Could the quaternion components be mixed in the roll, pitch, yaw equations?? That is beyond my pay grade.

Hi @Gord1 ,

Welcome to the weird and wonderful world of the ICM20948 DMP!

Agreed. I'm seeing the same thing. Looks like I've got a rotation in there.

My setup: SparkFun ESP32 Thing Plus C, connected to a SparkFun ICM-20948 breakout via I2C; Arduino IDE 1.8.19; Espressif ESP32 boards 3.0.1.

If I let Example11 run for a while, do the three rotations and six holds, the Yaw reports 0 degrees when the Accel/Gyro Y axis is pointing towards magnetic North. But, yes, roll and pitch are reversed. Also, Yaw goes negative when I rotate clockwise from North.

IIRC, I used the conversion code from here (or a very similar source):

https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2

In the example code, I assumed Q0/1/2/3 mapped onto Qw/x/y/z. That appears to be untrue...

I don't have time to dig into this right now. I will try and get to it ~soon. If you have time and can correct the sequence, please do and send us a Pull Request.

Best wishes,
Paul

Agreed. I'm seeing the same thing. Looks like I've got a rotation in there.

Hi @PaulZC

Many thanks for looking into this and confirming the issue.

In the example code, I assumed Q0/1/2/3 mapped onto Qw/x/y/z. That appears to be untrue...

The coordinate frame for the accelerometer and gyro are different than the magnetometer on the ICM20948. Do you have any information on the coordinate frame for the DMP and how it relates to the physical chip?

There are a lot of combinations to start trying them randomly shooting in the dark.

Thanks,
Gord

Hi Gord,

There are no clues about coordinate frames in the DMP application note...

But this may help:

https://github.com/ZaneL/quaternion_sensor_3d_nodejs/blob/34a289a251b9a593fd4d522e68a702f64de324d8/public/index.html#L56-L57

Best,
Paul

Hi @PaulZC,

In the example 11 code the 4th quaternion is calculated by
double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));

This is always taken as positive while it could be +/- as it is the square root. Depending on whether you take the positive q0 or negative, don't you get a different rotation?

Regards,
Gord

This seems to be the directions for both the Gyro and the Quaternion.

If I tilt the board around the x-axis both the gx and q1 increase for +ve rotation. The same for the y and z axis.

For the roll, pitch, yaw calculation equations in example 11, I'm guessing that the x-axis is defined as forward and not the y-axis as shown above.

I'm not familiar with quats. I guess we should we switching q1,q2 and q3 around.

Gord

Hi @PaulZC,

Here is the bit of code to swap the axis around so they work with the equations that are in standard aircraft dynamics form:

		// Scale to +/- 1
		//  -GW note The ICM 20948 chip has axes y-forward, x-right and Z-up
		//  -see Figure 12. Orientation of Axes of Sensitivity and Polarity of Rotation
		//	DS-000189-ICM-20948-v1.6.pdf  These are the axes for gyro and accel and quat
		//
		//  For conversion to roll, pitch and yaw for the equations below, the coordinate frame
		//  must be in aircraft reference frame.
		//  
		// we use the Tait Bryan angles (in terms of flight dynamics):
		// ref: https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles
		//
		// Heading – ψ : rotation about the Z-axis (+/- 180 deg.)
		// Pitch – θ : rotation about the new Y-axis (+/- 90 deg.)
		// Bank – ϕ : rotation about the new X-axis  (+/- 180 deg.)
		//
		// where the X-axis points forward (pin 1 on chip), Y-axis to the right and Z-axis downward.
		// In the conversion example above the rotation occurs in the order heading, pitch, bank. 
		// To get the roll, pitch and yaw equations to work properly we need to exchange the axes
		
		// Note when pitch approaches +/- 90 deg. the heading and bank become less meaningfull because the
		// device is pointing up/down.

		double qx, qy;  // some temp vars for switching axis

		qx = q1;
		qy = q2;

		// w no change
		q1= qy;  //x and y exchanged
		q2= qx;
		q3= -q3; //neg z

Now do conversion ...

Regards,
Gord

Hi Gord (@Gord1 ),

Thank you.

I went with the following, to avoid messing up the QUAT_ANIMATION:

SparkFun_ICM-20948_ArduinoLibrary/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino

Lines 346 to 367 in 168c18b

    
           double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); 
        
           double qw = q0; // See issue #145 - thank you @Gord1 
        
           double qx = q2; 
        
           double qy = q1; 
        
           double qz = -q3; 
        
           // roll (x-axis rotation) 
        
           double t0 = +2.0 * (qw * qx + qy * qz); 
        
           double t1 = +1.0 - 2.0 * (qx * qx + qy * qy); 
        
           double roll = atan2(t0, t1) * 180.0 / PI; 
        
           // pitch (y-axis rotation) 
        
           double t2 = +2.0 * (qw * qy - qx * qz); 
        
           t2 = t2 > 1.0 ? 1.0 : t2; 
        
           t2 = t2 < -1.0 ? -1.0 : t2; 
        
           double pitch = asin(t2) * 180.0 / PI; 
        
           // yaw (z-axis rotation) 
        
           double t3 = +2.0 * (qw * qz + qx * qy); 
        
           double t4 = +1.0 - 2.0 * (qy * qy + qz * qz); 
        
           double yaw = atan2(t3, t4) * 180.0 / PI;

I'll release these changes in a few minutes.

Thanks again,
Paul

Resolved by https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary/releases/tag/v1.2.13

	double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));

	double qw = q0; // See issue #145 - thank you @Gord1
	double qx = q2;
	double qy = q1;
	double qz = -q3;

	// roll (x-axis rotation)
	double t0 = +2.0 * (qw * qx + qy * qz);
	double t1 = +1.0 - 2.0 * (qx * qx + qy * qy);
	double roll = atan2(t0, t1) * 180.0 / PI;

	// pitch (y-axis rotation)
	double t2 = +2.0 * (qw * qy - qx * qz);
	t2 = t2 > 1.0 ? 1.0 : t2;
	t2 = t2 < -1.0 ? -1.0 : t2;
	double pitch = asin(t2) * 180.0 / PI;

	// yaw (z-axis rotation)
	double t3 = +2.0 * (qw * qz + qx * qy);
	double t4 = +1.0 - 2.0 * (qy * qy + qz * qz);
	double yaw = atan2(t3, t4) * 180.0 / PI;