imufilter

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

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

Описание

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

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

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

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

Создание

Синтаксис

FUSE = imufilter

FUSE = imufilter(Name,Value)

Описание

пример

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

пример

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

Пример: FUSE = imufilter('SampleRate',200,'GyroscopeNoise',1e-6) создает Системный объект, FUSE, с частотой дискретизации на 200 Гц и набором шума гироскопа к 1e-6 радианам в секунду придал квадратную форму.

Свойства

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

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

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

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

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

Частота дискретизации входных данных о датчике в Гц, заданном как положительный конечный скаляр.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Ковариационная матрица для шума процесса, заданного как 9 9 матрица. Значение по умолчанию:

  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

  Columns 7 through 9

                   0                   0                   0
                   0                   0                   0
                   0                   0                   0
                   0                   0                   0
                   0                   0                   0
                   0                   0                   0
   0.009623610000000                   0                   0
                   0   0.009623610000000                   0
                   0                   0   0.009623610000000

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

`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)

Описание

пример

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

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

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

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

Показания акселерометра в системе координат корпуса датчика в ^m/s2, заданном как 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

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

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

`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 в функции rotateframe, чтобы вращать количества от глобальной системы координат до системы координат корпуса датчика.

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

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

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

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

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

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

release(obj)

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

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

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

Примеры

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

Оцените Ориентацию от данных IMU

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

Загрузите файл rpy_9axis, который содержит записанный акселерометр, гироскоп, и данные о датчике магнитометра из устройства, колеблющегося в подаче (вокруг оси Y), затем отклоняются от курса (вокруг оси z), и затем прокручиваются (вокруг оси X). Файл также содержит частоту дискретизации записи.

load 'rpy_9axis.mat' sensorData Fs

accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;

Создайте Систему imufilter object™ с набором частоты дискретизации к частоте дискретизации данных о датчике. Задайте фактор десятикратного уменьшения два, чтобы уменьшать вычислительную стоимость алгоритма.

decim = 2;
fuse = imufilter('SampleRate',Fs,'DecimationFactor',decim);

Передайте показания акселерометра и показания гироскопа к объекту imufilter, fuse, чтобы выводить оценку ориентации корпуса датчика в зависимости от времени. По умолчанию ориентация выводится как вектор кватернионов.

q = fuse(accelerometerReadings,gyroscopeReadings);

Ориентация задана угловым смещением, требуемым вращать систему координаты вышестоящего элемента к системе координаты нижестоящего элемента. Постройте ориентацию в Углах Эйлера в градусах в зависимости от времени.

Сплав imufilter правильно оценивает изменение в ориентации от принятой стоящей с севером начальной ориентации. Однако ось X устройства указывала на юг, когда зарегистрировано. Чтобы правильно оценить ориентацию относительно истинной начальной ориентации или относительно NED, используйте 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')
xlabel('Time (s)')
ylabel('Rotation (degrees)')

Образцовый наклон Используя показания гироскопа и акселерометра

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

Смоделируйте наклон IMU, который содержит акселерометр и гироскоп с помощью Системы imuSensor object™. Используйте идеальные и реалистические модели, чтобы сравнить результаты отслеживания ориентации с помощью Системного объекта imufilter.

Загрузите struct, описывающий движение наземной истины и частоту дискретизации. Struct движения описывает последовательные вращения:

отклонение от курса: 120 градусов более чем две секунды
подача: 60 градусов более чем одна секунда
список: 30 градусов по половине второго
список:-30 градусов по половине второго
подача:-60 градусов более чем одна секунда
отклонение от курса:-120 градусов более чем две секунды

На последней стадии struct движения комбинирует 1-е, 2-е, и 3-и вращения во вращение одно оси. Ускорение, угловая скорость и ориентация заданы в локальной системе координат NED.

load y120p60r30.mat motion fs
accNED = motion.Acceleration;
angVelNED = motion.AngularVelocity;
orientationNED = motion.Orientation;

numSamples = size(motion.Orientation,1);
t = (0:(numSamples-1)).'/fs;

Создайте идеальный объект датчика IMU и объект фильтра IMU по умолчанию.

IMU = imuSensor('accel-gyro','SampleRate',fs);

aFilter = imufilter('SampleRate',fs);

В цикле:

Моделируйте IMU вывод путем питания движения наземной истины объект датчика IMU.
Отфильтруйте IMU вывод с помощью объекта фильтра IMU по умолчанию.

orientation = zeros(numSamples,1,'quaternion');
for i = 1:numSamples

    [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));

    orientation(i) = aFilter(accelBody,gyroBody);

end
release(aFilter)

Стройте ориентацию в зависимости от времени.

figure(1)
plot(t,eulerd(orientation,'ZYX','frame'))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation Estimation -- Ideal IMU Data, Default IMU Filter')
legend('Z-axis','Y-axis','X-axis')

Измените свойства своего imuSensor смоделировать реальные датчики. Запустите цикл снова и стройте оценку ориентации в зависимости от времени.

IMU.Accelerometer = accelparams( ...
    'MeasurementRange',19.62, ...
    'Resolution',0.00059875, ...
    'ConstantBias',0.4905, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',0.003924, ...
    'BiasInstability',0, ...
    'TemperatureBias', [0.34335 0.34335 0.5886], ...
    'TemperatureScaleFactor',0.02);
IMU.Gyroscope = gyroparams( ...
    'MeasurementRange',4.3633, ...
    'Resolution',0.00013323, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',8.7266e-05, ...
    'TemperatureBias',0.34907, ...
    'TemperatureScaleFactor',0.02, ...
    'AccelerationBias',0.00017809, ...
    'ConstantBias',[0.3491,0.5,0]);

orientationDefault = zeros(numSamples,1,'quaternion');
for i = 1:numSamples

    [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));

    orientationDefault(i) = aFilter(accelBody,gyroBody);

end
release(aFilter)

figure(2)
plot(t,eulerd(orientationDefault,'ZYX','frame'))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation Estimation -- Realistic IMU Data, Default IMU Filter')
legend('Z-axis','Y-axis','X-axis')

Способность imufilter отследить данные наземной истины значительно уменьшается при моделировании реалистического IMU. Чтобы улучшать производительность, измените свойства своего объекта imufilter. Эти значения были определены опытным путем. Запустите цикл снова и стройте оценку ориентации в зависимости от времени.

aFilter.GyroscopeNoise          = 7.6154e-7;
aFilter.AccelerometerNoise      = 0.0015398;
aFilter.GyroscopeDriftNoise     = 3.0462e-12;
aFilter.LinearAccelerationNoise = 0.00096236;
aFilter.InitialProcessNoise     = aFilter.InitialProcessNoise*10;

orientationNondefault = zeros(numSamples,1,'quaternion');
for i = 1:numSamples
    [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));

    orientationNondefault(i) = aFilter(accelBody,gyroBody);
end
release(aFilter)

figure(3)
plot(t,eulerd(orientationNondefault,'ZYX','frame'))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation Estimation -- Realistic IMU Data, Nondefault IMU Filter')
legend('Z-axis','Y-axis','X-axis')

Чтобы определить количество улучшенной производительности измененного imufilter, постройте расстояние кватерниона между движением наземной истины и ориентацией, как возвращено imufilter со свойствами не по умолчанию и по умолчанию.

qDistDefault = rad2deg(dist(orientationNED,orientationDefault));
qDistNondefault = rad2deg(dist(orientationNED,orientationNondefault));

figure(4)
plot(t,[qDistDefault,qDistNondefault])
title('Quaternion Distance from True Orientation')
legend('Realistic IMU Data, Default IMU Filter', ...
       'Realistic IMU Data, Nondefault IMU Filter')
xlabel('Time (s)')
ylabel('Quaternion Distance (degrees)')

Удалите смещение из углового скоростного измерения

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

Этот пример показывает, как удалить смещение гироскопа из IMU использование imufilter.

Используйте kinematicTrajectory, чтобы создать траекторию с постоянной угловой скоростью приблизительно y-и оси z.

duration = 60*5;
fs = 20;
numSamples = duration * fs;

angVelBody = repmat([0,0.5,0.25],numSamples,1);
accBody = zeros(numSamples,3);

traj = kinematicTrajectory('SampleRate',fs);

[~,qNED,~,accNED,angVelNED] = traj(accBody,angVelBody);

Создайте Систему imuSensor object™, IMU, с неидеальным гироскопом. Вызовите IMU с ускорением наземной истины, угловой скоростью и ориентацией.

IMU = imuSensor('accel-gyro', ...
    'Gyroscope',gyroparams('BiasInstability',0.003,'ConstantBias',0.3), ...
    'SampleRate',fs);

[accelReadings, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);

Создайте Системный объект imufilter, fuse. Вызовите fuse со смоделированными показаниями акселерометра и показаниями гироскопа.

fuse = imufilter('SampleRate',fs);

[~,angVelBodyRecovered] = fuse(accelReadings,gyroReadingsBody);

Постройте наземную истину угловая скорость, показания гироскопа и восстановленная угловая скорость для каждой оси.

Угловая скорость, возвращенная в imufilter, компенсирует эффект смещения гироскопа в зависимости от времени и сходится к истинной угловой скорости.

time = (0:numSamples-1)'/fs;

figure(1)
plot(time,angVelBody(:,3), ...
     time,gyroReadingsBody(:,3), ...
     time,angVelBodyRecovered(:,3))
title('X-axis')
legend('True Angular Velocity', ...
       'Gyroscope Readings', ...
       'Recovered Angular Velocity')
ylabel('Angular Velocity (rad/s)')

figure(2)
plot(time,angVelBody(:,2), ...
     time,gyroReadingsBody(:,2), ...
     time,angVelBodyRecovered(:,2))
title('Y-axis')
ylabel('Angular Velocity (rad/s)')

figure(3)
plot(time,angVelBody(:,1), ...
     time,gyroReadingsBody(:,1), ...
     time,angVelBodyRecovered(:,1))
title('Z-axis')
ylabel('Angular Velocity (rad/s)')
xlabel('Time (s)')

Алгоритмы

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

imufilter использует структуру Фильтра Калмана с шестью осями, описанную в [1]. Алгоритм пытается отследить ошибки в ориентации, смещении гироскопа и линейном ускорении, чтобы вывести итоговую ориентацию и угловую скорость. Вместо того, чтобы отследить ориентацию непосредственно, косвенный Фильтр Калмана моделирует ошибочный процесс, x, с рекурсивным обновлением:

$x_{k} = [\begin{matrix} θ_{k} \\ b_{k} \\ a_{k} \end{matrix}] = F_{k} [\begin{matrix} θ_{k - 1} \\ b_{k - 1} \\ a_{k - 1} \end{matrix}] + w_{k}$

где _xk 9 1 вектор, состоящий из:

_θk – Вектор ошибок ориентации 3 на 1, в градусах, во время k
_bk – Нулевой вектор смещения уровня гироскопа 3 на 1, в градусе/с, во время k
_ak – Ускоряющий вектор ошибок 3 на 1 измеряется в кадре датчика, в g, во время k
_wk – 9 1 аддитивный шумовой вектор
_Fk – модель изменения состояния

Поскольку 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 представляет итерацию, верхний индекс ⁺ представляет по опыту оценка, и верхний индекс ⁻ представляет априорную оценку.

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

Перед первой итерацией accelReadings и входные параметры gyroReadings разделяются на блоки в 1 3 кадры и 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.

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

$Δ Q_{N \times 1} = кватернион (Δ φ_{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 с предположением, что x - ось указывает север.

Оцените силу тяжести от ориентации

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

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

Смотрите ecompass для объяснения того, почему третий столбец 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}$

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

Ошибочная модель является различием между оценкой силы тяжести от показаний акселерометра и оценкой силы тяжести от показаний гироскопа: $z = g - g A c c e l$ .

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

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

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

Модель наблюдения сопоставляет 1 3 наблюдаемое состояние, g, в 3 9 истинное состояние, H.

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

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

где _gx, _gy и _gz является x - y - и z - элементы вектора силы тяжести, оцененного от ориентации, соответственно. κ является константой, определенной свойствами SampleRate и DecimationFactor: κ = DecimationFactor/SampleRate.

Смотрите разделы 7.3 и 7.4 из [1] для деривации модели наблюдения.

Инновационная ковариация

Инновационная ковариация является 3х3 матрицей, используемой, чтобы отследить изменчивость в измерениях. Инновационная ковариационная матрица вычисляется как:

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

где

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

$R_{3 \times 3} = (λ + ξ + κ (β + η)) [\begin{matrix} 100010001 \end{matrix}] .$
Следующие свойства задают модель наблюдения шумовое отклонение:
- κ – (DecimationFactor/SampleRate) ²
- β – GyroscopeDriftNoise
- η – GyroscopeNoise
- λ – AccelerometerNoise
- ξ – LinearAccelerationNoise

Обновите ошибочную оценочную ковариацию

Ошибочная оценочная ковариация 9 9, матрица раньше отслеживала изменчивость в состоянии.

Ошибочная оценочная ковариационная матрица обновляется как:

$P_{_{9 \times 9}}^{+} = P_{_{9 \times 9}}^{-} - (K_{9 \times 3}) (H_{3 \times 9}) (P_{_{9 \times 9}}^{-})$

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

Предскажите ошибочную оценочную ковариацию

Ошибочная оценочная ковариация 9 9, матрица раньше отслеживала изменчивость в состоянии. Априорная ошибочная оценочная ковариация, ^P−, установлена в ковариацию шума процесса, Q, определенный во время предыдущей итерации. Q вычисляется как функция по опыту ошибочная оценочная ковариация, ^P+. При вычислении Q условия взаимной корреляции приняты, чтобы быть незначительными по сравнению с условиями автокорреляции и обнуляются:

$Q = [\begin{matrix} P^{+} (1) + κ^{2} P^{+} (31) + β + η & 00 & - κ (P^{+} (31) + β) \\ 000000 & P^{+} (11) + κ^{2} P^{+} (41) + β + η & 00 & P^{+} (41) + β \\ 000000 & P^{+} (21) + κ^{2} P^{+} (51) + β + η & 00 & P^{+} (51) + β & 000 \\ - κ (P^{+} (31) + β) & 00 & P^{+} (31) + β \\ 000000 & P^{+} (41) + β & 00 & P^{+} (41) + β \\ 000000 & P^{+} (51) + β & 00 & P^{+} (51) + β \\ 000000000 & ν^{2} P^{+} (61) + ξ \\ 000000000 & ν^{2} P^{+} (71) + ξ \\ 000000000 & ν^{2} P^{+} (81) + ξ \end{matrix}]$

где

P ⁺ – является обновленным (по опыту) ошибочная оценочная ковариация
κ – DecimationFactor/SampleRate
β – GyroscopeDriftNoise
η – GyroscopeNoise
ν – LinearAcclerationDecayFactor
ξ – LinearAccelerationNoise

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

Кальман Гэйн

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

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

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

где

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

Обновите следующую ошибку

Следующая ошибочная оценка определяется путем объединения матрицы усиления Кальмана с ошибкой по оценкам вектора силы тяжести:

$x_{9 \times 1} = (K_{9 \times 3}) {(z_{1 \times 3})}^{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^{+}$

где

ν – LinearAcclerationDecayFactor

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

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

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

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

Ссылки

[1] Fusion датчика с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

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

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

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

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

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

Темы

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

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

imufilter

Описание

Создание

Синтаксис

Описание

Свойства

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

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

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

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

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

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

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

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

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

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

Синтаксис

Описание

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

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

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

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

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

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

Примеры

Оцените Ориентацию от данных IMU

Образцовый наклон Используя показания гироскопа и акселерометра

Удалите смещение из углового скоростного измерения

Алгоритмы

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

Модель

Предскажите ориентацию

Оцените силу тяжести от ориентации

Оцените силу тяжести от ускорения

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

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

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

Инновационная ковариация

Обновите ошибочную оценочную ковариацию

Предскажите ошибочную оценочную ковариацию

Кальман Гэйн

Обновите следующую ошибку

Правильный

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

Оцените линейное ускорение

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

Вычислите угловую скорость

Ссылки

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

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

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

Темы

Введенный в R2018b

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

Поддержка

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

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

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

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

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

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

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

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

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

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

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

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

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