GPS and Accelerometer Sensor Fusion with a Kalman Filter, a Practical Walkthrough (Golang)
Posted on January 23, 2017 by admin in Python | 23 Comments
The Up Front Video
Chances are, you’re reading this blog post while sitting on the toilet. That means I have a meager 30 seconds to 8 minutes to really grab your attention and show off what this post is all about. Therefore, peep the video first:
Please excuse the blimp icon for the actual car I’m traveling in. The actual project is sensor fusion for a blimp; I just happen to test the sensor fusion aspect by recording a trip to Taco Bell to pick up 3 crispty tacos, 2 cheesy gordita crunches, a large diet coke, and sure, I’ll try the new double stacked tacos nacho crunch.
5 star chefs as always working in the back over there. They’re the every day heroes of this world. I’m just a programmer that fell into the notorious airship game.
True Story Follows
So if you’re one of my avid followers, which I’m not sure is something that actually exists, then you might be privy to my autonomous blimp project. Well, it turns out that last weekend I was on the grind with none other than Larry Fleming AKA “Airshippimus Dirigiblamous”, and we were doing a test run of another version of code I had prepared. I told him to not worry, and that you might as well start calling me Rumpelstiltskin, because I straight spit out gold when I type on the keyboard. I had mentioned that we might as well codename this project “Fort Knox”, because that’s how much gold was now inside the code. I told him: I know I said this would work before, but now, this is definitely going to work. Like: I know we’re all born equal, and I put my pants on one leg at a time, but the only difference is that once my pants are on, I make gold. Well anyway, I came over after declaring all this, and the blimp autopilot didn’t work. In fact, it was actually worse than before.
But, the test flight was valuable from a number of intangible insights. In short, the blimp generally appeared to be attempting the right thing, but in all actions, there was pilot induced oscillation, which basically means that the blimp was overcompensating to correct a previous fault. Which basically means: the sensor updates are too slow and out of date. The autopilot, while responding appropriately, is basing its actions off of old data, and the implications of the corrected measures are not realized until seconds later, which only perpetuates the problem.
Now, we’re using reasonably cheap hardware, and meanwhile there are autopilot algorithms for quadcopters and such that use the same cheap hardware, so clearly I’m missing something. While I’m referencing velocity to calculate reactionary force, that velocity value is from the average of previous GPS data, which is clearly outdated. So, the most recent sensor I have after that is an accelerometer. However, an accelerometer will not be able to give me any usable information unless I have an initial velocity so that the integral can mean something. And, I cannot have an initial velocity because I don’t have a velocity sensor; therefore I must get the velocity from GPS. Ah! I am a genius! So somehow, I must “fuse”, if you will the inputs between GPS and accelerometer. Ah! Oh. Wait. Yeah. That’s already a solved problem. You just use a kalman filter. There’s like 50,000 white papers about it.
So, after feeling on top of the world for a few minutes after recognizing that I indeed must be on to something if the idea I have is already something smart people before me have already solved, I sought to understand and/or find sample code for what I was trying to do. All over the internets, the billboards read: “Use a kalman filter to merge GPS and accelerometer data”, but, as trivial as the internets made it out to be, there were no examples of EXACTLY what I was trying to do. There were white papers that explained concepts in the pure abstract, then there were a few blog posts that referenced said white papers and referenced said concept, and then there were a few examples on Github that were so intertwined with the hardware platform and matrix libraries that it was difficult to extract what I needed to know.
So, after a few days of reading through a few white papers, watching a few YouTube videos, and reading a few blog posts several times through, I managed to put together a Kalman Filter that works properly from sensors found exclusively on the Emlid Navio2.
Context For Following Example
It should be said that whatever information is displayed in this blog post requires upfront context. It should only serve to bridge gaps that I could not already find on the internets. Among the existing information:
What exactly a kalman filter is
Why a kalman filter works
The mathematical proofs behind a kalman filter
In abundance, there already appeared to be erudite information about what a kalman filter is, but there was hardly anything in concrete form to tie the abstract into something useful. With that said, the two posts that helped me out a ton in understanding kalman filters:
The example below also already relies on perfect inputs for sensor reads. Therefore, there’s a few pre-requisites there in order to get the example below working. One, you’re using an Emlid Navio2 to read sensor data. Then, you have a working implementation of a compass to get raw data from the accelerometer. I have an example of this code.
Since my previous post mentioned, I’ve added some updates to the compass code. As I left it, the compass could output a pitch, yaw, and roll angle, which, as it turns out, is very difficult to work with in a 3D space. Therefore, I went and learned about quaternions in order to learn how I could take my raw accelerometer data and simply turn it into an absolute acceleration vector in a three dimensional space.
For starters, accelerometer data needs to be converted to values stripped of acceleration due to gravity. To do this, you need to first:
Calibrate your accelerometer by reading its raw magnitude while at rest. This value should be equal to the force of gravity, so apply the appropriate divisor to ensure that this magnitude is indeed 1G
Strip out the acceleration due gravity. To do this:
A quaternion, which is an array of 4 values, can be converted into a rotation matrix. Hence, [x, y, z, y] can be converted to:
So now, the pre-requisites should be met. You’re using an Emlid Navio2 hat for raspberry pi (and if you’re not it’s actually fine, but you’ll need to plug in slightly different values throughout this example), and you have working compass code that outputs acceleration in absolute terms of North, East, and up. Moreover, you have working GPS reads working (from the Navio2).
The working code for this entire example can be found on GitHub. You should be able to use the individual module to create a kalman filter from an abstract position and an abstract acceleration. The only assumption is that this filter works in exactly one dimension. Therefore, if you have 2 or 3 dimensions, simply use 2 or 3 kalman filters, respectively. For simplicity’s sake, the file on Github is inclusive of the main function and some helper functions. The kalman filter then, distilled, with only inputs in terms of meters and acceleration in meters per second per second, is as follows in Golang:
Note that this code by itself has no dependencies, and this should work for all cases involving position and acceleration assuming the two are already using the same units for distance.