I had a problem concerning state estimation with quaternions, specifically with the practical application with an actual IMU. In my simulation in Matlab, my state estimation model worked fine (I used the angle-axis method to update the quaternion), however, when I actually hook up an IMU, I get problems when pitch approaches 90 degrees. Suddenly roll and yaw get crazy big values. Upon further investigation on the quaternion when pitch is 90 degrees, I found something interesting, which I don’t know how to solve:
For example, if I take one of the quaternions I get when pitching with 90 degrees, put it into matlab to figure out whether the angles given are right, this is what I receive (angles in rad/s):

For example this is a quaternion I got: [yaw, pitch, roll] = quat2angle([0.705, 0.00182, 0.706, 0.0013])

yaw =
1.8806
pitch =
1.5662
roll =
1.8813

Obviously, roll and yaw are non zero and way to big, the reason for this is that qx and qz aren’t exactly zero, only close to zero (accuracy problem), which delivers this result, if they were exactly zero this problem wouldn’t occur. I already tried running this through an Extended kalman Filter and it didn’t really help. Anyone got any solution to this issue? btw this issue occured only with pitching. Rolling was fine, and I haven’t tested yaw with it.

Here the code of the pure quaternion integration:
void mpu6050_PureQuaternionAxisAngle(mpu* kal, quatty* quat)
{
float T = (KALMAN_PREDICT_TIME)/1000.0f;

if ((HAL_GetTick() - timeP) >= KALMAN_PREDICT_TIME)
{
quat->PitchEstimate = asinf(2.0f*(quat->quat_y*quat->quat_s - quat->quat_x*quat->quat_z));
quat->RollEstimate = atan2f(2.0f*(quat->quat_y*quat->quat_z + quat->quat_x*quat->quat_s), (quat->quat_s*quat->quat_s - quat->quat_x*quat->quat_x - quat->quat_y*quat->quat_y + quat->quat_z*quat->quat_z));

printf("qs: %f n ",quat->quat_s);
printf("qx: %f n", quat->quat_x);
printf("qy: %f n", quat->quat_y);
printf("qz: %f n", quat->quat_z);

printf("Pitch Angle is %f n ", quat->PitchEstimate*RAD_TO_DEGREES);
printf("Roll Angle is %f n ", quat->RollEstimate*RAD_TO_DEGREES);


mpu6050_GyrRead_Struct(kal);

/* vector amount gyro */
float w = sqrtf(kal->GyroM[0]*kal->GyroM[0] + kal->GyroM[1]*kal->GyroM[1] + kal->GyroM[2]*kal->GyroM[2]);

if (w == 0)
{
    w = 0.01f;
}

kal->GyroM[0] = kal->GyroM[0]/w;
kal->GyroM[1] = kal->GyroM[1]/w;
kal->GyroM[2] = kal->GyroM[2]/w;

float angle = T*w;

float q_delt_s = cosf(angle/2.0f);
float q_delt_x = kal->GyroM[0]*sinf(angle/2.0f);
float q_delt_y = kal->GyroM[1]*sinf(angle/2.0f);
float q_delt_z = kal->GyroM[2]*sinf(angle/2.0f);


quat->quat_s = quat->quat_s*q_delt_s - quat->quat_x*q_delt_x - quat->quat_y*q_delt_y - quat->quat_z*q_delt_z;
quat->quat_x = quat->quat_s*q_delt_x + quat->quat_x*q_delt_s + quat->quat_y*q_delt_z - quat->quat_z*q_delt_y;
quat->quat_y = quat->quat_s*q_delt_y - quat->quat_x*q_delt_z + quat->quat_y*q_delt_s + quat->quat_z*q_delt_x;
quat->quat_z = quat->quat_s*q_delt_z + quat->quat_x*q_delt_y - quat->quat_y*q_delt_x + quat->quat_z*q_delt_s;


float norm = sqrtf(quat->quat_s*quat->quat_s + quat->quat_x*quat->quat_x + quat->quat_y*quat->quat_y + quat->quat_z*quat->quat_z);

/* Norming the quaternion */
quat->quat_s = quat->quat_s/norm;
quat->quat_x = quat->quat_x/norm;
quat->quat_y = quat->quat_y/norm;
quat->quat_z = quat->quat_z/norm;


timeP += KALMAN_PREDICT_TIME;
}

}