Habe zufällig etwas Erfahrung (Diplomarbeit) mit deinem Problem.
Radgebundene Odometrie Messungen bei inhomogenen und unebenen Untergrund ist stark fehleranfällig. Deswegen war meine Lösung damals eine Optische Odometrie zu verwenden.
Meine Erfahrungen damit sind sehr gut!
Du kannst du kannst die nicht triviale Aufgabe von der Komplexität einschränken in dem du mit aktiven Markern arbeiten so wie bei der Wii (Spielekonsole) und ein fertiges Kameramodul verwendest (CMUcam3).
Also mein Vorschlag ist mach es Optisch!
Gruß
Der Melchi