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
The specs indicate 72 MHz, overclockable to 96 MHz. So I'm gonna say 72.
The Arduino Mega is 16 MHz, so you wouldn't expect the factor of ten speedup I saw on the Teensy based solely on the clock. It's gotta be the 32 bit vs. 8 bit difference you mentioned in your earlier post, that's giving the additional speedup.
And since my Nucleo board just arrived from Mouser, I can code up an even more realistic test for STM32 users. At $10 a pop, I think they're under-pricing this baby to compete with Teensy :^) Much win!
Nice job Simon.
What frequency was the Teensy running?
This is so awesome! I'm working on some robotics projects, this will be so useful! The ability to have a Kalman Filter as a library is amazing. Thank you for your work!
@Phillip: Done! There's now an Examples/Benchmark that allows you to specify N. For simplicity, I use M = N, so this is a pure smoothing filter (no sensor fusion). The Arduino Uno memory craps out after N=5, but the Mega was able to do 32 updates per second, and Teensy3.2 does 324 (and in general is always 10X faster than ArduinoMega). Results for other N shown below.
@Ben: I appreciate the tips! I'm a long way from putting TinyEKF on an actual flight controller, but will keep in mind what you've suggested.
@Simon: I'm by no means a Kalman expert, but like you I've spent a decent amount of time trying to understand how they work (and have found so few examples). I have noticed, though, that there seems to be many different philosophies on how to use a Kalman for sensor fusion. One interesting approach I've seen with a standard IMU (accels and gyros) is to use the angular rates from the gyro as control inputs to a Kalman that's filtering orientation angles. The gyros are assumed to be accurate in the short-term and are integrated directly (multiplied by dt) in the B matrix. This allows you to "fuse" the data without having to store both angles and angle rates, making your matrices smaller and faster to compute. I'll try to post an example with some data this week.
Awesome, I'm looking forward to seeing your results.
If I could be so greedy, I would like to "pre-request" you post the 6dof test source code on you git account after you have your results.
@Ben: I considered adding control-signal code, but did not have a working example against which to compare my results. If you have any such example -- ideally, a spreadsheet of input sensor and control values and known correct outputs -- I would be grateful to see it.
@Ben, @Phillip: Excellent idea on timing tests. I intended TinyEKF mainly for 32-bit boards (STM32, Teensy3) running at higher rates (72MHz, 96MHz), but as always it is best to run some benchmarks. I am doing that this evening (test on Arduino Uno and Teensy 3.2) and plan on post something tonight.
As you (Ben) note, scaling up to 6dof would likely cause noticeable slowdown: the Cholesky decomposition (matrix inversion) stage is O(N^3) or something nearly as bad. But because the matrices are pretty sparse (typically only diagonal and a few other values are nonzero), it may be possible to add a little bit of code to deal with that behind the scenes. As ever: test first, then optimize!
@simon,
yes, i overlooked this ...
usual error when i'm too tired.
Wow, learn something new every day. I would probably still avoid it out of preference, but clearly it's not a problem.
I would be super surprised if this would run at a reasonable rate on an 8bit/16Mhz processor when doing 6dof motion fusion, but that's one of the things I would like to test when I have some time. Given my recent history of guessing at expected results, I wouldn't hold it against anyone who takes this prediction with a grain of salt. ;-)
Really nice tutorial! One of the best Kalman Filter implementations in C that I've seen.
One suggestion might be to add in the optional control input vector and matrix (commonly U and B). Along the same lines, splitting the predict and update steps into two separate functions would let you predict (with control inputs) and update at two different rates, depending on the application.
Have you run any tests with an Arduino to see what kind of peak update rates you can get? Doubles and floats can really bog down those smaller architectures, but it'd be cool to see.
Overall, nice job.
-
1
-
2
-
3
-
4
of 4 Next