Инерциальная система навигации

Необходим алгоритм и программный код, для обработки данных от акселерометра и гироскопа, для построения инерциальной системы навигации. На входе данный от датчиков и стартовая позиция (широта-долгота), на выходе набор координат (широта-долгота) за период обработки данных от датчиков.