Высота и ориентация от показаний MARG и альтиметра
The ahrs10filter объект взрывает данные MARG и датчика высоты для оценки высоты и ориентации устройства. Данные MARG (магнитная, угловая скорость, гравитация) обычно получают из магнитометра, гироскопа и датчиков акселерометра. Фильтр использует вектор состояния с 18 элементами, чтобы отслеживать кватернион ориентации, вертикальную скорость, вертикальное положение, смещения датчика MARG и геомагнитный вектор. The ahrs10filter объект использует расширенный фильтр Калмана, чтобы оценить эти величины.
FUSE = ahrs10filterFUSE, для слияния датчика MARG и показаний альтиметра для оценки высоты и ориентации устройства.
FUSE = ahrs10filter('ReferenceFrame',RF)RF. Задайте RF следующим 'NED' (Северо-Восток-Даун) или 'ENU' (Восток-Север-Вверх). Значение по умолчанию 'NED'.
FUSE = ahrs10filter(___,Name,Value)Name к заданной Value. Неопределенные свойства имеют значения по умолчанию.
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
fprintf('Quaternion Distance RMS Error\n');Quaternion Distance RMS Error
fprintf('\t%.2f (degrees)\n\n',qRMS);2.93 (degrees)