ahrs10filter
Высота и ориентация от MARG и показаний высотомера
Описание
Объект ahrs10filter плавит MARG и данные о датчике высотомера, чтобы оценить высоту устройства и ориентацию. MARG (магнитный, угловой уровень, сила тяжести) данные обычно выводятся от магнитометра, гироскопа и датчиков акселерометра. Фильтр использует вектор состояния с 18 элементами, чтобы отследить кватернион ориентации, вертикальную скорость, вертикальное положение, смещения датчика MARG и геомагнитный вектор. Объект ahrs10filter использует расширенный Фильтр Калмана, чтобы оценить эти количества.
Создание
Синтаксис
FUSE = ahrs10filterFUSE = ahrs10filter(Name,Value)Описание
FUSE = ahrs10filterFUSE, для сплава датчика MARG и показаний высотомера, чтобы оценить высоту устройства и ориентацию.
FUSE = ahrs10filter(Name,Value)Name к заданному Value. Незаданные свойства имеют значения по умолчанию.
Свойства
Функции объекта
predict | Обновите состояния с помощью данных о гироскопе и акселерометра |
fusemag | Правильные состояния с помощью данных о магнитометре |
fusealtimeter | Правильные состояния с помощью данных о высотомере |
correct | Правильные состояния с помощью прямых измерений состояния |
pose | Текущая ориентация и оценка положения |
reset | Сбросьте внутренние состояния |
stateinfo | Отобразите информацию о векторе состояния |
Примеры
Оцените положение UAV
Загрузите регистрируемые данные о датчике, наземное положение истины, и ковариацию начального состояния и начальное состояние. Вычислите количество выборок 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'); fprintf('\t%.2f (meters)\n\n',pRMS);
Altitude RMS Error 0.38 (meters)
Визуализируйте истинную и предполагаемую высоту в зависимости от времени.
t = (0:(numIMUSamples-1))/imuFs; plot(t,expectedHeight);hold on plot(t,estHeight);hold off legend('Ground Truth','Esimated Height','location','best') ylabel('Height (m)') xlabel('Time (s)') grid on fprintf('Quaternion Distance RMS Error\n'); fprintf('\t%.2f (degrees)\n\n',qRMS);
Quaternion Distance RMS Error 2.93 (degrees)
