AHRS

Для получения подробного описания каждого этапа обзора ознакомьтесь с алгоритмом.

Алгоритм моделирует ускорение и изменение угла как линейные процессы.

Прогнозирование ориентации

Ориентация для текущего кадра определяется путем предварительной оценки углового изменения по сравнению с предыдущим кадром:

где

Угловое изменение преобразуется в кватернионы с использованием синтаксиса построения кватернионов:

Предыдущая оценка ориентации обновляется путем поворота на :

В ходе первой итерации оценка ориентации инициализируется с помощью ecompass.

Оценка гравитации по ориентации

Вектор гравитации интерпретируется как третий столбец кватерниона в форме матрицы вращения:

Оценка гравитации по ускорению

Вторая оценка вектора гравитации производится путем вычитания оценки затухающего линейного ускорения из предыдущей итерации из показаний акселерометра:

Оценка магнитного вектора Земли

Магнитный вектор Земли оценивается путем поворота оценки магнитного вектора из предыдущей итерации на априорную оценку ориентации в виде матрицы поворота:

Модель ошибок объединяет две разности:

Поправка магнитометра определяет погрешность в оценке магнитного вектора и обнаруживает магнитные помехи.

Ошибка магнитного возмущения

Ошибка магнитного возмущения рассчитывается путем матричного умножения коэффициента усиления Калмана, связанного с магнитным вектором, на сигнал ошибки:

Коэффициент усиления Калмана — это коэффициент усиления Калмана, рассчитанный в текущей итерации.

Обнаружение магнитных помех

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

В уравнениях Калмана используется оценка гравитации , полученная из показаний гироскопа, оценка магнитного вектора , полученная из показаний гироскопа, и измерение процесса ошибки для обновления коэффициента усиления Калмана и промежуточных ковариационных матриц. Коэффициент усиления Калмана применяется к сигналу ошибки для получения апостериорной оценки ошибки .

Модель измерений

Модель измерений отображает наблюдаемые состояния размером на , и , в истинное состояние размером на .

Модель измерений строится следующим образом:

где

Ковариация невязок

Ковариация невязок — это матрица размером на , используемая для отслеживания изменчивости измерений. Матрица ковариации невязок рассчитывается следующим образом:

где

Обновление ковариации оценки ошибки

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

Матрица ковариации оценки ошибки обновляется следующим образом:

где

Прогнозируемая ковариация оценки ошибки

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

где

Коэффициент усиления Калмана

Матрица коэффициента усиления Калмана — это матрица размером на , используемая для взвешивания невязки. В этом алгоритме невязка интерпретируется как процесс ошибки .

Матрица коэффициента усиления Калмана строится следующим образом:

где

Обновление апостериорной ошибки

Апостериорная оценка ошибки определяется путем объединения матрицы коэффициентов усиления Калмана с ошибкой в оценках вектора гравитации и вектора магнитного поля:

Если в текущей итерации обнаружены магнитные помехи, то сигнал ошибки магнитного вектора игнорируется, и апостериорная оценка ошибки вычисляется следующим образом:

Оценка ориентации

Оценка ориентации обновляется путем умножения предыдущей оценки на ошибку:

Оценка линейного ускорения

Оценка линейного ускорения обновляется путем уменьшения оценки линейного ускорения из предыдущей итерации и вычитания ошибки:

где — коэффициент затухания линейного ускорения.

Оценка смещения гироскопа

Оценка смещения гироскопа обновляется путем вычитания ошибки смещения гироскопа из значения смещения, полученного на предыдущей итерации:

Для оценки угловой скорости усредняются данные , а затем вычитается смещение гироскопа, вычисленное на предыдущей итерации:

где — коэффициент децимации.

Оценка смещения гироскопа инициализируется нулями для первой итерации.

Если в текущей итерации магнитные помехи не были обнаружены, оценка магнитного вектора обновляется с использованием апостериорной ошибки магнитного возмущения и апостериорной ориентации.

Ошибка магнитного возмущения преобразуется в навигационную систему координат:

Ошибка магнитного возмущения в навигационной системе координат вычитается из предыдущей оценки магнитного вектора, а затем интерпретируется как наклонение:

Наклонение преобразуется в ограниченную оценку вектора магнитного поля для следующей итерации: