imufilter

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

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

Описание

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

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

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

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

Создание

Синтаксис

FUSE = imufilter

FUSE = imufilter (эталонный кадр, RF)

FUSE = imufilter (___, имя, значение)

Описание

пример

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

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

пример

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

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

Свойства

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

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

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

Дополнительные сведения об изменении значений свойств см. в разделе Проектирование системы в MATLAB с использованием системных объектов.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Дисперсия шума линейного ускорения в (^m/s2) 2, заданная как положительный действительный скаляр. Линейное ускорение моделируется как процесс белого шума с фильтрацией нижних частот.

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

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

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

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

`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-by-3-by-N.

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

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

Синтаксис

[orientation, angularVelocity] = FUSE (ускоренные показания, гиропоказания)

Описание

пример

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Число входных выборок, N и свойство DecimationFactor определяют М.

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

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

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

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

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

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

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

release(obj)

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

Специфично для `imufilter`

tune Мелодия imufilter параметры для уменьшения ошибки оценки

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

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

Примеры

свернуть все

Оценка ориентации на основе данных 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)')

Figure contains an axes. The axes with title Orientation Estimate contains 3 objects of type line. These objects represent Z-axis, Y-axis, X-axis.

Наклон модели с использованием показаний гироскопа и акселерометра

Открыть сценарий

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

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

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

На последнем этапе структура движения объединяет 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*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 contains an axes. The axes with title X-axis contains 3 objects of type line. These objects represent True Angular Velocity, Gyroscope Readings, Recovered Angular Velocity.

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

Figure contains an axes. The axes with title Y-axis contains 3 objects of type line.

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

Figure contains an axes. The axes with title Z-axis contains 3 objects of type line.

Алгоритмы

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

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

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

$_{xk} = \begin{matrix} _{[} \\ _{} \\ _{} \end{matrix} startkbkak]_{} = \begin{matrix} _{Fk} \\ [_{startk} \\ -_{} \end{matrix} 1bk -_{}$ 1ak − 1] + wk

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

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

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

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

$\begin{array}{l} _{xk}^{} -_{=}_{}^{Fkxk} \\ _{-}^{} 1_{+}_{Pk}^{-}_{=}^{}_{} \\ {FkPk}_{} -_{1} +_{}_{}^{FkT} \\ _{+}_{} Qkyk_{=}_{}^{zk}_{-}^{} \\ _{} {Hkxk}_{}^{-}_{}^{Sk} {=_{}}^{Rk} \\ +_{}^{}_{HkPk}^{} -_{}_{} \\ _{HkTkk}^{} =_{}^{Pk} -_{}_{} {HkT}_{}^{(} \end{array}$ Sk) − 1xk + = xk − + Kkykpk

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

$\begin{array}{l} _{xk}^{} - \\ =_{}^{} {0Pk}_{} \\ -_{} =_{} \\ _{} Qkyk =_{}_{} {zkSk}_{}^{=}_{}^{Rk} \\ _{+}_{}^{} {HkPk}_{}^{-} {_{}}^{HkTKk} \\ _{=}^{} {Pk}_{} -_{} \\ _{}^{HkT} (_{}^{Sk})_{-}_{}_{1xk}^{} \end{array}$ + = KkykPk + = Pk − − KkHkPk −

где

_xk − -- прогнозируемая (априорная) оценка состояния; процесс ошибки
_Pk − -- предсказанная (априорная) оценка ковариации
_yk -- инновации
_Sk - инновационная ковариация
_Kk -- Kalman gain
_xk + -- обновленная (a posteriori) оценка состояния
_Pk + -- обновленная (a posteriori) оценка ковариации

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

На рисунке и последующих шагах описывается итерация на основе одного кадра в алгоритме.

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

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

Выполните пошаговое описание алгоритма для пояснения каждого этапа подробного обзора.

Модель

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

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

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

$_{ΔфН ×} \frac{3 = (_{} гироРеады N \times 3 -_{}}{}$ gyroOffset1 × 3) fs

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

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

$_{ΔQN ×} 1 =_{кватернион (} ΔфН \times 3,'$ rotvec')

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

$_{q1 ×}^{} 1 -_{=}^{(} q1_{\times 1}^{} +)_{} ($ ∏n=1NΔQn)

Во время первой итерации оценка ориентации, q −, инициализируется ecompass с предположением, что ось X указывает на север.

Оценка силы тяжести по ориентации

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

$_{g1 ×} {3 = (rПриор}^{}$ (:, 3)) T

Посмотрите ecompass для объяснения того, почему третий столбец rPrior можно интерпретировать как вектор гравитации.

Оценка силы тяжести по ускорению

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

$_{}_{}_{gAccel1×3=accelReadings1×3−linAccelprior1×3}$

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

Модель погрешности представляет собой разность между оценкой гравитации по показаниям акселерометра и оценкой гравитации по показаниям гироскопа: $z = g -$ gAccel.

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

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

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

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

Модель наблюдения строится следующим образом:

$_{H3×9} = \begin{matrix} _{} & _{} & _{} & _{} \\ _{} & _{} & _{} & _{} \\ _{} & _{} & _{} & _{} \end{matrix} [0gz-gy0-κgzκgy100-gz0gxκgz0-κgx010gy-gx0-κgyκgx0001]$

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

Вывод модели наблюдения см. в разделах 7.3 и 7.4 [1].

Ковариация инноваций

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

$_{S3x3} =_{} R3x3_{+ (}_{_{H3x9)}}^{(} {_{P9x9 −})}^{}$ (H3x9) T

где

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

$_{R3 ×} 3 = (λ + (β + \begin{matrix} ()) \\ [ \end{matrix}$ 100010001].
Дисперсию шума модели наблюдения определяют следующие свойства:
- (DecimationFactor/SampleRate) 2
- β -- Гироскопический шум
- start-- Гироссовый шум
- λ -- Ускорение Шум
- start-- Линейный уровень шума

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

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

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

$_{_{P9×9}}^{+} =_{_{}}^{}_{}_{}_{_{}}^{} P9\times9--(K9\times3)(H3\times9)(P9\times9-)$

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

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

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

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

где

P + - обновленная (a posteriori) ковариация оценки ошибки
δ -- Коэффициент прореживания/SampleRate
β -- Гироскопический шум
start-- Гироссовый шум
start-- Коэффициент декайфактора LinearAcclerationDecayFactor
start-- Линейный уровень шума

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

Калман-Гари

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

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

$_{K9 ×} 3_{_{= (}}^{} P9 {\times_{9} -}^{)} {{(_{H3 ×}}^{9})}^{T}$ ((S3 × 3) T) − 1

где

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

Обновить ошибку Posteriori

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

$_{x9 ×} 1_{= (} {K9_{× 3})}^{}$ (z1 × 3) T

Правильный

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

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

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

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

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

$linAccelPrior = (_{linAccelPriork} - {1)}^{}$

где

start-- Коэффициент декайфактора LinearAcclerationDecayFactor

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

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

$gyroOffset =_{gyroOffsetk} -^{}$ 1 − a +

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

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

$_{} \frac{_{}}{}_{angularVelocity1×3=∑gyroReadingsN×3N−gyroOffset1×3}$

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

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

Ссылки

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

[2] Роетенберг, Д., Х.Ж. Луинге, К.Т.М. Батен и П.Х. Велтинк. «Компенсация магнитных возмущений улучшает инерционное и магнитное восприятие ориентации сегмента человеческого тела». Сделки IEEE по нейронным системам и инженерии реабилитации. Том 13. Выпуск 3, 2005, стр. 395-405.

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

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™

Примечания и ограничения по использованию:

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

См. также

ahrsfilter | ecompass | gpsSensor | imuSensor

Представлен в R2018b

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

imufilter

Описание

Создание

Синтаксис

Описание

Свойства

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

DecimationFactor - Коэффициент прореживания 1 (по умолчанию) | целочисленный скаляр

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

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

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

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

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

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

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

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

Синтаксис

Описание

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

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

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

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

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

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

Специфично для imufilter

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

Примеры

Оценка ориентации на основе данных IMU

Наклон модели с использованием показаний гироскопа и акселерометра

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

Алгоритмы

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

Модель

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

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

Правильный

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

Ссылки

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

Создание кода C/C + + Создайте код C и C++ с помощью MATLAB ® Coder™

См. также

Документация по панели инструментов навигации

Поддержка

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

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

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

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

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

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

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

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

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

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

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

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

Специфично для `imufilter`

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™