Я рискну предположить, ваша проблема в "дребезге" показаний GPS, отличающихся на несколько метров. Стоите вы в пробке 10 минут, например, а по показаниям GPS совершаете 5- 10-метровые "прыжки" около одной точки.
Присмотритесь к данным. Если всё так, как я говорю, прикрутите фильтрацию, которая будет адаптировать полученные данные к скорости вашего перемещения.
Начните с отсеивания данных, если расстояние между ними, поделенное на количество секунд, будет менее N метров (быстрее чем пешеход).
Добавляйте к каждому отрезку пути вектор перемещения. Это позволит отсекать отрезки незначительной длины, противоречащие последнему значимому вектору.
Да много всяких ухищрений можно придумать. Всё зависит от вашей ситуации.