Having received many positive emails about my Extended Kalman Filter Tutorial, I wanted to see whether I could write my own general-purpose EKF from scratch, suitable for running on a microcontroller like Arduino, Teensy, and the STM32 platform used on today's popular flight controllers (Pixhawk, Naze, CC3D). I wanted something that could be easily modified as new sensors were added, but that didn't use dynamic memory allocation or other techniques that are impractical in an embedded environment.
The result is TinyEKF, a C/C++ EKF implementation that takes care of most of the EKF algorithm for you. There is a C++ version for Arduino/Teensy, and a pure C version for STM32. For both versions, you use #define to specify the number of state values N and the number of sensor measurements M. You also provide a method to compute the output of the state-transition function f(x), the observation function h(x), and their respective Jacobian matrices F(x) and H(x). As you'll see from the examples, this involves perhaps ten lines of code for each implementation. TinyEKF takes care of the rest (prediction, update). This implementation is naturally not as efficient as the hard-coded EKFs you see in actual flight controllers, but I am hoping that with today's increasingly fast ARM processors, and perhaps some behind-the-scenes optimizations, TinyEKF will become a useful, scaleable way of implementing your own EKF.
I've provided an Arduino example using sensor fusion of a BMP180 barometer / temperature sensor with an LM35 temperature sensor. For those who don't have an Arduino but want to try it out anyway, there's also a GPS example in C, using realistic satellite data stored in a .CSV spreadsheet. Once I get my STM32 Nucleo board, I plan to port the Arduino sensor fusion example to that platform as well.