Estimar la orientación del teléfono mediante Sensor Fusion
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.
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