Fixed error resulting in NaNs
Changed velocity calculation.