Psitionserkennung

Hallo, wir arbeit im Rahmen eines Jahresprojektes an einem Autonomen Fahrzeug. Die Sensorik besteht aus Ultraschall und evtl. einer Bilderkennung.
Die Vorgaben sind: Es soll Pakete von einem Raum in einen anderen transportieren können. Das Leitsystem läuft unter JAVA
Meine Idee ist es jetzt eine Karte in der Software zu hinterlegen. Und anhand der Sensordaten die Schnittlinien mit den Kartendaten zu vergleichen.
Daraus müßte man eigendlich mit Hilfe zusätzliche OdometrieDaten die Position recht genau bestimmen können. (warscheinliche(laut Odometrie) und erechnete vergleichen.
Hat dazu jemand eine Meinung oder vieleicht schon Erfahrung.
Gibt es dazu neuere Papiere als das "where I am" ...Hilft dabei nicht wirklich weiter.

MfG Blackheart