ahrsfilter

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

Описание

ahrsfilter Система object™ плавит акселерометр, магнитометр и данные о датчике гироскопа, чтобы оценить ориентацию устройства.

Оценить ориентацию устройства:

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

Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты?

Создание

Синтаксис

FUSE = ahrsfilter

FUSE = ahrsfilter('ReferenceFrame',RF)

FUSE = ahrsfilter(___,Name,Value)

Описание

пример

FUSE = ahrsfilter возвращает косвенный Системный объект Фильтра Калмана, FUSE, для cочетания датчиков акселерометра, гироскопа и данных о магнитометре, чтобы оценить ориентацию устройства и скорость вращения. Фильтр использует вектор состояния с 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` — Отклонение акселерометра сигнализирует о шуме ((m/s²)²)
0.00019247 (значение по умолчанию) | положительный действительный скаляр

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

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

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

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

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

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

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

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

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

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

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

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

Отклонение смещения гироскопа дрейфует в (rad/s)²В виде положительного действительного скаляра.

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

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

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

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

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

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

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

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

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

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

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

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

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

Типы данных: 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` — Ожидаемая оценка силы магнитного поля (μT)
50 (значение по умолчанию) | действительная положительная скалярная величина

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

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

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

`OrientationFormat` — Выведите формат ориентации
`'quaternion'` (значение по умолчанию) | `'Rotation matrix'`

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

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

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

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

Синтаксис

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

Описание

пример

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

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

развернуть все

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

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

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

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

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

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

`magReadings` — Показания магнитометра в системе координат корпуса датчика (µT)
N-by-3 матрица

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

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

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

развернуть все

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

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

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

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

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

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

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

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

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

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

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

release(obj)

развернуть все

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

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

Характерный для всех системных объектов

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

Примеры

свернуть все

Оцените ориентацию Используя `ahrsfilter`

Скрипт Open 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 object. The axes object with title Orientation Estimate contains 3 objects of type line. These objects represent z-axis, y-axis, x-axis.

Симулируйте магнитный затор на `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')

Отследите дрожащий IMU с 9 осями

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

Этот пример использует 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} = [\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 измеряется в системе координат датчика, в µT, во время 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⁺ – обновленный (по опыту) оценивают ковариацию

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

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

Algorithm Flow Chart

Перед первой итерацией, accelReadings, gyroReadings, и magReadings входные параметры разделяются на блоки в DecimationFactor- 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⁺ – обновленный (по опыту) ошибочная оценочная ковариация
κ – 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] Cочетание датчиков С открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., Х.Дж. Линдж, C.T.M. Baten и П.Х. Велтинк. "Компенсация Магнитных Воздействий Улучшает Инерционное и Магнитное Обнаружение Ориентации Сегмента Человеческого тела". Транзакции IEEE в Нейронных Системах и Разработке Реабилитации. Издание 13. Выпуск 3, 2005, стр 395-405.

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

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

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

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

Темы

Определите ориентацию Используя инерционные датчики

Введенный в R2018b

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

ahrsfilter

Описание

Создание

Синтаксис

Описание

Свойства

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

DecimationFactor — Фактор децимации1 (значение по умолчанию) | положительное целое число

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

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

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

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

LinearAccelerationNoise — Отклонение линейного ускоряющего шума (m/s2)20.0096236 (значение по умолчанию) | положительный действительный скаляр

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

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

MagneticDisturbanceDecayFactor — Фактор затухания для магнитного воздействия0.5 (значение по умолчанию) | положительная скалярная величина в области значений [0,1]

InitialProcessNoise — Ковариационная матрица для шума процесса 12 12 матрица

ExpectedMagneticFieldStrength — Ожидаемая оценка силы магнитного поля (μT)50 (значение по умолчанию) | действительная положительная скалярная величина

OrientationFormat — Выведите формат ориентации 'quaternion' (значение по умолчанию) | 'Rotation matrix'

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

Синтаксис

Описание

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

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

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

magReadings — Показания магнитометра в системе координат корпуса датчика (µT) N-by-3 матрица

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

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

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

Характерный для ahrsfilter

Характерный для всех системных объектов

Примеры

Оцените ориентацию Используя ahrsfilter

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

Отследите дрожащий IMU с 9 осями

Алгоритмы

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

Модель

Ошибочная модель

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

Кальман Экуэйшнс

Правильный

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

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

Ссылки

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

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

Смотрите также

Темы

Документация Sensor Fusion and Tracking Toolbox

Поддержка

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

`DecimationFactor` — Фактор децимации
1 (значение по умолчанию) | положительное целое число

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

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

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

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

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

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

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

`MagneticDisturbanceDecayFactor` — Фактор затухания для магнитного воздействия
0.5 (значение по умолчанию) | положительная скалярная величина в области значений [0,1]

`InitialProcessNoise` — Ковариационная матрица для шума процесса
12 12 матрица

`ExpectedMagneticFieldStrength` — Ожидаемая оценка силы магнитного поля (μT)
50 (значение по умолчанию) | действительная положительная скалярная величина

`OrientationFormat` — Выведите формат ориентации
`'quaternion'` (значение по умолчанию) | `'Rotation matrix'`

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

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

`magReadings` — Показания магнитометра в системе координат корпуса датчика (µT)
N-by-3 матрица

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

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

Оцените ориентацию Используя `ahrsfilter`

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

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