ahrs10filter

Высота и ориентация от показаний MARG и альтиметра

Описание

The ahrs10filter объект взрывает данные MARG и датчика высоты для оценки высоты и ориентации устройства. Данные MARG (магнитная, угловая скорость, гравитация) обычно получают из магнитометра, гироскопа и датчиков акселерометра. Фильтр использует вектор состояния с 18 элементами, чтобы отслеживать кватернион ориентации, вертикальную скорость, вертикальное положение, смещения датчика MARG и геомагнитный вектор. The ahrs10filter объект использует расширенный фильтр Калмана, чтобы оценить эти величины.

Создание

Описание

FUSE = ahrs10filter возвращает расширенный объект фильтра Калмана, FUSE, для слияния датчика MARG и показаний альтиметра для оценки высоты и ориентации устройства.

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

пример

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

Свойства

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

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

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

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

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

Мультипликативное отклонение шума процесса от акселерометра в (м/с2)2, заданный как положительные вещественные конечные числа.

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

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

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

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

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

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

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

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

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

Вектор состояния расширенного фильтра Калмана. Значения состояния представляют:

ГосударствоМодулиИндекс
Ориентация (кватернионные части)Н/Д1:4
Высота над уровнем моря (NED или ENU)m5
Вертикальная скорость (NED или ENU)м/с6
Смещение угла дельты (XYZ)рад/с7:9
Смещение скорости в дельте (XYZ)м/с10:12
Геомагнитные Вектора поля (NED или ENU)μT13:15
Смещение магнитометра (XYZ)μT16:18

Начальное состояние по умолчанию соответствует объекту в покое, расположенному в [0 0 0] в геодезических координатах LLA.

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

Ковариация ошибки состояния для фильтра Калмана, заданная как матрица вещественных чисел с 18 на 18 элементами.

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

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

predictОбновите состояния, используя данные акселерометра и гироскопа для ahrs10filter
fusemagИсправьте состояния, используя данные магнитометра для ahrs10filter
fusealtimeterИсправьте состояния, используя данные о высотомере для ahrs10filter
correctИсправьте состояния, используя прямые измерения состояния для ahrs10filter
residualНевязки и остаточные ковариации от прямых измерений состояния для ahrs10filter
residualmagНевязки и остаточная ковариация от измерений магнитометра для ahrs10filter
residualaltimeterНевязки и остаточная ковариация от измерений высотомера для ahrs10filter
poseТекущая ориентация и оценка положения для ahrs10filter
resetСбросьте внутренние состояния для ahrs10filter
stateinfoОтобразите информацию о векторе состояния для ahrs10filter
tuneНастройка ahrs10filter параметры для уменьшения ошибки расчета
copyСоздание копии ahrs10filter

Примеры

свернуть все

Загрузите записанные данные датчика, положение основной истины и начальное состояние и начальное состояние ковариации. Рассчитать количество выборок БИНС на выборку альтиметра и количество выборок БИНС на выборку магнитометра.

load('fuse10exampledata.mat', ...
     'imuFs','accelData','gyroData', ...
     'magnetometerFs','magData', ...
     'altimeterFs','altData', ...
     'expectedHeight','expectedOrient', ...
     'initstate','initcov');

imuSamplesPerAlt = fix(imuFs/altimeterFs);
imuSamplesPerMag = fix(imuFs/magnetometerFs);

Создайте фильтр AHRS, который запирает показания MARG и альтиметра для оценки высоты и ориентации. Установите частоту дискретизации и шумы измерений датчиков. Значения определяли из таблиц данных и экспериментов.

filt = ahrs10filter('IMUSampleRate',imuFs, ...
                    'AccelerometerNoise',0.1, ...
                    'State',initstate, ...
                    'StateCovariance',initcov);

Ralt = 0.24;
Rmag = 0.9;

Предварительно выделите переменные для регистрации высоты и ориентации.

numIMUSamples = size(accelData,1);
estHeight = zeros(numIMUSamples,1);
estOrient = zeros(numIMUSamples,1,'quaternion');

Данные об акселерометре предохранителя, гироскопе, магнитометре и альтиметре. Внешний контур предсказывает фильтр вперед на уровне самой быстрой частоты дискретизации (частота дискретизации IMU).

for ii = 1:numIMUSamples
    
    % Use predict to estimate the filter state based on the accelometer and
    % gyroscope data.
    predict(filt,accelData(ii,:),gyroData(ii,:));
    
    % Magnetometer data is collected at a lower rate than IMU data. Fuse
    % magnetometer data at the lower rate.
    if ~mod(ii,imuSamplesPerMag)
        fusemag(filt,magData(ii,:),Rmag);
    end
    
    % Altimeter data is collected at a lower rate than IMU data. Fuse
    % altimeter data at the lower rate.
    if ~mod(ii, imuSamplesPerAlt)
        fusealtimeter(filt,altData(ii),Ralt);
    end
    
    % Log the current height and orientation estimate.
    [estHeight(ii),estOrient(ii)] = pose(filt);
end

Вычислите ошибки RMS между известной истинной высотой и ориентацией и выходом из фильтра AHRS.

pErr = expectedHeight - estHeight;
qErr = rad2deg(dist(expectedOrient,estOrient));

pRMS = sqrt(mean(pErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Altitude RMS Error\n');
Altitude RMS Error
fprintf('\t%.2f (meters)\n\n',pRMS);
	0.38 (meters)

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

t = (0:(numIMUSamples-1))/imuFs;
plot(t,expectedHeight);hold on
plot(t,estHeight);hold off
legend('Ground Truth','Estimated Height','location','best')
ylabel('Height (m)')
xlabel('Time (s)')
grid on

Figure contains an axes. The axes contains 2 objects of type line. These objects represent Ground Truth, Estimated Height.


fprintf('Quaternion Distance RMS Error\n');
Quaternion Distance RMS Error
fprintf('\t%.2f (degrees)\n\n',qRMS);
	2.93 (degrees)

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

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

.

См. также

|

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