corrected velocity function to include current position, parameter constants may still be wrong