ahrsfilter

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

Описание

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

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

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

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

Создание

Синтаксис

FUSE = ahrsfilter

FUSE = ahrsfilter('ReferenceFrame',RF)

FUSE = ahrsfilter(___,Name,Value)

Описание

пример

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

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

пример

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

Свойства

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

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

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

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

`SampleRate` - Входная частота выборки данных датчика (Гц)
`100` (по умолчанию) | положительная скалярная величина

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

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

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

`DecimationFactor` - Коэффициент десятикратного уменьшения
`1` (по умолчанию) | положительное целое число

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

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

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

`AccelerometerNoise` - Отклонение шума сигнала акселерометра ((м/с²)²)
`0.00019247` (по умолчанию) | положительный действительный скаляр

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

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

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

`MagnetometerNoise` - Отклонение шума сигнала магнитометра (мкТ²)
`0.1` (по умолчанию) | положительный действительный скаляр

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

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

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

`GyroscopeNoise` - Отклонение шума сигнала гироскопа ((рад/с)²)
`9.1385e-5` (по умолчанию) | положительный действительный скаляр

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

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

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

`GyroscopeDriftNoise` - Отклонение смещения гироскопа ((рад/с)²)
`3.0462e-13` (по умолчанию) | положительный действительный скаляр

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

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

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

`LinearAccelerationNoise` - Отклонение линейного шума ускорения (м/с²)²
`0.0096236` (по умолчанию) | положительный действительный скаляр

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

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

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

`LinearAccelerationDecayFactor` - Коэффициент затухания для дрейфа линейного ускорения
`0.5` (по умолчанию) | скаляром в области значений [0,1)

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

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

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

`MagneticDisturbanceNoise` - Отклонение магнитного нарушения порядка шума (мкТ²)
`0.5` (по умолчанию) | действительная конечная положительная скалярная величина

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

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

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

`MagneticDisturbanceDecayFactor` - Коэффициент распада для магнитного нарушения порядка
`0.5` (по умолчанию) | положительная скалярная величина в области значений [0,1]

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

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

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

`InitialProcessNoise` - Ковариационная матрица для технологического шума
Матрица 12 на 12

Ковариационная 0.000006092348396 0 0 0 0 0 0 0 0 0 0 Columns 7 through 12 0 0 0 0 0 0 0.009623610000000 0 0 0 0 матрица для технологического шума, заданная как матрица 12 на 12. Значение по умолчанию является:

  Columns 1 through 6 0                   0                   0                   0                   0 0   0.000006092348396                   0                   0                   0                   0 0   0.000006092348396                   0                   0                   0 0                   0   0.000076154354947                   0                   0 0                   0                   0   0.000076154354947                   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                   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.009623610000000                   0                   0                   0 0                   0   0.600000000000000                   0                   0 0                   0                   0   0.600000000000000                   0 0                   0                   0                   0   0.600000000000000

Начальная ковариационная матрица процесса учитывает ошибку в модели процесса.

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

`ExpectedMagneticFieldStrength` - Ожидаемая оценка напряженности магнитного поля (мкТ)
`50` (по умолчанию) | реальная положительная скалярная величина

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

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

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

`OrientationFormat` - Формат выходной ориентации
`'quaternion'` (по умолчанию) | `'Rotation matrix'`

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

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

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

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

Синтаксис

[orientation,angularVelocity] = FUSE(accelReadings,gyroReadings,magReadings)

Описание

пример

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

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

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

`accelReadings` - Показания акселерометра в системе координат тела датчика (м/с²)
N -by-3 матрица

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

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

`gyroReadings` - Показания гироскопа в системе координат тела датчика (рад/с)
N -by-3 матрица

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

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

`magReadings` - Показания магнитометра в системе координат тела датчика (мкТ)
N -by-3 матрица

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

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

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

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

`orientation` - Ориентация, которая вращает величины от локальной системы координат навигации к системе координат тела датчика
M массив кватернионов -by-1 (по умолчанию) | массив 3by-3-by- M

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

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

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

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

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

`angularVelocity` - Скорость вращения в системе координат тела датчика (рад/с)
M массив -by-3 (по умолчанию

)

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

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

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

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

release(obj)

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

Характерно для `ahrsfilter`

tune Настройка ahrsfilter параметры для уменьшения ошибки расчета

Общий для всех системных объектов

`step`	Запуск алгоритма системного объекта
`release`	Отпустите ресурсы и допустите изменения в значениях свойств системного объекта и входных характеристиках
`reset`	Сброс внутренних состояний Системного объекта

Примеры

свернуть все

Оцените ориентацию с помощью `ahrsfilter`

Открыть Live Script

Загрузите 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`

Открыть скрипт

Этот пример показывает, как эффективность 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')

Тряска трека 9-Axis БИНС

Открыть скрипт

Этот пример использует 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} = [\begin{matrix} θ_{k} \\ b_{k} \\ a_{k} \\ d_{k} \end{matrix}] = F_{k} [\begin{matrix} θ_{k - 1} \\ b_{k - 1} \\ a_{k - 1} \\ d_{k - 1} \end{matrix}] + w_{k}$

где 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, равна нулю. Это понимание приводит к следующему уменьшению стандартных уравнений Калмана:

Стандартные уравнения Калмана:

$\begin{array}{l} x_{k}^{-} = F_{k} x_{k - 1}^{+} \\ P_{k}^{-} = F_{k} P_{k - 1}^{+} F_{k}^{T} + Q_{k} \\ y_{k} = z_{k} - H_{k} x_{k}^{-} \\ S_{k} = R_{k} + H_{k} P_{k}^{-} H_{k}^{T} \\ K_{k} = P_{k}^{-} H_{k}^{T} {(S_{k})}^{- 1} \\ x_{k}^{+} = x_{k}^{-} + K_{k} y_{k} \\ P_{k}^{+} = P_{k}^{-} - K_{k} H_{k} P_{k}^{-} \end{array}$

Уравнения Калмана, используемые в этом алгоритме:

$\begin{array}{l} x_{k}^{-} = 0 \\ P_{k}^{-} = Q_{k} \\ y_{k} = z_{k} \\ S_{k} = R_{k} + H_{k} P_{k}^{-} H_{k}^{T} \\ K_{k} = P_{k}^{-} H_{k}^{T} {(S_{k})}^{- 1} \\ x_{k}^{+} = K_{k} y_{k} \\ P_{k}^{+} = P_{k}^{-} - K_{k} H_{k} P_{k}^{-} \end{array}$

где:

_xk⁻ - прогнозируемая (априори) оценка состояния; процесс ошибки
_Pk⁻ - предсказанная (априорная) оценка ковариации
_yk -- инновации
_Sk -- инновационная ковариация
_Kk -- коэффициент усиления Калмана
_xk⁺ - обновленная (апостериорная) оценка состояния
_Pk⁺ - обновленная (a posteriori) оценка ковариации

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

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

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

Подробный обзор

Пройдите по алгоритму для объяснения каждого этапа подробного обзора.

Модель

Алгоритм моделирует ускорение и угловое изменение как линейные процессы.

Прогнозирование ориентации

Ориентация для текущей системы координат предсказывается путем первой оценки углового изменения по сравнению с предыдущей системой координат:

$Δ φ_{N \times 3} = \frac{(g y r o R e a d i n g s_{N \times 3} - g y r o O f f s e t_{1 \times 3})}{f s}$

где N - коэффициент десятикратного уменьшения, заданный свойством DecimationFactor, а fs - частота дискретизации, заданная свойством SampleRate.

Угловое изменение преобразуется в кватернионы с помощью rotvec quaternion синтаксис конструкции:

$Δ Q_{N \times 1} = quaternion (Δ φ_{N \times 3},' r o t v e c')$

Предыдущая оценка ориентации обновляется путем поворота ее на И Q:

$q_{1 \times 1}^{-} = (q_{1 \times 1}^{+}) (\prod_{n = 1}^{N} Δ Q_{n})$

Во время первой итерации оценка ориентации, q⁻, инициализируется ecompass.

Оценка гравитации по ориентации

Вектор гравитации интерпретируется как третий столбец кватерниона, q⁻, в матричном виде вращения:

$g_{1 \times 3} = {(r P r i o r (:, 3))}^{T}$

См. [1] для объяснения того, почему третий столбец rPrior может быть интерпретирован как вектор гравитации.

Оценка свободного падения от ускорения

Вторую оценку вектора гравитации производят путем вычитания из показаний акселерометра истекшей оценки линейного ускорения предыдущей итерации:

$g A c c e l_{1 \times 3} = a c c e l R e a d i n g s_{1 \times 3} - l i n A c c e l p r i o r_{1 \times 3}$

Оценка магнитного вектора Земли

Магнитный вектор Земли оценивается вращением оценки магнитного вектора из предыдущей итерации априорной оценкой ориентации в матричной форме вращения:

$m G y r o_{1 \times 3} = {((r P r i o r) (m^{T}))}^{T}$

Модель ошибки

Модель ошибки сочетает в себе два различия:

Различие между оценкой силы тяжести из показаний акселерометра и оценкой силы тяжести из показаний гироскопа: $z_{g} = g - g A c c e l$
Различие между оценкой магнитного вектора из показаний гироскопа и оценкой магнитного вектора из магнитометра: $z_{m} = m G y r o - m a g R e a d i n g s$

Магнитометр правильный

Правильный магнитометр оценивает ошибку в оценке магнитного вектора и обнаруживает магнитные помехи.

Нарушение порядка магнитометра

Ошибка магнитного нарушения порядка вычисляется матричным умножением усиления Калмана, сопоставленного с магнитным вектором с сигналом ошибки:

$m E r r o r_{3 \times 1} = {((K {(10:12, :)}_{3 \times 6}) {(z_{1 \times 6})}^{T})}^{T}$

Коэффициент усиления Калмана, K, является коэффициентом усиления Калмана, вычисленным в текущей итерации.

Обнаружение магнитных помех

Магнитное заедание определяется путем проверки, что степень обнаруженного магнитного нарушения порядка меньше или равна четырём степеням ожидаемого напряжения магнитного поля:

$t f = {\begin{matrix} true \\ ложный \end{matrix} \begin{matrix} если \\ еще \end{matrix} \begin{matrix} {\sum | m E r r o r |}^{2} > (4) {(ExpectedMagneticFieldStrength)}^{2} \end{matrix}$

ExpectedMagneticFieldStrength является свойством ahrsfilter.

Уравнения Калмана

Уравнения Калмана используют оценку тяжести, полученную из показаний гироскопа, g, оценку магнитного вектора, полученную из показаний гироскопа, mGyro и наблюдения процесса ошибки, z, чтобы обновить усиление Калмана и промежуточные ковариационные матрицы. Коэффициент усиления Калмана применяется к сигналу ошибки, z, для вывода апостериорной оценки ошибки x⁺.

Модель наблюдения

Модель наблюдения преобразует наблюдаемые состояния 1 на 3, g и mGyro, в истинное состояние 6 на 12, H.

Модель наблюдения построена как:

$H_{3 \times 9} = [\begin{matrix} 0 & g_{z} & - g_{y} & 0 & - κ g_{z} & κ g_{y} & 1 & 0 & 0 & 0 & 0 & 0 \\ - g_{z} & 0 & g_{x} & κ g_{z} & 0 & - κ g_{x} & 0 & 1 & 0 & 0 & 0 & 0 \\ g_{y} & - g_{x} & 0 & - κ g_{y} & κ g_{x} & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & m_{z} & - m_{y} & 0 & - κ m_{z} & - κ m_{y} & 0 & 0 & 0 & - 1 & 0 & 0 \\ - m_{z} & 0 & m_{x} & κ m_{z} & 0 & - κ m_{x} & 0 & 0 & 0 & 0 & - 1 & 0 \\ m_{y} & - m_{x} & 0 & - κ m_{y} & κ m_{x} & 0 & 0 & 0 & 0 & 0 & 0 & - 1 \end{matrix}]$

где 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, используемой для отслеживания изменчивости измерений. Инновационная ковариационная матрица вычисляется как:

$S_{6 x 6} = R_{6 x 6} + (H_{6 x 12}) (P_{_{12 x 12}}^{-}) {(H_{6 x 12})}^{T}$

где

H - матрица модели наблюдения
P⁻ - предсказанная (априорная) оценка ковариации модели наблюдения, вычисленная в предыдущей итерации
R - ковариация шума модели наблюдения, рассчитанная как:

$R_{6 \times 6} = [\begin{matrix} a c c e l_{noise} & 0 & 0 & 0 & 0 & 0 \\ 0 & a c c e l_{noise} & 0 & 0 & 0 & 0 \\ 0 & 0 & a c c e l_{noise} & 0 & 0 & 0 \\ 0 & 0 & 0 & m a g_{noise} & 0 & 0 \\ 0 & 0 & 0 & 0 & m a g_{noise} & 0 \\ 0 & 0 & 0 & 0 & 0 & m a g_{noise} \end{matrix}]$
где

$a c c e l_{noise} = AccelerometerNoise + LinearAccelerationNoise + κ^{2} (GyroscopeDriftNoise + GyroscopeNoise)$
и

$m a g_{noise} = MagnetometerNoise + MagneticDisturbanceNoise + κ^{2} (GyroscopeDriftNoise + GyroscopeNoise)$
Следующие свойства определяют отклонение шума модели наблюдения:

Обновление оценки ошибки ковариации

Ковариация оценки ошибки является матрицей 12 на 12, используемой для отслеживания изменчивости состояния.

Ковариационная матрица оценки ошибки обновляется следующим образом:

$P_{_{12 \times 12}}^{+} = P_{_{12 \times 12}}^{-} - (K_{12 \times 6}) (H_{6 \times 12}) (P_{_{12 \times 12}}^{-})$

где K - коэффициент усиления Калмана, H - матрица измерений и P⁻ - ковариация оценки ошибки, вычисленная во время предыдущей итерации.

Предсказание оценки ошибки ковариации

Ковариация оценки ошибки является матрицей 12 на 12, используемой для отслеживания изменчивости состояния. Априорная оценка ошибки, ковариация, P⁻, устанавливается в ковариацию шума процесса, Q, определенную во время предыдущей итерации. Q вычисляется как функция ковариации оценки апостериорной ошибки, P⁺. При вычислении Q принято, что условия перекрестной корреляции незначительны по сравнению с терминами автокорреляции и равны нулю:

$Q = [\begin{matrix} P^{+} (1) + κ^{2} P^{+} (40) + β + η & 0 & 0 & - κ (P^{+} (40) + β) & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & P^{+} (14) + κ^{2} P^{+} (53) + β + η & 0 & 0 & - κ (P^{+} (53) + β) & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & P^{+} (27) + κ^{2} P^{+} (66) + β + η & 0 & 0 & - κ (P^{+} (66) + β) & 0 & 0 & 0 & 0 & 0 & 0 \\ - κ (P^{+} (40) + β) & 0 & 0 & P^{+} (40) + β & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & - κ (P^{+} (53) + β) & 0 & 0 & P^{+} (53) + β & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & - κ (P^{+} (66) + β) & 0 & 0 & P^{+} (66) + β & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (79) + ξ & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (92) + ξ & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (105) + ξ & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & σ^{2} P^{+} (118) + γ & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & σ^{2} P^{+} (131) + γ & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & σ^{2} P^{+} (144) + γ \end{matrix}]$

где

P⁺ - обновленная (a posteriori) оценка ошибки ковариация
κ -- DecimationFactor/SampleRate
β -- GyroscopeDriftNoise
η -- GyroscopeNoise
ν -- LinearAccelerationDecayFactor
ξ -- LinearAccelerationNoise
σ -- MagneticDisturbanceDecayFactor
γ -- MagneticDisturbanceNoise

Для вывода терминов матрицы ошибок процесса смотрите раздел 10.1 документа [1].

Калманское усиление

Матрица усиления Калмана является матрицей 12 на 6, используемой для взвешивания нововведения. В этом алгоритме нововведение интерпретируется как процесс ошибки, z.

Матрица усиления Калмана построена как:

$K_{12 \times 6} = (P_{_{12 \times 12}}^{-}) {(H_{6 \times 12})}^{T} {({(S_{6 \times 6})}^{T})}^{- 1}$

где

P⁻ -- предсказанная ковариация ошибок
H -- модель наблюдения
S -- инновационная ковариация

Обновление апостериорной ошибки

Апостериорная оценка ошибки определяется путем объединения матрицы усиления Калмана с ошибкой в оценках вектора гравитации и магнитного вектора:

$x_{12 \times 1} = (K_{12 \times 6}) {(z_{1 \times 6})}^{T}$

Если в текущей итерации обнаруживается магнитное глушение, сигнал ошибки магнитного вектора игнорируется, и оценка апостериорной ошибки вычисляется как:

$x_{9 \times 1} = (K (1:9, 1:3) {(z_{g})}^{T}$

Правильный

Оценка ориентации

Оценка ориентации обновляется путем умножения предыдущей оценки на ошибку:

$q^{+} = (q^{-}) (θ^{+})$

Оценка линейного ускорения

Линейная оценка ускорения обновляется путем распада линейной оценки ускорения из предыдущей итерации и вычитания ошибки:

$l i n A c c e l P r i o r = (l i n A c c e l P r i o r_{k - 1}) ν - b^{+}$

где

ν -- LinearAccelerationDecayFactor

Оценка смещения гироскопа

Оценка смещения гироскопа обновлена, вычтя ошибку смещения гироскопа из смещения гироскопа от предыдущей итерации:

$g y r o O f f s e t = g y r o O f f s e t_{k - 1} - a^{+}$

Вычисление скорости вращения

Чтобы оценить скорость вращения, система координат gyroReadings усредняются, и вычисленное в предыдущей итерации смещение гироскопа вычитается:

$a n g u l a r V e l o c i t y_{1 \times 3} = \frac{\sum g y r o R e a d i n g s_{N \times 3}}{N} - g y r o O f f s e t_{1 \times 3}$

где N - коэффициент десятикратного уменьшения, заданный DecimationFactor свойство.

Оценка смещения гироскопа инициализируется в нули для первой итерации.

Обновление магнитного вектора

Если магнитное глушение не было обнаружено в текущей итерации, оценка магнитного вектора, m, обновляется с помощью a posteriori ошибки магнитного нарушения порядка и a posteriori ориентации.

Ошибка магнитного нарушения порядка преобразуется в навигационную систему координат:

$m E r r o r N E D_{1 \times 3} = {({(r P o s t_{3 \times 3})}^{T} {(m E r r o r_{1 \times 3})}^{T})}^{T}$

Ошибка магнитного нарушения порядка в навигационной системе координат вычитается из предыдущей оценки магнитного вектора и затем интерпретируется как наклон:

$Μ = m - m E r r o r N E D$

$i n c l i n a t i o n = atan2 (Μ (3), Μ (1))$

Наклон преобразуется в оценку магнитного вектора с ограничениями для следующей итерации:

$\begin{array}{l} m (1) = (ExpectedMagneticFieldStrength) (\cos (i n c l i n a t i o n)) \\ m (2) = 0 \\ m (3) = (ExpectedMagneticFieldStrength) (\sin (i n c l i n a t i o n)) \end{array}$

ExpectedMagneticFieldStrength является свойством ahrsfilter.

Ссылки

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

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

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

Указания и ограничения по применению:

Смотрите Системные объекты в Генерации кода MATLAB (MATLAB Coder).

См. также

ecompass | gpsSensor | imufilter | imuSensor

Введенный в R2018b

Документация

ahrsfilter

Описание

Создание

Синтаксис

Описание

Свойства

SampleRate - Входная частота выборки данных датчика (Гц) 100 (по умолчанию) | положительная скалярная величина

DecimationFactor - Коэффициент десятикратного уменьшения 1 (по умолчанию) | положительное целое число

AccelerometerNoise - Отклонение шума сигнала акселерометра ((м/с2)2) 0.00019247 (по умолчанию) | положительный действительный скаляр

MagnetometerNoise - Отклонение шума сигнала магнитометра (мкТ2) 0.1 (по умолчанию) | положительный действительный скаляр

GyroscopeNoise - Отклонение шума сигнала гироскопа ((рад/с)2) 9.1385e-5 (по умолчанию) | положительный действительный скаляр

GyroscopeDriftNoise - Отклонение смещения гироскопа ((рад/с)2) 3.0462e-13 (по умолчанию) | положительный действительный скаляр

LinearAccelerationNoise - Отклонение линейного шума ускорения (м/с2)2 0.0096236 (по умолчанию) | положительный действительный скаляр

LinearAccelerationDecayFactor - Коэффициент затухания для дрейфа линейного ускорения 0.5 (по умолчанию) | скаляром в области значений [0,1)

MagneticDisturbanceNoise - Отклонение магнитного нарушения порядка шума (мкТ2) 0.5 (по умолчанию) | действительная конечная положительная скалярная величина

MagneticDisturbanceDecayFactor - Коэффициент распада для магнитного нарушения порядка 0.5 (по умолчанию) | положительная скалярная величина в области значений [0,1]

InitialProcessNoise - Ковариационная матрица для технологического шума Матрица 12 на 12

ExpectedMagneticFieldStrength - Ожидаемая оценка напряженности магнитного поля (мкТ) 50 (по умолчанию) | реальная положительная скалярная величина

OrientationFormat - Формат выходной ориентации 'quaternion' (по умолчанию) | 'Rotation matrix'

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

Синтаксис

Описание

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

accelReadings - Показания акселерометра в системе координат тела датчика (м/с2) N -by-3 матрица

gyroReadings - Показания гироскопа в системе координат тела датчика (рад/с) N -by-3 матрица

magReadings - Показания магнитометра в системе координат тела датчика (мкТ) N -by-3 матрица

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

angularVelocity - Скорость вращения в системе координат тела датчика (рад/с) M массив -by-3 (по умолчанию

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

Характерно для ahrsfilter

Общий для всех системных объектов

Примеры

Оцените ориентацию с помощью ahrsfilter

Симулируйте магнитные помехи на ahrsFilter

Тряска трека 9-Axis БИНС

Алгоритмы

Подробный обзор

Модель

Модель ошибки

Магнитометр правильный

Уравнения Калмана

Правильный

Вычисление скорости вращения

Обновление магнитного вектора

Ссылки

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

Генерация кода C/C + + Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

См. также

Документация по Navigation Toolbox

Поддержка

`SampleRate` - Входная частота выборки данных датчика (Гц)
`100` (по умолчанию) | положительная скалярная величина

`DecimationFactor` - Коэффициент десятикратного уменьшения
`1` (по умолчанию) | положительное целое число

`AccelerometerNoise` - Отклонение шума сигнала акселерометра ((м/с²)²)
`0.00019247` (по умолчанию) | положительный действительный скаляр

`MagnetometerNoise` - Отклонение шума сигнала магнитометра (мкТ²)
`0.1` (по умолчанию) | положительный действительный скаляр

`GyroscopeNoise` - Отклонение шума сигнала гироскопа ((рад/с)²)
`9.1385e-5` (по умолчанию) | положительный действительный скаляр

`GyroscopeDriftNoise` - Отклонение смещения гироскопа ((рад/с)²)
`3.0462e-13` (по умолчанию) | положительный действительный скаляр

`LinearAccelerationNoise` - Отклонение линейного шума ускорения (м/с²)²
`0.0096236` (по умолчанию) | положительный действительный скаляр

`LinearAccelerationDecayFactor` - Коэффициент затухания для дрейфа линейного ускорения
`0.5` (по умолчанию) | скаляром в области значений [0,1)

`MagneticDisturbanceNoise` - Отклонение магнитного нарушения порядка шума (мкТ²)
`0.5` (по умолчанию) | действительная конечная положительная скалярная величина

`MagneticDisturbanceDecayFactor` - Коэффициент распада для магнитного нарушения порядка
`0.5` (по умолчанию) | положительная скалярная величина в области значений [0,1]

`InitialProcessNoise` - Ковариационная матрица для технологического шума
Матрица 12 на 12

`ExpectedMagneticFieldStrength` - Ожидаемая оценка напряженности магнитного поля (мкТ)
`50` (по умолчанию) | реальная положительная скалярная величина

`OrientationFormat` - Формат выходной ориентации
`'quaternion'` (по умолчанию) | `'Rotation matrix'`

`accelReadings` - Показания акселерометра в системе координат тела датчика (м/с²)
N -by-3 матрица

`gyroReadings` - Показания гироскопа в системе координат тела датчика (рад/с)
N -by-3 матрица

`magReadings` - Показания магнитометра в системе координат тела датчика (мкТ)
N -by-3 матрица

`angularVelocity` - Скорость вращения в системе координат тела датчика (рад/с)
M массив -by-3 (по умолчанию

Характерно для `ahrsfilter`

Оцените ориентацию с помощью `ahrsfilter`

Симулируйте магнитные помехи на `ahrsFilter`

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®