![]() |
Главная Случайная страница Контакты | Мы поможем в написании вашей работы! | |
|
(входная информация: - начальное значение кватерниона ориентации из задачи начальной выставки;
- от блока гироскопов после предварительной обработки;
- значения вектора угловой скорости географического трехгранника
- из навигационной задачи;
- оценки погрешностей ориентации из фильтровой задачи)
Искомый кватернион , определяющий ориентацию измерительного блока
относительно осей
, ищем в виде
или
,
где - приращение кватернионов на шаге
;
для дискретного алгоритма получим
, (4.3)
Вычисление приращений кватернионов может быть осуществлено следующим образом.
- формирование приращения вектора Эйлера
Приведем рекуррентный алгоритм задачи ориентации при использовании дискретного алгоритма 4-го порядка [2] для вычисления вектора Эйлера Т по квазикоординатам
и их разностям:
, (4.4)
где ;
- формирование кватерниона
(4.5)
- формирование приращения вектора Эйлера по значениям вектора угловой скорости
из задачи преобразования сигналов акселерометров на навигационные оси и первого интегрирования (в соответствии с разложением (4.4)):
;
;
- формирование кватерниона
(4.6)
- формирование кватерниона
; (4.7)
при
;
- коррекция нормы кватерниона
где – норма кватерниона.
- формирование матрицы направляющих косинусов
; (4.8)
- формированиеуглов
;
где - матрица привязки осей
ИБ БИИМ к осям
объекта.
Из элемента находим выражение для угла тангажа (килевой качки)
:
; (4.9)
Элементы и
позволяют определить угол крена (бортовой качки)
:
. (4.10)
Поскольку модули углов и
меньше
, то приведенные выше выражения однозначно определяют значения углов килевой и бортовой качек.
Для нахождения соотношения, однозначно определяющего курс , воспользуемся элементами матриц
и функцией Matlab atan2:
при ,
. (4.11)
Дата публикования: 2014-10-19; Прочитано: 673 | Нарушение авторского права страницы | Мы поможем в написании вашей работы!