Ориентация от показаний акселерометра, гироскопа и магнитометра
The ahrsfilter Система object™ предохраняет данные акселерометра, магнитометра и гироскопа для оценки ориентации устройства.
Для оценки ориентации устройства:
Создайте ahrsfilter Объекту и установите его свойства.
Вызывайте объект с аргументами, как будто это функция.
Дополнительные сведения о работе системных объектов см. в разделе «Что такое системные объекты?».
FUSE = ahrsfilterFUSEдля слияния датчика акселерометра, гироскопа и данных магнитометра для оценки ориентации и скорости вращения устройства. Фильтр использует вектор состояния с 12 элементами, чтобы отследить ошибку расчета для ориентации, смещения гироскопа, линейного ускорения и магнитного нарушения порядка.
FUSE = ahrsfilter('ReferenceFrame',RF)RF. Задайте RF как 'NED' (Северо-Восток-Даун) или 'ENU' (Восток-Север-Вверх). Значение по умолчанию является 'NED'.
FUSE = ahrsfilter(___,Name,Value)Name к заданной Value. Неопределенные свойства имеют значения по умолчанию.
[ fuses accelerometer, гироскоп и данные магнитометра для вычисления ориентации и скорости вращения измерений. Алгоритм принимает, что устройство стационарно перед первым вызовом.orientation,angularVelocity] = FUSE(accelReadings,gyroReadings,magReadings)
Чтобы использовать функцию объекта, задайте системный объект в качестве первого входного параметра. Например, чтобы освободить системные ресурсы системного объекта с именем obj, используйте следующий синтаксис:
release(obj)
ahrsfilterЗагрузите 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Этот пример показывает, как эффективность 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, с рекурсивным обновлением:
где 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− - прогнозируемая (априори) оценка состояния; процесс ошибки
Pk− - предсказанная (априорная) оценка ковариации
yk -- инновации
Sk -- инновационная ковариация
Kk -- коэффициент усиления Калмана
xk+ - обновленная (апостериорная) оценка состояния
Pk+ - обновленная (a posteriori) оценка ковариации
k представляет итерацию, надстрочный индекс + представляет апостериорную оценку и надстрочный индекс − представляет априорную оценку.
Графические и последующие шаги описывают итерацию на основе одного кадра через алгоритм.
Перед первой итерацией accelReadings, gyroReadings, и magReadings входы шунтируются в DecimationFactor-by-3 системы координат. Для каждого фрагмента алгоритм использует самые актуальные показания акселерометра и магнитометра, соответствующие фрагменту показаний гироскопа.
Пройдите по алгоритму для объяснения каждого этапа подробного обзора.
Алгоритм моделирует ускорение и угловое изменение как линейные процессы.
Ориентация для текущей системы координат предсказывается путем первой оценки углового изменения по сравнению с предыдущей системой координат:
где N - коэффициент десятикратного уменьшения, заданный свойством DecimationFactor, а fs - частота дискретизации, заданная свойством SampleRate.
Угловое изменение преобразуется в кватернионы с помощью rotvec
quaternion синтаксис конструкции:
Предыдущая оценка ориентации обновляется путем поворота ее на И Q:
Во время первой итерации оценка ориентации, q−, инициализируется ecompass.
Вектор гравитации интерпретируется как третий столбец кватерниона, q−, в матричном виде вращения:
См. [1] для объяснения того, почему третий столбец rPrior может быть интерпретирован как вектор гравитации.
Вторую оценку вектора гравитации производят путем вычитания из показаний акселерометра истекшей оценки линейного ускорения предыдущей итерации:
Магнитный вектор Земли оценивается вращением оценки магнитного вектора из предыдущей итерации априорной оценкой ориентации в матричной форме вращения:
Модель ошибки сочетает в себе два различия:
Различие между оценкой силы тяжести из показаний акселерометра и оценкой силы тяжести из показаний гироскопа:
Различие между оценкой магнитного вектора из показаний гироскопа и оценкой магнитного вектора из магнитометра:
Правильный магнитометр оценивает ошибку в оценке магнитного вектора и обнаруживает магнитные помехи.
Ошибка магнитного нарушения порядка вычисляется матричным умножением усиления Калмана, сопоставленного с магнитным вектором с сигналом ошибки:
Коэффициент усиления Калмана, K, является коэффициентом усиления Калмана, вычисленным в текущей итерации.
Магнитное заедание определяется путем проверки, что степень обнаруженного магнитного нарушения порядка меньше или равна четырём степеням ожидаемого напряжения магнитного поля:
ExpectedMagneticFieldStrength является свойством ahrsfilter.
Уравнения Калмана используют оценку тяжести, полученную из показаний гироскопа, g, оценку магнитного вектора, полученную из показаний гироскопа, mGyro и наблюдения процесса ошибки, z, чтобы обновить усиление Калмана и промежуточные ковариационные матрицы. Коэффициент усиления Калмана применяется к сигналу ошибки, z, для вывода апостериорной оценки ошибки x+.
Модель наблюдения преобразует наблюдаемые состояния 1 на 3, g и mGyro, в истинное состояние 6 на 12, H.
Модель наблюдения построена как:
где g x, g y и g z являются x -, y - и z - элементами вектора тяжести, оцененными из априорной ориентации, соответственно. m x, m y и m z являются x -, y - и z - элементами магнитного вектора, оцененными из априорной ориентации, соответственно. κ является константой, определяемой свойствами SampleRate и DecimationFactor: κ = DecimationFactor/ SampleRate.
Вывод модели наблюдения см. в разделах 7.3 и 7.4 документа [1].
Инновационная ковариация является матрицей 6 на 6, используемой для отслеживания изменчивости измерений. Инновационная ковариационная матрица вычисляется как:
где
H - матрица модели наблюдения
P− - предсказанная (априорная) оценка ковариации модели наблюдения, вычисленная в предыдущей итерации
R - ковариация шума модели наблюдения, рассчитанная как:
где
и
Следующие свойства определяют отклонение шума модели наблюдения:
Ковариация оценки ошибки является матрицей 12 на 12, используемой для отслеживания изменчивости состояния.
Ковариационная матрица оценки ошибки обновляется следующим образом:
где K - коэффициент усиления Калмана, H - матрица измерений и P− - ковариация оценки ошибки, вычисленная во время предыдущей итерации.
Ковариация оценки ошибки является матрицей 12 на 12, используемой для отслеживания изменчивости состояния. Априорная оценка ошибки, ковариация, P−, устанавливается в ковариацию шума процесса, Q, определенную во время предыдущей итерации. Q вычисляется как функция ковариации оценки апостериорной ошибки, P+. При вычислении Q принято, что условия перекрестной корреляции незначительны по сравнению с терминами автокорреляции и равны нулю:
где
P+ - обновленная (a posteriori) оценка ошибки ковариация
β -- GyroscopeDriftNoise
η -- GyroscopeNoise
Для вывода терминов матрицы ошибок процесса смотрите раздел 10.1 документа [1].
Матрица усиления Калмана является матрицей 12 на 6, используемой для взвешивания нововведения. В этом алгоритме нововведение интерпретируется как процесс ошибки, z.
Матрица усиления Калмана построена как:
где
P− -- предсказанная ковариация ошибок
H -- модель наблюдения
S -- инновационная ковариация
Апостериорная оценка ошибки определяется путем объединения матрицы усиления Калмана с ошибкой в оценках вектора гравитации и магнитного вектора:
Если в текущей итерации обнаруживается магнитное глушение, сигнал ошибки магнитного вектора игнорируется, и оценка апостериорной ошибки вычисляется как:
Оценка ориентации обновляется путем умножения предыдущей оценки на ошибку:
Линейная оценка ускорения обновляется путем распада линейной оценки ускорения из предыдущей итерации и вычитания ошибки:
где
Оценка смещения гироскопа обновлена, вычтя ошибку смещения гироскопа из смещения гироскопа от предыдущей итерации:
[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.