Using STM32F446, ISM330DHCX and MotionFX, computed yaw angle is correct after a controlled 90-degree right rotation, but the yaw angle is off by ~3 degrees after each 90-degree left rotation. What could be causing this issue?

[SLibe]

The MotionFX Library is initialized as follows:

MotionFX_getKnobs(&iKnobs);

/* Modify knobs settings */

// Configure knobs with non-default values

iKnobs.output_type = MFX_ENGINE_OUTPUT_ENU;

iKnobs.modx  = 1; // No decimation

iKnobs.ATime = 10; // kalman_settings.static_accel_time_coeff;

iKnobs.FrTime = 10; // kalman_settings.dynamic_accel_time_coeff;

iKnobs.LMode = 1; // Enable gyro bias calculation during static motion

iKnobs.gbias_gyro_th_sc_6X = 0.0036; // Per datasheet, this is empirically determined

  MotionFX_setKnobs(&iKnobs);

When new data arrives, it's processed like this:

MotionFX_propagate(&data_out, &imu, &delta_s);

MotionFX_update(&data_out, &imu, &delta_s, NULL);

Interstingly, if instead I first make the left-hand rotation, it has no significant error, but after making the right-hand rotation, a similar ~3 degree error is detected.

Each time motion occurs in the direction in which the error is present, the error increases, so the error is compounding over time.

It seems that whichever direction the first turn is in, error is not seen when turning in the direction, but turning in the opposite direction has continually-compounding error.

What could account for, immediately after intialization completes, a second rotation in the opposite direction having several degrees of error, but the first rotation in the opposite direction does not exhibit error?

[Eleon BORLINI]

Hi @SLi.9bert​ ,

before running the MotionFX library, I suggest you to run a calibration step with the MotionAC accelerometer calibration library, in order to minimize the residual offset of the sensor, that could sum during the integration steps.

I'm wondering if the error is summing in one direction (the direction of the possible offset) and subtracting in the opposite one...

-Eleon

[SLibe]

Thank you Eleon, for your reply and suggestion.

Can you please answer follow-up questions to ensure my clear understanding of accelerometer fusion?

• Question 1: Should modifying ATime knob to 10.0 minimize the impact of accelerometer offset error on yaw angle during integration?

• Question 2: This product uses 6-axis fusion on mobile robots, which do ~not~ have pitch or roll changes often. For our use case, do you think the "dynamic" accelerometer calibration is sufficient - or for our use case will a 6-point tumble calibration give better results?

[SLibe]

Hi @Eleon BORLINI​ 

MotionAC "dynamic" calibration is now added to our STM32F4 firmware, as you suggested. MotionAC reports the quality is "MAC_CALQSTATUSGOOD".

Our goal: achieve highest possible orientation accuracy using 6-axis MotionFX fusion.

However, after adding MotionAC calibration, we still see several degrees error after 90-degree rotations, especially in left-hand direction.

Question 1: Can you please suggest what other factor could be causing ~2.5 degrees of error on each left-hand 90 degree rotation?

We also see some error (~1 degree) with Right-hand rotation - and the error amount may vary based upon temperature. I believe MotionFX LMode=1 zero rate offset correction should help this - but maybe it is not? Additional note: We are now researching decreasing GBIAS_GYRO_TH_SC_6X - since that seems to improve the right-hand rotation accuracy - and maybe decrease the StDev, too...

Question 2: Can you please suggest what could be causing the Right-hand rotation error? Is some sort of additional temperature compensation required?

Below are results of controlled testing from several test runs:

6-axis heading statistics (7 trials in each run; estimated "truth" accuracy .25 degrees on each rotation):

Run 1 (Saturday, 9:30 am) - avg IMU temp: 30.1 - 7 trials

Rotate Right 90 degrees: avg 88.63 degrees (StDev .22; error% -1.53)

Then Rotate Left 90 degrees: avg 1.89 degrees (StDev .34; error% -2.10)

Run 2 - (Saturday, 2:00 pm) avg IMU temp: 29.3 - 7 trials

Rotate Right 90 degrees: avg 90.26 degrees (StDev .12; error% 0.29)

Then Rotate Left 90 degrees: avg 2.73 degrees (StDev .34; error% -3.03)

Run 3 - (Sunday, 9:30 am) avg temp: 29.45 - 7 trials

Rotate Right 90 degrees: avg 89.52 degrees (StDev -.48; error% -0.54)

Then Rotate Left 90 degrees: avg -2.29 degrees (StDev .57; error% -2.55)

Summary:

• Left-hand rotations experience approximately -2.5 degrees error per 90 degree rotation. This is a key issue.
• Left-hand rotations experience a higher StDev of error than right-hand rotations
• If Left-hand rotations occur without first Right rotations, the degrees error per 90 degree rotation is similar about -2.5 degrees
• Run 2 has highest right-hand rotation accuracy. Right-hand rotation error is much less than left-hand rotation.
• Larger right-hand rotation error appears to occur at higher IMU temperatures.
• The low StDev in reported heading angles indicates a high repeatability of these results.

*** CONFIGURATION ***

IMU: ISM330DHCX

Gyro ODR: 416

Accel ODR: 416

Gyro FSR: 2000 dps

Accel FSR: 2 G

MotionFX Settings:

Thresholds:

#define GBIAS_ACC_TH_SC_6X       (2.0f*0.000765f)

#define GBIAS_GYRO_TH_SC_6X       (2.0f*0.002f)

#define GBIAS_MAG_TH_SC_6X       (2.0f*0.001500f)

ATime = 0.5;

LMode = 1;

output_type = MFX_ENGINE_OUTPUT_ENU

modx = 1;

start_automatic_gbias_calculation = 1;

MotionFX propagate/update deltat_s = 0.0024

Delta T is calculated by using 25-us resolution timestamp acquired via FIFO from ISM330DHCX with each gyro/accel sample. We have verified that the delta_t never changes during acquisition of data from the IMU.

[Eleon BORLINI]

Hi Scott,

can we refer to this other post for the follow up of the above one?

-Eleon

[SLibe]

Hi @Eleon BORLINI​ , Yes, if you want to follow up on the MotionAC question in the other post, please do so.

For the MotionFX error when rotating left, please answer these questions in this post.

Thanks,

• scott

[Eleon BORLINI]

Hi Scott,

I believe it's not an issue related to the algorithm, since it should be symmetric with respect to the left and to the right.

It could be related to the specific device, especially if the offset is not exactly zero: it is calibrated in production but there could be a residual coming from the soldering process. You can place the device in you default "zero" position, read the dataout and write the (averaged) residual in the X_OFS_USR (73h), Y_OFS_USR (74h) and Z_OFS_USR (75h) registers (datasheet, p.84).

If this doesn't work, do you have the possibility to test another ISM330DHCX to check if it is a "device-related" issue?

-Eleon

[SLibe]

Hi @Eleon BORLINI​ ,

• The accelerometer offset correction is now implemented (via X_OFS_USR/Y_OFS_USR/Z_OFS_USR registers)
• The MotionAC accelerometer calibration is also now implemented.
• As suggested, we have run testing on 4 different devices from our production line: 2 boards using ISM330DHCX, 2 boards using LSM6DSM
• As detailed further in the test results below:
  • The addition of accelerometer calibration has not changed the fundamental issues, which are:
    • Apparent scale factor (gain) error, which is a different magnitude on each board
    • Non-symmetrical error, when rotating in first one direction, then the opposite direction, which is a different magnitude on each board - suggesting bias error
    • Different amounts of error on the second rotation (i.e., after changing directions in a single run)

The attached graphic summarizes the types and differing amounts of error in each board. All error values are averages of 7 separate runs. Each graph has one point for each of these 5 steps in the test:

1) Reset board, Rotate 90 degrees (Right), acquire MotionFX yaw angle measurement

2) then, Change Direction ("cd"), rotate -90 degrees (Left), acquire MotionFX yaw angle measurement

3) Reset board, Rotate -90 degrees (Left), acquire MotionFX yaw angle measurement

4) then, Change Direction ("cd"), rotate 90 degrees (Right), acquire MotionFX yaw angle measurement

5) Reset board, Rotate 90 degrees (Right), acquire MotionFX yaw angle measurement. This verifies repeatability of step 1).

 

Ideally, all lines in these graphs should be flat (0 degrees error no matter which rotation is made or if directions are reversed).

Detailed test data and summary are in this Excel Spreadsheet, this includes for each board details about data/fusion rates, output data rate, full-scale range, etc.

Please let me know what are some additional factors that can be causing this.

(a) Is it possible that the zero-rate offset calibration performed by MotionFX is not working correctly?

(b) Why should we see different error amounts for rotations occurring initially after reset, vs, rotations occurring after changing direction?

Thank you for any expertise you can provide to help us solve these issues.

• scott

P.S. Test Conditions are documented in the Excel spreadsheet.

[Eleon BORLINI]

Hi Scott,

not many further suggestions, indeed...

Can you try with changing the ODR (lowering it from the 416Hz current value?) it might be that a too fast ODR is worsening integration errors in time.

In ST algorithm lower ODRs are sometimes used...

-Eleon

[SLibe]

Hi @Eleon BORLINI​ ,

1) The ODR has been modified to 104Hz, as you suggested; this appears to slightly improve the ISM330DHCX yaw angle accuracy - sometimes.

2) Also, we found that the LSM6DSM data previously reported was not correct, because while the ISM330DHCX subtracts the contents of the X_OFS_USR, Y_OFS_USR, Z_OFS_USR registers, the LSM6DSM Adds them. Now that this is corrected, we are receiving improved values on LSM6DSM, similar to that of ISM330DHCX.

However, we still see many times when the yaw angle is off by up to 4 degrees after a single 90-degree rotation - and especially after changing directions (90 degrees one direction and than 90 degrees the opposite direction).

We have been reviewing the gyro biases, this seems like it could be a problem area. Our accel/gyro orientation is 'e' 'n' 'u', and the MotionFX Output orientation is MFX_ENGINE_OUTPUT_ENU. We have a real-time display that outputs the raw gyro x/y/z values from the sensor, as well as the MotionFX gyro bias retrieved via MotionFX_getGbias(). Here is an example:

Raw Gyro X,Y,Z values (DPS): .21, -.49, -.28 (X+, Y-, Z-)

MotionFX GBias values (DPS): .46, -.20, -.31 (Y inverted, X inverted, Z)

This seems odd, doesn't it?

The MotionFX GBIas value orientations don't match the Raw Gyro value orientations; I believe the MotionFX GBias values appear to be SWU orientation - not ENU orientation like the raw gyro values.

Question 1): Is it expected that MotionFX GBias values are in a different orientation than Raw Gyro values from ISM330DHCX? If so, why?

Question 2): Since we are suspicious that MotionFX GBias values are incorrect and introducing yaw angle error, we are considering using the MotionGC library to calculate gyro biases - and using MotionFX LMode=0. If we do use MotionGC instead and set LMode=0, how should we apply the gyro biases - by regularly invoking MotionFX_setGBias()? Or instead by removing the biases before passing the gyro values to MotionFX_propagate()/update()?

Thanks again for your help,

• scott