Strange trend of Euler Angles with sensor fusion algorithm (LSM6DSV16X device)
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 Hz
ODR: 120 Hz
HAODR
HAODR
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?