made some small changes to getEstimateRobotPose()