Not able to read absolute orientation (tilt) using DMP

[bernhardHartleb]

Hi Paul,

now that the compass data is available to the DMP, I tried to verify the output against a BNO055 board from production.

In modes ORIENTATION and ROTATION_VECTOR the heading (x-axis) will drift to the a magnetic direction, but roll and pitch axis always start at 0°. Even if the sensor is kept at a constant 90° tilt (vertical), the output stays permanently at the initial ~0° value in those modes.

Only the mode GEOMAGNETIC_ROTATION_VECTOR seems to give an absolute orientation and outputs the 90° tilt correctly.
However, for some reason this mode is extremely slow to update. A 90° change in tilt takes over 10sec to settle, even tough the DMP is supposed to run at 55Hz.

Other 9-axis modes show large rotations instantly + get the heel and pitch axis correct, but do not give absolute orientation. Even worse, if you tilt the sensor by only 45° the heading suddenly changes. It should be steady as long as it "points" in the same direction. This makes none of the modes very useful for real world applications where absolute orientation is important.

In summary, the following things do not work for me when using DMP results:

1. Incorrect magnetic heading. For example +90° yaw causes output to go from 80° to 170° (ok) but then drift to 110° (wrong)
2. Heading is not independent of tilt, in any mode. For example, roll by -45° and the heading changes by 160° (woah!)
3. Output is not referenced to absolute orientation (gravity), which should be provided by the Accelerometer

This leads me to believe that at least the axis configuration of of the magnetometer could be wrong.

For testing we have to be able to convert the Quaternion output into Euler angles.
I would like to contribute the following code for that:

#include <cmath>

// heading, roll, pitch in degrees
struct Vector3D { double x, y, z; }

const double RAD2DEG = (180.0 / M_PI);
const double DEG2RAD = (M_PI / 180.0);

Vector3D toEuler( double w, double x, double y, double z)
{
    Vector3D ret;
    double sqw = w*w;
    double sqx = x*x;
    double sqy = y*y;
    double sqz = z*z;

    ret.x = atan2(2.0*(x*y+z*w),(sqx-sqy-sqz+sqw)) * RAD2DEG;
    ret.z = atan2(2.0*(y*z+x*w),(-sqx-sqy+sqz+sqw)) * RAD2DEG;
    double siny = -2.0*(x*z-y*w)/(sqx+sqy+sqz+sqw);

    // prevent NaN and use 90 deg when out of range
    if (fabs(siny) >= 1.0)
        ret.y = copysign(90.0, siny);
    else
        ret.y = asin(siny) * RAD2DEG;

    return ret;
}

Sorry for the bother with these issues, but I would really like to get some "truth" out of this sensor.
I know this IMU is especially difficult to configure, but I have a good reference, some experience, and would like to help :)

Best regards,
Bernhard

[Niv1900]

In an older version of this lib I remember using different formulas written by @PaulZC :

// Scale to +/- 1
double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));

double q2sqr = q2 * q2;

// roll (x-axis rotation)
double t0 = +2.0 * (q0 * q1 + q2 * q3);
double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr);
Roll = atan2(t0, t1) * 180.0 / PI;

// pitch (y-axis rotation)
double t2 = +2.0 * (q0 * q2 - q3 * q1);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
Pitch = asin(t2) * 180.0 / PI;

// yaw (z-axis rotation)
double t3 = +2.0 * (q0 * q3 + q1 * q2);
double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
Yaw = atan2(t3, t4) * 180.0 / PI;

But these computations could also be false regarding #55 (q0) ? With my small tests (note: I'm quite a noob here between all of you) I didn't see some weird roll, pitch and yaw values

Maybe @PaulZC is computing these angles in a different reference frame ?

(This is example is from a Quat6 example)

[bernhardHartleb]

@Niv1900
How to compute Euler angles from the Quaternion is unlikely the issue here.
Especially since the output is correct in the 6-axis modes, or before the magnetometer has been enabled in v1.2.5

To be honest, I completely overlooked that Example7 actually has code to compute the Euler angles.
I will verify it against my own function to be certain...

TLDR of my issue is two things:

• When my board is vertical, I turn on power and read the orientation from DMP then either
  Roll or Pitch should be +/-90° (absolute) not all ~0° (relative to whatever it is during initialization)

• Magnetometer axis seem to be wrong because
  a) Heading in degrees is wrong (0° is not north) and otherwise erratic (simple offset does not fix it)
  b) Heading is influenced by Pitch and Roll when it should not
  All while the raw magnetometer data is correct.

Please let me know if I can investigate a different mode or setting.
Maybe you do not experience the same issues?

[PaulZC]

Hi Bernhard,

Thank you for taking the time to study this - I appreciate the help.

I know that the DMP requires calibration, like you used to have to do with the earlier Bosch devices, but, yes, I would expect it to be able to get the orientation correct using the accelerometer at power up.
Have you tried: rotating the sensor around all three axes; and holding it stationary in all six orientations? Does the performance improve after that?

Sadly, I have no more information about the difference between ROTATION_VECTOR and GEOMAGNETIC_ROTATION_VECTOR. I assume the difference is the frame of reference, but I am not sure.

The code which sets the magnetometer axes is here. I completely understand why you might think it has been set incorrectly, based on how the chip behaves, but, to me, the settings look completely correct. They match the settings I captured from the InvenSense Nucleo code (on the I2C bus) and the InvenSense code comments mention the same 'magic number' of 161061273 for the scaling. The minuses at MTX_11 and MTX_22 also look correct given the different orientation of the magnetometer vs the accel and gyro (mag Y and Z are inverted):

The settings I am less confident in are these. I have used the values from the InvenSense application note but the I2C traffic I captured from the Nucleo example code showed them being set very differently:

 *  Set register 0x7E (Memory Bank Select) to 0x01
 *  Set register 0x7C (Memory Start Address) to 0x0C
 *  Write 0x00E8BA2E to memory (16 * 16 + 12 == Accel Only Gain)
 *  Set register 0x7E (Memory Bank Select) to 0x05
 *  Set register 0x7C (Memory Start Address) to 0xB0
 *  Write 0x06666666 to memory (91 * 16 + 0 == Accel Alpha Var)
 *  Set register 0x7C (Memory Start Address) to 0xC0
 *  Write 0x3999999A to memory (92 * 16 + 0 == Accel A Var)
 *  Set register 0x7C (Memory Start Address) to 0xE4
 *  Write 0x0000 to memory (94 * 16 + 4 == Accel Cal Rate)

Looking at the code, I am using the correct values for ALPHA_VAR and A_VAR. I am a little worried about the ACCEL_ONLY_GAIN but only a little. The I2C traffic suggests the Nucleo code was using 'default' values for ALPHA_VAR and A_VAR and ACCEL_ONLY_GAIN.

While I was investigating the I2C Master ODR configuration issue, I noticed a setting called CPASS_RADIUS_3D_THRESH_ANOMALY. It is defined here in the Nucleo code and here in our library. I do not set it. There is nothing in the application note which says what it should be set to. And I did not notice it being set in the Nucleo I2C traffic. The function which sets it seems not to get called. So, just something to be aware of.

Good luck - and thanks again for the help,
Paul

[bernhardHartleb]

Hi Paul,

thank you for all these references! Very useful to have it laid out like this.
I have to take a break for now, but will come back in a few days with anything I can learn.

Best wishes,
Bernhard

[adamgarbo]

Hi @bernhardHartleb,

Please keep us updated on your efforts. I am also very keen to know if absolute heading (tilt-compensated) can be obtained from the ICM-20948. In theory, we shouldn't actually need sensor fusion for this, though we do need to calibrate the magnetometer.

Cheers,
Adam

[PaulZC]

Hi Bernhard (@bernhardHartleb ),

I have, I think, solved this issue.

By going byte by byte through the SPI data from the InvenSense example, I found that: the I2C Master, accel and gyro were all being placed in low power mode initially (our library did the same thing); but that later the setting was changed leaving only the I2C Master in low power mode. Having the accel and gyro in low power mode was causing the erroneous behavior you were seeing. Using the Quat9 example, I can now power up the ICM-20948 in a random orientation and - after rotating it around all three axes and holding it stationary in all six orientations - the quaternions become well behaved, returning to the same absolute values on every occasion.

If you are interested in the root cause, please see dmp.md and do a search on that page for LP_CONFIG. We were setting LP_CONFIG to 0x70, but had missed that it is later changed to 0x40. Those two bits were causing all our troubles...

Please try v1.2.6. I think you will be pleased!

Best wishes,
Paul

[Eemac]

Hey Everyone,

I figured I'd add my two cents to the discussion - I ran the 1.1.6 and 1.1.7 versions on three different chips, and unfortunately for me, those versions did not work on any of them. I was having the same problem (the sensors would respond beautifully to quicker rotations but then within about 3 seconds drift to a seemingly random yet consistent value that wasn't the actual orientation of the chip). This seems like a similar error to what the OP was having.

What ended up fixing the problem for me was to set the Gyro offset registers (0x03-0x08) in User Bank 2. The formula for calculating these offset biases is to take the gyro offset for each axis (I got this by averaging 10 measurements for each gyro axis using the "Example 1: Basics" sketch included in the Sparkfun library) and divide this by the constant 0.031. Note that for this to work the chip has to be still.

For me, the drift values I read were:
X_Axis: 10 DPS Y_Axis: -1 DPS Z_Axis: -7 DPS

The Corresponding biases were (drift/0.031) - rounded to nearest whole:
X_Axis: 323 Y_Axis: -32 Z_Axis: -226

And the corresponding (signed!) hex:
X_Axis: 0x0143 Y_Axis: 0xFFE0 Z_Axis: 0xFF1E

I wrote those values into bank two and, after the mag calibration finished, the 9 axis magically worked.
While admittedly this is not a very sleek solution, I don't think it would be too hard to automate the raw data collection/bias calculation. Interestingly, Invensense's spaghetti codebase does seem to have a partial implementation of this - they have a function called "inv_selftest_read_samples" that ingests 200 samples and averages them, and another called "inv_icm20948_set_offset" that sets the gyro offset cancellation registers properly. Unfortunately there is no "bias calculation" function.

The thing that really puzzles me is that there are also bias registers in the DMP image/memory as well! So why the need for both?

[PaulZC]

Hi @Eemac ,

This is great stuff - thank you!

Just to be clear, did you mean v1.2.6 and v1.2.7? v1.2.7 is the latest and greatest. The previous versions were all flawed in one way or another.

Would you like to submit a Pull Request to add a setGyroOffset function? Or are you happy for me to run with your idea and credit you in the release notes?

Very best wishes,
Paul

[Eemac]

Hey Paul,

Yes, I meant to say v1.2.6/7. Sorry about that!

Unfortunately I'm going to be away for a while, so I don't think I'll be able to contribute much in the near future. Hopefully my "fix" works for you guys too!

Cheers,
Ian

[msrivast]

Hey @PaulZC ,

Thank you for your excellent work on this library. I am using v 1.2.9. I don't see the gyro settling down to an inconsistent value(using quat9 RV). Has this issue been fixed or have I just been lucky with my testing so far?
(I don't see any mention of setGyroOffset function in the library.)

Cheers,
Manu

[Eemac]

It turns out that this problem can be avoided all together if the DMP calibration algorithms are enabled. If a no-movement state is detected (literally holding the chip still), the gyro will auto calibrate. The registers I was referring to are for external to the DMP and shouldn't be used if you want DMP functionality.

Sorry about the confusion!
Ian

[joshgalvan]

@Eemac In the current Quat9 example are you able to get non-drifting quaternion values? And how do you enable these DMP calibration algorithms? Currently my issue is that I can place my IMU steady on all 3 axes (x pointing down, y pointing down, z pointing down), and then find a steady state (quaternion values stop changing) in any position. But when I move from position to position it always takes a handful of seconds for the quaternion to settle to concrete values.

I could be doing something wrong. Do you know how you got your values to be accurate?