MPU9150Lib and gimbal lock?

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);
}

You could try compiling with `#define MPULIB_DEBUG 1` to see if a NaN is generated by atan2. — Salix alba, Jul 01 '14 at 08:31

score 0 · Accepted Answer · edited Jun 20 '20 at 09:12

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.

MPU9150Lib and gimbal lock?

1 Answers1