Ориентация от показаний акселерометра, гироскопа и магнитометра
The ahrsfilter
Система object™ предохраняет данные акселерометра, магнитометра и гироскопа для оценки ориентации устройства.
Для оценки ориентации устройства:
Создайте ahrsfilter
Объекту и установите его свойства.
Вызывайте объект с аргументами, как будто это функция.
Дополнительные сведения о работе системных объектов см. в разделе «Что такое системные объекты?».
возвращает косвенный фильтр Калмана Системного объекта, FUSE
= ahrsfilterFUSE
для слияния датчика акселерометра, гироскопа и данных магнитометра для оценки ориентации и скорости вращения устройства. Фильтр использует вектор состояния с 12 элементами, чтобы отследить ошибку расчета для ориентации, смещения гироскопа, линейного ускорения и магнитного нарушения порядка.
возвращает Системный объект ahrsfilter, которая запирает данные акселерометра, гироскопа и магнитометра, чтобы оценить ориентацию устройства относительно систему координат FUSE
= ahrsfilter('ReferenceFrame'
,RF
)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
- Отклонение шума сигнала акселерометра ((м/с2)2)0.00019247
(по умолчанию) | положительный действительный скалярОтклонение шума сигнала акселерометра в (м/с2)2, заданный как положительный действительный скаляр.
Настраиваемый: Да
Типы данных: single
| double
MagnetometerNoise
- Отклонение шума сигнала магнитометра (мкТ2)0.1
(по умолчанию) | положительный действительный скалярОтклонение шума сигнала магнитометра в мкТ2, заданный как положительный действительный скаляр.
Настраиваемый: Да
Типы данных: single
| double
GyroscopeNoise
- Отклонение шума сигнала гироскопа ((рад/с)2)9.1385e-5
(по умолчанию) | положительный действительный скалярОтклонение шума сигнала гироскопа в (рад/с)2, заданный как положительный действительный скаляр.
Настраиваемый: Да
Типы данных: single
| double
GyroscopeDriftNoise
- Отклонение смещения гироскопа ((рад/с)2)3.0462e-13
(по умолчанию) | положительный действительный скалярОтклонение дрейфа смещения гироскопа в (рад/с)2, заданный как положительный действительный скаляр.
Настраиваемый: Да
Типы данных: single
| double
LinearAccelerationNoise
- Отклонение линейного шума ускорения (м/с2)20.0096236
(по умолчанию) | положительный действительный скалярОтклонение линейного шума ускорения в (м/с2)2, заданный как положительный действительный скаляр. Линейное ускорение моделируется как процесс белого шума с фильтрацией методом lowpass.
Настраиваемый: Да
Типы данных: single
| double
LinearAccelerationDecayFactor
- Коэффициент затухания для дрейфа линейного ускорения0.5
(по умолчанию) | скаляром в области значений [0,1)Коэффициент распада для дрейфа линейного ускорения, заданный как скаляр в области значений [0,1). Если линейное ускорение изменяется быстро, задайте LinearAcclerationDecayFactor
к более низкому значению. Если линейное ускорение изменяется медленно, установите LinearAcclerationDecayFactor
к более высокому значению. Дрейф линейного ускорения моделируется как процесс белого шума с фильтрацией методом lowpass.
Настраиваемый: Да
Типы данных: single
| double
MagneticDisturbanceNoise
- Отклонение магнитного нарушения порядка шума (мкТ2)0.5
(по умолчанию) | действительная конечная положительная скалярная величинаОтклонение магнитного нарушения порядка шума в мкТ2, заданный как действительная конечная положительная скалярная величина.
Настраиваемый: Да
Типы данных: single
| double
MagneticDisturbanceDecayFactor
- Коэффициент распада для магнитного нарушения порядка0.5
(по умолчанию) | положительная скалярная величина в области значений [0,1]Коэффициент распада для магнитных нарушений порядка, заданный как положительная скалярная величина в область значений [0,1]. Магнитное нарушение порядка моделируется как марковский процесс первого порядка.
Настраиваемый: Да
Типы данных: single
| double
InitialProcessNoise
- Ковариационная матрица для технологического шумаКовариационная матрица для технологического шума, заданная как матрица 12 на 12. Значение по умолчанию является:
Columns 1 through 6 0.000006092348396 0 0 0 0 0 0 0.000006092348396 0 0 0 0 0 0 0.000006092348396 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0.000076154354947 0 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 Columns 7 through 12 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 0.009623610000000 0 0 0 0 0 0 0.009623610000000 0 0 0 0 0 0 0.600000000000000 0 0 0 0 0 0 0.600000000000000 0 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
[
fuses accelerometer, гироскоп и данные магнитометра для вычисления ориентации и скорости вращения измерений. Алгоритм принимает, что устройство стационарно перед первым вызовом.orientation
,angularVelocity
] = FUSE(accelReadings
,gyroReadings
,magReadings
)
accelReadings
- Показания акселерометра в системе координат тела датчика (м/с2)Показания акселерометра в системе координат тела датчика в м/с2, заданный как матрица N -by-3. N - количество выборок и три столбца accelReadings
представление измерений [x
y z
]. Показания акселерометра приняты соответствующими скорости дискретизации, заданной свойством SampleRate.
Типы данных: single
| double
gyroReadings
- Показания гироскопа в системе координат тела датчика (рад/с)Показания гироскопа в системе координат тела датчика в рад/с, заданные как матрица N-на-3. N - количество выборок и три столбца gyroReadings
представление измерений [x
y z
]. Показания гироскопа приняты соответствующими скорости дискретизации, заданной свойством SampleRate.
Типы данных: single
| double
magReadings
- Показания магнитометра в системе координат тела датчика (мкТ)Показания магнитометра в системе координат тела датчика в мкт, заданные как матрица N-на-3. N - количество выборок и три столбца magReadings
представление измерений [x
y z
]. Показания магнитометра приняты соответствующими скорости дискретизации, заданной свойством SampleRate.
Типы данных: single
| double
orientation
- Ориентация, которая вращает величины от локальной системы координат навигации к системе координат тела датчикаОриентация, которая может поворачивать величины из локальной системы координат навигации в систему координат тела, возвращаемую как кватернионы или массив. Размер и тип orientation
зависит от того, задано ли для свойства OrientationFormat значение 'quaternion'
или 'Rotation matrix'
:
'quaternion'
- выходы являются вектором кватернионов M-на-1 с таким же базовым типом данных, как и входы
'Rotation matrix'
- выводится массив вращений M 3 на 3 байта, матриц того же типа, что и входы
Количество входа отсчетов, N и свойство DecimationFactor определяют M.
Можно использовать orientation
в rotateframe
функция для поворота величин из локальной системы навигации в систему координат тела датчика.
Типы данных: quaternion
| single
| double
angularVelocity
- Скорость вращения в системе координат тела датчика (рад/с) Скорость вращения с смещением гироскопа, удаленным в системе координат тела датчика в рада/с, возвращается как массив M на 3. Количество входа образцов, N и DecimationFactor
свойство определяет M.
Типы данных: single
| double
Чтобы использовать функцию объекта, задайте системный объект в качестве первого входного параметра. Например, чтобы освободить системные ресурсы системного объекта с именем obj
, используйте следующий синтаксис:
release(obj)
ahrsfilter
tune | Настройка ahrsfilter параметры для уменьшения ошибки расчета |
ahrsfilter
Загрузите rpy_9axis
файл, содержащий зарегистрированные данные акселерометра, гироскопа и датчика магнитометра от устройства, колеблющегося с тангажом (вокруг оси Y), затем рыскание (вокруг оси Z), а затем крен (вокруг оси X). Файл также содержит частоту дискретизации записи.
load 'rpy_9axis' sensorData Fs accelerometerReadings = sensorData.Acceleration; gyroscopeReadings = sensorData.AngularVelocity; magnetometerReadings = sensorData.MagneticField;
Создайте ahrsfilter
Системная object™ с SampleRate
установите значение скорости дискретизации данных о датчике. Задайте коэффициент десятикратного уменьшения два, чтобы уменьшить вычислительные затраты алгоритма.
decim = 2; fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);
Передайте показания акселерометра, показания гироскопа и показания магнитометра в ahrsfilter
объект, fuse
для вывода оценки ориентации тела датчика с течением времени. По умолчанию ориентация выводится как вектор кватернионов.
q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);
Ориентация определяется угловым перемещением, необходимым для поворота родительской системы координат к дочерней системе координат. Постройте график ориентации углов Эйлера в степенях с течением времени.
ahrsfilter
правильно оценивает изменение ориентации с течением времени, включая исходную ориентацию, обращенную к югу.
time = (0:decim:size(accelerometerReadings,1)-1)/Fs; plot(time,eulerd(q,'ZYX','frame')) title('Orientation Estimate') legend('z-axis', 'y-axis', 'x-axis') ylabel('Rotation (degrees)')
ahrsFilter
Этот пример показывает, как эффективность ahrsfilter
На object™ системы влияет магнитное заедание.
Загрузка StationaryIMUReadings
, который содержит показания акселерометра, магнитометра и гироскопа от стационарного БИНС.
load 'StationaryIMUReadings.mat' accelReadings magReadings gyroReadings SampleRate numSamples = size(accelReadings,1);
The ahrsfilter
использует силу магнитного поля, чтобы стабилизировать его ориентацию относительно предполагаемого постоянного магнитного поля Земли. Однако существует много естественных и техногенных объектов, которые формируют магнитные поля и могут запутать алгоритм. Для учета наличия переходных магнитных полей можно задать MagneticDisturbanceNoise
свойство на ahrsfilter
объект.
Создайте ahrsfilter
объект с коэффициентом десятикратного уменьшения, установленным на 2, и обратите внимание на ожидаемую напряженность магнитного поля по умолчанию.
decim = 2; FUSE = ahrsfilter('SampleRate',SampleRate,'DecimationFactor',decim);
Предохраните показания БИНС с помощью фильтра системы ссылки ориентации и курса (AHRS), а затем визуализируйте ориентацию корпуса датчика с течением времени. Ориентация колеблется в начале и стабилизируется примерно через 60 секунд.
orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); time = (0:decim:(numSamples-1))'/SampleRate; figure(1) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') legend('z-axis','y-axis','x-axis') title('Filtered IMU Data')
Мимическое магнитное заедание путем добавления переходного, сильного магнитного поля к магнитному полю, записанному в magReadings
. Визуализируйте помехи магнитного поля.
jamStrength = [10,5,2]; startStop = (50*SampleRate):(150*SampleRate); jam = zeros(size(magReadings)); jam(startStop,:) = jamStrength.*ones(numel(startStop),3); magReadings = magReadings + jam; figure(2) plot(time,magReadings(1:decim:end,:)) xlabel('Time (s)') ylabel('Magnetic Field Strength (\mu T)') title('Simulated Magnetic Field with Jamming') legend('z-axis','y-axis','x-axis')
Запустите симуляцию снова, используя magReadings
с магнитными помехами. Постройте график результатов и отметьте снижение эффективности в оценке ориентации.
reset(FUSE) orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); figure(3) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') legend('z-axis','y-axis','x-axis') title('Filtered IMU Data with Magnetic Disturbance and Default Properties')
Магнитные помехи были неверно истолкованы фильтром AHRS, и ориентация тела датчика была неправильно оценена. Можно компенсировать глушение, увеличивая MagneticDisturbanceNoise
свойство. Увеличение MagneticDisturbanceNoise
свойство увеличивает принятую область значений шума для магнитных нарушений порядка, и весь сигнал магнитометра взвешивается меньше в базовом алгоритме слияния ahrsfilter
.
Установите MagneticDisturbanceNoise
на 200
и запустите симуляцию еще раз.
Результат оценки ориентации из ahrsfilter
является более точным и менее затронуто магнитным переходным процессом. Однако, поскольку сигнал магнитометра взвешивается меньше в базовом алгоритме слияния, алгоритму может потребоваться больше времени, чтобы перезапустить.
reset(FUSE) FUSE.MagneticDisturbanceNoise = 20; orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); figure(4) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') legend('z-axis','y-axis','x-axis') title('Filtered IMU Data with Magnetic Disturbance and Modified Properties')
Этот пример использует ahrsfilter
Система object™, чтобы сплавить 9-осевые данные БИНС от тела датчика, который встряхивается. Постройте график кватернионного расстояния между объектом и его конечным положением покоя, чтобы визуализировать эффективность и как быстро фильтр сходится к правильному положению покоя. Затем настройте параметры ahrsfilter
так что фильтр сходится быстрее в положение покоя «основная истина».
Загрузка IMUReadingsShaken
в текущую рабочую область. Эти данные были записаны из БИНС, который был встряхнут затем помещен в положение покоя. Визуализируйте ускорение, магнитное поле и скорость вращения, как записано датчиками.
load 'IMUReadingsShaken' accelReadings gyroReadings magReadings SampleRate numSamples = size(accelReadings,1); time = (0:(numSamples-1))'/SampleRate; figure(1) subplot(3,1,1) plot(time,accelReadings) title('Accelerometer Reading') ylabel('Acceleration (m/s^2)') subplot(3,1,2) plot(time,magReadings) title('Magnetometer Reading') ylabel('Magnetic Field (\muT)') subplot(3,1,3) plot(time,gyroReadings) title('Gyroscope Reading') ylabel('Angular Velocity (rad/s)') xlabel('Time (s)')
Создайте ahrsfilter
и затем сплавьте данные БИНС, чтобы определить ориентацию. Ориентация возвращается как вектор кватернионов; преобразуйте кватернионы в углы Эйлера в степенях. Визуализируйте ориентацию тела датчика с течением времени путем построения графика углов Эйлера, необходимых на каждом временном шаге, чтобы повернуть глобальную систему координат к системе координат тела датчика.
fuse = ahrsfilter('SampleRate',SampleRate); orientation = fuse(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,'ZYX','frame'); figure(2) plot(time,orientationEulerAngles(:,1), ... time,orientationEulerAngles(:,2), ... time,orientationEulerAngles(:,3)) xlabel('Time (s)') ylabel('Rotation (degrees)') title('Orientation over Time') legend('Rotation around z-axis', ... 'Rotation around y-axis', ... 'Rotation around x-axis')
В записи БИНС тряска прекращается примерно через шесть секунд. Определите ориентацию покоя, чтобы вы могли охарактеризовать, насколько быстро ahrsfilter
сходится.
Чтобы определить ориентацию покоя, вычислите средние значения магнитного поля и ускорения за последние четыре секунды и затем используйте ecompass
функция для сплавления данных.
Визуализируйте кватернионное расстояние от положения покоя с течением времени.
restingOrientation = ecompass(mean(accelReadings(6*SampleRate:end,:)), ... mean(magReadings(6*SampleRate:end,:))); figure(3) plot(time,rad2deg(dist(restingOrientation,orientation))) hold on xlabel('Time (s)') ylabel('Quaternion Distance (degrees)')
Измените значение по умолчанию ahrsfilter
свойства так, что фильтр сходится к гравитации быстрее. Увеличьте GyroscopeDriftNoise
на 1e-2
и уменьшите LinearAccelerationNoise
на 1e-4
. Это инструктирует ahrsfilter
алгоритм, чтобы взвесить данные гироскопа меньше и данные акселерометра больше. Потому что accelerometer
данные обеспечивают стабилизирующий и последовательный вектор тяжести, результирующая ориентация сходится быстрее.
Сбросьте фильтр, сплавьте данные и постройте график результатов.
fuse.LinearAccelerationNoise = 1e-4; fuse.GyroscopeDriftNoise = 1e-2; reset(fuse) orientation = fuse(accelReadings,gyroReadings,magReadings); figure(3) plot(time,rad2deg(dist(restingOrientation,orientation))) legend('Default AHRS Filter','Tuned AHRS Filter')
Примечание: Следующий алгоритм применяется только к опорной системе координат NED.
The ahrsfilter
использует девятиосную структуру фильтра Калмана, описанную в [1]. Алгоритм пытается отследить ошибки в ориентации, смещении гироскопа, линейном ускорении и магнитном нарушении порядка, чтобы вывести конечную ориентацию и скорость вращения. Вместо непосредственного отслеживания ориентации косвенный фильтр Калмана моделирует процесс ошибки, x, с рекурсивным обновлением:
где x k является вектором 12 на 1, состоящим из:
θk -- вектор ошибки ориентации 3 на 1, в степенях, в момент времени k
bk -- гироскоп 3 на 1 нуле вектор смещения угловой скорости, в град/с, в момент времени k
ak -- вектор ошибки ускорения 3 на 1, измеренный в системе координат датчика, в g, в момент времени k
dk -- 3 на 1 вектор ошибки магнитного нарушения порядка, измеренный в системе координат датчика, в мкТ, в момент времени k
и где w k является аддитивным вектором шума 12 на 1, и F k является моделью перехода состояния.
Поскольку x k задан как процесс ошибки, априорная оценка всегда равна нулю, и, следовательно, модель перехода состояния, F k, равна нулю. Это понимание приводит к следующему уменьшению стандартных уравнений Калмана:
Стандартные уравнения Калмана:
Уравнения Калмана, используемые в этом алгоритме:
где:
xk− - прогнозируемая (априори) оценка состояния; процесс ошибки
Pk− - предсказанная (априорная) оценка ковариации
yk -- инновации
Sk -- инновационная ковариация
Kk -- коэффициент усиления Калмана
xk+ - обновленная (апостериорная) оценка состояния
Pk+ - обновленная (a posteriori) оценка ковариации
k представляет итерацию, надстрочный индекс + представляет апостериорную оценку и надстрочный индекс − представляет априорную оценку.
Графические и последующие шаги описывают итерацию на основе одного кадра через алгоритм.
Перед первой итерацией accelReadings
, gyroReadings
, и magReadings
входы шунтируются в DecimationFactor
-by-3 системы координат. Для каждого фрагмента алгоритм использует самые актуальные показания акселерометра и магнитометра, соответствующие фрагменту показаний гироскопа.
Пройдите по алгоритму для объяснения каждого этапа подробного обзора.
Алгоритм моделирует ускорение и угловое изменение как линейные процессы.
Ориентация для текущей системы координат предсказывается путем первой оценки углового изменения по сравнению с предыдущей системой координат:
где N - коэффициент десятикратного уменьшения, заданный свойством DecimationFactor, а fs - частота дискретизации, заданная свойством SampleRate.
Угловое изменение преобразуется в кватернионы с помощью rotvec
quaternion
синтаксис конструкции:
Предыдущая оценка ориентации обновляется путем поворота ее на И Q:
Во время первой итерации оценка ориентации, q−, инициализируется ecompass
.
Вектор гравитации интерпретируется как третий столбец кватерниона, q−, в матричном виде вращения:
См. [1] для объяснения того, почему третий столбец rPrior может быть интерпретирован как вектор гравитации.
Вторую оценку вектора гравитации производят путем вычитания из показаний акселерометра истекшей оценки линейного ускорения предыдущей итерации:
Магнитный вектор Земли оценивается вращением оценки магнитного вектора из предыдущей итерации априорной оценкой ориентации в матричной форме вращения:
Модель ошибки сочетает в себе два различия:
Различие между оценкой силы тяжести из показаний акселерометра и оценкой силы тяжести из показаний гироскопа:
Различие между оценкой магнитного вектора из показаний гироскопа и оценкой магнитного вектора из магнитометра:
Правильный магнитометр оценивает ошибку в оценке магнитного вектора и обнаруживает магнитные помехи.
Ошибка магнитного нарушения порядка вычисляется матричным умножением усиления Калмана, сопоставленного с магнитным вектором с сигналом ошибки:
Коэффициент усиления Калмана, K, является коэффициентом усиления Калмана, вычисленным в текущей итерации.
Магнитное заедание определяется путем проверки, что степень обнаруженного магнитного нарушения порядка меньше или равна четырём степеням ожидаемого напряжения магнитного поля:
ExpectedMagneticFieldStrength является свойством ahrsfilter
.
Уравнения Калмана используют оценку тяжести, полученную из показаний гироскопа, g, оценку магнитного вектора, полученную из показаний гироскопа, mGyro и наблюдения процесса ошибки, z, чтобы обновить усиление Калмана и промежуточные ковариационные матрицы. Коэффициент усиления Калмана применяется к сигналу ошибки, z, для вывода апостериорной оценки ошибки x+.
Модель наблюдения преобразует наблюдаемые состояния 1 на 3, g и mGyro, в истинное состояние 6 на 12, H.
Модель наблюдения построена как:
где g x, g y и g z являются x -, y - и z - элементами вектора тяжести, оцененными из априорной ориентации, соответственно. m x, m y и m z являются x -, y - и z - элементами магнитного вектора, оцененными из априорной ориентации, соответственно. κ является константой, определяемой свойствами SampleRate и DecimationFactor: κ = DecimationFactor
/ SampleRate
.
Вывод модели наблюдения см. в разделах 7.3 и 7.4 документа [1].
Инновационная ковариация является матрицей 6 на 6, используемой для отслеживания изменчивости измерений. Инновационная ковариационная матрица вычисляется как:
где
H - матрица модели наблюдения
P− - предсказанная (априорная) оценка ковариации модели наблюдения, вычисленная в предыдущей итерации
R - ковариация шума модели наблюдения, рассчитанная как:
где
и
Следующие свойства определяют отклонение шума модели наблюдения:
Ковариация оценки ошибки является матрицей 12 на 12, используемой для отслеживания изменчивости состояния.
Ковариационная матрица оценки ошибки обновляется следующим образом:
где K - коэффициент усиления Калмана, H - матрица измерений и P− - ковариация оценки ошибки, вычисленная во время предыдущей итерации.
Ковариация оценки ошибки является матрицей 12 на 12, используемой для отслеживания изменчивости состояния. Априорная оценка ошибки, ковариация, P−, устанавливается в ковариацию шума процесса, Q, определенную во время предыдущей итерации. Q вычисляется как функция ковариации оценки апостериорной ошибки, P+. При вычислении Q принято, что условия перекрестной корреляции незначительны по сравнению с терминами автокорреляции и равны нулю:
где
P+ - обновленная (a posteriori) оценка ошибки ковариация
β -- GyroscopeDriftNoise
η -- GyroscopeNoise
Для вывода терминов матрицы ошибок процесса смотрите раздел 10.1 документа [1].
Матрица усиления Калмана является матрицей 12 на 6, используемой для взвешивания нововведения. В этом алгоритме нововведение интерпретируется как процесс ошибки, z.
Матрица усиления Калмана построена как:
где
P− -- предсказанная ковариация ошибок
H -- модель наблюдения
S -- инновационная ковариация
Апостериорная оценка ошибки определяется путем объединения матрицы усиления Калмана с ошибкой в оценках вектора гравитации и магнитного вектора:
Если в текущей итерации обнаруживается магнитное глушение, сигнал ошибки магнитного вектора игнорируется, и оценка апостериорной ошибки вычисляется как:
Оценка ориентации обновляется путем умножения предыдущей оценки на ошибку:
Линейная оценка ускорения обновляется путем распада линейной оценки ускорения из предыдущей итерации и вычитания ошибки:
где
Оценка смещения гироскопа обновлена, вычтя ошибку смещения гироскопа из смещения гироскопа от предыдущей итерации:
Чтобы оценить скорость вращения, система координат gyroReadings
усредняются, и вычисленное в предыдущей итерации смещение гироскопа вычитается:
где N - коэффициент десятикратного уменьшения, заданный DecimationFactor
свойство.
Оценка смещения гироскопа инициализируется в нули для первой итерации.
Если магнитное глушение не было обнаружено в текущей итерации, оценка магнитного вектора, m, обновляется с помощью a posteriori ошибки магнитного нарушения порядка и a posteriori ориентации.
Ошибка магнитного нарушения порядка преобразуется в навигационную систему координат:
Ошибка магнитного нарушения порядка в навигационной системе координат вычитается из предыдущей оценки магнитного вектора и затем интерпретируется как наклон:
Наклон преобразуется в оценку магнитного вектора с ограничениями для следующей итерации:
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.
Указания и ограничения по применению:
Смотрите Системные объекты в Генерации кода MATLAB (MATLAB Coder).
ecompass
| gpsSensor
| imufilter
| imuSensor
| quaternion
У вас есть измененная версия этого примера. Вы хотите открыть этот пример с вашими правками?
1. Если смысл перевода понятен, то лучше оставьте как есть и не придирайтесь к словам, синонимам и тому подобному. О вкусах не спорим.
2. Не дополняйте перевод комментариями “от себя”. В исправлении не должно появляться дополнительных смыслов и комментариев, отсутствующих в оригинале. Такие правки не получится интегрировать в алгоритме автоматического перевода.
3. Сохраняйте структуру оригинального текста - например, не разбивайте одно предложение на два.
4. Не имеет смысла однотипное исправление перевода какого-то термина во всех предложениях. Исправляйте только в одном месте. Когда Вашу правку одобрят, это исправление будет алгоритмически распространено и на другие части документации.
5. По иным вопросам, например если надо исправить заблокированное для перевода слово, обратитесь к редакторам через форму технической поддержки.