Fix the algorithm.
Refactor motion equations.