TinyEKF: Lightweight C/C++ Extended Kalman Filter for microcontrollers

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.”