AnsweredAssumed Answered

Problem with Drift in Orientation with SensorFusion 7.0

Question asked by Jonathan Cox on Apr 17, 2017

I am developing an application that uses  a 9-DOF MEMS navigation device (InvenSense MPU-9250, hopefully containing NXP parts??) to provide real-time orientation on custom designed hardware, software and drivers. Since everything is custom, I had to strip down the SensorFusion 7.0 library to the core routines provided in the SDK. In addition, I call the initialization functions directly and wrote my own function to load the data into a 1 element FIFO for gyro, magnetometer and accelerometer.


The axes of the MPU-9250 are defined as shown in the image below. Therefore, I rotate the magnetometer axes to make it the same as for the gyro and accelerometer. I do this by negating the Z-axis and swapping X and Y axes.

I have also checked to confirm that the units are correct (acceleration in g, rotational velocity in degrees/s, and magnetometer in micro Teslas). I set the conversion factors for "counts per unit" and "units per count" for integer and floats appropriately (or so I believe).


I am running the fRun_9DOF_GBY_KALMAN algorithm at 200 Hz and loading new data/samples into my 1 element FIFO also at 200 Hz for all 9 axes. I find that after moving the device around a bit, the iFirstAccelMagLock flag will switch from 0 to 1 (which would seem to be a good sign). 


However, I am finding that the various angular outputs of the Kalman filter, which are

  • fPhiPl; ///< roll (deg)
  • fThePl; ///< pitch (deg)
  • fPsiPl; ///< yaw (deg)
  • fRhoPl; ///< compass (deg)
  • fChiPl; ///< tilt from vertical (deg)
  • fDeltaPl; ///< a posteriori inclination angle from Kalman filter (deg)

seem to be drifting in space and not responding to actual rotation of the device.


I tried measuring the DC offset of my gyro with long term averaging at rest. It is about 0.5 deg/s, which I subtract out, but this drift and lack of response persists.


Thus, I have the following questions regarding the usage of the SensorFusion 7.0 library:

  1. Where is it specified exactly what the expected input axes and units are for all 9 degrees of freedom, including rotational direction for the gyro? I assume that X, Y and Z axes should all be consistent for gyro, mag and accel, and that the right hand rule should be satisfied for the gyro rotational direction. But again, is this documented precisely somewhere?
  2. How sensitive is the algorithm to gryo DC offset / drift? Do I need to zero this out precisely for my hardware?
  3. Am I missing any calibration routines in my main loop?
  4. How sensitive is SensorFusion (fRun_9DOF_GBY_KALMAN) to the calibration of the magnetometer? I noticed my magnetometer seems to be off by a factor of 10x, and its near a lot of metal, so I don't know if the field components (X,Y,Z) are also off. Should I expect the algorithm to not operate properly unless the magnetometer reading and field distortion is minimal? I saw some hard/soft iron and calibration routines in the code, so I'm wondering how good of a job these do for compensation for an uncalibrated magnetometer.
  5. What does it mean when iFirstAccelMagLock = 1? Does this mean that SensorFusion believes my magnetometer and gyro data is working correctly together, or could iFirstAccelMagLock = 1 even if the gyro and magnetometer data is screwed up somehow. In other words, how reliable an indication of proper operation is this variable?
  6. I am sampling my 9-DOF device at 200 Hz and loading data at 200 Hz into SensorFusion with a 1 element FIFO. The analog bandwidth of my samples is around 50 or 100 Hz (I can configure this). I am reading the output of SV_9DOF_GBY_KALMAN also at 200 Hz. Are there any problems with this approach?
  7. Any other pointers you can share with me to get this working properly?