Ориентация от показаний акселерометра, гироскопа и магнитометра
ahrsfilter Система object™ плавит акселерометр, магнитометр и данные датчика гироскопа для оценки ориентации устройства.
Для оценки ориентации устройства:
Создать ahrsfilter и задайте его свойства.
Вызовите объект с аргументами, как если бы это была функция.
Дополнительные сведения о работе системных объектов см. в разделе Что такое системные объекты?.
FUSE = ahrsfilterFUSEдля сенсорного слияния данных акселерометра, гироскопа и магнитометра для оценки ориентации устройства и угловой скорости. Фильтр использует 12-элементный вектор состояния для отслеживания ошибки оценки для ориентации, смещения гироскопа, линейного ускорения и магнитного возмущения.
FUSE = ahrsfilter('ReferenceFrame',RF)RF. Определить RF как 'NED' (Север-Восток-вниз) или 'ENU' (Восток-Север-Вверх). Значением по умолчанию является NED.
FUSE = ahrsfilter(___,Name,Value)Name к указанному Value. Неопределенные свойства имеют значения по умолчанию.
[ плавит акселерометр, гироскоп и данные магнитометра для вычисления измерений ориентации и угловой скорости. Алгоритм предполагает, что устройство неподвижно перед первым вызовом.orientation,angularVelocity] = FUSE(accelReadings,gyroReadings,magReadings)
Чтобы использовать функцию объекта, укажите объект System в качестве первого входного аргумента. Например, для освобождения системных ресурсов объекта System с именем 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™ плавить 9-осевые данные IMU от корпуса датчика, который встряхивается. Постройте график расстояния кватерниона между объектом и его конечным положением покоя для визуализации производительности и того, как быстро фильтр сходится в правильное положение покоя. Затем выполните настройку параметров 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 с рекурсивным обновлением:
− 1dk − 1] + wk
где xk - вектор 12 на 1, состоящий из:
startk -- вектор ошибки ориентации 3 на 1, в градусах, в момент времени k
bk -- вектор смещения нулевой угловой скорости гироскопа 3 на 1, в град/с, в момент времени k
ak -- вектор ошибки ускорения 3 на 1, измеренный в кадре датчика, в g, в момент времени k
dk -- вектор ошибки магнитного возмущения 3 на 1, измеренный в кадре датчика, в мкТ, в момент времени k
и где wk является вектором аддитивного шума 12 на 1, а Fk является моделью перехода состояния.
Поскольку xk определяется как процесс ошибки, априорная оценка всегда равна нулю, и поэтому модель перехода состояния Fk равна нулю. Это понимание приводит к следующему уменьшению стандартных уравнений Калмана:
Стандартные уравнения Калмана:
Sk) − 1xk + = xk − + Kkykpk
Уравнения Калмана, используемые в этом алгоритме:
+ = KkykPk + = Pk − − KkHkPk −
где:
xk − -- прогнозируемая (априорная) оценка состояния; процесс ошибки
Pk − -- предсказанная (априорная) оценка ковариации
yk -- инновации
Sk - инновационная ковариация
Kk -- Kalman gain
xk + -- обновленная (a posteriori) оценка состояния
Pk + -- обновленная (a posteriori) оценка ковариации
k представляет итерацию, верхний индекс + представляет апостериорную оценку, а верхний индекс − представляет априорную оценку.
На рисунке и последующих шагах описывается итерация на основе одного кадра в алгоритме.
Перед первой итерацией accelReadings, gyroReadings, и magReadings входы секционированы в DecimationFactor-на-3 кадра. Для каждого блока алгоритм использует самые текущие показания акселерометра и магнитометра, соответствующие фрагменту показаний гироскопа.
Пройдите по алгоритму для пояснения каждого этапа подробного обзора.
Алгоритм моделирует ускорение и угловое изменение как линейные процессы.
Ориентацию для текущего кадра прогнозируют путем первой оценки углового изменения по сравнению с предыдущим кадром:
gyroOffset1 × 3) fs
где N - коэффициент прореживания, заданный свойством DecimationFactor, а fs - частота дискретизации, заданная свойством SampleRate.
Угловое изменение преобразуется в кватернионы с помощью rotvec
quaternion строительный синтаксис:
rotvec')
Предыдущая оценка ориентации обновляется путем ее поворота на ΔQ:
∏n=1NΔQn)
Во время первой итерации оценка ориентации, q −, инициализируется ecompass.
Вектор гравитации интерпретируется как третий столбец кватерниона, q −, в матричной форме вращения:
(:, 3)) T
Объяснение того, почему третий столбец rPrior можно интерпретировать как вектор гравитации, см. в [1].
Вторую оценку вектора гравитации производят путем вычитания оценки затухшего линейного ускорения предыдущей итерации из показаний акселерометра:
Магнитный вектор Земли оценивается вращением оценки магнитного вектора из предыдущей итерации на априорную оценку ориентации, в матричной форме вращения:
mT)) T
Модель ошибок объединяет два отличия:
Разность между оценкой гравитации по показаниям акселерометра и оценкой гравитации по показаниям гироскопа: gAccel
Разность между оценкой магнитного вектора по показаниям гироскопа и оценкой магнитного вектора по магнитометру: magReadings
Магнитометр правильно оценивает погрешность в оценке магнитного вектора и обнаруживает магнитные помехи.
Вычисляют погрешность магнитного возмущения матричным умножением коэффициента усиления Калмана, связанного с магнитным вектором, на сигнал ошибки:
(z1 × 6) T) T
Коэффициент Калмана K - коэффициент Калмана, вычисленный в текущей итерации.
Магнитное заклинивание определяют путем проверки, что мощность обнаруженного магнитного возмущения меньше или равна четырехкратной мощности ожидаемой напряженности магнитного поля:
2
ExpectedDepectedStartStrength является свойством ahrsfilter.
Уравнения Калмана используют оценку гравитации, полученную из показаний гироскопа, g, оценку магнитного вектора, полученную из показаний гироскопа, mGyro, и наблюдение процесса ошибки, z, для обновления матриц усиления Калмана и промежуточной ковариации. Коэффициент усиления Калмана применяется к сигналу ошибки z для вывода апостериорной оценки ошибки x +.
Модель наблюдения отображает наблюдаемые состояния 1 на 3, g и mGyro, в истинное состояние 6 на 12, Н.
Модель наблюдения строится следующим образом:
где gx, gy и gz - x-, y- и z-элементы вектора гравитации, оцененные по априорной ориентации соответственно. mx, my и mz - x-, y- и z-элементы магнитного вектора, оцененные по априорной ориентации соответственно. δ - константа, определяемая свойствами SampleRate и DecimationFactor: DecimationFactor/SampleRate.
Инновационная ковариация представляет собой матрицу 6 на 6, используемую для отслеживания изменчивости измерений. Инновационную ковариационную матрицу рассчитывают как:
H6x12) T
где
H - матрица модели наблюдения
P − - предсказанная (априорная) оценка ковариации модели наблюдения, вычисленная в предыдущей итерации
R - ковариация шума модели наблюдения, вычисленная как:
где
Гиросвязь Шум)
и
)
Дисперсию шума модели наблюдения определяют следующие свойства:
Ковариация оценки ошибок представляет собой матрицу 12 на 12, используемую для отслеживания изменчивости состояния.
Ковариационная матрица оценки ошибок обновляется следующим образом:
где K - коэффициент усиления Калмана, H - матрица измерения, а P − - ковариация оценки ошибок, вычисленная во время предыдущей итерации.
Ковариация оценки ошибок представляет собой матрицу 12 на 12, используемую для отслеживания изменчивости состояния. Априорная оценка ошибки ковариации, P −, устанавливается на ковариацию шума процесса, Q, определенную во время предыдущей итерации. Q вычисляется как функция ковариации оценки апостериорной ошибки P +. При вычислении Q предполагается, что члены взаимной корреляции ничтожны по сравнению с членами автокорреляции и установлены в нуль:
+ γ000000000000σ2P + (131) + γ000000000000σ2P + (144) + γ]
где
P + - обновленная (a posteriori) ковариация оценки ошибки
β -- Гироскопический шум
start-- Гироссовый шум
start-- Линейная поддержка DecayFactor
start-- Линейный уровень шума
γ -- Шум возмущения
Деривация терминов матрицы ошибок процесса приведена в разделе 10.1 [1].
Матрица усиления Калмана - это матрица 12 на 6, используемая для взвешивания нововведения. В этом алгоритме нововведение интерпретируется как процесс ошибки, z.
Матрица усиления Калмана построена как:
((S6 × 6) T) − 1
где
P − -- предсказанная ковариация ошибки
H -- модель наблюдения
S -- инновационная ковариация
Заднюю оценку погрешности определяют объединением матрицы усиления Калмана с погрешностью в оценках вектора гравитации и магнитного вектора:
(z1 × 6) T
Если в итерации тока обнаружено магнитное заклинивание, сигнал ошибки магнитного вектора игнорируется, и оценка задней ошибки вычисляется как:
3) (zg) T
Оценка ориентации обновляется путем умножения предыдущей оценки на ошибку:
(λ +)
Оценка линейного ускорения обновляется путем затухания оценки линейного ускорения из предыдущей итерации и вычитания ошибки:
где
start-- Линейный коэффициент DecayFactor
Оценка смещения гироскопа обновлена, вычтя ошибку смещения гироскопа из смещения гироскопа от предыдущего повторения:
1 − a +
[1] Слияние датчиков с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs
[2] Роетенберг, Д., Х.Ж. Луинге, К.Т.М. Батен и П.Х. Велтинк. «Компенсация магнитных возмущений улучшает инерционное и магнитное восприятие ориентации сегмента человеческого тела». Сделки IEEE по нейронным системам и инженерии реабилитации. Том 13. Выпуск 3, 2005, стр. 395-405.
ecompass | gpsSensor | imufilter | imuSensor | quaternion