ahrsfilter

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

Описание

The ahrsfilter Система object™ предохраняет данные акселерометра, магнитометра и гироскопа для оценки ориентации устройства.

Для оценки ориентации устройства:

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

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

Дополнительные сведения о работе системных объектов см. в разделе «Что такое системные объекты?».

Создание

Описание

пример

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

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

пример

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

Свойства

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Типы данных: 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

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

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

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

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

  • 'quaternion' -- Выход представляет собой N -by-1 quaternion.

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

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

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

Описание

пример

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

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

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

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

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

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

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

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

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

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

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

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

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

  • 'Rotation matrix' - выводится массив вращений M 3 на 3 байта, матриц того же типа, что и входы

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

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

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

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

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

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

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

release(obj)

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

tuneНастройка ahrsfilter параметры для уменьшения ошибки расчета
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)')

Figure contains an axes. The axes with title Orientation Estimate contains 3 objects of type line. These objects represent z-axis, y-axis, x-axis.

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

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

load 'StationaryIMUReadings.mat' accelReadings magReadings gyroReadings SampleRate

numSamples = size(accelReadings,1);

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

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

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

Предохраните показания БИНС с помощью фильтра системы ссылки ориентации и курса (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™, чтобы сплавить 9-осевые данные БИНС от тела датчика, который встряхивается. Постройте график кватернионного расстояния между объектом и его конечным положением покоя, чтобы визуализировать эффективность и как быстро фильтр сходится к правильному положению покоя. Затем настройте параметры ahrsfilter так что фильтр сходится быстрее в положение покоя «основная истина».

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

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 и затем сплавьте данные БИНС, чтобы определить ориентацию. Ориентация возвращается как вектор кватернионов; преобразуйте кватернионы в углы Эйлера в степенях. Визуализируйте ориентацию тела датчика с течением времени путем построения графика углов Эйлера, необходимых на каждом временном шаге, чтобы повернуть глобальную систему координат к системе координат тела датчика.

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')

В записи БИНС тряска прекращается примерно через шесть секунд. Определите ориентацию покоя, чтобы вы могли охарактеризовать, насколько быстро 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.

The 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 вектор ошибки магнитного нарушения порядка, измеренный в системе координат датчика, в мкТ, в момент времени 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+ - обновленная (a posteriori) оценка ковариации

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

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

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

Ссылки

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

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

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