Fixed yaw + or - 180 issue in position_controller.py