Задача пространственной ориентации объекта

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

Искомый кватернион , определяющий ориентацию измерительного блока относительно осей , ищем в виде

или

,

где - приращение кватернионов на шаге ;

для дискретного алгоритма получим

, (4.3)

Вычисление приращений кватернионов может быть осуществлено следующим образом.

- формирование приращения вектора Эйлера

Приведем рекуррентный алгоритм задачи ориентации при использовании дискретного алгоритма 4-го порядка [2] для вычисления вектора Эйлера ^Т по квазикоординатам и их разностям:

, (4.4)

где ;

- формирование кватерниона

(4.5)

- формирование приращения вектора Эйлера по значениям вектора угловой скорости из задачи преобразования сигналов акселерометров на навигационные оси и первого интегрирования (в соответствии с разложением (4.4)):

;

;

- формирование кватерниона

(4.6)

- формирование кватерниона

; (4.7)

при ;

- коррекция нормы кватерниона

где – норма кватерниона.

- формирование матрицы направляющих косинусов

; (4.8)

- формированиеуглов

;

где - матрица привязки осей ИБ БИИМ к осям объекта.

Из элемента находим выражение для угла тангажа (килевой качки) :

; (4.9)

Элементы и позволяют определить угол крена (бортовой качки) :

. (4.10)

Поскольку модули углов и меньше , то приведенные выше выражения однозначно определяют значения углов килевой и бортовой качек.

Для нахождения соотношения, однозначно определяющего курс , воспользуемся элементами матриц и функцией Matlab atan2:

при , . (4.11)