Much cleaner/more efficient way to compute path distances.
Compute path error only, rather than velocity error.