Why are you dividing the difference between sample end and sample start by 1000? You are losing all your precision. Doing that will give you only whole seconds to work with. So when go to calculate speed, you will only use 1 second, 2 seconds, etc, but 1.234 would be truncated to 1 second. Which will give you a completely wrong result when you do the math.