Fix bug in velocity update