Высота и ориентация по показаниям МАРГ и высотомера
ahrs10filter объект плавит данные датчика MARG и высотомера для оценки высоты и ориентации устройства. Данные MARG (магнитная, угловая скорость, сила тяжести) обычно получают из датчиков магнитометра, гироскопа и акселерометра. Фильтр использует 18-элементный вектор состояния для отслеживания кватерниона ориентации, вертикальной скорости, вертикального положения, смещений датчика MARG и геомагнитного вектора. 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 |
Загрузить записанные данные датчика, позу истинности земли, а также ковариацию начального состояния и исходного состояния. Вычислите количество образцов 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
Вычислите среднеквадратичные ошибки между известной истинной высотой и ориентацией и выходными данными фильтра 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)