I’ve been working on my inertial navigation library. It actually beats ArduPilot’s navigation subsystem in my benchmarks. I’ve been working on these algorithms since my PhD thesis and now I am releasing the stuff as open source embedded C and Python library:

https://github.com/jnz/inslib