ahrs10filter

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

Описание

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

Создание

Описание

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

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

пример

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

Свойства

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

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

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

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

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

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

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

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

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

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

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

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

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

Шум аддитивного процесса для магнитометра смещает в μT2, заданном как положительные действительные конечные числа.

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

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

СостояниеМодулиИндекс
Ориентация (части кватерниона)Нет данных1:4
Высота (NED или ENU)m5
Вертикальная скорость (NED или ENU)m/s6
Угловое смещение Delta (XYZ)rad/s7:9
Скоростное смещение Delta (XYZ)m/s10:12
Геомагнитный полевой вектор (NED или ENU)μT13:15
Смещение магнитометра (XYZ)μT16:18

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

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

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

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

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

predictОбновите состояния с помощью данных о гироскопе и акселерометра
fusemagПравильные состояния с помощью данных о магнитометре
fusealtimeterПравильные состояния с помощью данных о высотомере
correctПравильные состояния с помощью прямых измерений состояния
poseТекущая ориентация и оценка положения
resetСбросьте внутренние состояния
stateinfoОтобразите информацию о векторе состояния

Примеры

свернуть все

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

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


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

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

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

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

|

Введенный в R2019a

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