¿Cómo obtener el azimut de un teléfono con lecturas de brújula y lecturas de giroscopio?

El sensor de orientación obtiene realmente sus lecturas del magnetómetro real y del acelerómetro.

Supongo que tal vez esta es la fuente de la confusión. ¿Dónde se indica esto en la documentación? Más importante aún, ¿la documentación en algún lugar explícitamente indicar que las lecturas de giro es ignorado? Por lo que sé el método descrito en este video se implementa:

Sensor Fusion en dispositivos Android: una revolución en el procesamiento de movimiento

Este método utiliza los giroscopios e integra sus lecturas. Esto prácticamente hace que el resto de la cuestión sea discutible; Sin embargo trataré de responder.

El sensor de orientación ya está integrando las lecturas giroscópicas para usted , así es como obtiene la orientación. No entiendo por qué lo haces tú mismo.

Usted no está haciendo la integración de las lecturas del giroscopio correctamente , es más complicado que CurrentOrientation += dR (que es incorrecto). Si necesita integrar las lecturas del giro (no veo por qué, el SensorManager ya lo está haciendo por usted) por favor lea la Dirección Coseno Matriz IMU: Teoría cómo hacerlo correctamente (Ecuación 17).

No trate de integrarse con los ángulos de Euler (también conocido como acimut, pitch, roll), no saldrá nada bueno.

Utilice matrices de rotación o cuaterniones en sus cálculos en lugar de ángulos de Euler. Si trabaja con matrices de rotación, siempre puede convertirlas en ángulos de Euler, consulte

Computación de ángulos de Euler a partir de una matriz de rotación por Gregory G. Slabaugh

(Lo mismo es cierto para los cuaterniones.) Hay (en el caso no degenerativo) dos formas de representar una rotación, es decir, obtendrá dos ángulos de Euler. Elija el que está en el rango que necesita. (En caso de bloqueo cardán , hay infinitamente muchos ángulos de Euler, véase el PDF de arriba). Simplemente prométete que no volverás a usar ángulos de Euler de nuevo en tus cálculos después de la matriz de rotación a la conversión de ángulos de Euler.

No está claro lo que está haciendo con el filtro complementario. Usted puede implementar una fusión de sensor bastante maldita buena basada en la Dirección Cosine Matrix IMU: Teoría manuscrito, que es básicamente un tutorial. No es trivial hacerlo, pero no creo que encuentres un tutorial mejor y más comprensible que este manuscrito.

Una cosa que tuve que descubrir a mí mismo cuando implementé la fusión de sensores basado en este manuscrito fue que la llamada liquidación integral puede ocurrir. Me ocupé de ello limitando la TotalCorrection (página 27). Usted comprenderá de lo que estoy hablando si implementa esta fusión de sensores.

ACTUALIZACIÓN: Aquí respondo a sus preguntas que publicó en los comentarios después de aceptar la respuesta.

Creo que la brújula me da mi orientación actual usando la gravedad y el campo magnético, ¿no? ¿Se utiliza giroscopio en la brújula?

Sí, si el teléfono está más o menos estacionario durante al menos medio segundo, puede obtener una buena estimación de orientación utilizando sólo la gravedad y la brújula. Aquí está cómo hacerlo: ¿Puede alguien decirme si el sensor de la gravedad es como un sensor de la inclinación para mejorar la exactitud de la dirección?

No, los giroscopios no se usan en la brújula.

¿Podría usted por favor amablemente explicar por qué la integración hecha por mí está mal? Entiendo que si el tono de mi teléfono apunta hacia arriba, el ángulo euler falla. Pero, ¿hay otras cosas mal con mi integración?

Hay dos cosas no relacionadas: (i) la integración debe hacerse de manera diferente, (ii) los ángulos de Euler son problemas debido a la cerradura de Gimbal. Repito, estos dos no están relacionados.

En cuanto a la integración: aquí está un ejemplo simple cómo usted puede ver realmente cuál es incorrecto con su integración. Sea xey los ejes del plano horizontal en la habitación. Obtener un teléfono en sus manos. Gire el teléfono alrededor del eje x (de la habitación) en 45 grados, luego alrededor del eje y (de la habitación) en 45 grados. A continuación, repita estos pasos desde el principio, pero ahora gire alrededor del eje y primero y luego alrededor del eje x. El teléfono termina en una orientación totalmente diferente. Si haces la integración de acuerdo con CurrentOrientation += dR no verás ninguna diferencia! Por favor, lea la dirección Cosine Matrix IMU: Teoría manuscrita si desea hacer la integración correctamente.

En cuanto a los ángulos de Euler: atornillan la estabilidad de la aplicación y es suficiente para mí no utilizarlos para rotaciones arbitrarias en 3D.

Todavía no entiendo por qué usted está tratando de hacerlo usted mismo, por qué no desea utilizar la estimación de orientación proporcionada por la plataforma. Lo más probable es que no se puede hacer mejor que eso.

Creo que deberías evitar el "Sensor de Orientación" despreciado, y usar métodos de fusión de sensores como getRotationVector, getRotationMatrix que ya implementan algoritmos de fusión especialmente de Invensense, que ya utilizan datos giroscópicos.

Si desea un simple algoritmo de fusión de sensor llamado filtro de equilibrio (consulte http://www.filedump.net/dumped/filter1285099462.pdf ) puede utilizarse. El enfoque es como en

http://postimg.org/image/9cu9dwn8z/

Esto integra el giroscopio para obtener ángulo, luego el filtro de paso alto filtra el resultado para eliminar la deriva, y lo añade a los resultados acelerómetro y brújula suavizados. Los datos integrados del giroscopio con paso alto y los datos del acelerómetro / brújula se añaden de tal manera que las dos partes se suman a uno, de modo que la salida es una estimación exacta en unidades que tienen sentido. Para el filtro de equilibrio, la constante de tiempo puede ajustarse para ajustar la respuesta. Cuanto más corta sea la constante de tiempo, mejor será la respuesta pero más ruido de aceleración se permitirá pasar.

Para ver cómo funciona esto, imagine que tiene el punto de datos giroscópico más reciente (en rad / s) almacenado en girocompás, la medición de ángulo más reciente del acelerómetro se almacena en angle_acc y es el tiempo desde el último girocompás hasta ahora. Entonces su nuevo ángulo se calcularía usando

Ángulo = b * (ángulo + giro * dt) + (1 – b) * (ángulo_acc);

Usted puede comenzar intentando b = 0.98 por ejemplo. También es probable que desee utilizar un giroscopio rápido tiempo de medición dt para que el girocompás no deriva más de un par de grados antes de la siguiente medición se toma. El filtro de equilibrio es útil y sencillo de implementar, pero no es el enfoque ideal de fusión de sensores. El enfoque de Invensense involucra algunos algoritmos inteligentes y probablemente alguna forma de filtro de Kalman.

Fuente: Professional Android Sensor Programming, Adam Stroud.

Si el valor de azimut es impreciso debido a la interferencia magnética, no hay nada que usted puede hacer para eliminarlo hasta donde yo sé. Para obtener una lectura estable del acimut, debe filtrar los valores del acelerómetro si TYPE_GRAVITY no está disponible. Si TYPE_GRAVITY no está disponible, entonces estoy bastante seguro de que el dispositivo no tiene un girocompás, por lo que el único filtro que puede utilizar es filtro de paso bajo. El código siguiente es una implementación de una brújula estable utilizando TYPE_GRAVITY y TYPE_MAGNETIC_FIELD.

 public class Compass implements SensorEventListener { public static final float TWENTY_FIVE_DEGREE_IN_RADIAN = 0.436332313f; public static final float ONE_FIFTY_FIVE_DEGREE_IN_RADIAN = 2.7052603f; private SensorManager mSensorManager; private float[] mGravity; private float[] mMagnetic; // If the device is flat mOrientation[0] = azimuth, mOrientation[1] = pitch // and mOrientation[2] = roll, otherwise mOrientation[0] is equal to Float.NAN private float[] mOrientation = new float[3]; private LinkedList<Float> mCompassHist = new LinkedList<Float>(); private float[] mCompassHistSum = new float[]{0.0f, 0.0f}; private int mHistoryMaxLength; public Compass(Context context) { mSensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); // Adjust the history length to fit your need, the faster the sensor rate // the larger value is needed for stable result. mHistoryMaxLength = 20; } public void registerListener(int sensorRate) { Sensor magneticSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD); if (magneticSensor != null) { mSensorManager.registerListener(this, magneticSensor, sensorRate); } Sensor gravitySensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY); if (gravitySensor != null) { mSensorManager.registerListener(this, gravitySensor, sensorRate); } } public void unregisterListener() { mSensorManager.unregisterListener(this); } @Override public void onAccuracyChanged(Sensor sensor, int accuracy) { } @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_GRAVITY) { mGravity = event.values.clone(); } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) { mMagnetic = event.values.clone(); } if (!(mGravity == null || mMagnetic == null)) { mOrientation = getOrientation(); } } private void getOrientation() { float[] rotMatrix = new float[9]; if (SensorManager.getRotationMatrix(rotMatrix, null, mGravity, mMagnetic)) { float inclination = (float) Math.acos(rotMatrix[8]); // device is flat if (inclination < TWENTY_FIVE_DEGREE_IN_RADIAN || inclination > ONE_FIFTY_FIVE_DEGREE_IN_RADIAN) { float[] orientation = sensorManager.getOrientation(rotMatrix, mOrientation); mCompassHist.add(orientation[0]); mOrientation[0] = averageAngle(); } else { mOrientation[0] = Float.NAN; clearCompassHist(); } } } private void clearCompassHist() { mCompassHistSum[0] = 0; mCompassHistSum[1] = 0; mCompassHist.clear(); } public float averageAngle() { int totalTerms = mCompassHist.size(); if (totalTerms > mHistoryMaxLength) { float firstTerm = mCompassHist.removeFirst(); mCompassHistSum[0] -= Math.sin(firstTerm); mCompassHistSum[1] -= Math.cos(firstTerm); totalTerms -= 1; } float lastTerm = mCompassHist.getLast(); mCompassHistSum[0] += Math.sin(lastTerm); mCompassHistSum[1] += Math.cos(lastTerm); float angle = (float) Math.atan2(mCompassHistSum[0] / totalTerms, mCompassHistSum[1] / totalTerms); return angle; } } 

En su actividad instancia de un objeto Compass decir en onCreate, registerListener en onResume y unregisterListener en onPause

 private Compass mCompass; @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); mCompass = new Compass(this); } @Override protected void onPause() { super.onPause(); mCompass.unregisterListener(); } @Override protected void onResume() { super.onResume(); mCompass.registerListener(SensorManager.SENSOR_DELAY_NORMAL); } 

Su mejor dejar la puesta en práctica del androide de la detección de la orientación lo maneja. Ahora, sí los valores que obtienes son de -PI a PI, y puedes convertirlos en grados (0-360). He creado una clase de propósito general para hacer esto (se puede encontrar aquí ), algunas partes relevantes:

Almacenamiento de datos a procesar:

 @Override public void onSensorChanged(SensorEvent sensorEvent) { switch (sensorEvent.sensor.getType()) { case Sensor.TYPE_ACCELEROMETER: mAccValues[0] = sensorEvent.values[0]; mAccValues[1] = sensorEvent.values[1]; mAccValues[2] = sensorEvent.values[2]; break; case Sensor.TYPE_MAGNETIC_FIELD: mMagValues[0] = sensorEvent.values[0]; mMagValues[1] = sensorEvent.values[1]; mMagValues[2] = sensorEvent.values[2]; break; } } 

Cálculo del rollo, tono y guiñada (azimut). mI y mI son arrys para mantener matrices de rotación e inclinación, mO es una matriz temporal. La matriz mResults tiene los valores en grados, al final:

  private void updateData() { SensorManager.getRotationMatrix(mR, mI, mAccValues, mMagValues); /** * arg 2: what world(according to app) axis , device's x axis aligns with * arg 3: what world(according to app) axis , device's y axis aligns with * world x = app's x = app's east * world y = app's y = app's north * device x = device's left side = device's east * device y = device's top side = device's north */ switch (mDispRotation) { case Surface.ROTATION_90: SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, mR2); break; case Surface.ROTATION_270: SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_MINUS_Y, SensorManager.AXIS_X, mR2); break; case Surface.ROTATION_180: SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_MINUS_X, SensorManager.AXIS_MINUS_Y, mR2); break; case Surface.ROTATION_0: default: mR2 = mR; } SensorManager.getOrientation(mR2, mO); //--upside down when abs roll > 90-- if (Math.abs(mO[2]) > PI_BY_TWO) { //--fix, azimuth always to true north, even when device upside down, realistic -- mO[0] = -mO[0]; //--fix, roll never upside down, even when device upside down, unrealistic -- //mO[2] = mO[2] > 0 ? PI - mO[2] : - (PI - Math.abs(mO[2])); //--fix, pitch comes from opposite , when device goes upside down, realistic -- mO[1] = -mO[1]; } CircleUtils.convertRadToDegrees(mO, mOut); CircleUtils.normalize(mOut); //--write-- mResults[0] = mOut[0]; mResults[1] = mOut[1]; mResults[2] = mOut[2]; }