Оцените ориентацию телефона с помощью Sensor Fusion

Открыть Live Script

MATLAB Mobile™ сообщает данные о датчике с акселерометра, гироскопа и магнитометра на мобильных устройствах Apple или Android. Необработанные данные из каждого датчика или сплавленные данные ориентации могут быть получены. Эти примеры показывают, как сравнить слитые данные ориентации с телефона с оценкой ориентации из ahrsfilter (Navigation Toolbox) объект.

Чтение Accelerometer, гироскопа, магнитометра и углов Эйлера

Считайте данные записанного датчика телефона. Файл MAT samplePhoneData.mat содержит данные о датчике, регистрируемом на iPhone со частотой дискретизации 100 Гц. Чтобы запустить этот пример с вашими собственными телефонными данными, смотрите Набор данных о датчике с MATLAB Mobile или MATLAB Online.

matfile = 'samplePhoneData.mat';
SampleRate = 100; % This must match the data rate of the phone.

[Accelerometer, Gyroscope, Magnetometer, EulerAngles] ...
    = exampleHelperProcessPhoneData(matfile);

Преобразуйте в координатную систему координат Северо-Востока-Вниз (NED)

MATLAB Mobile использует конвенцию, показанную на следующем изображении. Для обработки данных о датчике с помощью ahrsfilter объект, преобразуйте в NED, правую систему координат с перемещением по часовой стрелке вокруг осей, соответствующих положительным вращениям. Смените оси X и Y и отмените ось Z для различных данных о датчике. Обратите внимание, что показания акселерометра отрицаются, поскольку показания имеют противоположный знак в двух соглашениях.

Accelerometer = -[Accelerometer(:,2), Accelerometer(:,1), -Accelerometer(:,3)];
Gyroscope = [Gyroscope(:,2), Gyroscope(:,1), -Gyroscope(:,3)];
Magnetometer = [Magnetometer(:,2), Magnetometer(:,1), -Magnetometer(:,3)];
qTrue = quaternion([EulerAngles(:,3), -EulerAngles(:,2), EulerAngles(:,1)], ...
    'eulerd', 'ZYX', 'frame');

Правильное начальное вращение телефона

Телефон может иметь случайное вращательное смещение. Не зная смещения, вы не можете сравнить ahrsfilter объект и результаты телефона. Используйте первые четыре выборки, чтобы определить вращательное смещение, затем поверните данные телефона назад к желаемым значениям.

% Get a starting guess at orientation using ecompass. No coefficients
% required. Use the initial orientation estimates to figure out what the
% phone's rotational offset is.
q = ecompass(Accelerometer, Magnetometer);

Navg = 4;
qfix = meanrot(q(1:Navg))./meanrot(qTrue(1:Navg));
Orientation = qfix*qTrue; % Rotationally corrected phone data.

Настройка фильтра AHRS

Чтобы оптимизировать параметры шума для телефона, настройте ahrsfilter объект. Параметры фильтра должны быть настроены для конкретного БИНС на телефоне, который регистрировал данные в MAT-файле. Используйте tune (Navigation Toolbox) функция с записанными в журнал данными ориентации как основная истина.

orientFilt = ahrsfilter('SampleRate', SampleRate);
groundTruth = table(Orientation);
sensorData = table(Accelerometer, Gyroscope, Magnetometer);

tc = tunerconfig('ahrsfilter', "MaxIterations", 30, ...
    'ObjectiveLimit', 0.001, 'Display', 'none');
tune(orientFilt, sensorData, groundTruth, tc);

Данные о датчике предохранителя с фильтром

Оцените ориентацию устройства с помощью настроенной ahrsfilter объект.

reset(orientFilt);
qEst = orientFilt(Accelerometer,Gyroscope,Magnetometer);

Графическое изображение результатов

Постройте график углов Эйлера для каждой оценки ориентации и кватернионного расстояния между двумя оценками ориентации. Расстояние кватерниона измеряется как угол между двумя кватернионами. Это расстояние может использоваться в качестве метрики ошибки для оценки ориентации.

numSamples = numel(Orientation);
t = (0:numSamples-1).'/SampleRate;

d = rad2deg(dist(qEst, Orientation));

figure
plot(t, eulerd(qEst, 'ZYX', 'frame'))
legend yaw pitch roll
title('ahrsfilter Euler Angles')
ylabel('Degrees')
xlabel('Time (s)')

Figure contains an axes. The axes with title ahrsfilter Euler Angles contains 3 objects of type line. These objects represent yaw, pitch, roll.

figure
plot(eulerd(Orientation, 'ZYX', 'frame'))
legend yaw pitch roll
title('Phone Euler Angles')
ylabel('Degrees')
xlabel('Time (s)')

Figure contains an axes. The axes with title Phone Euler Angles contains 3 objects of type line. These objects represent yaw, pitch, roll.

figure
plot(t, d)
title('Orientation Error')
ylabel('Degrees')
xlabel('Time (s)')
% Add RMS error
rmsval = sqrt(mean(d.^2));
line(t, repmat(rmsval,size(t)),'LineStyle','-.','Color','red');
text(t(1),rmsval + 0.7,"RMS Error = " + rmsval,'Color','red')

Figure contains an axes. The axes with title Orientation Error contains 3 objects of type line, text.

Используйте HelperOrientationViewer класс, чтобы просмотреть оценки ориентации телефона как 3-D прямоугольник.

fig = figure;
viewer = HelperOrientationViewer("AppWindow", fig, "BoxDimensions", [2.0514, 1.0129, 0.11]);

for i = 1:numel(qEst)
    viewer(qEst(i));
end

Figure contains an axes. The axes contains an object of type patch.

Документация