Wie ist eigentlich die Anzahl der Gesamtimpulse Links/Rechts nach 1m? Und wie sind diese auf den ersten 10cm? Es genügt ein kleiner Unterschied beim Anfahren, und schon macht er eine kleine Kurve und fährt dann den Rest der Strecke gerade weiter.

Die Berechnungen könnte man etwas aufräumen, zB einmal am Anfang right_minus_left / AXLE_LENGTH berechnen, und nicht in jedem Schritt einzeln.
Ich hab auf meiner Homepage eine ähnliche Berechnung für Odometrie angeben, vielleicht kannst diese mal vergleichen http://members.aon.at/wstocke6/Positionsbestimmung2.pdf

LG!