Hi all,

I ported the uNav Attitude and Heading Reference System (AHRS), which is a 7 state Extended Kalman Filter (EKF) used to estimate attitude and heading from IMU data, to work well as an Arduino library.
https://github.com/bolderflight/uNavAHRS

This particular filter was developed by Jung Soon Jang at Stanford University and was used at the University of Minnesota UAS Research Labs since 2006. It hasn't been used as a primary navigation filter at the University of Minnesota in a long time, the 15 state EKF which include GPS data is used for that, but it provides good estimates of attitude and heading without requiring a GPS. It uses gyro measurements to propagate the state; accelerometers are used as a measurement update on the pitch and roll channels and magnetometers as a measurement update on the yaw channel to constrain drift. Optionally, airspeed measurements may be provided to correct apparent accelerations that can lead to errors in AHRS filters used on air vehicles.

Performance is great on a Teensy 3.6, it takes about 125 us for a time and measurement update. I've included an example of using this filter with the MPU-9250 IMU.

Best,
Brian