How LSM6DSV16X enables sensor fusion low power (SFLP) algorithm

[Denise SANFILIPPO]

Summary      

The LSM6DSV16X device is the first 6-axis IMU that supports data fusion in a MEMS sensor. Sensor fusion is widely used in drones, wearables, TWS, AR/VR and other products. The sensor fusion algorithm can accurately identify the posture of objects in space motion. The LSM6DSV16X integrates a data fusion function, which aims to reduce the user’s algorithm development process and improve product competitiveness. This allows customers to quickly develop applications. The LSM6DSV16X adopts gyroscope dynamic calibration, greatly improving the stability of the algorithm and providing excellent performance.

 

1. How to compute data fusion based on an IMU?

Rotation matrices can be obtained from these three examples using matrix multiplication. For example, the product:

 

Represents a rotation whose yaw, pitch, and roll angles are α, β and γ, respectively. More formally, it is an intrinsic rotation whose Tait–Bryan angles are α, β, γ, about axes z, y, x, respectively. Similarly, the product:

 

 

2. What is the SFLP algorithm in the LSM6DSV16X?

A sensor fusion low-power (SFLP) block is available in the LSM6DSV16X for generating the following data based on the accelerometer and gyroscope data processing:

1. Game rotation vector, which provides a quaternion representing the attitude of the device.
2. Gravity vector, which provides a three-dimensional vector representing the direction of gravity.
3. Gyroscope bias, which provides a three-dimensional vector representing the gyroscope bias.

Sensor fusion performance and time required to reach steady state

 

3. SFLP demo and tools

The hardware setup is STEVAL-MKI109V3 and DIL24 adapter board STEVAL-MKI227KA.

STEVAL-MKI109V3DIL24 adapter

 

The software used is Unico-GUI.

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

When you shake the demo board, the image follows the actual position displayed. SFLP has integrated quaternion, so customers can directly get quaternion and convert it into euler angles.

4. Yaw angle automatic calibration flow

Due to the long-term operation of the gyroscope, the integration error caused by zero deviation becomes increasingly large. The yaw angle may experience significant drift due to lack of calibration of the magnetometer. To solve this problem, the SFLP algorithm adopts a dynamic self-calibration method to ensure yaw angle stability.

 

    

5. Automatic calibration flow

 

6. How to configure the SFLP?

Coordinates:
Pitch rotation around the X-axisΩP
Roll rotation around the Y-axis ΩR
Heading/Yaw rotation around the Z-axisΩY

• Game rotation vector, which provides a quaternion representing the attitude of the device.
• Gravity vector, which provides a three-dimensional vector representing the direction of gravity.
• Gyroscope bias, which provides a three-dimensional vector representing the gyroscope bias.    

SFLP Register description:

Register bit config	Bit description
SFLP_GAME_EN	Enables SFLP
SFLP_GBIAS_FIFO_EN	Enables Gbias in FIFO mode
SFLP_GRAVITY_FIFO_EN	Enables gravity vector in FIFO mode
SFLP_GAME_FIFO_EN	Enables game rotation vector in FIFO mode
SFLP_GAME_ODR_[2:0]	Configures SFLP ODR

SFLP data format:

	TAG	X_L	X_H	Y_L	Y_H	Z_L	Z_H	Axis format
Game rotation vector	13h	X		Y		Z		Half precision floating-point
Gyroscope bias	16h	X		Y		Z		int16_t (raw, 125 dps sensitivity)
Gravity vector	17h	X		Y		Z		int16_t (raw, 2 g sensitivity)

Code configuration:

 /* Check device ID */
 lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);

 if (whoamI != LSM6DSV16X_ID)
 while (1);

 /* Restore default configuration */
 lsm6dsv16x_reset_set(&dev_ctx, LSM6DSV16X_RESTORE_CTRL_REGS);
 do {
 lsm6dsv16x_reset_get(&dev_ctx, &rst);
 } while (rst != LSM6DSV16X_READY);

 /* Enable Block Data Update */
 lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
 /* Set full scale */
 lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
 lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_2000dps);

 /*
 * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
 * stored in FIFO) to FIFO_WATERMARK samples
 */
 lsm6dsv16x_fifo_watermark_set(&dev_ctx, FIFO_WATERMARK);

 /* Set FIFO batch of sflp data */
 fifo_sflp.game_rotation = 1;
 fifo_sflp.gravity = 1;
 fifo_sflp.gbias = 1;
 lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);

 /* Set FIFO mode to Stream mode (aka Continuous Mode) */
 lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);

 /* Set Output Data Rate */
 lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);
 lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);
 lsm6dsv16x_sflp_data_rate_set(&dev_ctx, LSM6DSV16X_SFLP_30Hz);

 lsm6dsv16x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);

More details about SFLP software are available on the GitHub repo sensor-fusion-code.

Conclusion

In this article, we described the principles of sensor fusion in the context of an inertial measurement unit and how to use sensor fusion in the LSM6DSV16X device.  The LSM6DSV16X integrates a complete set of acceleration and gyroscope algorithms in the sensor. This allows customers to obtain high-precision fusion algorithm results without specifying specific algorithm implementations.

Customers can use STEVAL-MKI109V3 and LSM6DSV16X to evaluate the SFLP performance and how to configure SFLP on the demo board. Users can quickly deploy algorithms, test, and evaluate performance.

You may also be interested in reading the following knowledge article: How to save power with LSM6DSV16X by leveraging on its adaptive self-configuration with MLC and FSM 

[marti3110]

Hi @Denise SANFILIPPO ,thanks for the explanation.

[Stefan3]

Hi Denis

I have set the all the registers
It looks like it works.

I can read the FIFO values

The values change but not yet properly. I just read the the six FIFO_data_out byte

I have not yet understood what this table means, 

 

[Denise SANFILIPPO]

Hi @Stefan3 ,

Here is a short explanation about the FIFO and SFLP block inside LSM6DSV16X:

The FIFO can be configured to store the SFLP game rotation vector, gravity vector, gyroscope bias.

The reconstruction of a FIFO stream is a simple task thanks to the TAG_SENSOR_[4:0] field of the FIFO_DATA_OUT_TAG register that allows recognizing the meaning of a word in FIFO.

For SFLP, the table below contains the values for the TAG_SENSOR_[4:0] field:

TAG_SENSOR_[4:0]	Sensor name	Sensor name	Description
0x13	Game rotation vector	Virtual	SFLP-generated game rotation vector
0x16	Gyroscope bias	Virtual	SFLP-generated gyroscope bias
0x17	Gravity vector	Virtual	SFLP-generated gravity vector

Data can be retrieved from the FIFO through six dedicated registers, from address 79h to 7Eh, in particular: 

• FIFO_DATA_OUT_X[15:0] = FIFO_DATA_OUT_X_L + FIFO_DATA_OUT_X_H << 8
• FIFO_DATA_OUT_Y[15:0] = FIFO_DATA_OUT_Y_L + FIFO_DATA_OUT_Y_H << 8
• FIFO_DATA_OUT_Z[15:0] = FIFO_DATA_OUT_Z_L + FIFO_DATA_OUT_Z_H << 8

A dedicated sensor fusion block (SFLP) is available for generating the following virtual sensors based on
processing the accelerometer and gyroscope data:
• Game rotation vector, which provides a quaternion representing the attitude of the device
• Gravity vector, which provides a three dimensional vector representing the direction of gravity
• Gyroscope bias, which provides a three dimensional vector representing the gyroscope bias

SFLP-generated sensors are read only from FIFO and they are selectively enabled:
• Game rotation vector is batched by setting the SFLP_GAME_FIFO_EN bit of the EMB_FUNC_FIFO_EN_A
register to 1.
• Gravity vector is batched by setting the SFLP_GRAVITY_FIFO_EN bit of EMB_FUNC_FIFO_EN_A
register to 1.
• Gyroscope bias is batched by setting the SFLP_GBIAS_FIFO_EN bit of the EMB_FUNC_FIFO_EN_A
register to 1.

If batching in FIFO is enabled, the SFLP-generated sensors are stored in FIFO according to the SFLP output data rate.

The format for the SFLP-generated sensors in FIFO is listed below:
• Game rotation vector: X, Y, and Z axes (vector part of the quaternion) are stored in half-precision floating point format, where w (scalar part of the quaternion) is computed in software after reading the data from
the FIFO, since the game rotation vector is a unit quaternion.
• Gravity vector: X, Y, and Z axes are stored as 16-bit two's complement number with ±2 g sensitivity.
• Gyroscope bias: X, Y, and Z axes are stored as 16-bit two's complement number with ±125 dps sensitivity.

I suggest looking at the following example lsm6dsv16x_sensor_fusion available inside our official GitHub: it can be useful to understand the various steps needed to use SFLP with LSM6DSV16X. 

Please refer also to AN5763 for further info. 

[Stefan3]

Hi Denis

I have a very nice signal from the Accellero and from Gyroscope.

From the SFLP I get some problems.

[Denise SANFILIPPO]

Hi @Stefan3 ,

can you provide more details about your plots? 
In particular, how are you reading SFLP data and which is the unit of measurement of your plots?

[Stefan3]

Hi Denis

I read with an Interrupt 30Hz the six register and display the read value 1000 times in Chart. I do not move the sensor. For the first test I read only Gyro data, for the second only Acceloero data and for the third test only the fusion data.
If I move the sensor all values change, that would be ok so far

 

[Denise SANFILIPPO]

Hi @Stefan3 ,

Are you converting the SFLP output data from half-precision floating-point format (float16) to float format?

You should interpret SFLP data as follows:

Please note that the SFLP output data is quaternions, not angles. 

[Stefan3]

Hi Denis

I guess I still have to read FIFO_DATA_OUT_TAG (78h) and wait until the value there is 0x13, 0x16 or 0x17.

Or how do I know which data to read?

 

FIFO_DATA_OUT_TAG (78h)

[Denise SANFILIPPO]

Hi @Stefan3,

yes, by reading the FIFO_DATA_OUT_TAG register, it is possible to understand to which sensor the data of the current reading belongs and to check if data are consistent. In fact, the reconstruction of a FIFO stream is a simple task thanks to the TAG_SENSOR field of the FIFO_DATA_OUT_TAG register that allows recognizing the meaning of a word in FIFO.

[Stefan3]

Hi Denis

With the attention of the TAG I can now read SFLP.
But in the data I regularly had wrong values.

For example 20,21,24. in all the 3Channel

Every time I got these values, I read the data again and then they were ok again.

Now I am trying to convert the values into roll, pitch, yaw