I am attempting to use quaternions to calculate angles in the global frame.
I have written all the code and it mostly seems to work. Except when I move the sensor on more than one axis the outputs become somewhat unstable and will occasionally output a "nan" error.
I am pretty sure this is due to a math error that I am unable to pinpoint. Does anyone know what could be causing this?
For what it is worth I have tried several methods of converting quaternions to angles and all of them have shown the behavior described above. So I am pretty sure the error lies with generating the qw, qx, qy, qz components.
Most people integrate the gyro rate term directly into the quaternion, assuming small angular steps per time step (so sin(x) ~ x in radians). The code for the Mahony or Madwick fusion filters is a good example and works very well (for small time steps). This example is for a 6DOF sensor (no magnetometer).
//--------------------------------------------------------------------------------------------------
// Mahony scheme uses proportional and integral filtering on
// the error between estimated reference vector (gravity) and measured one.
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// last update 07/09/2020 SJR minor edits
//--------------------------------------------------------------------------------------------------
// IMU algorithm update
// ***acceleration vector must be normalized***
void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
float recipNorm;
float vx, vy, vz;
float ex, ey, ez; //error terms
float qa, qb, qc;
static float ix = 0.0, iy = 0.0, iz = 0.0; //integral feedback terms
// Estimated direction of gravity in the body frame (factor of two divided out)
vx = q[1] * q[3] - q[0] * q[2]; //to normalize these terms, multiply each by 2.0
vy = q[0] * q[1] + q[2] * q[3];
vz = q[0] * q[0] - 0.5f + q[3] * q[3];
// Error is cross product between estimated and measured direction of gravity in body frame
// (half the actual magnitude)
ex = (ay * vz - az * vy);
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
// Compute and apply to gyro term the integral feedback, if enabled
if (Ki > 0.0f) {
ix += Ki * ex * deltat; // integral error scaled by Ki
iy += Ki * ey * deltat;
iz += Ki * ez * deltat;
gx += ix; // apply integral feedback
gy += iy;
gz += iz;
}
// Apply proportional feedback to gyro term
gx += Kp * ex;
gy += Kp * ey;
gz += Kp * ez;
// Integrate rate of change of quaternion, q_dot = (1/2) q cross omega
// https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/
deltat = 0.5 * deltat;
gx *= deltat; // pre-multiply common time step
gy *= deltat;
gz *= deltat;
qa = q[0];
qb = q[1];
qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz);
q[1] += (qa * gx + qc * gz - q[3] * gy);
q[2] += (qa * gy - qb * gz + q[3] * gx);
q[3] += (qa * gz + qb * gy - qc * gx);
// renormalise quaternion
recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] * recipNorm;
q[1] = q[1] * recipNorm;
q[2] = q[2] * recipNorm;
q[3] = q[3] * recipNorm;
}
Both will fail if the time step is too large. Plan on updating the filter at 50 Hz, minimum.
Even then, sudden large changes in orientation can be a problem.
Still no luck. I got the code running at just under 200 Hz and I am experiencing the same error.
Let me explain with a little more detail.
When the code starts running the pitch, yaw, and roll outputs are all stable. When I move the sensor on one axis (say the pitch axis) the angular output is stable and accurate.
Then I move the sensor back to its starting position and move it on the axis perpendicular to the axis I first moved it on (In this case the yaw axis) all the angular outputs change significantly. About 110 degrees on the pitch axis despite me not moving the sensor along this axis. A similar behavior occurs when I move the sensor on more than one axis.
When I return the sensor to zero all the angles return to zero.
Maybe the conversion to Euler angles is not correct. This is what I use, and it works as I expect (except where gimbal lock singularity is encountered). If I recall correctly, yaw is zero for Z up, X pointing at magnetic North.
Of course, the IMU coordinate system has to be right handed, with axes of all sensors in agreement. That is not the case for the manufacturer's chosen axis labels, on many 9DOF sensors
I'm going to guess that it is not, but don't have the patience to work through it for you. You are taking the long way around with these calculations, and one single typo or incorrect sign invalidates the entire series of operations.
When I turn the sensor on and rotate it along its pitch axis the x values shows the rotation along the pitch axis. Same for yaw.
However, when I roll the sensor 90 degrees and rotate the sensor along its global pitch axis the y angle will show the movement.
3D angular operations are neither distributive or commutative, and the order of operations matters a great deal. There are 12 different definitions of Euler angles in common usage, which is one reason why people are moving away from using them.
Which one did you choose for input, and was the same used for output?
(*) Watch out especially for rotations of 90 degrees, because some orientations lead to gimbal lock, where two of the angles become linearly dependent.