Algoritme ist siemlich einfach : Mit den GPS daten wird die gewunschte "Heading" jedes mal neu berechnet. An Hand von das Kompass wird dan die Ist Heading und Soll Heading verglichen, und den Steur servo wird nachgeregelt.
es gib für arduino eine gps bibliothek 'TinyGPS' die macht alles. bei zwei gps punkten kann man die richtung und entfenung ausrechnen.