Updated code for pose estimation

Fixed boundaries for the angles: theta is between pi and -pi