Updated Pose estimation to include refinement of the rotation matrix