on - edited on by
Laurids_PETERSE
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:
- 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.
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-MKI109V3
DIL24 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
@Denise SANFILIPPO I managed to visualize the quaternions through the output terminal and saved the data.
The commans I used are:
"
fifo_sflp.game_rotation = 1;
fifo_sflp.gravity = 1;
fifo_sflp.gbias = 1;
lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);
lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);
lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz); // ODR dell'accelerometro
lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz); // ODR del giroscopio
lsm6dsv16x_sflp_data_rate_set(&dev_ctx, LSM6DSV16X_SFLP_30Hz);
lsm6dsv16x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);
lsm6dsv16x_sflp_gbias_t gbias;
gbias.gbias_x = 0.0f;
gbias.gbias_y = 0.0f;
gbias.gbias_z = 0.0f;
lsm6dsv16x_sflp_game_gbias_set(&dev_ctx, &gbias);
while (1)
{
// Controlla lo stato del FIFO
lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
if (fifo_status.fifo_th == 1) {
// Leggi i dati dal FIFO
uint16_t num_samples = fifo_status.fifo_level;
while (num_samples--) {
lsm6dsv16x_fifo_out_raw_t raw_data;
lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &raw_data);
int16_t *axis;
float gravity_mg[3];
float gbias_mdps[3];
// Se i dati sono del Game Rotation Vector
if (raw_data.tag == LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG) {
// Converti i dati grezzi in quaternioni
sflp2q(quaternion, (uint16_t*)&raw_data.data[0]);
// Invia i dati via UART
snprintf((char*)tx_buffer, sizeof(tx_buffer), "Game Rotation \tX: %2.3f\tY: %2.3f\tZ: %2.3f\tW: %2.3f\r\n", quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
tx_com(tx_buffer, strlen((char const*)tx_buffer));
}
else if (raw_data.tag == LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG)
{
axis = (int16_t *)&raw_data.data[0];
gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[1]);
gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[2]);
snprintf((char *)tx_buffer, sizeof(tx_buffer), "GBIAS [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
(double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);
tx_com(tx_buffer, strlen((char const *)tx_buffer));
}
else if (raw_data.tag == LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG)
{
axis = (int16_t *)&raw_data.data[0];
gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0]);
gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[1]);
gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[2]);
snprintf((char *)tx_buffer, sizeof(tx_buffer), "Gravity [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
(double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);
tx_com(tx_buffer, strlen((char const *)tx_buffer));
}
}
}
MX_MEMS_Process();
}"
The commands I used to get Euler angles from quaternions in Matlab are the following:
"q=quaternion(W_values,X_values,Y_values,Z_values);
angles=eulerd(q,'ZYX','frame');
roll_eulerd=angles(:,3);
pitch_eulerd=angles(:,2);
yaw_eulerd=angles(:,1);
yaw_eulerd = yaw_eulerd - yaw_eulerd(1);"
In the first acquisition I performed a roll:+90, pitch:+90, yaw:+90 rotation and in the second acquisition I performed a roll:-90, pitch:-90, yaw:-90 rotation and I get the attached graphs.
The pitch angle corresponds to the rotations made but the Roll angle varies even when it varies pitch angle and the Yaw angle instead always has a strange trend.
I also encountered this problem when implementing the Motion FX 6x and Motion FX 9x libraries and getting quaternions as output.
Since for my project I need to obtain very precise angles that do not vary when there is rotation around a single axis, what can I do?
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,
Hi Denis
I have a very nice signal from the Accellero and from Gyroscope.
From the SFLP I get some problems.
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
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.
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
Hi Denis, Hi Eliane
I read the game rotation vectors from the LSM9
But unfortunately no fourth value. This is called the scalar part. float sw = quat[3];
How do I calculate this scalar part or how do I read it?
float sx = quat[1];
float sy = quat[2];
float sz = quat[0];
float sw = quat[3];
Hi Denis
It now works from configuring the LSM6, reading from FIFO, sending to USB and receiving in the App.
Also the conversion from FP16 to FB32 and the calculation of pitch, yaw, roll and each in grad degres is perfect.
A small problem is still, regular spikes in the data.
In decimal the values are about 20-40 und if I convert to FP32 it's looks like a very small number or somtimes very high number
I suppose I read a counter or something similar instead of the quaternion.
Hi Denis
Ok before I had only read 1 byte and if that was correct then I have read the 6 others.
With your suggestion to read 7 at once and then when the tag is right I use it, it works very well and also faster.
I can read and transmit at 15Hz, 30Hz, 60Hz, 120Hz, 240Hz, 480Hz without any problems.
The old chart shows Pitch, Yaw, Roll in Grad degrees.
Here a new chart with both, the convertet Float32 value (top) and the chart with the Pitch, Yaw, Roll (bottom). The x (pitch) is 358 grad. It changes the value when I turn, but I suspect not in the right range
