AHRS

Ориентация от показаний акселерометра, гироскопа и магнитометра

Библиотека:
Navigation Toolbox Toolbox/Мультисенсорное позиционирование/Навигационные фильтры
Sensor Fusion and Tracking Toolbox/Мультисенсорные позиционные/навигационные фильтры

Описание

The AHRS Simulink^® блок использует акселерометр, магнитометр и данные гироскопического датчика для оценки ориентации устройства.

Порты

Вход

расширить все

`Accel` - Показания акселерометра в системе координат тела датчика (м/с²)
N -by-3 матрица действительного скаляра

Показания акселерометра в системе координат тела датчика в м/с², заданный как N-на-3 матрица действительных скаляров. N - количество выборок и три столбца Accel представляют измерения [x y z], соответственно.

Типы данных: single | double

`Gyro` - Показания гироскопа в системе координат тела датчика (рад/с)
N -by-3 матрица действительного скаляра

Показания гироскопа в системе координат тела датчика в рад/с, заданные как матрица N-на-3 действительных скаляров. N - количество выборок и три столбца Gyro представляют измерения [x y z], соответственно.

Типы данных: single | double

`Mag` - Показания магнитометра в системе координат тела датчика (мкТ)
N -by-3 матрица действительного скаляра

Показания магнитометра в системе координат тела датчика в мкТ, заданные как N-на-3 матрица действительных скаляров. N - количество выборок и три столбца magReadings представляют измерения [x y z], соответственно.

Типы данных: single | double

Выход

расширить все

`Orientation` - Ориентация каркаса кузова датчика относительно навигационной системы координат
M -by-4 массива вращения скаляра | 3 на 3-by- M -элемента

Ориентация датчика каркаса кузова относительно навигационной системы координат, возвращается как M-на-4 массив скаляров или 3х3-бай- M массив матриц вращения. Каждая строка массива N -by-4 принята как четыре элементов массиваquaternion (Sensor Fusion and Tracking Toolbox). Количество входных выборок, N и параметр Decimation Factor определяют выходной M размера.

Типы данных: single | double

`Angular Velocity` - Скорость вращения в системе координат тела датчика (рад/с)
M массив -by-3 из действительного скаляра (по умолчанию

)

Скорость вращения с смещением гироскопа, удаленным в системе координат тела датчика в рад/с, возвращенная как M-на-3 массив действительных скаляров. Количество входных выборок, N и параметр Decimation Factor определяют выходной M размера.

Типы данных: single | double

Параметры

расширить все

Главный

`Reference frame` - Навигационная система координат
`NED` (по умолчанию) | `ENU`

Навигационная система координат, заданная как NED (Северо-Восток-Даун) или ENU (Восток-Север-Вверх).

`Decimation factor` - Коэффициент десятикратного уменьшения
`1` (по умолчанию) | положительное целое число

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

Количество строк входов - Accel, Gyro и Mag - должно быть кратным коэффициенту десятикратного уменьшения.

Типы данных: single | double

`Initial process noise` - Начальный технологический шум
`ahrsfilter.defaultProcessNoise` (по умолчанию) | матрицу 12 на 12 действительного скаляра

Начальный 0.000006092348396 0 0 0 0 0 0 0 0 0 0 Columns 7 through 12 0 0 0 0 0 0 0.009623610000000 0 0 0 0 шум процесса, заданный как матрица 12 на 12 действительных скаляров. Значение по умолчанию, ahrsfilter.defaultProcessNoise, является диагональной матрицей 12 на 12 как:

  Columns 1 through 6 0                   0                   0                   0                   0 0   0.000006092348396                   0                   0                   0                   0 0   0.000006092348396                   0                   0                   0 0                   0   0.000076154354947                   0                   0 0                   0                   0   0.000076154354947                   0 0                   0                   0                   0   0.000076154354947 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0                   0                   0                   0                   0 0   0.009623610000000                   0                   0                   0                   0 0   0.009623610000000                   0                   0                   0 0                   0   0.600000000000000                   0                   0 0                   0                   0   0.600000000000000                   0 0                   0                   0                   0   0.600000000000000

Типы данных: single | double

`Orientation format` - Формат выхода ориентации
`'quaternion'` (по умолчанию) | `'Rotation matrix'`

Выход выходной ориентации, заданный как 'quaternion' или 'Rotation matrix':

'quaternion' - Выход является M -на-4 массивом действительных скаляров. Каждая строка массива представляет четыре компонента quaternion (Sensor Fusion and Tracking Toolbox).
'Rotation matrix' -- Выход является матрицей вращения M 3 на 3 байта.

Выходной размер M зависит от входного размера N и параметра Decimation Factor.

Типы данных: char | string

`Simulate using` - Тип выполняемой симуляции
`Interpreted Execution` (по умолчанию) | `Code Generation`

Interpreted execution - Симулируйте модель с помощью MATLAB^® интерпретатор. Эта опция сокращает время запуска. В Interpreted execution mode, можно отлаживать исходный код блока.
Code generation - Симулируйте модель с помощью сгенерированного кода C. При первом запуске симуляции Simulink генерирует код С для блока. Код С повторно используется для последующих симуляций, пока модель не меняется. Эта опция требует дополнительного времени запуска.

Измерительный шум

`Шум акселерометра ((м/с²)²)` - Отклонение шума сигнала акселерометра ((м/с²)²)
`0.00019247` (по умолчанию) | положительный действительный скаляр

Отклонение шума сигнала акселерометра в (м/с²)², заданный как положительный действительный скаляр.

Типы данных: single | double

`Шум гироскопа ((рад/с)²)` - Отклонение шума сигнала гироскопа ((рад/с)²)
`9.1385e-5` (по умолчанию) | положительный действительный скаляр

Отклонение шума сигнала гироскопа в (рад/с)², заданный как положительный действительный скаляр.

Типы данных: single | double

`Шум магнитометра (мкТ²)` - Отклонение шума сигнала магнитометра (мкТ²)
`0.1` (по умолчанию) | положительный действительный скаляр

Отклонение шума сигнала магнитометра в мкТ², заданный как положительный действительный скаляр.

Типы данных: single | double

`Gyroscope drift noise (rad/s)` - Отклонение смещения гироскопа ((рад/с)²)
`3.0462e-13` (по умолчанию) | положительный действительный скаляр

Отклонение дрейфа смещения гироскопа в (рад/с)², заданный как положительный действительный скаляр.

Типы данных: single | double

Экологический шум

`Линейный шум ускорения ((м/с²)²)` - Отклонение линейного шума ускорения (м/с²)²
`0.0096236` (по умолчанию) | положительный действительный скаляр

Отклонение линейного шума ускорения в (м/с²)², заданный как положительный действительный скаляр. Линейное ускорение моделируется как процесс белого шума с фильтрацией методом lowpass.

Типы данных: single | double

`Магнитный возмущающий шум (мкТ²)` - Отклонение магнитного нарушения порядка шума (мкТ²)
`0.5` (по умолчанию) | действительная конечная положительная скалярная величина

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

Типы данных: single | double

`Linear acceleration decay factor` - Коэффициент затухания для дрейфа линейного ускорения
`0.5` (по умолчанию) | скаляром в области значений [0,1)

Коэффициент распада для дрейфа линейного ускорения, заданный как скаляр в области значений [0,1). Если линейное ускорение изменяется быстро, установите этот параметр на более низкое значение. Если линейное ускорение изменяется медленно, установите этот параметр на более высокое значение. Дрейф линейного ускорения моделируется как процесс белого шума с фильтрацией методом lowpass.

Типы данных: single | double

`Magnetic disturbance decay factor` - Коэффициент распада для магнитного нарушения порядка
`0.5` (по умолчанию) | положительная скалярная величина в области значений [0,1]

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

Типы данных: single | double

`Magnetic field strength (μT)` - Напряженность магнитного поля (мкТ)
`50` (по умолчанию) | реальная положительная скалярная величина

Напряженность магнитного поля в мкТ, заданная как действительная положительная скалярная величина. Напряженность магнитного поля является оценкой напряженности магнитного поля Земли в текущем месте.

Типы данных: single | double

Примеры моделей

Комплексирование БИНС с Simulink

Сгенерируйте и предохраните данные БИНС с помощью Simulink ®. Можно точно смоделировать поведение акселерометра, гироскопа и магнитометра и сплавить их выходы для вычисления ориентации.

Алгоритмы

расширить все

Примечание: Следующий алгоритм применяется только к опорной системе координат NED.

Блок AHRS использует девятиосевую структуру фильтра Калмана, описанную в [1] (Sensor Fusion and Tracking Toolbox). Алгоритм пытается отследить ошибки в ориентации, смещении гироскопа, линейном ускорении и магнитном нарушении порядка, чтобы вывести конечную ориентацию и скорость вращения. Вместо непосредственного отслеживания ориентации косвенный фильтр Калмана моделирует процесс ошибки, x, с рекурсивным обновлением:

$x_{k} = [\begin{matrix} θ_{k} \\ b_{k} \\ a_{k} \\ d_{k} \end{matrix}] = F_{k} [\begin{matrix} θ_{k - 1} \\ b_{k - 1} \\ a_{k - 1} \\ d_{k - 1} \end{matrix}] + w_{k}$

где x k является вектором 12 на 1, состоящим из:

_θk -- вектор ошибки ориентации 3 на 1, в степенях, в момент времени k
_bk -- гироскоп 3 на 1 нуле вектор смещения угловой скорости, в град/с, в момент времени k
_ak -- вектор ошибки ускорения 3 на 1, измеренный в системе координат датчика, в g, в момент времени k
_dk -- 3 на 1 вектор ошибки магнитного нарушения порядка, измеренный в системе координат датчика, в мкТ, в момент времени k

и где w k является аддитивным вектором шума 12 на 1, и _F k является моделью перехода состояния.

Поскольку x k задан как процесс ошибки, априорная оценка всегда равна нулю, и, следовательно, модель перехода состояния, _F k, равна нулю. Это понимание приводит к следующему уменьшению стандартных уравнений Калмана:

Стандартные уравнения Калмана:

$\begin{array}{l} x_{k}^{-} = F_{k} x_{k - 1}^{+} \\ P_{k}^{-} = F_{k} P_{k - 1}^{+} F_{k}^{T} + Q_{k} \\ y_{k} = z_{k} - H_{k} x_{k}^{-} \\ S_{k} = R_{k} + H_{k} P_{k}^{-} H_{k}^{T} \\ K_{k} = P_{k}^{-} H_{k}^{T} {(S_{k})}^{- 1} \\ x_{k}^{+} = x_{k}^{-} + K_{k} y_{k} \\ P_{k}^{+} = P_{k}^{-} - K_{k} H_{k} P_{k}^{-} \end{array}$

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

$\begin{array}{l} x_{k}^{-} = 0 \\ P_{k}^{-} = Q_{k} \\ y_{k} = z_{k} \\ S_{k} = R_{k} + H_{k} P_{k}^{-} H_{k}^{T} \\ K_{k} = P_{k}^{-} H_{k}^{T} {(S_{k})}^{- 1} \\ x_{k}^{+} = K_{k} y_{k} \\ P_{k}^{+} = P_{k}^{-} - K_{k} H_{k} P_{k}^{-} \end{array}$

где:

_xk⁻ - прогнозируемая (априори) оценка состояния; процесс ошибки
_Pk⁻ - предсказанная (априорная) оценка ковариации
_yk -- инновации
_Sk -- инновационная ковариация
_Kk -- коэффициент усиления Калмана
_xk⁺ - обновленная (апостериорная) оценка состояния
_Pk⁺ - обновленная (a posteriori) оценка ковариации

k представляет итерацию, надстрочный индекс ⁺ представляет апостериорную оценку и надстрочный индекс ⁻ представляет априорную оценку.

Графические и последующие шаги описывают итерацию на основе одного кадра через алгоритм.

Перед первой итерацией accelReadings, gyroReadings, и magReadings входы шунтируются в DecimationFactor-by-3 системы координат. Для каждого фрагмента алгоритм использует самые актуальные показания акселерометра и магнитометра, соответствующие фрагменту показаний гироскопа.

Подробный обзор

Пройдите по алгоритму для объяснения каждого этапа подробного обзора.

Модель

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

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

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

$Δ φ_{N \times 3} = \frac{(g y r o R e a d i n g s_{N \times 3} - g y r o O f f s e t_{1 \times 3})}{f s}$

где N - коэффициент десятикратного уменьшения, заданный коэффициентом десятикратного уменьшения, и fs является частотой дискретизации.

Угловое изменение преобразуется в кватернионы с помощью rotvec quaternion (Sensor Fusion and Tracking Toolbox) синтаксис конструкции:

$Δ Q_{N \times 1} = quaternion (Δ φ_{N \times 3},' r o t v e c')$

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

$q_{1 \times 1}^{-} = (q_{1 \times 1}^{+}) (\prod_{n = 1}^{N} Δ Q_{n})$

Во время первой итерации оценка ориентации, q⁻, инициализируется ecompass (Sensor Fusion and Tracking Toolbox).

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

Вектор гравитации интерпретируется как третий столбец кватерниона, q⁻, в матричном виде вращения:

$g_{1 \times 3} = {(r P r i o r (:, 3))}^{T}$

Оценка свободного падения от ускорения

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

$g A c c e l_{1 \times 3} = a c c e l R e a d i n g s_{1 \times 3} - l i n A c c e l p r i o r_{1 \times 3}$

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

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

$m G y r o_{1 \times 3} = {((r P r i o r) (m^{T}))}^{T}$

Модель ошибки

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

Различие между оценкой силы тяжести из показаний акселерометра и оценкой силы тяжести из показаний гироскопа: $z_{g} = g - g A c c e l$
Различие между оценкой магнитного вектора из показаний гироскопа и оценкой магнитного вектора из магнитометра: $z_{m} = m G y r o - m a g R e a d i n g s$

Магнитометр правильный

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

Нарушение порядка магнитометра

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

$m E r r o r_{3 \times 1} = {((K {(10:12, :)}_{3 \times 6}) {(z_{1 \times 6})}^{T})}^{T}$

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

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

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

$t f = {\begin{matrix} true \\ ложный \end{matrix} \begin{matrix} если \\ еще \end{matrix} \begin{matrix} {\sum | m E r r o r |}^{2} > (4) {(ExpectedMagneticFieldStrength)}^{2} \end{matrix}$

ExpectedMagneticFieldStrength (Sensor Fusion and Tracking Toolbox) является свойством ahrsfilter.

Уравнения Калмана

Уравнения Калмана используют оценку тяжести, полученную из показаний гироскопа, g, оценку магнитного вектора, полученную из показаний гироскопа, mGyro и наблюдения процесса ошибки, z, чтобы обновить усиление Калмана и промежуточные ковариационные матрицы. Коэффициент усиления Калмана применяется к сигналу ошибки, z, для вывода апостериорной оценки ошибки x⁺.

Модель наблюдения

Модель наблюдения преобразует наблюдаемые состояния 1 на 3, g и mGyro, в истинное состояние 6 на 12, H.

Модель наблюдения построена как:

$H_{3 \times 9} = [\begin{matrix} 0 & g_{z} & - g_{y} & 0 & - κ g_{z} & κ g_{y} & 1 & 0 & 0 & 0 & 0 & 0 \\ - g_{z} & 0 & g_{x} & κ g_{z} & 0 & - κ g_{x} & 0 & 1 & 0 & 0 & 0 & 0 \\ g_{y} & - g_{x} & 0 & - κ g_{y} & κ g_{x} & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & m_{z} & - m_{y} & 0 & - κ m_{z} & - κ m_{y} & 0 & 0 & 0 & - 1 & 0 & 0 \\ - m_{z} & 0 & m_{x} & κ m_{z} & 0 & - κ m_{x} & 0 & 0 & 0 & 0 & - 1 & 0 \\ m_{y} & - m_{x} & 0 & - κ m_{y} & κ m_{x} & 0 & 0 & 0 & 0 & 0 & 0 & - 1 \end{matrix}]$

где g x, _g y и g z являются x -, y - и z - элементами вектора тяжести, оцененными из априорной ориентации, соответственно. m x, m _y и m z являются x -, y - и z - элементами магнитного вектора, оцененными из априорной ориентации, соответственно. κ является константой, определяемой свойствами Sample rate и Decimation factor: κ = Decimation factor/ Sample rate.

Инновационная ковариация

Инновационная ковариация является матрицей 6 на 6, используемой для отслеживания изменчивости измерений. Инновационная ковариационная матрица вычисляется как:

$S_{6 x 6} = R_{6 x 6} + (H_{6 x 12}) (P_{_{12 x 12}}^{-}) {(H_{6 x 12})}^{T}$

где

H - матрица модели наблюдения
P⁻ - предсказанная (априорная) оценка ковариации модели наблюдения, вычисленная в предыдущей итерации
R - ковариация шума модели наблюдения, рассчитанная как:

$R_{6 \times 6} = [\begin{matrix} a c c e l_{noise} & 0 & 0 & 0 & 0 & 0 \\ 0 & a c c e l_{noise} & 0 & 0 & 0 & 0 \\ 0 & 0 & a c c e l_{noise} & 0 & 0 & 0 \\ 0 & 0 & 0 & m a g_{noise} & 0 & 0 \\ 0 & 0 & 0 & 0 & m a g_{noise} & 0 \\ 0 & 0 & 0 & 0 & 0 & m a g_{noise} \end{matrix}]$
где

$a c c e l_{noise} = AccelerometerNoise + LinearAccelerationNoise + κ^{2} (GyroscopeDriftNoise + GyroscopeNoise)$
и

$m a g_{noise} = MagnetometerNoise + MagneticDisturbanceNoise + κ^{2} (GyroscopeDriftNoise + GyroscopeNoise)$

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

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

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

$P_{_{12 \times 12}}^{+} = P_{_{12 \times 12}}^{-} - (K_{12 \times 6}) (H_{6 \times 12}) (P_{_{12 \times 12}}^{-})$

где K - коэффициент усиления Калмана, H - матрица измерений и P⁻ - ковариация оценки ошибки, вычисленная во время предыдущей итерации.

Предсказание оценки ошибки ковариации

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

$Q = [\begin{matrix} P^{+} (1) + κ^{2} P^{+} (40) + β + η & 0 & 0 & - κ (P^{+} (40) + β) & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & P^{+} (14) + κ^{2} P^{+} (53) + β + η & 0 & 0 & - κ (P^{+} (53) + β) & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & P^{+} (27) + κ^{2} P^{+} (66) + β + η & 0 & 0 & - κ (P^{+} (66) + β) & 0 & 0 & 0 & 0 & 0 & 0 \\ - κ (P^{+} (40) + β) & 0 & 0 & P^{+} (40) + β & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & - κ (P^{+} (53) + β) & 0 & 0 & P^{+} (53) + β & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & - κ (P^{+} (66) + β) & 0 & 0 & P^{+} (66) + β & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (79) + ξ & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (92) + ξ & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (105) + ξ & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & σ^{2} P^{+} (118) + γ & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & σ^{2} P^{+} (131) + γ & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & σ^{2} P^{+} (144) + γ \end{matrix}]$

где

P⁺ - обновленная (a posteriori) оценка ошибки ковариация
κ -- Коэффициент десятикратного уменьшения, разделенный на частоту дискретизации.
β -- Шум дрейфа гироскопа.
η -- Гироскоп шум.
ν - Коэффициент распада линейного ускорения.
ξ -- Линейный шум ускорения.
σ -- Коэффициент распада магнитных нарушений порядка.
γ -- Магнитное нарушение порядка шум.

Калманское усиление

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

Матрица усиления Калмана построена как:

$K_{12 \times 6} = (P_{_{12 \times 12}}^{-}) {(H_{6 \times 12})}^{T} {({(S_{6 \times 6})}^{T})}^{- 1}$

где

P⁻ -- предсказанная ковариация ошибок
H -- модель наблюдения
S -- инновационная ковариация

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

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

$x_{12 \times 1} = (K_{12 \times 6}) {(z_{1 \times 6})}^{T}$

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

$x_{9 \times 1} = (K (1:9, 1:3) {(z_{g})}^{T}$

Правильный

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

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

$q^{+} = (q^{-}) (θ^{+})$

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

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

$l i n A c c e l P r i o r = (l i n A c c e l P r i o r_{k - 1}) ν - b^{+}$

где

ν -- Коэффициент распада линейного ускорения

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

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

$g y r o O f f s e t = g y r o O f f s e t_{k - 1} - a^{+}$

Вычисление скорости вращения

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

$a n g u l a r V e l o c i t y_{1 \times 3} = \frac{\sum g y r o R e a d i n g s_{N \times 3}}{N} - g y r o O f f s e t_{1 \times 3}$

где N - коэффициент десятикратного уменьшения, заданный DecimationFactor свойство.

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

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

Если магнитное глушение не было обнаружено в текущей итерации, оценка магнитного вектора, m, обновляется с помощью a posteriori ошибки магнитного нарушения порядка и a posteriori ориентации.

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

$m E r r o r N E D_{1 \times 3} = {({(r P o s t_{3 \times 3})}^{T} {(m E r r o r_{1 \times 3})}^{T})}^{T}$

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

$Μ = m - m E r r o r N E D$

$i n c l i n a t i o n = atan2 (Μ (3), Μ (1))$

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

$\begin{array}{l} m (1) = (ExpectedMagneticFieldStrength) (\cos (i n c l i n a t i o n)) \\ m (2) = 0 \\ m (3) = (ExpectedMagneticFieldStrength) (\sin (i n c l i n a t i o n)) \end{array}$

Ссылки

[1] Комплексирование датчиков с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., H.J. Luinge, C.T.M. Бэтен и П.Х. Велтинк. Компенсация магнитных нарушений порядка улучшает инерционное и магнитное измерение ориентации сегмента тела человека. Транзакции IEEE по нейронным системам и реабилитационной инженерии. Том 13. Выпуск 3, 2005, с. 395-405.

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ Simulink ®

См. также

ahrsfilter | ecompass | gpsSensor | imufilter | imuSensor

Введенный в R2020a

Документация

AHRS

Описание

Порты

Вход

Accel - Показания акселерометра в системе координат тела датчика (м/с2) N -by-3 матрица действительного скаляра

Gyro - Показания гироскопа в системе координат тела датчика (рад/с) N -by-3 матрица действительного скаляра

Mag - Показания магнитометра в системе координат тела датчика (мкТ) N -by-3 матрица действительного скаляра

Выход

Orientation - Ориентация каркаса кузова датчика относительно навигационной системы координат M -by-4 массива вращения скаляра | 3 на 3-by- M -элемента

Angular Velocity - Скорость вращения в системе координат тела датчика (рад/с) M массив -by-3 из действительного скаляра (по умолчанию

Параметры

Reference frame - Навигационная система координат NED (по умолчанию) | ENU

Decimation factor - Коэффициент десятикратного уменьшения 1 (по умолчанию) | положительное целое число

Initial process noise - Начальный технологический шум ahrsfilter.defaultProcessNoise (по умолчанию) | матрицу 12 на 12 действительного скаляра

Orientation format - Формат выхода ориентации 'quaternion' (по умолчанию) | 'Rotation matrix'

Simulate using - Тип выполняемой симуляции Interpreted Execution (по умолчанию) | Code Generation

Шум акселерометра ((м/с2)2) - Отклонение шума сигнала акселерометра ((м/с2)2) 0.00019247 (по умолчанию) | положительный действительный скаляр

Шум гироскопа ((рад/с)2) - Отклонение шума сигнала гироскопа ((рад/с)2) 9.1385e-5 (по умолчанию) | положительный действительный скаляр

Шум магнитометра (мкТ2) - Отклонение шума сигнала магнитометра (мкТ2) 0.1 (по умолчанию) | положительный действительный скаляр

Gyroscope drift noise (rad/s) - Отклонение смещения гироскопа ((рад/с)2) 3.0462e-13 (по умолчанию) | положительный действительный скаляр

Линейный шум ускорения ((м/с2)2) - Отклонение линейного шума ускорения (м/с2)2 0.0096236 (по умолчанию) | положительный действительный скаляр

Магнитный возмущающий шум (мкТ2) - Отклонение магнитного нарушения порядка шума (мкТ2) 0.5 (по умолчанию) | действительная конечная положительная скалярная величина

Linear acceleration decay factor - Коэффициент затухания для дрейфа линейного ускорения 0.5 (по умолчанию) | скаляром в области значений [0,1)

Magnetic disturbance decay factor - Коэффициент распада для магнитного нарушения порядка 0.5 (по умолчанию) | положительная скалярная величина в области значений [0,1]

Magnetic field strength (μT) - Напряженность магнитного поля (мкТ) 50 (по умолчанию) | реальная положительная скалярная величина

Примеры моделей

Комплексирование БИНС с Simulink

Алгоритмы

Подробный обзор

Модель

Модель ошибки

Магнитометр правильный

Уравнения Калмана

Правильный

Вычисление скорости вращения

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

Ссылки

Расширенные возможности

Генерация кода C/C + + Сгенерируйте код C и C++ с помощью Coder™ Simulink ®

См. также

Документация по Navigation Toolbox

Поддержка

`Accel` - Показания акселерометра в системе координат тела датчика (м/с²)
N -by-3 матрица действительного скаляра

`Gyro` - Показания гироскопа в системе координат тела датчика (рад/с)
N -by-3 матрица действительного скаляра

`Mag` - Показания магнитометра в системе координат тела датчика (мкТ)
N -by-3 матрица действительного скаляра

`Orientation` - Ориентация каркаса кузова датчика относительно навигационной системы координат
M -by-4 массива вращения скаляра | 3 на 3-by- M -элемента

`Angular Velocity` - Скорость вращения в системе координат тела датчика (рад/с)
M массив -by-3 из действительного скаляра (по умолчанию

`Reference frame` - Навигационная система координат
`NED` (по умолчанию) | `ENU`

`Decimation factor` - Коэффициент десятикратного уменьшения
`1` (по умолчанию) | положительное целое число

`Initial process noise` - Начальный технологический шум
`ahrsfilter.defaultProcessNoise` (по умолчанию) | матрицу 12 на 12 действительного скаляра

`Orientation format` - Формат выхода ориентации
`'quaternion'` (по умолчанию) | `'Rotation matrix'`

`Simulate using` - Тип выполняемой симуляции
`Interpreted Execution` (по умолчанию) | `Code Generation`

`Шум акселерометра ((м/с²)²)` - Отклонение шума сигнала акселерометра ((м/с²)²)
`0.00019247` (по умолчанию) | положительный действительный скаляр

`Шум гироскопа ((рад/с)²)` - Отклонение шума сигнала гироскопа ((рад/с)²)
`9.1385e-5` (по умолчанию) | положительный действительный скаляр

`Шум магнитометра (мкТ²)` - Отклонение шума сигнала магнитометра (мкТ²)
`0.1` (по умолчанию) | положительный действительный скаляр

`Gyroscope drift noise (rad/s)` - Отклонение смещения гироскопа ((рад/с)²)
`3.0462e-13` (по умолчанию) | положительный действительный скаляр

`Линейный шум ускорения ((м/с²)²)` - Отклонение линейного шума ускорения (м/с²)²
`0.0096236` (по умолчанию) | положительный действительный скаляр

`Linear acceleration decay factor` - Коэффициент затухания для дрейфа линейного ускорения
`0.5` (по умолчанию) | скаляром в области значений [0,1)

`Magnetic disturbance decay factor` - Коэффициент распада для магнитного нарушения порядка
`0.5` (по умолчанию) | положительная скалярная величина в области значений [0,1]

`Magnetic field strength (μT)` - Напряженность магнитного поля (мкТ)
`50` (по умолчанию) | реальная положительная скалярная величина

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ Simulink ®