Ein sehr einfaches Algoritme das gut functioniert geht wie folgt :
De senkrechte Abweichung nimmst du als Mass für eine extra Winkel-correctur. Beispiel :
Winkelcorrectur = Seitliche Abweichung*Verstaerkings-factor. Diesen Wert muss dann noch limitiert werden, ich verwende dort max correctur zwischen +/- 25°.
Damit bekommen sie immer eine kleinere correctur bei kleinere seitliche Abweichung. Ich habe das gestern noch versucht mit meine GPS-car, und es functioniert gut. Nur soll die Verstaerkungs-factor abhangig sein von Geschwindigkeit. Bei hohere Geschwindigkeit haben sie mehr "Overshoot".

[c]
breedte=(waypoint[track_point_nr].latitude-gps.latitude)*111000;// 1 breedtegraad = 111 km, richting Noord = +
lengte=(waypoint[track_point_nr].longitude-gps.longitude)*68400; // 1 lengtegraad = 68,4 km, richting Oost = +
hoek=atan2(lengte,breedte)*1800/PI;//in deze richting moet gereden worden voor het volgende trackpoint !
//afstand + dwarsafwijking berekenen tov waypoint /ideale koers
afstand=sqrt(lengte*lengte+breedte*breedte);
offset_course=sin((waypoint[track_point_nr].heading-hoek)*PI/180)*afstand*75;//p-gain=75
offset_course=min_max((int)(offset_course),-25,25);//beperken tussen +/-25°
[/c]