AHRS

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

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

Описание

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

Порты

Входной параметр

развернуть все

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

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

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

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

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

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

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

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

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

Вывод

развернуть все

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

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

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

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

Скорость вращения со смещением гироскопа, удаленным в системе координат корпуса датчика в rad/s, возвращенном как M-by-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-by-4 массив действительных скаляров. Каждая строка массива представляет четыре компонента quaternion.
'Rotation matrix' – Выход является 3 3 M матрицей вращения.

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

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

`Simulate using` — Тип симуляции, чтобы запуститься
`Interpreted Execution` (значение по умолчанию) | `Code Generation`

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

Шум измерения

`^{Accelerometer noise ((m/s2)2)}` — Отклонение акселерометра сигнализирует о шуме (^(m/s2) ²)
0.00019247 (значение по умолчанию) | положительный действительный скаляр

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

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

`^{Gyroscope noise ((rad/s)2)}` — Отклонение гироскопа сигнализирует о шуме ((rad/s) ²)
`9.1385e-5` (значение по умолчанию) | положительный действительный скаляр

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

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

`^{Magnetometer noise (μT2)}` — Отклонение магнитометра сигнализирует о шуме ^(μT2)
0.1 (значение по умолчанию) | положительный действительный скаляр

Отклонение магнитометра сигнализирует о шуме в ^μT2 в виде положительного действительного скаляра.

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

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

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

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

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

`^{Linear acceleration noise ((m/s2)2)}` — Отклонение линейного ускоряющего шума ^(m/s2) ²
0.0096236 (значение по умолчанию) | положительный действительный скаляр

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

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

`^{Magnetic disturbance noise (μT2)}` — Отклонение магнитного шума воздействия ^(μT2)
0.5 (значение по умолчанию) | действительная конечная положительная скалярная величина

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

Типы данных: 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)` — Сила магнитного поля (μT)
50 (значение по умолчанию) | действительная положительная скалярная величина

Сила магнитного поля в μT в виде действительной положительной скалярной величины. Сила магнитного поля является оценкой силы магнитного поля Земли в текущем местоположении.

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

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

Fusion датчика IMU с Simulink

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

Открыть скрипт

Алгоритмы

развернуть все

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

Блок AHRS использует структуру Фильтра Калмана с девятью осями, описанную в [1]. Алгоритм пытается отследить ошибки в ориентации, смещении гироскопа, линейном ускорении и магнитном воздействии, чтобы вывести итоговую ориентацию и скорость вращения. Вместо того, чтобы отследить ориентацию непосредственно, косвенный Фильтр Калмана моделирует ошибочный процесс, 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 измеряется в системе координат датчика, в µT, во время 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 ⁺ – обновленный (по опыту) оценивает ковариацию

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

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

Перед первой итерацией, accelReadings, gyroReadings, и magReadings входные параметры разделяются на блоки в DecimationFactor- 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 синтаксис конструкции:

$Δ 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.

Оцените силу тяжести от ориентации

Вектор силы тяжести интерпретирован как третий столбец кватерниона, 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 является свойством 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 - элементы магнитного вектора, оцененного от априорной ориентации, соответственно. κ является константой, определенной свойствами фактора Частоты дискретизации и Децимации: κ = 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 ⁺ – является обновленным (по опыту) ошибочная оценочная ковариация
κ – Фактор децимации разделен на частоту дискретизации.
β – Шум дрейфа гироскопа.
η – Шум гироскопа.
ν – Линейный ускоряющий фактор затухания.
ξ – Линейный ускоряющий шум.
σ – Магнитный фактор затухания воздействия.
γ – Магнитный шум воздействия.

Кальман Гэйн

Матрица усиления Кальмана 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] Fusion датчика с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., Х.Дж. Линдж, C.T.M. Baten и П.Х. Велтинк. "Компенсация Магнитных Воздействий Улучшает Инерционное и Магнитное Обнаружение Ориентации Сегмента Человеческого тела". Транзакции IEEE в Нейронных Системах и Разработке Реабилитации. Издание 13. Выпуск 3, 2005, стр 395-405.

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

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

Темы

Определите ориентацию Используя инерционные датчики

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

AHRS

Описание

Порты

Входной параметр

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

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

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

Вывод

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

Angular Velocity — Скорость вращения в системе координат корпуса датчика (rad/s) 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

Шум измерения

Accelerometer noise ((m/s2)2) — Отклонение акселерометра сигнализирует о шуме ((m/s2) 2)0.00019247 (значение по умолчанию) | положительный действительный скаляр

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

Magnetometer noise (μT2) — Отклонение магнитометра сигнализирует о шуме (μT2)0.1 (значение по умолчанию) | положительный действительный скаляр

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

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

Linear acceleration noise ((m/s2)2) — Отклонение линейного ускоряющего шума (m/s2) 20.0096236 (значение по умолчанию) | положительный действительный скаляр

Magnetic disturbance noise (μT2) — Отклонение магнитного шума воздействия (μT2)0.5 (значение по умолчанию) | действительная конечная положительная скалярная величина

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

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

Magnetic field strength (μT) — Сила магнитного поля (μT)50 (значение по умолчанию) | действительная положительная скалярная величина

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

Fusion датчика IMU с Simulink

Алгоритмы

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

Модель

Предскажите ориентацию

Оцените силу тяжести от ориентации

Оцените силу тяжести от ускорения

Оцените магнитный вектор земли

Ошибочная модель

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

Ошибка воздействия магнитометра

Магнитное создающее затор обнаружение

Кальман Экуэйшнс

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

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

Обновите ошибочную оценочную ковариацию

Предскажите ошибочную оценочную ковариацию

Кальман Гэйн

Обновите по опыту ошибку

Правильный

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

Оцените линейное ускорение

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

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

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

Ссылки

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

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

Смотрите также

Темы

Введенный в R2020a

Документация Sensor Fusion and Tracking Toolbox

Поддержка

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

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

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

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

`Angular Velocity` — Скорость вращения в системе координат корпуса датчика (rad/s)
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`

`^{Accelerometer noise ((m/s2)2)}` — Отклонение акселерометра сигнализирует о шуме (^(m/s2) ²)
0.00019247 (значение по умолчанию) | положительный действительный скаляр

`^{Gyroscope noise ((rad/s)2)}` — Отклонение гироскопа сигнализирует о шуме ((rad/s) ²)
`9.1385e-5` (значение по умолчанию) | положительный действительный скаляр

`^{Magnetometer noise (μT2)}` — Отклонение магнитометра сигнализирует о шуме ^(μT2)
0.1 (значение по умолчанию) | положительный действительный скаляр

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

`^{Linear acceleration noise ((m/s2)2)}` — Отклонение линейного ускоряющего шума ^(m/s2) ²
0.0096236 (значение по умолчанию) | положительный действительный скаляр

`^{Magnetic disturbance noise (μT2)}` — Отклонение магнитного шума воздействия ^(μT2)
0.5 (значение по умолчанию) | действительная конечная положительная скалярная величина

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

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

`Magnetic field strength (μT)` — Сила магнитного поля (μT)
50 (значение по умолчанию) | действительная положительная скалярная величина

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