Solution for INS and GPS integration

Question

I have a GPS module and an IMU (gyro, accelerometer and magnetometer) and I need to build an autonomous navigation system for a quadcopter. It must know its position at any time so that it can track a predefined path. I know that, in order to improve precision, I need to merge both sensors data through a Kalman Filter (or any other technique for that matter, the thing is that the Kalman Filter is way more common according to my research). The problem is that I am seriously stuck and I know this might be something very simple but I don't seem to find a solution or at least the answer for some of the most basic questions. As a start, I know how to get the position from the accelerometer readings. I have some filters that help eliminate noise and minimize the integration errors. I also have the GPS readings in latitude and longitude. The first question is, during sensor fusion, how can I make both measurements compatible? The latitude and longitude from the GPS won't simply mix with the displacement given by the accelerometer, so what is the starting point for all of this? Should I calculate the displacement from the GPS readings or should I assume a starting latitude and longitude and then update it with the accelerometer prior to applying the filter?

I have once developed a simple Kalman Filter in which I could plug the new reading values to obtain the next estimate position of a two wheeled car. Now I have two sources of inputs. How should I merge those two together? Will the filter have two inputs or should I find a function that will somehow get the best estimate (average, maybe?) from the accelerometer and GPS? I am really lost here.

Do you guys have any examples of code that I could use to learn? It is really easy to find articles full of boxes with arrows pointing the direction in which data must flow and some really long equations that start to get confusing very soon such as those presented on this article: http://isas.uka.de/Material/Samba-Papierkorb/vorl2014_15/SI/Terejanu_tutorialUKF.pdf (I have no problems with equations, seriously) but I have never seen a real life example of such implementation.

Any help on this topic would be deeply appreciated.

Thank you very much.

Answer 1

I was in exactly the same boat with my master's thesis; wave-based imaging (sonar and radar) seemed so common that all the papers I read assumed you knew the fundamental concept and they were going to show an advanced technique, or they set out deriving everything from Maxwell's equations! That said, I hope this helps.

Essentially, you have two measurements: one that should be really accurate in the short term, but prone to integration error/drift in the long run (the IMU accelerometer readings); and you have one that should be extremely accurate in the long term but is very noisy in the short term (GPS).

I would recommend you look at a complimentary filter, which is an extremely easy way to combine two measurements. Basically:

$$ y_{\mbox{est}} = g * y_1 + (1-g) y_2 \\ $$

That's it. $g$ is a value between 0 and 1 that controls how much weight or "trust" you put in a measurement. $y_1$ would be the more trustworthy input, and $y_2$ would be the less trustworthy input. In your case, maybe try g=0.9 with the IMU as y1 and the GPS as y2. Remember, in the short term - sample to sample - IMU integration should be more accurate.

Now you can of course filter individual measurements before, or output after, or both, but try it first with just the raw measurements.

If you're really into pain and desperately want to use the Kalman filter because it's what all the other academics are doing, this paper (PDF) may be of help as it provides a discrete implementation of a fusing Kalman filter. I wouldn't do it, though. If you're doing something tangible it's engineering, and as engineers we want quality, reliable results, whatever the means! The complimentary filter is literally the one line I wrote, it's super easy to debug, minimal processor requirement, it's intuitive, it works, and did I mention easy to debug?

Try looking around online for "complimentary vs Kalman filter"; I'm not the only advocate.