Estimar la orientación del teléfono mediante Sensor Fusion
MATLAB ® Mobile™ informa datos de los 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 de sensores 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, conviértalos a NED, un sistema de coordenadas para diestros. En el marco NED, un ángulo de rotación positivo corresponde a una rotación en el sentido de las agujas del reloj alrededor de un eje cuando se mira desde el origen a lo largo de la dirección positiva del eje. La convención de la mano derecha es equivalente, donde la rotación positiva se indica mediante la dirección en la que los dedos de la mano derecha se curvan cuando el pulgar apunta en la dirección del eje de rotación. Intercambie los ejes x y 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.
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 pueden comparar los resultados del objeto ahrsfilter y 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 a 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);
Datos del sensor de fusibles con filtro
Estime la orientación del dispositivo utilizando el objeto ajustado ahrsfilter.
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 3D.
figure pp = poseplot("MeshFileName", "phoneMesh.stl"); for i = 1:numel(qEst) set(pp, "Orientation", qEst(i)); drawnow end
