Высота и ориентация от MARG и показаний высотомера
ahrs10filter возразите плавит MARG и данные о датчике высотомера, чтобы оценить высоту устройства и ориентацию. MARG (магнитный, угловой уровень, сила тяжести) данные обычно выводятся из магнитометра, гироскопа и датчиков акселерометра. Фильтр использует вектор состояния с 18 элементами, чтобы отследить кватернион ориентации, вертикальную скорость, вертикальное положение, смещения датчика MARG и геомагнитный вектор. ahrs10filter возразите использует расширенный Фильтр Калмана, чтобы оценить эти количества.
FUSE = ahrs10filterFUSE, для сплава датчика MARG и показаний высотомера, чтобы оценить высоту устройства и ориентацию.
FUSE = ahrs10filter('ReferenceFrame',RF)RF. Задайте РФ как 'NED' (Северо-восток вниз) или 'ENU' (Восточный Север). Значением по умолчанию является 'NED'.
FUSE = ahrs10filter(___,Name,Value)Name к заданному Value. Незаданные свойства имеют значения по умолчанию.
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)