Tracking mittels Kondensationsalgorithmus

Hallo zusammen,

ich habe gerade das Tutorial zum Tracking mittels Condensationalgorithm 
(eine Art des Partikelfilters) herumtergeladen und implementiert. (gibts 
hier als Beispielcode: 
http://www.robots.ox.ac.uk/~misard/condensation.html)

Leider habe ich (wie man auf der Grafik sieht) die Vermutung(!), dass 
das Tracking näher an den gemessenen Werten liegt als an den "echten" 
Daten. Eigentlich sollte dieser Algorithmus doch Rückschlüsse auf die 
echten Messwerte geben?! Das macht er aber nicht wirklich. Ist der 
Algorithmus so schlecht oder liegt es an der Implementierung? Vielleicht 
habt ihr tolle Erfahrungswerte für mich oder könnt mal über die 
Implementierung drüber schauen.

Im Anhang ist noch der Code von mir. Ich habe den bereits etwas 
verändert, damit ich überhaupt Durchblick bekomme.

Vielen Dank!