Got rid of velocity calculation, now gives a not so shaky trajectory