GPSログの処理

GPSログの処理で、積算距離を求める。
ヒュベニの距離計算式
これで2点間の距離を求め、距離を積算できました。
で、問題は次のようなログ。

実際は10分ほど休憩しているのですが、
誤差のため、ポイントが飛び回っています(泣。
■次で解決できる？
A, B点があった場合に、その距離をd(A, B)とすると、
　d(A, C) > d(A, B) + d(B, C)
の場合にはB点を省く。
許容する最大距離と時間を制限すれば、何とかなりそうな？