Moves the velocity correction completely into C