LSM6DSV half precision error at ~180 degrees

[Gorbit99]

Hi,

We're using the lsm6dsv's SFLP algorithm to calculate IMU rotation. Recently however we noticed that the resulting rotation has big gaps at around the 180degree mark. We believe this is because of the half float precision, specifically because there aren't a lot of representable values near 1. See below image for our calculated accuracy results which match the actual results we're seeing:

 

[phly]

Need an update on this

[Andrea VITALI]

You are correct: the issue you are facing is related to the quantization of the half-precision float components of the quaternion stored in the FIFO.

The quaternion is made of four components: the scalar component qw = cos(angle/2), and the vector component qx qy qz = sin(angle/2) multiplied by the normalized direction of rotation [x,y,z]. Quaternion components satisfy the relation qw^2 + qx^2 + qy^2 + qz^2 = 1. Also, Q = [qw, qx, qy, qz] represents the same orientation as -Q. The SFLP output quaternion is chosen to have qw >= 0 always.

For efficiency, in LSM6DSV sensors the FIFO only stores 3 components: qx, qy, qz. The 4th component qw is recomputed as +sqrt(1 - qx^2 - qy^2 - qz^2) - newer generation sensors have been improved, see note below.

When any of the vector components, qx qy or qz, is near +/-1, the accuracy of qw is reduced as you have observed. 

The workaround is to readout the quaternion components from the embedded advanced features registers before the next accelerometer and gyroscope data sample is taken; the readout must be performed when the computation of the embedded functions is completed, otherwise the embedded functions will not work as expected and the device must be rebooted.

- All 4 quaternion components are stored in page 3, from 0x4C to 0x53 (qw, qx, qy, qz).

- Wait for the rising edge of INT2_EMB_FUNC_ENDOP to be sure readout is completed after the computation of the embedded functions is completed

- Advanced features registers must be read one byte at a time (multiple-byte readout operation is not available)

Example application code is attached.

Newer generation sensors store all 4 quaternion components in the FIFO. Contact sales and marketing for more information.

[flaufer]

Hi Andrea,

thank you very much for the valuable insights!

How about the ISM330BX? Are the quaternion components also stored in such embedded advanced features registers and which are they?

Kind regards,

Felix

[Andrea VITALI]

Yes, it should work - I had no time to test it. Please try the attached application example.

You will notice that there is an additional step during the configuration to enable SFLP in the correct way for ISM330BX as specified in AN6109 section 6.5.

[Andrea VITALI]

The sensor fusion can also be performed on the STM32 microcontroller: see MotionFX in X-Cube-MEMS software package. User manual UM2220.

MotionFX is available as 6-axis sensor fusion (accelerometer and gyroscope) or 9-axis sensor fusion (accelerometer, gyroscope, magnetometer). 

[flaufer]

Hi @Andrea VITALI ,

thank you very much, this seems to work.

However, I noticed quite some delay/lag between the quaternions obtained regularly from the FIFO and via the embedded advanced features registers. While the FIFO orientations are up to date, the ones obtained from registers 0x4C - 0x53 seem to be kind of low-pass filtered. This becomes an issue when I select a higher SFLP frequency such as 240Hz. For instance, at 240Hz when I flip the device by 90° it takes several seconds until the output orientation settles and approaches the correct values. It's better at 120Hz but still the orintation is delayed compared to the FIFO data. Any idea why this is the case?

EDIT: Nevermind. My I2C reads and writes were too slow causing the delays. Now it works just fine. Thanks again!

 

[Andrea VITALI]

Exactly :) the I2C slow readout process can cause delays.

The quaternion you read from the memory is identical to the quaternion you read from the FIFO, except that the one in the FIFO has the sign changed so that the qw component is always positive.

(Q = [qw,qx,qy,qz] represents the same orientation as -Q = [-qw, -qx, -qy, -qz], we need qw to have positive sign when we store qx,qy,qz in the FIFO so that we can reconstruct qw using +sqrt as discussed before)

[flaufer]

Another question: Actually I'm interested in the following data: timestamp, acceleration, angular velocity, gyro bias (in order to subtract it) and an accurate quaternion. Everything at 120Hz. So I guess the most efficient way in terms of I2C transactions would be to configure the FIFO for most of that data and just obtain the quaternion's real part from the embedded register.

Would you recommend to wait for INT2_EMB_FUNC_ENDOP in this case as well? After that I could read the FIFO and the two remaining embedded registers for q.w.

[phly]

This new approach is working well so far. Thank you!

[phly]

To maximize sflp accuracy, should sensor modes be configured to use high performance mode, or high-accuracy ODR mode? Antialiasing doesn't appear to be enabled in high-accuracy ODR mode.

Can we change sflp rebiasing behavior? EG can we opt to set biases ourselves and disable automatic rebiasing?

[Andrea VITALI]

HA-ODR (high-accuracy ODR) do uses the anti-alias filter: AN5763 paragraph 3.1 page 17, "When the accelerometer is configured in high-accuracy ODR (HAODR) mode, its reading chain is always on. The antialiasing filter is enabled and three sets of accelerometer ODRs are available, ..."

SFLP uses the actual time interval to perform the gyro integration, so accuracy will be similar when enabling HA-ODR.

SFLP gbias estimation can be set/reset as shown in AN5763 paragraph 6.5.1 page 70.

Gbias is a useful by-product of the SFLP algorithm. Disabling gbias update would cause SFLP performance to drop because gbias would not be tracked as it needs to be. Note that gyroscope bias always changes when there is a power toggle, and may also change for a number of factors (temperature, mechanical stress, etc).

To discuss SFLP tuning further you would need to move the discussion to a private channel through marketing which will put you through a dedicated FAE if volume of business justify the customization.

[Syntech]

Can you please submit the code you're using as your driver, as this sounds like a combined issue?