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.

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.

Views: 15509

Comment by Patrick Poirier on November 20, 2015 at 2:11pm

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

Comment by Simon D. Levy on November 20, 2015 at 2:27pm

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.

Comment by robert bouwens on November 21, 2015 at 12:15am

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

thanks for sharing.

https://github.com/lacker/ikalman

this is what i am using at the moment.

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

Comment by Patrick Poirier on November 21, 2015 at 7:12am

LOL !!

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

Thanks for your help 

Comment by Daniel Nugent on November 21, 2015 at 5:50pm
Very cool Simon. I've been meaning to learn more about ekfs. I am sure this will be good reference material.
Comment by Phillip Schmidt on November 23, 2015 at 9:43am

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.

Comment by Simon D. Levy on November 23, 2015 at 12:15pm

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.

}

Comment by Simon D. Levy on November 23, 2015 at 12:16pm

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

Comment by robert bouwens on November 23, 2015 at 12:22pm

paranoid :-)

colums and rows are better as defines.

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

Comment by Simon D. Levy on November 23, 2015 at 12:30pm
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++;
*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);
}

Comment

You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2017   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service