added the adapted function using velocities instead of numerical differentiation.