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 Simon! Thanks for a super awesome tutorial and nice library!
I have one question though. The hx() is a function that should map the current state vector to an estimated sensor readings (observations) vector just like the matrix C in regular KF (http://home.wlu.edu/~levys/kalman_tutorial/kalman_13.html) , right? So this implies that the output of hx(xk, vk) (which should be zk) is a vector with the length that is equal to the number of sensors (observations), no? But in https://github.com/simondlevy/TinyEKF/blob/master/examples/tinyekf_... and in the comment of https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.h the hx size is set to the number of states ( then again in https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf_struct.h the size seems to be correct in my mind ). Is this intentional? Am I missing something?
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
-
1
-
2
-
3
-
4
of 4 Next