Ориентация от показаний акселерометра, гироскопа и магнитометра
Панель инструментов навигации Панель инструментов/Мультисенсорные фильтры позиционирования/Навигационные фильтры
Панель инструментов слияния и отслеживания датчиков/мультисенсорные позиционирующие/навигационные фильтры
AHRS Блок Simulink ® предохраняет акселерометр, магнитометр и данные датчика гироскопа для оценки ориентации устройства.
Примечание.Следующий алгоритм применяется только к опорному кадру NED.
Блок AHRS использует девятиосную структуру фильтра Калмана, описанную в [1]. Алгоритм пытается отследить ошибки ориентации, смещения гироскопа, линейного ускорения и магнитного возмущения для вывода конечной ориентации и угловой скорости. Вместо непосредственного отслеживания ориентации косвенный фильтр Калмана моделирует процесс ошибок x с рекурсивным обновлением:
− 1dk − 1] + wk
где xk - вектор 12 на 1, состоящий из:
startk -- вектор ошибки ориентации 3 на 1, в градусах, в момент времени k
bk -- вектор смещения нулевой угловой скорости гироскопа 3 на 1, в град/с, в момент времени k
ak -- вектор ошибки ускорения 3 на 1, измеренный в кадре датчика, в g, в момент времени k
dk -- вектор ошибки магнитного возмущения 3 на 1, измеренный в кадре датчика, в мкТ, в момент времени k
и где wk является вектором аддитивного шума 12 на 1, а Fk является моделью перехода состояния.
Поскольку xk определяется как процесс ошибки, априорная оценка всегда равна нулю, и поэтому модель перехода состояния Fk равна нулю. Это понимание приводит к следующему уменьшению стандартных уравнений Калмана:
Стандартные уравнения Калмана:
Sk) − 1xk + = xk − + Kkykpk
Уравнения Калмана, используемые в этом алгоритме:
+ = KkykPk + = Pk − − KkHkPk −
где:
xk − -- прогнозируемая (априорная) оценка состояния; процесс ошибки
Pk − -- предсказанная (априорная) оценка ковариации
yk -- инновации
Sk - инновационная ковариация
Kk -- Kalman gain
xk + -- обновленная (a posteriori) оценка состояния
Pk + -- обновленная (a posteriori) оценка ковариации
k представляет итерацию, верхний индекс + представляет апостериорную оценку, а верхний индекс − представляет априорную оценку.
На рисунке и последующих шагах описывается итерация на основе одного кадра в алгоритме.
Перед первой итерацией accelReadings, gyroReadings, и magReadings входы секционированы в DecimationFactor-на-3 кадра. Для каждого блока алгоритм использует самые текущие показания акселерометра и магнитометра, соответствующие фрагменту показаний гироскопа.
Пройдите по алгоритму для пояснения каждого этапа подробного обзора.
Алгоритм моделирует ускорение и угловое изменение как линейные процессы.
Ориентацию для текущего кадра прогнозируют путем первой оценки углового изменения по сравнению с предыдущим кадром:
gyroOffset1 × 3) fs
где N - коэффициент прореживания, заданный коэффициентом прореживания, а fs - частота дискретизации.
Угловое изменение преобразуется в кватернионы с помощью rotvec
quaternion строительный синтаксис:
rotvec')
Предыдущая оценка ориентации обновляется путем ее поворота на ΔQ:
∏n=1NΔQn)
Во время первой итерации оценка ориентации, q −, инициализируется ecompass.
Вектор гравитации интерпретируется как третий столбец кватерниона, q −, в матричной форме вращения:
(:, 3)) T
Вторую оценку вектора гравитации производят путем вычитания оценки затухшего линейного ускорения предыдущей итерации из показаний акселерометра:
Магнитный вектор Земли оценивается вращением оценки магнитного вектора из предыдущей итерации на априорную оценку ориентации, в матричной форме вращения:
mT)) T
Модель ошибок объединяет два отличия:
Разность между оценкой гравитации по показаниям акселерометра и оценкой гравитации по показаниям гироскопа: gAccel
Разность между оценкой магнитного вектора по показаниям гироскопа и оценкой магнитного вектора по магнитометру: magReadings
Магнитометр правильно оценивает погрешность в оценке магнитного вектора и обнаруживает магнитные помехи.
Вычисляют погрешность магнитного возмущения матричным умножением коэффициента усиления Калмана, связанного с магнитным вектором, на сигнал ошибки:
(z1 × 6) T) T
Коэффициент Калмана K - коэффициент Калмана, вычисленный в текущей итерации.
Магнитное заклинивание определяют путем проверки, что мощность обнаруженного магнитного возмущения меньше или равна четырехкратной мощности ожидаемой напряженности магнитного поля:
2
ExpectedDepectedStartStrength является свойством ahrsfilter.
Уравнения Калмана используют оценку гравитации, полученную из показаний гироскопа, g, оценку магнитного вектора, полученную из показаний гироскопа, mGyro, и наблюдение процесса ошибки, z, для обновления матриц усиления Калмана и промежуточной ковариации. Коэффициент усиления Калмана применяется к сигналу ошибки z для вывода апостериорной оценки ошибки x +.
Модель наблюдения отображает наблюдаемые состояния 1 на 3, g и mGyro, в истинное состояние 6 на 12, Н.
Модель наблюдения строится следующим образом:
где gx, gy и gz - x-, y- и z-элементы вектора гравитации, оцененные по априорной ориентации соответственно. mx, my и mz - x-, y- и z-элементы магнитного вектора, оцененные по априорной ориентации соответственно. δ - константа, определяемая свойствами скорости выборки и коэффициента прореживания: Decimation factor/Sample rate.
Инновационная ковариация представляет собой матрицу 6 на 6, используемую для отслеживания изменчивости измерений. Инновационную ковариационную матрицу рассчитывают как:
H6x12) T
где
H - матрица модели наблюдения
P − - предсказанная (априорная) оценка ковариации модели наблюдения, вычисленная в предыдущей итерации
R - ковариация шума модели наблюдения, вычисленная как:
где
Гиросвязь Шум)
и
)
Ковариация оценки ошибок представляет собой матрицу 12 на 12, используемую для отслеживания изменчивости состояния.
Ковариационная матрица оценки ошибок обновляется следующим образом:
где K - коэффициент усиления Калмана, H - матрица измерения, а P − - ковариация оценки ошибок, вычисленная во время предыдущей итерации.
Ковариация оценки ошибок представляет собой матрицу 12 на 12, используемую для отслеживания изменчивости состояния. Априорная оценка ошибки ковариации, P −, устанавливается на ковариацию шума процесса, Q, определенную во время предыдущей итерации. Q вычисляется как функция ковариации оценки апостериорной ошибки P +. При вычислении Q предполагается, что члены взаимной корреляции ничтожны по сравнению с членами автокорреляции и установлены в нуль:
+ γ000000000000σ2P + (131) + γ000000000000σ2P + (144) + γ]
где
P + - обновленная (a posteriori) ковариация оценки ошибки
t - Коэффициент прореживания, деленный на частоту выборки.
β - Шум дрейфа гироскопа.
start-- Шум гироскопа.
start-- Коэффициент затухания линейного ускорения.
δ -- Шум линейного ускорения.
λ - Коэффициент затухания магнитного возмущения.
γ - Шум магнитного возмущения.
Матрица усиления Калмана - это матрица 12 на 6, используемая для взвешивания нововведения. В этом алгоритме нововведение интерпретируется как процесс ошибки, z.
Матрица усиления Калмана построена как:
((S6 × 6) T) − 1
где
P − -- предсказанная ковариация ошибки
H -- модель наблюдения
S -- инновационная ковариация
Заднюю оценку погрешности определяют объединением матрицы усиления Калмана с погрешностью в оценках вектора гравитации и магнитного вектора:
(z1 × 6) T
Если в итерации тока обнаружено магнитное заклинивание, сигнал ошибки магнитного вектора игнорируется, и оценка задней ошибки вычисляется как:
3) (zg) T
Оценка ориентации обновляется путем умножения предыдущей оценки на ошибку:
(λ +)
Оценка линейного ускорения обновляется путем затухания оценки линейного ускорения из предыдущей итерации и вычитания ошибки:
где
start-- Коэффициент затухания линейного ускорения
Оценка смещения гироскопа обновлена, вычтя ошибку смещения гироскопа из смещения гироскопа от предыдущего повторения:
1 − a +
[1] Слияние датчиков с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs
[2] Роетенберг, Д., Х.Ж. Луинге, К.Т.М. Батен и П.Х. Велтинк. «Компенсация магнитных возмущений улучшает инерционное и магнитное восприятие ориентации сегмента человеческого тела». Сделки IEEE по нейронным системам и инженерии реабилитации. Том 13. Выпуск 3, 2005, стр. 395-405.
ahrsfilter | ecompass | gpsSensor | imufilter | imuSensor | quaternion
