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:

 

[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.

[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)

[phly]

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

[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.

[Andrea VITALI]

The wait for ENDOP is only needed to read the quaternion from the embedded register. Other data registers can be read at any time.

Of course, to be sure not to read stale data just before it is updated, and to read in sync with the quaternion, one should wait for the data ready flag. No polling is necessary, the sensor can be configured to send an interrupt when new data samples are ready. When the interrupt is received, one starts the operation to readout the data from the output registers for accel gyro and timestamp, and then waits for the ENDOP, and finally reads the quaternion.

Actually, reading the timestamps is not strictly necessary. Timestamps can be computed by measuring the actual output data rate of the sensor or computing the actual data rate (ODR) based on the INTERNAL_FREQ_FINE register. Given the actual ODR, the time interval is 1/ActualODR, and the n-th data sample will have an associated timestamp equal to n/ActualODR.

Every device has its own output data rate (ODR) due to natural spread in the production process. For example, you may configure the sensor for 120Hz, but the actual ODR can be 110 or 130Hz. One can measure it (count the number of samples in one second, assuming the microcontroller has an accurate clock, best if based on crystal). Or one can compute it as ActualODR = (1 + 0.0013 * INTERNAL_FREQ_FINE) * NominalODR.

If you need to get the data at a specific ODR, you may also want to evaluate the High-Accuracy ODR: when you enable it, the ODR error is reduced (maximum ODR accuracy when gyro is ON). For example, when you program 120Hz, you will get anything in the range 118 or 122 Hz. The other way would be to perform async resampling on the MCU which is much more difficult. 

(there is also another mode which is ODR-triggered by an external signal on a dedicated pin, this allows for maximum accuracy, if the trigger signal is 120Hz, you will get ODR 120Hz, but it is not compatible with SFLP sensor fusion, you would need to run the sensor fusion on the MCU using a dedicated library like MotionFX in X-Cube-MEMS)

FIFO is only utilized when the MCU cannot serve interrupts on a regular basis and you want to batch the processing. May be convenient in some application but increases the overall latency.