Velocity

The current implementation to calcualate the velocity is not appropriate. We need some sort of smoothing and most likely float is a problem ...