# Madgwick/Mahony AHRS code for AltIMU-9 and -10 sensors

**URL:** https://forum.pololu.com/t/madgwick-mahony-ahrs-code-for-altimu-9-and-10-sensors/19293
**Category:** Sensors
**Created:** [April 26, 2020, 7:59pm UTC](https://forum.pololu.com/t/madgwick-mahony-ahrs-code-for-altimu-9-and-10-sensors/19293 "2020-04-26T19:59:56Z")
**Posts on this page:** 2
**Page:** 1

<div class="post-metadata">

### Author: ![Jim\_Remington](https://forum.pololu.com/letter_avatar_proxy/v4/letter/j/a3d4f5/32.png) [@Jim\_Remington](https://forum.pololu.com/u/Jim_Remington)
#### Post date: [April 26, 2020, 7:59pm UTC](https://forum.pololu.com/t/madgwick-mahony-ahrs-code-for-altimu-9-and-10-sensors/19293/1 "2020-04-26T19:59:56Z")

</div>

Since I have some extra time on my hands these days, I implemented the popular Madgwick/Mahony AHRS fusion filters for the AltIMU-10 v3. The present version uses the Mahony algorithm, which makes the most sense to me. (Aside: the Kalman filter is provably the “best”, but only in terms of a _specific_ system model, which is assumed to be accurate. Other fusion filter proposals do not require a specific system model.)

The Mahony filter is quite fast (about 180 updates/second on a 16 MHz Arduino Pro Mini), responsive and perfectly stable when the sensor is not moving, so I’m very pleased with the performance. Both quaternion and Tait-Bryan (one type of Euler) orientation angles can be output. Minor modifications will be needed to work with other AltIMU versions, due to sensor differences.

**Sensor calibration is all-important!**

Most fusion filter implementations suffer from poor sensor calibration, which is absolutely essential for best performance. In many cases, the magnetometer data can be so far out of calibration that the filter results are completely meaningless. So, I’ve written simple Arduino code to collect data for calibrating both the accelerometer and the magnetometer in one go, and point readers to procedures that make the best possible use of the calibration data.

The Github link is [GitHub - jremington/AltIMU-AHRS: Arduino Madgwick/Mahoney AHRS filters for the Pololu AltIMU9 and 10 series of 9 and 10DOF sensors. Includes code for sensor calibration.](https://github.com/jremington/AltIMU-AHRS)

The calibration procedure involves full 3D ellipsoid fitting, resulting in a 9+3 parameter matrix and offset that can correct for severe environmental effects on sensor performance. I described that procedure in some detail, in this post on correcting the Balboa magnetometer:

> [@Correcting the Balboa magnetometer](https://forum.pololu.com/t/correcting-the-balboa-magnetometer/14315):
>
> Balboa appears to be an extremely well engineered robot – nice job, folks! However, a few tests, shown below, reveal that the magnetometer is subject to severe hard iron distortion, perhaps due to the vicinity of the battery casings (I’m using alkaline AAs in my early tests and haven’t tried any others yet). Fortunately, this situation can be fixed. The basic test is simple: just hold the robot in the air and wave it around, covering as many 3D points as possible, while outputting the raw mag…

See also this tutorial and general overview:

> **[Tutorial: How to calibrate a compass (and accelerometer) with Arduino](https://thecavepearlproject.org/2015/05/22/calibrating-any-compass-or-accelerometer-for-arduino/)**
>
> When I started building a flow sensor based on the drag/tilt principle, I knew that leaving sensors on their default factory calibration settings was not optimal, but I had so many other things to …

Keep in mind that **the sensor should be calibrated in its final installation.**

**For those interested in AHRS fusion filter theory** , Olli W’s page hosts a really excellent discussion and comparison of the various popular algorithms. It includes links to the original papers.  
[http://www.olliw.eu/2013/imu-data-fusing/](http://www.olliw.eu/2013/imu-data-fusing/)

---

<div class="post-metadata">

### Author: ![kevin](https://forum.pololu.com/user_avatar/forum.pololu.com/kevin/32/200_2.png) [@kevin](https://forum.pololu.com/u/kevin)
#### Post date: [May 5, 2020, 11:49pm UTC](https://forum.pololu.com/t/madgwick-mahony-ahrs-code-for-altimu-9-and-10-sensors/19293/2 "2020-05-05T23:49:12Z")

</div>

Hi, Jim.

Thanks for sharing your software! I hope to try it out soon and I also hope it will be useful to others looking to implement an AHRS.

Kevin
