А на сколько подруливать, и как часто, и как часто проверять где точка? На самом деле все алгоритмы давно созданы и описаны. Литературы всякой полно. Я чесно говоря не понимаю зачем городить огород, если полазив в Гугле можно найти и алгоритмы, и тексты программ с фильтрами Калмана, и многое другое.