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.
Comments
It might have been simpler to do a simple substitution { Ns = Mobs } in Nsta to get Mobsta. Those genetic algorithms can be complicated, or do you have a library for that, too?
I ran Mobs and Nsta through a genetic algorithm and got a Mobsta! ;^)
Oh, good, you beat me to it - I was still typing the response. That will make it work no matter which of Nsta or Mobs is larger. Thanks for updating the code.
PS: Sorry, I need to adopt your tmp0 solution (keep tmp1[Nsta][Msta] and add a new variable tmp0[Nsta][Nsta], to accommodate the two different updates. I'm doing that now.
Thanks for your helpful suggestions, Dan. I have changed N to Nsta and M to Mobs everywhere, which will avoid interfering with the use of the names N and M for variables in your own code.
As for the incorrect sizing of the tmp1 variable, I believe that it was a holdover from a previous version; I have changed it now to [Nsta][Nsta].
Simon, Thanks for your EKF implementation and tutorial. They are both very helpful in understanding and getting started using Kalman filters. I do have a couple of suggestions for making improvements that you may consider:
1. Revisiting the #define N and M issue previously discussed, you are correct that it doesn't cause problems with text substitutions like "Nfoo" in your test. However, it does cause collisions with C++ template code, which by convention normally use single capital letters as template parameters, such as one like "template<typename T, int N> ...". Although simple software programs may not be using templates, more complicated ones will fail to compile (as I discovered).
2. The tmp1[N][M] temporary storage array is not the correct size for the predict uncertainty calculation in ekf_step(). The mulmat(ekfp.F, ekf.P, ekf.tmp1, n, n, n) call expects tmp1 to be NxN, but it is defined as NxM. The update uncertainty step also expects tmp1 to be NxN. This causes problems when N > M, which for example would happen if the filter state is defined as 9 values like positionX, positionY, positionZ, velocityX, velocityY, velocityZ, accelerationX, accelerationY, accelerationZ, but the filter is updated with measurements defined with just 3 values (like positionX, positionY, positionZ). I got things working by simply adding a tmp0[N][N] array for that calculation, adding it to the ekf_t pointers in tiny_ekf.c and the unpack() function, and using tmp0 instead of tmp1 in the predict uncertainty and update uncertainty steps, and just using tmp1 in the update gain step which needs a NxM array.
I hope this is clear enough to be of help.
Thank you for your kind remarks and for finding this typo, Sander. I have corrected it and updated the repository.
Hi Jonas,
Yes, I'd love to help you with that excellent use case -- much more useful than the Arduino example I posted! I have worked a lot with NMEA and even have a little C++ parser/generator package I wrote for it. (We can also use tinygps if you prefer).
Anyway, email me at simon.d.levy@gmail.com when you have some time to give more details about your project.
Hello,
first of all thank you for your good job of this work ! - very good to understand and good implementation for arduino !
I tried to convert your lib. for my usecase - fusing the GPS Data with acc and gyro with 20HZ. The actual Problem is the description of the system and also the use of the position data from nmea msg. Are you interested in working together to have a add. example for that case ? - I think a lot of people want to have that kind of example. Greetings from Germany
Thanks, Oleksandr! The reason I did not use bold letters for vectors is that I could not figure out how to do this in MathJax, the system I used for typesetting the math notation in the tutorial. I tried \matbf{x} and \bold{x}, but it looked just like the non-bold version. So I gave up trying to do it. If you know how I can do it the right way, please let me know.
-
1
-
2
-
3
-
4
of 4 Next