Реализовать алгоритм бинс

На основании полученных соотношений и материала можно реализовать алгоритм БИНС по следующей схеме:
1.Задание начальных углов рысканья, крена и тангажа. Задание начальных координат и высоты (абсолютной). Начальные скорости задать нулевыми.
2.Расчет матрицы перехода C из связанной в нормальную систему координат по углам ориентации.
3.Прием показаний акселерометров в ССК ax ay az , пересчет их в НСК с помощью матрицы перехода C.
4.Расчет приращений скорости (10) на основании (5), (7), (8), (9).
5.Прибавить к текущей относительной скорости ее приращение из п.3. (реализация интегрирования (11)).
6.По новым значениям скорости обновить значения географических координат (12).
7.Прием показаний гироскопов x y z , расчет по ним относительной угловой скорости xотн yотн zотн (вращение связанной относительно НСК). Расчет приращений углов ориентации по уравнениям Эйлера-Крылова (см. лекцию 3).
8.Переход к п.2.