Arduino + MPU-9250 + AHRS = ???

This question is a little bit off topic, but still robotics related, and a lot of the same concepts are used frequently in FRC. I couldn’t get a response on the Teensy forums, so I thought I’d ask here.

I have a Teensy LC hooked up to this MPU9250 (using i2c + the sensor’s own integrated pullups), and I can’t figure out which AHRS library and MPU-9250 libraries I should be using to merge the accel/gyro/mag output into one heading.

I found this MPU-9250 library:
And I found this Madgwick filter based AHRS library (although it seems to only take gyro/accel as input):

Do you guys have any recommendations/suggestions for combining gyro/accel/mag input to one heading? My plan is to use this as the feedback for a PID loop. I’m assuming the PID loop has to refresh at the same speed that the AHRS filter does?

If I use the Madgwick filter library I linked, which only works with gyro/accel, would heading drift be noticeably bad? It looks like the FRC ADXRS450 Gyro uses a pretty simple integration-esque type algorithm to retrieve angle, maybe I should try something like that? I’m just worried about drift adding up…

The project I’m putting this in is a micro robot which navigates a maze, and needs to hold certain headings relative to starting heading. The entire period of the mini-robot’s driving will be 10-15 seconds max. Thanks in advance :slight_smile:

Edit: also found this page, which suggests it can be done:

To get a fused heading estimate from 9DOF orientation, you’ll need something like a Kalman filter. They’re pretty tricky if you don’t have experience in that realm of control systems, but there should exist some premade solutions. I’m surprised there isn’t one for the 9250, given its popularity. You could also take a look at a complementary filter. They’re not very amazing, but they work well enough. There should be a genericish Madgewick implementation for 9DOF somewhere — I used it in a project a long time ago.

For the record, the 9250 is intrinsically extremely flawed; much better solutions exist for not that much more money. Adafruit sells the BNO055 for $35, which has built-in sensor fusion as well as better specs and performance than the 9250. Additionally, there’s their FXOS8700 + FXAS21002 breakout that warrants a custom filter but promises better theoretical performance than the 055. However, for ease of use’s sake, I’d highly recommend the 055 for your application.

If you’re not using the built in algorithms of something like the 055, you’ll have to do some sort of integration to get heading from rotational velocity (which is what the gyro measures). Various signal filtering methods simply serve as a method to combine the three onboard sensors’ readings. The ADXRS450 doesn’t have more than one sensor (just 3DOF gyro iirc) so it doesn’t have any reason to use filtering.

I already own an MPU-9250 which is why I’m using it. I’ll keep those alternatives in mind for the future though :slight_smile:

If I just used the MPU-6050 gyro/accel “component” of the MPU-9250 with that gyro/accel only Madgwick filter I found, do you think that would work reasonably well?

Should work, albeit suboptimal. MahonyAHRS and MadgwickAHRS both look solid, albiet only 6DOF.

If it’s good enough for 254, it’s good enough for me.

Wait, I had no idea the NavX used an MPU-9250.

Now I’m convinced there’s a way to do this…

The firmware used on the NavX-MXP is open source, available on the Kauai Labs github, including the MPU-9250 bits. FRC Championship robots for last two years running are using it. It’s designed to run on a STM32, but the MPU-9250 bits should be portable to Arduino.

Feel free to message me with questions, we are here to help.

Just took a look at source… not sure if I could rewrite thousands of lines :smiley:

You didn’t say you wanted easy. :wink:

The interesting parts for interacting with the MPU-9250 are in this file:

This code uses the Invensense Motion Driver 6.1 SDK. There’s also some code to intialize the sensor, manage calibration and orientation configuration data, etc - but the key bits are in the mpucontroller.cpp file.

Send an email to and we can provide some high-level guidance if you’re ready to roll up your sleeves and dig in.

  • scott

Sure, it works fine, but so did the intensely nonlinear 884s and anemic cRIO. At least in my opinion, if you’re doing something from scratch, you might as well do it as well as possible. Just cause the 9250 works well enough doesn’t mean it’s wrong or a waste of time to try to achieve more performance.

I’m really interested in different orientation sensors so I’m curious what is flawed about the 9250. Also, it sounds like you’ve worked with the BNO055 a bit. How often or how important is it to calibrate? Will it work if you don’t ever do the calibration sequence?


What are the performance specifications the BNO055 can achieve that the MPU-9250 cannot, and how specifically do those differences impact FRC robot perfomance?

With the goal of constant improvement and promoting engineering discipline, I’d appreciate your giving the community the chance to analyze the data upon which the opinion you’ve stated was formed.

And as someone focused on designing next-gen Kauai Labs products, I’m all ears.


  • scott

It’s true. Scott takes feedback seriously and listens to his community and customers.

In fact, pretty much all of the competitive robot manufacturers do this and make enhancements based on community suggestions. It’s a rare thing in this world.

To answer your questions regarding calibration of the BNO055:

You need to recalibrate the BNO055 every time that power is lost and reapplied to the sensor, as it doesn’t have any built-in EEPROM. However, it seems to work fairly well for getting orientation (I don’t know your application) even without calibration. This is because the gyro gets calibrated on startup as long as the sensor is being held still, which it usually is. Magnetometer ends up calibrating itself to some extent as you move it around. The only tricky one is the accelerometer, as it seems like you do need to rotate it around one of its axes and hold it in place for a few seconds every 45 degrees.

TL;DR: If all you need is orientation, calibration probably won’t be an issue. But if you need other info like which direction gravity is, you will need to do some more work.

If you have any questions about Adafruit’s breakout, please PM me as I have experience with it.

There are libraries that load a calibration data set into the BNO055 at start up just as the MPU-9250 libraries do.

We use the 9250 in the Pigeon IMU.

We’ve used Pigeon for several applications successfully (Yaw/Pitch servo, turns, go-straights, hit-detection, etc…).

In fact our intern put together a new demo bot using it…

works for me :slight_smile:

More Pigeon testing below (jump to the middle)…

I am also trying to make the IMU9250 system work in a robot where I only need relative (not absolute) heading (yaw) information.

I’m using the Sparkfun 9250 breakout with a Teensy 3.2, and I’m using Kris Winer’s MPU925BasicAHRS code (slightly modified for Teensy I2C) for testing. When I rotate the sensor around the Z-axis, the yaw output changes appropriately for the first few complete rotations, but then the output decreases dramatically after that, to the point where a full manual rotation only produces a 50 degree change in the yaw heading value (see attached excel plot).

Any idea what I’m doing wrong here?