Keep heading error within [-180,180]

Can you check if this change works?