I’m planning on using an extended Kalman filter (EKF) to fuse a bunch of sensors on my robot to give me a better position estimate than I would get just from just GPS alone. The common online wisdom (see DIYdrones, for example) is that Kalman filters are too expensive for an Arduino, but that’s not true. In fact, I’ve been pretty surprised by the speed of numerics on these chips.

I’ve set up a five-state filter and implemented it in straight C with no optimizations at all. I don’t take advantage of symmetric matrices or the fact that a lot of the matrix elements in the filter are zero. I just do a textbook implementation of an EKF calling functions to implement every matrix math operation (e.g. matrixAdd(a,b,c) where c=a+b).

The bottom line is that the filter runs at 54 Hz on an Arduino Mega. Speed isn’t the issue at all, but memory is. The matrices take up about 1.5K and my first attempt at timing the code on an Arduino Nano wasn’t successful. I got a lot of bizarre problems until I remembered that the Mega has 8K, but the Nano only has 2K of RAM.

My GPS runs at 4 Hz and the rest of the sensors can run at just about any rate so a multiple of 4 Hz makes sense. Maybe 20 Hz for the filter. That leaves about half of the horsepower to run a simple control law.