Нужно запрограммировать навигационный алгоритм

Нужно запрограммировать навигационный алгоритм. Для начала можно предположить, что БИНС неподвижна относительно земли, а ее оси совпадают с осями географической системы координат (одна в горизонте смотрит на восток, другая в горизонте смотрит на север, третья - по местной вертикали вверх), и смоделировать показания акселерометров (первый и второй - нулевые, третий = g) и ДУС (первый = 0, второй = U cos(широты Москвы), третий = U sin(широты Москвы)).