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