AHRS

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

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

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

Описание

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

Порты

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

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

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

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

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

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

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

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

Вывод

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

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

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

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

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

Параметры

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

Основной

Система координат навигации в виде NED (Северо-восток вниз) или ENU (Восточный Север).

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

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

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

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

  Columns 1 through 6

   0.000006092348396                   0                   0                   0                   0                   0
                   0   0.000006092348396                   0                   0                   0                   0
                   0                   0   0.000006092348396                   0                   0                   0
                   0                   0                   0   0.000076154354947                   0                   0
                   0                   0                   0                   0   0.000076154354947                   0
                   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

  Columns 7 through 12

                   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   0.009623610000000                   0                   0                   0                   0
                   0                   0   0.009623610000000                   0                   0                   0
                   0                   0                   0   0.600000000000000                   0                   0
                   0                   0                   0                   0   0.600000000000000                   0
                   0                   0                   0                   0                   0   0.600000000000000

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

Выведите формат ориентации в виде 'quaternion' или 'Rotation matrix':

  • 'quaternion' – Выходом является M-by-4 массив действительных скаляров. Каждая строка массива представляет четыре компонента quaternion.

  • 'Rotation matrix' – Выход является 3 3 M матрицей вращения.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Алгоритмы

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

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

Блок AHRS использует структуру Фильтра Калмана с девятью осями, описанную в [1]. Алгоритм пытается отследить ошибки в ориентации, смещении гироскопа, линейном ускорении и магнитном воздействии, чтобы вывести итоговую ориентацию и скорость вращения. Вместо того, чтобы отследить ориентацию непосредственно, косвенный Фильтр Калмана моделирует ошибочный процесс, x, с рекурсивным обновлением:

xk=[θkbkakdk]=Fk[θk1bk1ak1dk1]+wk

где 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, является нулем. Это понимание приводит к следующему сокращению стандарта уравнения Кальмана:

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

xk=Fkxk1+Pk=FkPk1+FkT+Qkyk=zkHkxkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=xk+KkykPk+=PkKkHkPk

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

xk=0Pk=Qkyk=zkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=KkykPk+=PkKkHkPk

где:

  • xk – предсказанный (априорно) утверждают оценку; ошибочный процесс

  • Pk – предсказанный (априорно) оценивают ковариацию

  • yk – инновации

  • Sk – инновационная ковариация

  • Kk – Усиление Кальмана

  • xk + – обновленный (по опыту) утверждает оценку

  • Pk + – обновленный (по опыту) оценивает ковариацию

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

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

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

Ссылки

[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™.

Введенный в R2020a

Для просмотра документации необходимо авторизоваться на сайте