Оцените телефонную ориентацию Используя cочетание датчиков

Скрипт Open Live Script

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

Считайте акселерометр, гироскоп, магнитометр и углы Эйлера

Считайте регистрируемые телефонные данные о датчике. 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 объект. Параметры на фильтре должны быть настроены для определенного IMU по телефону, который регистрировал данные в 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 object. The axes object 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 object. The axes object 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 object. The axes object with title Orientation Error contains 3 objects of type line, text.

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

figure
pp = poseplot("MeshFileName", "phoneMesh.stl");

for i = 1:numel(qEst)
    set(pp, "Orientation", qEst(i));
    drawnow
end

Figure contains an axes object. The axes object is empty.

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