센서 융합을 사용하여 휴대폰 방향 추정하기

MATLAB Mobile™은 Apple이나 Android 모바일 기기의 가속도계, 자이로스코프, 자력계에서 얻은 센서 데이터를 보고합니다. 각 센서로부터 원시 데이터를 얻거나 융합된 방향 데이터를 얻을 수 있습니다. 이 예제는 휴대폰에서 얻은 융합된 방향 데이터와 ahrsfilter 객체로 구한 방향 추정값을 비교하는 방법을 보여줍니다.

가속도계, 자이로스코프, 자력계, 오일러 각 읽어오기

휴대폰에 기록된 센서 데이터를 읽어옵니다. MAT 파일 samplePhoneData.mat는 iPhone에서 100Hz 샘플링 레이트로 기록된 센서 데이터를 포함합니다. 이 예제를 본인의 휴대폰 데이터를 사용해서 실행하려면 MATLAB Mobile을 사용한 센서 데이터 수집 항목을 참조하십시오.

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로 변환합니다. 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 객체와 휴대폰의 결과를 비교할 수 없습니다. 처음 4개의 샘플을 사용하여 회전 오프셋을 확인한 다음 휴대폰 데이터를 적합한 값으로 회전시킵니다.

% 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 파일에 데이터를 기록한 휴대폰의 IMU에 맞게 조정해야 합니다. 기록된 방향 데이터를 ground truth로 지정해서 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
plot(eulerd(Orientation, 'ZYX', 'frame'))
legend yaw pitch roll
title('Phone Euler Angles')
ylabel('Degrees')
xlabel('Time (s)')

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')

휴대폰의 방향 추정값을 3차원 사각형으로 보려면 poseplot을 사용합니다.

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

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