Оценка ориентации телефона с помощью слияния датчиков

Открыть сценарий в реальном времени

MATLAB Mobile™ сообщает данные датчиков акселерометра, гироскопа и магнитометра на мобильных устройствах Apple или Android. Можно получить необработанные данные от каждого датчика или данные плавкой ориентации. В этом примере показано, как сравнить данные о плавкой ориентации из телефона с оценкой ориентации из ahrsfilter объект.

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

Считывайте записанные данные датчика телефона. Файл 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 функция с записанными данными ориентации в качестве истинного состояния земли.

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.

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