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.

E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • void ekf_init(void * v, int n, int m)
    /* retrieve n, m and set them in incoming data structure */
    int * ptr = (int *)v;
    *ptr = n;
    *ptr = m;
    /* unpack rest of incoming structure for initlization */
    ekf_t ekf;
    unpack(v, &ekf, n, m);
    /* zero-out matrices */
    zeros(ekf.P, n, n);
    zeros(ekf.Q, n, n);
    zeros(ekf.R, m, m);
    zeros(ekf.G, n, m);
    zeros(ekf.F, n, n);
    zeros(ekf.H, m, n);
  • paranoid :-)

    colums and rows are better as defines.

    but i would initialize all memory - memory might look different after a warmstart.

  • Sorry, I meant "you cannot use those symbols elsewhere in your code".

  • Thanks for the suggestion, Phillip.   I was concerned about that same issue, so I tested the code below and found that it compiles (using gcc) and runs just fine.  Of course, using N and M as #define'd constants means you can use those symbols elsewhere in your code, but I figured that that was a small price to pay to keep the code simple and aligned with the notation in the literature.

    #define N 8

    #define M 4

    #include "tinyekf.h"

    int main(int argc, char ** argv)
        int Nfoo = 0;             // Will this survive #define N 8 ?
        printf("%d\n", Nfoo);

       // etc.


  • Very interesting.  I will have to play with this more.

      Using a #define to change a single letter into a number seems like a recipe for compile errors if any variable in the project starts with that letter.  I would change them to something more unique (i.e. M to M_SENSOR_INPUTS and N to N_STATE_VALUES).  I might be paranoid.

  • Very cool Simon. I've been meaning to learn more about ekfs. I am sure this will be good reference material.
  • LOL !!

    Yeah right, I guess that "Lithium" batteries might have some incidence with this behavior change on my "circuits" :-)

    Thanks for your help 

  • yes, that's my next weekend test as i am going to work on a new flicghtcontroller.

    thanks for sharing.


    this is what i am using at the moment.

    at some time i am going to rewrite it using the dsp lib.

    An iPhone-friendly Kalman filter written in C. Contribute to lacker/ikalman development by creating an account on GitHub.
  • 20 years? Now that's what I call a "trigger warning"!  ;^)

    Srsly, I'll be interested in seeing if you can get it working on a real IMU.  Happy to help out if I can.

  • Cool stuff , I might give it a try on my homebrew IMU.

    Funny though, It took me 20 years to step from Scmitt Trigger to Kalman Filter... 

This reply was deleted.