imufilter

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

Описание

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

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

  1. Создайте imufilter объект и набор его свойства.

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

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

Создание

Описание

пример

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

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

пример

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

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

Свойства

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

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

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

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

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

Ковариационная матрица для шума процесса в виде 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

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

Типы данных: single | double | uint8 | uint16 | uint32 | uint64 | int8 | int16 | int32 | int64

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

  • 'quaternion' – Выходом является N-by-1 quaternion.

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

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

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

Описание

пример

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

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

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

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

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

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

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

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

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

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

  • 'quaternion' – Выходом является M-by-1 вектор кватернионов с тем же базовым типом данных как входные параметры.

  • 'Rotation matrix' – Выход является 3 3 M массивом матриц вращения совпадающий тип данных как входные параметры.

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

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

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

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

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

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

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

release(obj)

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

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

Примеры

свернуть все

Загрузите 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 движения описывает последовательные вращения:

  1. отклонение от курса: 120 градусов более чем две секунды

  2. тангаж: 60 градусов более чем одна секунда

  3. крен: 30 градусов по половине второго

  4. крен:-30 градусов по половине второго

  5. тангаж:-60 градусов более чем одна секунда

  6. отклонение от курса:-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);

В цикле:

  1. Симулируйте IMU выход путем питания движения основной истины объект датчика IMU.

  2. Отфильтруйте 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*8;
fs = 20;
numSamples = duration * fs;
rng('default') % Seed the RNG to reproduce noisy sensor measurements.

initialAngVel = [0,0.5,0.25];
finalAngVel = [-0.2,0.6,0.5];
constantAngVel = repmat(initialAngVel,floor(numSamples/2),1);
varyingAngVel = [linspace(initialAngVel(1), finalAngVel(1), ceil(numSamples/2)).', ...
    linspace(initialAngVel(2), finalAngVel(2), ceil(numSamples/2)).', ...
    linspace(initialAngVel(3), finalAngVel(3), ceil(numSamples/2)).'];

angVelBody = [constantAngVel; varyingAngVel];
accBody = zeros(numSamples,3);

traj = kinematicTrajectory('SampleRate',fs);

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

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

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

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

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

fuse = imufilter('SampleRate',fs, 'GyroscopeDriftNoise', 1e-6);

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

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

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

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

figure(1)
plot(time,angVelBody(:,1), ...
     time,gyroReadingsBody(:,1), ...
     time,angVelBodyRecovered(:,1))
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(:,3), ...
     time,gyroReadingsBody(:,3), ...
     time,angVelBodyRecovered(:,3))
title('Z-axis')
ylabel('Angular Velocity (rad/s)')
xlabel('Time (s)')

Алгоритмы

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

Примечание: следующий алгоритм только применяется к системе координат NED.

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

xk=[θkbkak]=Fk[θk1bk1ak1]+wk

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

  • θk – Вектор ошибок ориентации 3 на 1, в градусах, во время k

  • bk – Нуль гироскопа 3 на 1 угловой вектор смещения уровня, в градусе/с, во время k

  • ak – Ускоряющий вектор ошибок 3 на 1 измеряется в системе координат датчика, в g, во время k

  • wk – 9 1 аддитивный шумовой вектор

  • Fk – модель изменения состояния

Поскольку x k задан как ошибочный процесс, априорная оценка всегда является нулем, и поэтому модель изменения состояния, F k, является нулем. Это понимание приводит к следующему сокращению стандарта уравнения Кальмана:

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

xk=Fkxk1+Pk=FkPk1+FkT+Qkyk=zkHkxkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=xk+KkykPk+=PkKkHkPk

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

xk=0Pk=Qkyk=zkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=KkykPk+=PkKkHkPk

где

  • xk – предсказанный (априорно) утверждают оценку; ошибочный процесс

  • Pk – предсказанный (априорно) оценивают ковариацию

  • yk – инновации

  • Sk – инновационная ковариация

  • Kk – Усиление Кальмана

  • xk + – обновленный (по опыту) утверждает оценку

  • Pk + – обновленный (по опыту) оценивает ковариацию

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

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

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

Ссылки

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

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

Введенный в R2018b

Для просмотра документации необходимо авторизоваться на сайте