Из рисунка 4—5 видно, что
Угол ориентации объекта можно рассматривать как совокупность трех углов поворота, где угол поворота вокруг оси z записывается как угол курса, угол поворота вокруг оси y записывается как угол тангажа y, и угол поворота вокруг оси x называется углом крена 0.
После выпуска матрицы преобразования ориентации параметры матрицы формул (15) и (16) могут быть сопоставлены один к одному после последующего процесса фильтрации для определения трехосного угла ориентации. Конкретный процесс заключается в следующем.
Сопоставьте параметры уравнений (15) и (16) один за другим, возьмем три элемента в третьей строке уравнения (16), приравняем их g1, g2 и g3 соответственно и возьмем первый элемент уравнения ( 16). Первые два элемента столбца – это g5 и g4 соответственно.
Его значения следующие:
Затем я подставил кватернион в последний момент в формулу угла ориентации (19), чтобы найти эти три угла ориентации. Формула для окончательного получения угла ориентации выглядит следующим образом:
4.2. Решение параметров кватернионов.
Затем преобразуйте его в матричную форму и получим уравнение (24).
4.3. Процесс фильтрации EKF. Расширенный фильтр Калмана (EKF) – это прикладное расширение фильтра Калмана, которое использует данные наблюдений системы для оптимальной оценки состояния нелинейной системы и фильтрации помех, таких как шум и дрейф. Этот алгоритм использует локальную линеаризацию для решения нелинейных задач путем вывода нелинейных уравнений.
Затем мы инициализируем фильтр и устанавливаем матрицу шума процесса Q, матрицу ошибок измерения R, матрицу состояний X и ковариационную матрицу P.
Поскольку параметры кватернионов в матрице преобразования отношения являются каноническими кватернионами и должны рассчитываться как нормализованные значения, полученные значения необходимо рассчитывать как нормализованные значения.
Берем значение акселерометра и нормализуем его.
Затем нормируем данные ускорения и магнитометра и вычисляем опорный магнитный вектор, векторная матрица которого обозначается как B.
Итак, возьмем частную производную матрицы H по матрице X. Среди них значения четырех вышеуказанных параметров могут быть получены по формуле решения теоретического ускорения (36) и формуле решения теоретического вектора магнитного поля (37), а значения акселерометра и магнитометра можно подставить