ahrsfilter

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

Описание

ahrsfilter Система object™ плавит акселерометр, магнитометр и данные о датчике гироскопа, чтобы оценить ориентацию устройства.

Оценить ориентацию устройства:

  1. Создайте ahrsfilter объект и набор его свойства.

  2. Вызовите объект с аргументами, как будто это была функция.

Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты? MATLAB.

Создание

Описание

пример

FUSE = ahrsfilter возвращает косвенный Системный объект Фильтра Калмана, FUSE, для сплава датчика акселерометра, гироскопа и данных о магнитометре, чтобы оценить ориентацию устройства и скорость вращения. Фильтр использует вектор состояния с 12 элементами, чтобы отследить ошибку оценки для ориентации, смещения гироскопа, линейного ускорения и магнитного воздействия.

FUSE = ahrsfilter('ReferenceFrame',RF) возвращает ahrsfilter Системный объект, который плавит акселерометр, гироскоп и данные о магнитометре, чтобы оценить ориентацию устройства относительно системы координат RF. Задайте RF как 'NED' (Северо-восток вниз) или 'ENU' (Восточный Север). Значением по умолчанию является 'NED'.

пример

FUSE = ahrsfilter(___,Name,Value) наборы каждое свойство Name к заданному Value. Незаданные свойства имеют значения по умолчанию.

Свойства

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

Если в противном случае не обозначено, свойства являются ненастраиваемыми, что означает, что вы не можете изменить их значения после вызова объекта. Объекты блокируют, когда вы вызываете их и release функция разблокировала их.

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

Для получения дополнительной информации об изменении значений свойств смотрите Разработку системы в MATLAB Используя Системные объекты (MATLAB).

Введите частоту дискретизации данных о датчике в Гц в виде положительной скалярной величины.

Настраиваемый: нет

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

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

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

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

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

Настраиваемый: да

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

Ковариационная матрица для шума процесса в виде 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

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

Настраиваемый: да

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

Выведите формат ориентации в виде 'quaternion' или 'Rotation matrix'. Размер выхода зависит от входного размера, N и выходного формата ориентации:

  • 'quaternion' – Выходом является N-by-1 quaternion.

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

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

Использование

Описание

пример

[orientation,angularVelocity] = FUSE(accelReadings,gyroReadings,magReadings) акселерометр предохранителей, гироскоп и данные о магнитометре, чтобы вычислить ориентацию и измерения скорости вращения. Алгоритм принимает, что устройство является стационарным перед первым вызовом.

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

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

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

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

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

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

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

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

Выходные аргументы

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

Ориентация, которая может вращать количества от локальной системы координат навигации до системы координат тела, возвратилась как кватернионы или массив. Размер и тип orientation зависит от того, установлено ли свойство OrientationFormat в 'quaternion' или 'Rotation matrix':

  • 'quaternion' – выходом является M-by-1 вектор кватернионов с тем же базовым типом данных как входные параметры

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

Количество входных выборок, N и свойства DecimationFactor определяет M.

Можно использовать orientation в rotateframe функция, чтобы вращать количества от локальной системы навигации до системы координат корпуса датчика.

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

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

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

Функции объекта

Чтобы использовать объектную функцию, задайте Системный объект как первый входной параметр. Например, чтобы выпустить системные ресурсы Системного объекта под названием obj, используйте этот синтаксис:

release(obj)

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

stepЗапустите алгоритм Системного объекта
releaseВысвободите средства и позвольте изменения в значениях свойств Системного объекта и введите характеристики
resetСбросьте внутренние состояния Системного объекта

Примеры

свернуть все

Загрузите rpy_9axis файл, который содержит записанный акселерометр, гироскоп и данные о датчике магнитометра из устройства, колеблющегося в подаче (вокруг оси Y), затем отклоняется от курса (вокруг оси z), и затем прокручивается (вокруг оси X). Файл также содержит частоту дискретизации записи.

load 'rpy_9axis' sensorData Fs
accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;
magnetometerReadings = sensorData.MagneticField;

Создайте ahrsfilter Система object™ с SampleRate установите на частоту дискретизации данных о датчике. Задайте фактор децимации два, чтобы уменьшать вычислительную стоимость алгоритма.

decim = 2;
fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);

Передайте показания акселерометра, показания гироскопа и показания магнитометра к ahrsfilter объект, fuse, выводить оценку ориентации корпуса датчика в зависимости от времени. По умолчанию ориентация выводится как вектор кватернионов.

q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);

Ориентация задана угловым смещением, требуемым вращать систему координаты вышестоящего элемента к системе координаты нижестоящего элемента. Постройте ориентацию в Углах Эйлера в градусах в зависимости от времени.

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

time = (0:decim:size(accelerometerReadings,1)-1)/Fs;

plot(time,eulerd(q,'ZYX','frame'))
title('Orientation Estimate')
legend('z-axis', 'y-axis', 'x-axis')
ylabel('Rotation (degrees)')

В этом примере показано, как производительность ahrsfilter На систему object™ воздействует магнитный затор.

Загрузите StationaryIMUReadings, который содержит акселерометр, магнитометр и показания гироскопа от стационарного IMU.

load 'StationaryIMUReadings.mat' accelReadings magReadings gyroReadings SampleRate

numSamples = size(accelReadings,1);

ahrsfilter сила магнитного поля использования, чтобы стабилизировать ее ориентацию против принятого постоянного магнитного поля Земли. Однако существует много естественных и искусственных объектов, которые выводят магнитные поля и могут перепутать алгоритм. Чтобы составлять присутствие переходных магнитных полей, можно установить MagneticDisturbanceNoise свойство на ahrsfilter объект.

Создайте ahrsfilter объект с набором фактора децимации к 2 и примечание значение по умолчанию ожидал силу магнитного поля.

decim = 2;
FUSE = ahrsfilter('SampleRate',SampleRate,'DecimationFactor',decim);

Плавьте показания IMU с помощью фильтра и заголовка ссылочной системы (AHRS) отношения, и затем визуализируйте ориентацию корпуса датчика в зависимости от времени. Ориентация колеблется вначале и стабилизировалась приблизительно после 60 секунд.

orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');
time = (0:decim:(numSamples-1))'/SampleRate;

figure(1)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data')

Подражайте магнитному затору путем добавления переходного, сильного магнитного поля в магнитное поле, зарегистрированное в magReadings. Визуализируйте затор магнитного поля.

jamStrength = [10,5,2];
startStop = (50*SampleRate):(150*SampleRate);
jam = zeros(size(magReadings));
jam(startStop,:) = jamStrength.*ones(numel(startStop),3);

magReadings = magReadings + jam;

figure(2)
plot(time,magReadings(1:decim:end,:))
xlabel('Time (s)')
ylabel('Magnetic Field Strength (\mu T)')
title('Simulated Magnetic Field with Jamming')
legend('z-axis','y-axis','x-axis')

Запустите симуляцию снова с помощью magReadings с магнитным затором. Постройте результаты и отметьте уменьшенную производительность по оценке ориентации.

reset(FUSE)
orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(3)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data with Magnetic Disturbance and Default Properties')

Магнитный затор был неправильно истолкован фильтром AHRS, и ориентация корпуса датчика была неправильно оценена. Можно компенсировать затор путем увеличения MagneticDisturbanceNoise свойство. Увеличение MagneticDisturbanceNoise свойство увеличивает принятую шумовую область значений для магнитного воздействия, и целый сигнал магнитометра взвешивается меньше в базовом алгоритме сплава ahrsfilter.

Установите MagneticDisturbanceNoise к 200 и запустите симуляцию снова.

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

reset(FUSE)
FUSE.MagneticDisturbanceNoise = 20;

orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(4)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data with Magnetic Disturbance and Modified Properties')

Этот пример использует ahrsfilter Система object™, чтобы объединить данные IMU с 9 осями от корпуса датчика, который встряхивается. Постройте расстояние кватерниона между объектом и его положением отдыха финала, чтобы визуализировать производительность и как быстро фильтр сходится к правильной позиции отдыха. Затем настройки параметров ahrsfilter так, чтобы фильтр сходился более быстро к позиции отдыха основной истины.

Загрузите IMUReadingsShaken в вашу текущую рабочую область. Эти данные были зарегистрированы от IMU, который встряхнулся затем положенный в покоящемся положении. Визуализируйте ускорение, магнитное поле и скорость вращения, как зарегистрировано датчиками.

load 'IMUReadingsShaken' accelReadings gyroReadings magReadings SampleRate
numSamples = size(accelReadings,1);
time = (0:(numSamples-1))'/SampleRate;

figure(1)
subplot(3,1,1)
plot(time,accelReadings)
title('Accelerometer Reading')
ylabel('Acceleration (m/s^2)')

subplot(3,1,2)
plot(time,magReadings)
title('Magnetometer Reading')
ylabel('Magnetic Field (\muT)')

subplot(3,1,3)
plot(time,gyroReadings)
title('Gyroscope Reading')
ylabel('Angular Velocity (rad/s)')
xlabel('Time (s)')

Создайте ahrsfilter и затем объедините данные IMU, чтобы определить ориентацию. Ориентация возвращена как вектор кватернионов; преобразуйте кватернионы в Углы Эйлера в градусах. Визуализируйте ориентацию корпуса датчика в зависимости от времени путем графического вывода требуемых Углов Эйлера, на каждом временном шаге, вращать глобальную систему координат к системе координат корпуса датчика.

fuse = ahrsfilter('SampleRate',SampleRate);
orientation = fuse(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(2)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation over Time')
legend('Rotation around z-axis', ...
       'Rotation around y-axis', ...
       'Rotation around x-axis')

В записи IMU, дрожащие остановки приблизительно после шести секунд. Определите покоящуюся ориентацию так, чтобы можно было охарактеризовать как быстро ahrsfilter сходится.

Чтобы определить покоящуюся ориентацию, вычислите средние значения магнитного поля и ускорения на итоговые четыре секунды и затем используйте ecompass функция, чтобы объединить данные.

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

restingOrientation = ecompass(mean(accelReadings(6*SampleRate:end,:)), ...
                              mean(magReadings(6*SampleRate:end,:)));

figure(3)
plot(time,rad2deg(dist(restingOrientation,orientation)))
hold on
xlabel('Time (s)')
ylabel('Quaternion Distance (degrees)')

Измените ahrsfilter по умолчанию свойства так, чтобы фильтр сходился к силе тяжести более быстро. Увеличьте GyroscopeDriftNoise к 1e-2 и уменьшите LinearAccelerationNoise к 1e-4. Это сообщает ahrsfilter алгоритм, чтобы взвесить данные о гироскопе меньше и данные об акселерометре больше. Поскольку accelerometer данные обеспечивают стабилизацию и сопоставимый вектор силы тяжести, получившаяся ориентация сходится более быстро.

Сбросьте фильтр, объедините данные и постройте результаты.

fuse.LinearAccelerationNoise = 1e-4;
fuse.GyroscopeDriftNoise     = 1e-2;
reset(fuse)

orientation = fuse(accelReadings,gyroReadings,magReadings);

figure(3)
plot(time,rad2deg(dist(restingOrientation,orientation)))
legend('Default AHRS Filter','Tuned AHRS Filter')

Алгоритмы

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

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

ahrsfilter использует структуру Фильтра Калмана с девятью осями, описанную в [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.

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

Введенный в R2018b

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