Solved minor problem in the calculation of the speed vectors.