Strange trend of Euler Angles with sensor fusion algorithm (LSM6DSV16X device)

[marti3110]

I am using a STM32F401RE board with the X-NUCLEO-IKS4A1 expansion board. I configure SFLP on the board in order to get the quaternion values ​. 

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 tried changing the ODR from 30Hz to 120Hz and also tested the HAODR but the results don't change much.

ODR: 120 HzODR: 120 HzHAODRHAODR

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?

[Federica Bossi · Accepted solution]

Hi @marti3110 ,

This is not a problem with the algorithm (neither SFLP nor MotionFX 6X / 9X), but with the representation of the output using Euler angles, which by nature suffer from the well-known ‘gimbal lock’ issue.

If you try SFLP with MEMS Studio which visualizes the outputs via a 3D model (rotated using the quaternion generated by the sensor), you will see that the algorithm works more than well and without jumps.