new_pose2D initialization bug fix