Bug fix for vel_target calculation