Some fixes for the yaw
Added direction for the stable move