Sensor Fusion and Tracking Toolbox™ позволяет вам объединить данные, считанные из инерциального измерительного блока (IMU), чтобы оценить ориентацию и скорость вращения:
ecompass
– Плавьте показания акселерометра и магнитометра
imufilter
– Плавьте показания гироскопа и акселерометр
ahrsfilter
– Плавьте акселерометр, гироскоп и показания магнитометра
Больше датчиков на IMU приводит к более устойчивой оценке ориентации. Данные о датчике могут быть перекрестный подтверждены, и информация, которую передают датчики, ортогональная.
Этот пример предоставляет обзор инерционного сплава датчика для IMUs в Sensor Fusion and Tracking Toolbox.
Чтобы изучить, как смоделировать инерционные датчики и GPS, смотрите IMU Модели, GPS и INS/GPS. Чтобы изучить, как сгенерировать движение основной истины что модели датчика дисков, смотрите waypointTrajectory
и kinematicTrajectory
.
Можно также плавить показания IMU с показаниями GPS, чтобы оценить положение. Смотрите Определяют Положение Используя Инерционные Датчики и GPS для обзора.
В этом примере показано, как использовать алгоритмы сплава с 9 осями и с 6 осями, чтобы вычислить ориентацию. Существует несколько алгоритмов, чтобы вычислить ориентацию из инерционных модулей измерения модулей магнитно-угловой силы тяжести уровня (MARG) и (IMUs). Этот пример покрывает основы ориентации и как использовать эти алгоритмы.
Ориентация
Ориентация объекта описывает свое вращение относительно некоторой системы координат, иногда названной системой координаты вышестоящего элемента, в трех измерениях.
Для следующих алгоритмов фиксированная используемая система координаты вышестоящего элемента является Северо-востоком вниз (NED). NED иногда упоминается как глобальная система координат или система координат. В системе координат NED Ось X указывает север, Ось Y указывает восток, и ось Z указывает вниз. Плоскость X-Y NED считается локальной плоскостью касательной Земли. В зависимости от алгоритма север может быть или магнитным северным или истинным севером. Алгоритмы в этом примере используют магнитный север.
Если задано, следующие алгоритмы могут оценить ориентацию относительно системы координаты вышестоящего элемента "восточного севера" (ENU) вместо NED.
Объект может считаться наличием его собственной системы координат, часто названной система координаты нижестоящего элемента или локальная координата. Эта система координаты нижестоящего элемента вращается с объектом относительно системы координаты вышестоящего элемента. Если нет никакого перевода, источников обоих перекрытий систем координат.
Вычисленное количество ориентации является вращением, которое берет количества от родительской системы координат до дочерней системы координат. Вращение представлено матрицей вращения или кватернионом.
Типы датчиков
Для оценки ориентации обычно используются три типа датчиков: акселерометры, гироскопы и магнитометры. Акселерометры измеряют соответствующее ускорение. Гироскопы измеряют скорость вращения. Магнитометры измеряют локальное магнитное поле. Различные алгоритмы используются, чтобы плавить различные комбинации датчиков, чтобы оценить ориентацию.
Данные о датчике
Через большую часть этого примера используется тот же набор данных о датчике. Акселерометр, гироскоп и данные о датчике магнитометра были зарегистрированы, в то время как устройство вращало приблизительно три различных оси: сначала вокруг его локальной Оси Y, затем вокруг его оси Z, и наконец вокруг ее Оси X. Ось X устройства обычно указывалась на юг на время эксперимента.
ld = load('rpy_9axis.mat');
acc = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;
mag = ld.sensorData.MagneticField;
viewer = HelperOrientationViewer;
Fusion магнитометра акселерометра
ecompass
функционируйте акселерометр предохранителей и данные о магнитометре. Это - алгоритм без памяти, который не требует никакой настройки параметра, но алгоритм очень восприимчив к шуму датчика.
qe = ecompass(acc, mag); for ii=1:size(acc,1) viewer(qe(ii)); pause(0.01); end
Обратите внимание на то, что ecompass
алгоритм правильно находит местоположение севера. Однако, потому что функция без памяти, предполагаемое движение не является гладким. Алгоритм мог использоваться в качестве шага инициализации в фильтре ориентации или некоторые методы, представленные в Ориентации Фильтра Lowpass Используя Кватернион, SLERP мог использоваться, чтобы сглаживать движение.
Fusion гироскопа акселерометра
Следующие объекты оценивают ориентацию с помощью или Фильтра Калмана состояния ошибки или дополнительного фильтра. Фильтр Калмана состояния ошибки является стандартным фильтром оценки и позволяет, чтобы много различных аспектов системы были настроены с помощью соответствующих шумовых параметров. Дополнительный фильтр может использоваться вместо систем с ограничениями памяти и имеет минимальные настраиваемые параметры, который допускает более легкую настройку за счет более прекрасной настройки.
imufilter
и complementaryFilter
Система objects™ плавит данные о гироскопе и акселерометр. imufilter
использует внутренний Фильтр Калмана состояния ошибки и complementaryFilter
использует дополнительный фильтр. Фильтры способны к удалению шума смещения гироскопа, который дрейфует в зависимости от времени.
ifilt = imufilter('SampleRate', ld.Fs); for ii=1:size(acc,1) qimu = ifilt(acc(ii,:), gyro(ii,:)); viewer(qimu); pause(0.01); end
% Disable magnetometer input. cfilt = complementaryFilter('SampleRate', ld.Fs, 'HasMagnetometer', false); for ii=1:size(acc,1) qimu = cfilt(acc(ii,:), gyro(ii,:)); viewer(qimu); pause(0.01); end
Несмотря на то, что imufilter
и complementaryFilter
алгоритмы производят значительно более сглаженные оценки движения, по сравнению с ecompass
, они правильно не оценивают направление севера. imufilter
не обрабатывает данные о магнитометре, таким образом, они просто принимают, что Ось X устройства первоначально указывает к северу. Оценка движения дана imufilter
относительно оцененной ориентации начальной буквы. complementaryFilter
делает то же предположение когда HasMagnetometer
свойство установлено в false
.
Fusion магнитометра гироскопа акселерометра
И заголовок ссылочной системы (AHRS) отношения состоит из 9 систем координат, которые используют акселерометр, гироскоп и магнитометр, чтобы вычислить ориентацию. ahrsfilter
и complementaryFilter
Система objects™ комбинирует лучший из предыдущих алгоритмов, чтобы произвести гладко изменяющуюся оценку ориентации устройства, правильно оценивая направление севера. complementaryFilter
использует тот же дополнительный алгоритм фильтра как прежде, с дополнительным шагом, чтобы включать магнитометр и улучшить оценку ориентации. Как imufilter
, ahrsfilter
алгоритм также использует Фильтр Калмана состояния ошибки. В дополнение к гироскопу смещают удаление, ahrsfilter
имеет некоторую способность обнаружить и отклонить умеренный магнитный затор.
ifilt = ahrsfilter('SampleRate', ld.Fs); for ii=1:size(acc,1) qahrs = ifilt(acc(ii,:), gyro(ii,:), mag(ii,:)); viewer(qahrs); pause(0.01); end
cfilt = complementaryFilter('SampleRate', ld.Fs); for ii=1:size(acc,1) qahrs = cfilt(acc(ii,:), gyro(ii,:), mag(ii,:)); viewer(qahrs); pause(0.01); end
Настройка параметров фильтра
complementaryFilter
, imufilter
, и ahrsfilter
Система objects™ все имеет настраиваемые параметры. Настройка параметров на основе заданных используемых датчиков может улучшать производительность.
complementaryFilter
параметры AccelerometerGain
и MagnetometerGain
может быть настроен, чтобы изменить сумму, измерения каждого датчика влияют на оценку ориентации. Когда AccelerometerGain
установлен в 0
, только гироскоп используется для ориентации оси Y и x-. Когда AccelerometerGain
установлен в 1
, только акселерометр используется для ориентации оси Y и x-. Когда MagnetometerGain
установлен в 0
, только гироскоп используется для ориентации оси z. Когда MagnetometerGain
установлен в 1
, только магнитометр используется для ориентации оси z.
ahrsfilter
и imufilter
Система objects™ имеет больше параметров, которые могут позволить фильтрам более тесно совпадать с определенными аппаратными датчиками. Среда датчика также важна, чтобы учесть. imufilter
параметры являются подмножеством ahrsfilter
параметры. AccelerometerNoise
, GyroscopeNoise
, MagnetometerNoise
, и GyroscopeDriftNoise
шумы измерения. Таблицы данных датчиков помогают определить те значения.
LinearAccelerationNoise
и LinearAccelerationDecayFactor
управляйте ответом фильтра на линейное (поступательное) ускорение. Сотрясение устройства является простым примером добавления линейного ускорения.
Рассмотрите как imufilter
с LinearAccelerationNoise
из 9e-3 отвечает на дрожащую траекторию, по сравнению с одной с LinearAccelerationNoise
из 9e-4.
ld = load('shakingDevice.mat'); accel = ld.sensorData.Acceleration; gyro = ld.sensorData.AngularVelocity; viewer = HelperOrientationViewer; highVarFilt = imufilter('SampleRate', ld.Fs, ... 'LinearAccelerationNoise', 0.009); qHighLANoise = highVarFilt(accel, gyro); lowVarFilt = imufilter('SampleRate', ld.Fs, ... 'LinearAccelerationNoise', 0.0009); qLowLANoise = lowVarFilt(accel, gyro);
Один способ видеть эффект LinearAccelerationNoise
должен посмотреть на выходной вектор силы тяжести. Вектор силы тяжести является просто третьим столбцом матрицы вращения ориентации.
rmatHigh = rotmat(qHighLANoise, 'frame'); rmatLow = rotmat(qLowLANoise, 'frame'); gravDistHigh = sqrt(sum( (rmatHigh(:,3,:) - [0;0;1]).^2, 1)); gravDistLow = sqrt(sum( (rmatLow(:,3,:) - [0;0;1]).^2, 1)); figure; plot([squeeze(gravDistHigh), squeeze(gravDistLow)]); title('Euclidean Distance to Gravity'); legend('LinearAccelerationNoise = 0.009', ... 'LinearAccelerationNoise = 0.0009');
lowVarFilt
имеет низкий LinearAccelerationNoise
, таким образом, это ожидает быть в среде с низким линейным ускорением. Поэтому это более восприимчиво к линейному ускорению, как проиллюстрировано большими изменениями ранее графика. Однако, потому что это ожидает быть в среде с низким линейным ускорением, более высокое доверие помещается в сигнал акселерометра. По сути, оценка ориентации сходится быстро назад к вертикали, если сотрясение закончилось. Обратное верно для highVarFilt
. Фильтр менее затронут путем сотрясения, но оценка ориентации занимает больше времени, чтобы сходиться к вертикали, когда сотрясение остановилось.
MagneticDisturbanceNoise
свойство позволяет моделировать магнитные воздействия (негеомагнитные источники шума) почти таким же способом LinearAccelerationNoise
модели линейное ускорение.
Два свойства фактора затухания (MagneticDisturbanceDecayFactor
и LinearAccelerationDecayFactor
) смоделируйте уровень изменения шумов. Для медленно различных источников шума, установленных эти параметры на значение ближе к 1. Для того, чтобы быстро варьироваться, некоррелированые шумы, устанавливает эти параметры ближе на 0. Более низкий LinearAccelerationDecayFactor
позволяет оценке ориентации найти "вниз" более быстро. Более низкий MagneticDisturbanceDecayFactor
позволяет оценке ориентации найти север более быстро.
Очень большие, короткие магнитные воздействия отклоняются почти полностью ahrsfilter
. Рассмотрите импульс [0 250 0], единое время применялось при записи от стационарного датчика. Идеально, в оценке ориентации не должно быть никакого изменения.
ld = load('magJamming.mat'); hpulse = ahrsfilter('SampleRate', ld.Fs); len = 1:10000; qpulse = hpulse(ld.sensorData.Acceleration(len,:), ... ld.sensorData.AngularVelocity(len,:), ... ld.sensorData.MagneticField(len,:)); figure; timevec = 0:ld.Fs:(ld.Fs*numel(qpulse) - 1); plot( timevec, eulerd(qpulse, 'ZYX', 'frame') ); title(['Stationary Trajectory Orientation Euler Angles' newline ... 'Magnetic Jamming Response']); legend('Z-rotation', 'Y-rotation', 'X-rotation'); ylabel('Degrees'); xlabel('Seconds');
Обратите внимание на то, что фильтр почти полностью отклоняет этот магнитный импульс как интерференцию. Любая сила магнитного поля, больше, чем четыре раза ExpectedMagneticFieldStrength
рассматривается создающим затор источником, и сигнал магнитометра проигнорирован для тех выборок.
Заключение
Алгоритмы, представленные здесь, когда правильно настроено, включают оценку ориентации и устойчивы против экологических источников шума. Важно рассмотреть ситуации, в которых датчики используются и настраивают фильтры соответственно.
ahrsfilter
| ecompass
| imufilter
| imuSensor
| insfilter