Problem with Drift in Orientation with SensorFusion 7.0

joncox — Tue, 03 Feb 2026 21:01:35 GMT

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:

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?
How sensitive is the algorithm to gryo DC offset / drift? Do I need to zero this out precisely for my hardware?
Am I missing any calibration routines in my main loop?
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.
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?
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?
Any other pointers you can share with me to get this working properly?

Re: Problem with Drift in Orientation with SensorFusion 7.0

michaelestanley — Fri, 21 Apr 2017 18:32:42 GMT

Jonathan,

We don't normally offer support for ports on competitive parts (Invensense is NXP's competition), but I'll try to answer a few of your questions (and others are welcome to jump in with additional suggestions). In order:

1. Section 5.3 of the user guide discusses hardware abstraction issues.

2. The Kalman filter includes gyro offsets as 3 of the variables (XY&Z) it tracks. So long as your device has some rotation, it will continuously update those estimates. The magnetometer and gyro reference vectors should keep things stable when the device is at rest.

3. Sorry, I can't offer code reviews.

4. Proper magnetic calibration is very important. Don't expect good 9-axis results without it. The routines we include generally do a pretty good job handling hard/soft iron sources that are fixed spatially with respect to the sensor. We include some code to handle general interference, although there are limitations to just how well that can be done.

5. If you check the code comments, you will see that iFirstAccelMagLock is just a boolean variable used to ensure that some initialization code runs only once. Don't infer any general health from it's status.

6. I see no problem with the approach you outline so long as your MCU has enough MIPS to keep up.

7. Read the manual. I tried to put as much "collective wisdom" there as I possibly could.

Regards,

Mike

topic Re: Problem with Drift in Orientation with SensorFusion 7.0 in Sensors

Problem with Drift in Orientation with SensorFusion 7.0

Re: Problem with Drift in Orientation with SensorFusion 7.0