imufilter

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

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

Описание

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

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

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

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

Создание

Синтаксис

FUSE = imufilter

FUSE = imufilter('ReferenceFrame',RF)

FUSE = imufilter(___,Name,Value)

Описание

пример

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 Использование Системных объектов.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Отклонение линейного шума ускорения в (м/с²)², заданный как положительный действительный скаляр. Линейное ускорение моделируется как процесс 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' -- Выход является матрицей вращения N 3 на 3 байта.

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

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

Синтаксис

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

Описание

пример

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

)

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

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

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

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

release(obj)

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

Характерно для `imufilter`

tune Настройка imufilter параметры для уменьшения ошибки расчета

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

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

Примеры

свернуть все

Оценка ориентации из данных БИНС

Открыть Live Script

Загрузите 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.

Моделируйте наклон с помощью показаний гироскопа и акселерометра

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

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

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

рыскание: 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);

В цикле:

Симулируйте выход БИНС путем подачи движения основная истина к объекту датчика БИНС.
Фильтрация выхода БИНС с помощью объекта фильтра БИНС по умолчанию.

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 отслеживать достоверные данные значительно уменьшается при моделировании реалистичного БИНС. Чтобы улучшить эффективность, измените свойства своего 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 со свойствами по умолчанию и nondefault.

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

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

Открыть Live Script

В этом примере показано, как удалить смещение гироскопа из БИНС с помощью 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.

The 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⁺ - обновленная (a posteriori) оценка ковариации

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

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

Перед первой итерацией accelReadings и gyroReadings входы chunked в системы координат 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 свойство.

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

$Δ Q_{N \times 1} = quaternion (Δ φ_{N \times 3},' r o t v e c')$

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

$q_{1 \times 1}^{-} = (q_{1 \times 1}^{+}) (\prod_{n = 1}^{N} Δ Q_{n})$

Во время первой итерации оценка ориентации, q⁻, инициализируется ecompass при предположении, что 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} & 1 & 0 & 0 \\ - g_{z} & 0 & g_{x} & κ g_{z} & 0 & - κ g_{x} & 0 & 1 & 0 \\ g_{y} & - g_{x} & 0 & - κ g_{y} & κ g_{x} & 0 & 0 & 0 & 1 \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} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \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) + β + η & 0 & 0 & - κ (P^{+} (31) + β) & 0 & 0 & 0 & 0 & 0 \\ 0 & P^{+} (11) + κ^{2} P^{+} (41) + β + η & 0 & 0 & - κ (P^{+} (41) + β) & 0 & 0 & 0 & 0 \\ 0 & 0 & P^{+} (21) + κ^{2} P^{+} (51) + β + η & 0 & 0 & - κ (P^{+} (51) + β) & 0 & 0 & 0 \\ - κ (P^{+} (31) + β) & 0 & 0 & P^{+} (31) + β & 0 & 0 & 0 & 0 & 0 \\ 0 & - κ (P^{+} (41) + β) & 0 & 0 & P^{+} (41) + β & 0 & 0 & 0 & 0 \\ 0 & 0 & - κ (P^{+} (51) + β) & 0 & 0 & P^{+} (51) + β & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (61) + ξ & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (71) + ξ & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (81) + ξ \end{matrix}]$

где

P⁺ - обновленная (a posteriori) оценка ошибки ковариация
κ -- 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] Комплексирование датчиков с открытым исходным кодом. 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.

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

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

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

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

См. также

ahrsfilter | ecompass | gpsSensor | imuSensor | quaternion

Темы

Определите ориентацию с помощью инерционных датчиков

Введенный в 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

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

Примеры

Оценка ориентации из данных БИНС

Моделируйте наклон с помощью показаний гироскопа и акселерометра

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

Алгоритмы

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

Модель

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

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

Правильный

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

Ссылки

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

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

См. также

Темы

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

Поддержка

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

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

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

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

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

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

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

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

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

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

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

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

Характерно для `imufilter`

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