ungeachtet deines speziellen problems, hier ein Link zu einem "standardwerk" der Roboter positionsbestimmung.
http://www-personal.umich.edu/~johan...d/pos96rep.pdf
mfg robert