adding robot position and orientation estimation