Zustandsschätzung

Hallo,

mein Team und ich haben ein Studendenprojekt, und zwar ist unsere Aufgabe ein Auto, das automatisch seitlich einparken soll, also so eine Art Einparkhilfe.
Ich bin dabei hauptsächlich für die Software-Seite zuständig.
Jetzt hänge ich gerade an der Stelle, wo die Pose des Autos (ist ein umgebautes Modellauto) anhand Lenkwinkel und zurückgelegter Strecke geschätzt werden soll. Die Pose besteht aus 2 Koordinaten (x,y) und der Orientierung. Zum einen sind die Steuersignale an das Auto und zum anderem die Messwerte vom Auto (die Odometrie) verfügbar. Das Bewegungsmodell (Errechnen der neuen Pose durch Lenkwinkel und Strecke) an sich ist kein Problem, nur hänge ich gerade an der Filterung des ganzen, d.h. wie ich von den Steuersignalen und der (fehlerbehafteten) Odometrie zum geschätzten Zustand komme. Wir haben vor, ein Kalmanfilter einzusetzen, wissen aber leider noch nicht genau wie.
Hat jemand einen Tipp oder einen Vorschlag?
Wäre für jede noch so kleine Anregung dankbar!

Grüße,
McMarco