Ориентация от акселерометра, гироскопа и показаний магнитометра
ahrsfilter Система object™ плавит акселерометр, магнитометр и данные о датчике гироскопа, чтобы оценить ориентацию устройства.
Оценить ориентацию устройства:
Создайте ahrsfilter объект и набор его свойства.
Вызовите объект с аргументами, как будто это была функция.
Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты? MATLAB.
FUSE = ahrsfilterFUSE, для сплава датчика акселерометра, гироскопа и данных о магнитометре, чтобы оценить ориентацию устройства и скорость вращения. Фильтр использует вектор состояния с 12 элементами, чтобы отследить ошибку оценки для ориентации, смещения гироскопа, линейного ускорения и магнитного воздействия.
FUSE = ahrsfilter('ReferenceFrame',RF)RF. Задайте RF как 'NED' (Северо-восток вниз) или 'ENU' (Восточный Север). Значением по умолчанию является 'NED'.
FUSE = ahrsfilter(___,Name,Value)Name к заданному Value. Незаданные свойства имеют значения по умолчанию.
[ акселерометр предохранителей, гироскоп и данные о магнитометре, чтобы вычислить ориентацию и измерения скорости вращения. Алгоритм принимает, что устройство является стационарным перед первым вызовом.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, который содержит акселерометр, магнитометр и показания гироскопа от стационарного 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, с рекурсивным обновлением:
где 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 − – предсказанный (априорно) утверждают оценку; ошибочный процесс
Pk − – предсказанный (априорно) оценивают ковариацию
yk – инновации
Sk – инновационная ковариация
Kk – Усиление Кальмана
xk + – обновленный (по опыту) утверждает оценку
Pk + – обновленный (по опыту) оценивает ковариацию
k представляет итерацию, верхний индекс + представляет по опыту оценка, и верхний индекс − представляет априорную оценку.
Графические и следующие шаги описывают одну основанную на системе координат итерацию через алгоритм.
Перед первой итерацией, accelReadings, gyroReadings, и magReadings входные параметры разделяются на блоки в DecimationFactor- 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 + – является обновленным (по опыту) ошибочная оценочная ковариация
η – GyroscopeNoise
Смотрите раздел 10.1 из [1] для деривации условий ошибочной матрицы процесса.
Матрица усиления Кальмана 12 6, матрица раньше взвешивала инновации. В этом алгоритме инновации интерпретированы как ошибочный процесс, z.
Матрица усиления Кальмана создается как:
где
P − – предсказанная ошибочная ковариация
H модель наблюдения
S инновационная ковариация
Следующая ошибочная оценка определяется путем объединения матрицы усиления Кальмана с ошибкой в силе тяжести векторные и магнитные векторные оценки:
Если магнитный затор обнаруживается в текущей итерации, магнитный векторный сигнал ошибки проигнорирован, и, следующая ошибочная оценка вычисляется как:
Оценка ориентации обновляется путем умножения предыдущей оценки ошибкой:
Линейная ускоряющая оценка обновляется путем затухания линейной ускоряющей оценки от предыдущей итерации и вычитания ошибки:
где
Оценка смещения гироскопа обновляется путем вычитания ошибки смещения гироскопа из смещения гироскопа от предыдущей итерации:
[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.
ecompass | gpsSensor | imuSensor | imufilter | quaternion