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.