MPU9150Lib and gimbal lock? - locking

Question

I'm currently using the MPU9150Lib on my Arduino project to read data from my IMU (of course a MPU9150). It works great so far. But I noticed some small glitches sometimes. I think it's a gimbal lock problem. When I look at the library, it seems they fuse the data and afterwards simply convert the resulting Euler angles to a quaternion. Could this cause a gimbal lock?
If so, could you guys help me rewrite the code? I don't really understand the math behind the data fusion. So I don't know how to correct the library.
You can find it here:
https://github.com/zarthcode/MPU9150Lib/tree/master/libraries/MPU9150Lib
The library contains the following function. I'm not sure if the problem lies within the function, but to me it looks strange since in the end it convert euler coordinates to a quaternion.
void MPU9150Lib::dataFusion()
{
float qMag[4];
float deltaDMPYaw, deltaMagYaw;
float newMagYaw, newYaw;
float temp1[4], unFused[4];
float unFusedConjugate[4];
// *** NOTE *** pitch direction swapped here
m_fusedEulerPose[VEC3_X] = m_dmpEulerPose[VEC3_X];
m_fusedEulerPose[VEC3_Y] = -m_dmpEulerPose[VEC3_Y];
m_fusedEulerPose[VEC3_Z] = 0;
MPUQuaternionEulerToQuaternion(m_fusedEulerPose, unFused); // create a new quaternion
deltaDMPYaw = -m_dmpEulerPose[VEC3_Z] + m_lastDMPYaw; // calculate change in yaw from dmp
m_lastDMPYaw = m_dmpEulerPose[VEC3_Z]; // update that
qMag[QUAT_W] = 0;
qMag[QUAT_X] = m_calMag[VEC3_X];
qMag[QUAT_Y] = m_calMag[VEC3_Y];
qMag[QUAT_Z] = m_calMag[VEC3_Z];
// Tilt compensate mag with the unfused data (i.e. just roll and pitch with yaw 0)
MPUQuaternionConjugate(unFused, unFusedConjugate);
MPUQuaternionMultiply(qMag, unFusedConjugate, temp1);
MPUQuaternionMultiply(unFused, temp1, qMag);
// Now fuse this with the dmp yaw gyro information
newMagYaw = -atan2(qMag[QUAT_Y], qMag[QUAT_X]);
if (newMagYaw != newMagYaw) { // check for nAn
#ifdef MPULIB_DEBUG
Serial.println("***nAn\n");
#endif
return; // just ignore in this case
}
if (newMagYaw < 0)
newMagYaw = 2.0f * (float)M_PI + newMagYaw; // need 0 <= newMagYaw <= 2*PI
newYaw = m_lastYaw + deltaDMPYaw; // compute new yaw from change
if (newYaw > (2.0f * (float)M_PI)) // need 0 <= newYaw <= 2*PI
newYaw -= 2.0f * (float)M_PI;
if (newYaw < 0)
newYaw += 2.0f * (float)M_PI;
deltaMagYaw = newMagYaw - newYaw; // compute difference
if (deltaMagYaw >= (float)M_PI)
deltaMagYaw = deltaMagYaw - 2.0f * (float)M_PI;
if (deltaMagYaw <= -(float)M_PI)
deltaMagYaw = (2.0f * (float)M_PI + deltaMagYaw);
newYaw += deltaMagYaw/4; // apply some of the correction
if (newYaw > (2.0f * (float)M_PI)) // need 0 <= newYaw <= 2*PI
newYaw -= 2.0f * (float)M_PI;
if (newYaw < 0)
newYaw += 2.0f * (float)M_PI;
m_lastYaw = newYaw;
if (newYaw > (float)M_PI)
newYaw -= 2.0f * (float)M_PI;
m_fusedEulerPose[VEC3_Z] = newYaw; // fill in output yaw value
MPUQuaternionEulerToQuaternion(m_fusedEulerPose, m_fusedQuaternion);
}

Answer 1

I was able to fix the problem. I found another library that was uploaded for the sparkfun breakout board of the mpu9150. I couldn't get this one to work either, but for other reasons. The raw data was very noisy and I couldn't fix it by tweaking the options. But in the example that comes with the library is a function for the data fusion. I copied it into the MPU9150Lib and adjusted the variables. So basically I use the MPU9150Lib now with the data fusion algorithm from the sparkfun breakout board. Now everything works like a charme.
You can find the sparkfun lib here:
https://github.com/sparkfun/MPU-9150_Breakout/tree/master/firmware/MPU6050/Examples
The example in there is finally what I was looking for all the time. A well documented way to use the mpu9150. Took me some time to get this thing up and running the way I wanted to.