Main Content

Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Estimar la orientación del teléfono mediante Sensor Fusion

Abrir script en vivo

MATLAB Mobile™ informa datos de sensores del acelerómetro, giroscopio y magnetómetro en dispositivos móviles Apple o Android. Se pueden obtener datos sin procesar de cada sensor o datos de orientación fusionados. Este ejemplo muestra cómo comparar los datos de orientación fusionados del teléfono con la estimación de orientación del objeto ahrsfilter .

Lea el acelerómetro, giroscopio, magnetómetro y ángulos de Euler

Lea los datos registrados del sensor del teléfono. El archivo MAT samplePhoneData.mat contiene datos del sensor registrados en un iPhone a una frecuencia de muestreo de 100 Hz. Para ejecutar este ejemplo con los datos de su propio teléfono, consulte Recopilación de datos de sensores con MATLAB Mobile.

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

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

Convertir a marco de coordenadas noreste-abajo (NED)

MATLAB Mobile utiliza la convención que se muestra en la siguiente imagen. Para procesar los datos del sensor con el objeto ahrsfilter , convierta a NED, un sistema de coordenadas diestro con movimiento en el sentido de las agujas del reloj alrededor de los ejes correspondientes a rotaciones positivas. Intercambie los ejes x e y y niegue el eje z para los distintos datos del sensor. Tenga en cuenta que las lecturas del acelerómetro se niegan ya que las lecturas tienen el signo opuesto en las dos convenciones.

phoneAxes.png

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

Rotación inicial correcta del teléfono

El teléfono puede tener un desplazamiento rotacional aleatorio. Sin conocer el desplazamiento, no se puede comparar el objeto ahrsfilter y los resultados del teléfono. Utilice las primeras cuatro muestras para determinar el desplazamiento de rotación y luego gire los datos del teléfono a los valores deseados.

% 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.

Ajusteel filtro AHRS

Para optimizar los parámetros de ruido del teléfono, ajuste el objeto ahrsfilter . Los parámetros del filtro deben ajustarse para la IMU específica del teléfono que registró los datos en el archivo MAT. Utilice la función tune con los datos de orientación registrados como ground-truth. 

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

Combinar datos del sensor con filtro

Calcule la orientación del dispositivo utilizando el objeto ahrsfilter ajustado.

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

Representar los resultados

Trazar los ángulos de Euler para cada estimación de orientación y la distancia del cuaternión entre las dos estimaciones de orientación. La distancia de los cuaterniones se mide como el ángulo entre dos cuaterniones. Esta distancia se puede utilizar como métrica de error para la estimación de la orientación.

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

Utilice poseplot para ver las estimaciones de orientación del teléfono como un rectángulo 3-D.

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

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