La Fusión de Sensores video se ve muy bien, pero no hay código:
http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

Aquí está mi código que sólo utiliza el acelerómetro y la brújula. Yo también uso un filtro de Kalman en el 3 la orientación de los valores, pero eso es demasiado código para mostrar aquí. En última instancia, esto funciona bien, pero el resultado es demasiado nervioso o demasiado lag dependiendo de lo que debo hacer con los resultados y de lo bajo que realizar el filtrado de los factores.

/** Just accelerometer and magnetic sensors */
public abstract class SensorsListener2
implements
SensorEventListener
{
/** The lower this is, the greater the preference which is given to previous values. (slows change) */
private static final float accelFilteringFactor = 0.1f;
private static final float magFilteringFactor = 0.01f;
public abstract boolean getIsLandscape();
@Override
public void onSensorChanged(SensorEvent event) {
Sensor sensor = event.sensor;
int type = sensor.getType();
switch (type) {
case Sensor.TYPE_MAGNETIC_FIELD:
mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor);
mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor);
mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor);
isReady = true;
break;
case Sensor.TYPE_ACCELEROMETER:
accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor);
accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor);
accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor);
break;
default:
return;
}
if(mags != null && accels != null && isReady) {
isReady = false;
SensorManager.getRotationMatrix(rot, inclination, accels, mags);
boolean isLandscape = getIsLandscape();
if(isLandscape) {
outR = rot;
} else {
//Remap the coordinates to work in portrait mode.
SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
}
SensorManager.getOrientation(outR, values);
double x180pi = 180.0 / Math.PI;
float azimuth = (float)(values[0] * x180pi);
float pitch = (float)(values[1] * x180pi);
float roll = (float)(values[2] * x180pi);
//In landscape mode swap pitch and roll and invert the pitch.
if(isLandscape) {
float tmp = pitch;
pitch = -roll;
roll = -tmp;
azimuth = 180 - azimuth;
} else {
pitch = -pitch - 90;
azimuth = 90 - azimuth;
}
onOrientationChanged(azimuth,pitch,roll);
}
}
private float[] mags = new float[3];
private float[] accels = new float[3];
private boolean isReady;
private float[] rot = new float[9];
private float[] outR = new float[9];
private float[] inclination = new float[9];
private float[] values = new float[3];
/**
Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West
Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis.
Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
*/
public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}

Traté de averiguar cómo agregar giroscopio de datos, pero simplemente no estoy haciendo lo correcto. El google doc en http://developer.android.com/reference/android/hardware/SensorEvent.html muestra algo de código para obtener un delta de la matriz de la giroscopio de datos. La idea parece ser que me gustaría girar abajo de los filtros para el acelerómetro y sensores magnéticos para que fueran realmente estable. Que mantenga un seguimiento de largo plazo de orientación.

A continuación, me gustaría mantener un historial de los últimos N delta matrices a partir de la giroscopio. Cada vez que tengo una nueva que me gustaría dejar la más antigua y se multiplican todos ellos juntos para llegar a una final de la matriz de la que me gustaría multiplicar contra el establo de la matriz devuelta por el acelerómetro y sensores magnéticos.

Esto no parece funcionar. O, al menos, la implementación de mi no funciona. El resultado es mucho más nerviosa que sólo el acelerómetro. Aumentar el tamaño de la giroscopio de la historia, en realidad, aumenta el jitter que me hace pensar que no soy el cálculo de los valores de la derecha desde el giroscopio.

public abstract class SensorsListener3
implements
SensorEventListener
{
/** The lower this is, the greater the preference which is given to previous values. (slows change) */
private static final float kFilteringFactor = 0.001f;
private static final float magKFilteringFactor = 0.001f;
public abstract boolean getIsLandscape();
@Override
public void onSensorChanged(SensorEvent event) {
Sensor sensor = event.sensor;
int type = sensor.getType();
switch (type) {
case Sensor.TYPE_MAGNETIC_FIELD:
mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor);
mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor);
mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor);
isReady = true;
break;
case Sensor.TYPE_ACCELEROMETER:
accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor);
accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor);
accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor);
break;
case Sensor.TYPE_GYROSCOPE:
gyroscopeSensorChanged(event);
break;
default:
return;
}
if(mags != null && accels != null && isReady) {
isReady = false;
SensorManager.getRotationMatrix(rot, inclination, accels, mags);
boolean isLandscape = getIsLandscape();
if(isLandscape) {
outR = rot;
} else {
//Remap the coordinates to work in portrait mode.
SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
}
if(gyroUpdateTime!=0) {
matrixHistory.mult(matrixTmp,matrixResult);
outR = matrixResult;
}
SensorManager.getOrientation(outR, values);
double x180pi = 180.0 / Math.PI;
float azimuth = (float)(values[0] * x180pi);
float pitch = (float)(values[1] * x180pi);
float roll = (float)(values[2] * x180pi);
//In landscape mode swap pitch and roll and invert the pitch.
if(isLandscape) {
float tmp = pitch;
pitch = -roll;
roll = -tmp;
azimuth = 180 - azimuth;
} else {
pitch = -pitch - 90;
azimuth = 90 - azimuth;
}
onOrientationChanged(azimuth,pitch,roll);
}
}
private void gyroscopeSensorChanged(SensorEvent event) {
//This timestep's delta rotation to be multiplied by the current rotation
//after computing it from the gyro sample data.
if(gyroUpdateTime != 0) {
final float dT = (event.timestamp - gyroUpdateTime) * NS2S;
//Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
//Calculate the angular speed of the sample
float omegaMagnitude = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
//Normalize the rotation vector if it's big enough to get the axis
if(omegaMagnitude > EPSILON) {
axisX /= omegaMagnitude;
axisY /= omegaMagnitude;
axisZ /= omegaMagnitude;
}
//Integrate around this axis with the angular speed by the timestep
//in order to get a delta rotation from this sample over the timestep
//We will convert this axis-angle representation of the delta rotation
//into a quaternion before turning it into the rotation matrix.
float thetaOverTwo = omegaMagnitude * dT / 2.0f;
float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * axisX;
deltaRotationVector[1] = sinThetaOverTwo * axisY;
deltaRotationVector[2] = sinThetaOverTwo * axisZ;
deltaRotationVector[3] = cosThetaOverTwo;
}
gyroUpdateTime = event.timestamp;
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
//User code should concatenate the delta rotation we computed with the current rotation
//in order to get the updated rotation.
//rotationCurrent = rotationCurrent * deltaRotationMatrix;
matrixHistory.add(deltaRotationMatrix);
}
private float[] mags = new float[3];
private float[] accels = new float[3];
private boolean isReady;
private float[] rot = new float[9];
private float[] outR = new float[9];
private float[] inclination = new float[9];
private float[] values = new float[3];
//gyroscope stuff
private long gyroUpdateTime = 0;
private static final float NS2S = 1.0f / 1000000000.0f;
private float[] deltaRotationMatrix = new float[9];
private final float[] deltaRotationVector = new float[4];
//TODO: I have no idea how small this value should be.
private static final float EPSILON = 0.000001f;
private float[] matrixMult = new float[9];
private MatrixHistory matrixHistory = new MatrixHistory(100);
private float[] matrixTmp = new float[9];
private float[] matrixResult = new float[9];
/**
Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West 
Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. 
Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
*/
public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}
public class MatrixHistory
{
public MatrixHistory(int size) {
vals = new float[size][];
}
public void add(float[] val) {
synchronized(vals) {
vals[ix] = val;
ix = (ix + 1) % vals.length;
if(ix==0)
full = true;
}
}
public void mult(float[] tmp, float[] output) {
synchronized(vals) {
if(full) {
for(int i=0; i<vals.length; ++i) {
if(i==0) {
System.arraycopy(vals[i],0,output,0,vals[i].length);
} else {
MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
System.arraycopy(tmp,0,output,0,tmp.length);
}
}
} else {
if(ix==0)
return;
for(int i=0; i<ix; ++i) {
if(i==0) {
System.arraycopy(vals[i],0,output,0,vals[i].length);
} else {
MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
System.arraycopy(tmp,0,output,0,tmp.length);
}
}
}
}
}
private int ix = 0;
private boolean full = false;
private float[][] vals;
}

El segundo bloque de código que contiene mis cambios desde el primer bloque de código que añadir el giroscopio a la mezcla.

Específicamente, el filtrado de factor de accel se hace más pequeña (lo que el valor más estable). El MatrixHistory clase sigue la pista de los últimos 100 giroscopio deltaRotationMatrix valores que se calculan en el gyroscopeSensorChanged método.

He visto muchas preguntas en este sitio sobre este tema. Ellos me han ayudado a llegar a este punto, pero no puedo averiguar qué hacer a continuación. Realmente deseo que la Fusión de Sensores chico acababa de publicar el código en algún lugar. Obviamente, él tenía todos juntos.

  • De acuerdo a la «Profesional Android Programación del Sensor del» libro, InvenSense del Sensor de algoritmos de Fusión son de propiedad exclusiva, por lo que casi no es posible encontrar el código fuente de acceso público. La biblioteca está incluido en la mayoría de los dispositivos modernos a nivel de sistema, de modo que el SENSOR.TYPE_ROTATION ya proporciona mediciones con respecto a gyro basado en corto tiempo de corrección. Creo que la más elaborada de las fuentes públicas para el asunto de que se este. No estoy seguro de si es un buen reemplazo.
  • También hay varios artículos académicos relacionados con la fusión de sensores utilizando filtros kalman. Por lo general, no contienen código fuente, pero debe tener la técnica, las matemáticas y la información que usted necesita. scholar.google.com
  • ¿Por qué el filtro de paso bajo los valores magnéticos?

2 Comentarios

  1. 51

    Bien, +1 a ustedes para saber siquiera lo que es un filtro de Kalman es. Si quieres, voy a editar este post y te dan el código que escribí hace un par de años para hacer lo que estamos tratando de hacer.

    Pero primero, voy a decirle por qué usted no lo necesita.

    Las implementaciones modernas de Android sensor de uso de pila la Fusión de Sensores, como Stan se mencionó anteriormente. Esto solo significa que todos los datos disponibles — accel, mag, gyro-se recoge juntos en un algoritmo y, a continuación, todos los resultados son leer en el formulario de Android sensores.

    Edit: me tropecé en esta magnífica Google Tech Talk sobre el tema: La Fusión de sensores en los Dispositivos Android: Una Revolución en el Procesamiento del Movimiento de. Bien vale la pena el de 45 minutos para ver si usted está interesado en el tema.

    En esencia, la Fusión de Sensores es una caja negra. He mirado el código fuente de la aplicación Android, y es un gran filtro de Kalman escrito en C++. Algunos muy buenos código que hay, y mucho más sofisticado que cualquier filtro que he escrito, y probablemente más sofisticado que lo que estás escribiendo. Recuerde, estos chicos están haciendo esto para vivir.

    También sé que al menos uno de los chipset fabricante tiene su propio sensor de fusión de la aplicación. El fabricante del dispositivo, a continuación, elige entre el Androide y el proveedor de la aplicación basándose en su propio criterio.

    Finalmente, como Stan se mencionó anteriormente, Invensense tiene su propia fusión de sensores en la implementación a nivel de chip.

    De todos modos, lo que todo se reduce a es que el incorporado en la fusión de sensores en el dispositivo es probable que sea superior a cualquier cosa que usted o yo podríamos improvisar. Así que lo que realmente quiero hacer es acceder a eso.

    En Android, hay física y virtual de los sensores. Los sensores virtuales son las que se sintetizan a partir de la disposición física de los sensores. El ejemplo más conocido es TYPE_ORIENTATION que lleva acelerómetro y magnetómetro y crea roll/pitch/rúbrica de salida. (Por cierto, usted no debe usar este sensor; tiene demasiadas limitaciones.)

    Pero lo importante es que las versiones más recientes de Android contienen estos dos nuevos sensores virtuales:

    TYPE_GRAVITY es la entrada de acelerómetro con el efecto de movimiento filtrados
    TYPE_LINEAR_ACCELERATION es el acelerómetro con la gravedad de componentes filtrados.

    Estos dos sensores virtuales son sintetizados a través de una combinación de entrada de acelerómetro y giroscopio de entrada.

    Otro notable sensor es TYPE_ROTATION_VECTOR que es una Cuádrupla sintetizados a partir de acelerómetro, magnetómetro, y giroscopio. Representa la 3-d de la orientación del dispositivo con los efectos de la aceleración lineal filtrados.

    Sin embargo, los Cuaterniones son un poco abstracto para la mayoría de las personas, y dado que es probable trabajando con 3-d de las transformaciones de todos modos, el mejor enfoque es combinar TYPE_GRAVITY y TYPE_MAGNETIC_FIELD a través de SensorManager.getRotationMatrix().

    Un punto más: si estás trabajando con un dispositivo que ejecuta una versión anterior de Android, usted necesita para detectar que no está recibiendo TYPE_GRAVITY eventos y el uso TYPE_ACCELEROMETER lugar. Teóricamente, este sería un lugar para usar su propio filtro de kalman, pero si su dispositivo no dispone de fusión de sensores integrados, probablemente no tiene gyros bien.

    De todos modos, aquí está el código de ejemplo para mostrar cómo lo hago.

      //Requires 1.5 or above
    class Foo extends Activity implements SensorEventListener {
    SensorManager sensorManager;
    float[] gData = new float[3];           //Gravity or accelerometer
    float[] mData = new float[3];           //Magnetometer
    float[] orientation = new float[3];
    float[] Rmat = new float[9];
    float[] R2 = new float[9];
    float[] Imat = new float[9];
    boolean haveGrav = false;
    boolean haveAccel = false;
    boolean haveMag = false;
    onCreate() {
    //Get the sensor manager from system services
    sensorManager =
    (SensorManager)getSystemService(Context.SENSOR_SERVICE);
    }
    onResume() {
    super.onResume();
    //Register our listeners
    Sensor gsensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
    Sensor asensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
    Sensor msensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
    sensorManager.registerListener(this, gsensor, SensorManager.SENSOR_DELAY_GAME);
    sensorManager.registerListener(this, asensor, SensorManager.SENSOR_DELAY_GAME);
    sensorManager.registerListener(this, msensor, SensorManager.SENSOR_DELAY_GAME);
    }
    public void onSensorChanged(SensorEvent event) {
    float[] data;
    switch( event.sensor.getType() ) {
    case Sensor.TYPE_GRAVITY:
    gData[0] = event.values[0];
    gData[1] = event.values[1];
    gData[2] = event.values[2];
    haveGrav = true;
    break;
    case Sensor.TYPE_ACCELEROMETER:
    if (haveGrav) break;    //don't need it, we have better
    gData[0] = event.values[0];
    gData[1] = event.values[1];
    gData[2] = event.values[2];
    haveAccel = true;
    break;
    case Sensor.TYPE_MAGNETIC_FIELD:
    mData[0] = event.values[0];
    mData[1] = event.values[1];
    mData[2] = event.values[2];
    haveMag = true;
    break;
    default:
    return;
    }
    if ((haveGrav || haveAccel) && haveMag) {
    SensorManager.getRotationMatrix(Rmat, Imat, gData, mData);
    SensorManager.remapCoordinateSystem(Rmat,
    SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, R2);
    //Orientation isn't as useful as a rotation matrix, but
    //we'll show it here anyway.
    SensorManager.getOrientation(R2, orientation);
    float incl = SensorManager.getInclination(Imat);
    Log.d(TAG, "mh: " + (int)(orientation[0]*DEG));
    Log.d(TAG, "pitch: " + (int)(orientation[1]*DEG));
    Log.d(TAG, "roll: " + (int)(orientation[2]*DEG));
    Log.d(TAG, "yaw: " + (int)(orientation[0]*DEG));
    Log.d(TAG, "inclination: " + (int)(incl*DEG));
    }
    }
    }

    Hmmm; si le sucede que tiene una Cuádrupla de la biblioteca a la mano, es probablemente la más sencilla simplemente para recibir TYPE_ROTATION_VECTOR y convertir en un array.

    • ¿Hay alguna posibilidad de que uno puede acceder a estos datos en un lateral nativo? La mayoría de las cosas no son expuestos a través de NDK en el sensor.h
    • ¿Alguna vez todos de trabajo, el mío todavía está nervioso y tengo un filtro de paso bajo
    • Hola Eduardo, esta es una explicación muy buena, algo que he estado tratando de encontrar por algún tiempo. Puedes por favor decirme ¿por qué es necesario el uso de SensorManager.remapCoordinateSystem? Gracias.
    • El sistema de sensores en Android no sabe o cuidado de la forma en que el dispositivo se gira. Cada dispositivo tiene su propio sistema de coordenadas con +X a la derecha, +Y, y +Z hacia usted. Si se gira el dispositivo, por ejemplo, un teléfono en orientación horizontal, es necesario transformar el sensor de coordenadas en valores que den sentido a la forma en que usted está sosteniendo el dispositivo. Qué es exactamente lo que significa que depende de su aplicación. remapcoordinateSystem() es una función auxiliar que puede ser útil aquí. Tengo algunas notas más en efalk.org/Docs/Android/sensors_0.html#remapCoordinateSystem
    • Gracias por su respuesta de Edward. Sólo una última pregunta. Yo miraba a TYPE_GRAVITY de salida y debo decir que es muy buena. Así, en el código, necesito filtrar el pitch final de salida? O tengo que filtrar la TYPE_MAGNETIC_FILED antes de que se utiliza? Gracias.
    • Me gustaría escribir tu aplicación sin filtrar y, a continuación, ver si los resultados son lo suficientemente estable como para usted. Si no, trate de aplicar algunos de filtrado. La GRAVEDAD probablemente no necesita de filtrado, pero MAGNETIC_FIELD probablemente no. Usted también debe considerar el uso de la ROTACIÓN y de la conversión de los cuaterniones a una matriz. ROTACIÓN prácticamente lo hace todo por usted. La conversión a una matriz es casi trivial.
    • Gracias por su respuesta. Así que escribí el código para combinar la GRAVEDAD y MAGNETIC_FIELD para la orientación de la estimación y la salida es más estable cuando puedo filtrar la MAGNETIC_FIELD. Supongo que los cuaterniones obtenidos a partir de la ROTACIÓN es en el de la orientación del dispositivo. Así que en ese caso, no es necesario para mí para obtener la orientación, que es? Gracias
    • Sí, los Cuaterniones realmente tiene todo lo que desea; es sólo una cuestión de la conversión de una matriz para convertir coordenadas de dispositivo de coordenadas del mundo o viceversa.
    • Gracias Edward, que ayudaron mucho 🙂
    • Hola Edward, por favor, puedes mirar este problema que tengo. Gracias. stackoverflow.com/questions/30529272/…
    • he Aquí un sensor de fusión de la biblioteca que puede ser interesante.
    • Gracias, acabo de probarlo.
    • Acabo de encontrar este excelente video sobre el tema: youtube.com/watch?v=C7JQ7Rpwn2k
    • Una desventaja de esto puede ser una locura de salida de algunos de estos incorporado sensores virtuales en ciertos escenarios. Por ejemplo, en mi Samsung Galaxy S5, toda la virtual/compuesto de sensores producen inversa resultados para un par de segundos después de dar el dispositivo de un swing duro/temblar cuando el tiempo de votación de 55000 microsegundos o superior. Otro ejemplo con el mismo dispositivo: una vez que entró en un extraño estado en el que el sensor de las lecturas de los sensores virtuales fueron permanentemente hacia atrás hasta que el dispositivo se reinicia. En ambos casos, la materia prima (no virtual) valores de los sensores estaban bien.

  2. 5

    A la pregunta de dónde encontrar el código completo, he aquí una implementación por defecto en Android jelly bean: https://android.googlesource.com/platform/frameworks/base/+/jb-de-prensa/servicios/sensorservice/
    Comience por comprobar la fusión.cpp/h.
    Utiliza Modificado Rodrigues Parámetros (cerca de los ángulos de Euler) en lugar de cuaterniones. Además de la orientación del filtro de Kalman estima gyro a la deriva. Para la medición de las actualizaciones utiliza magnetómetro y, un poco incorrectamente, la aceleración (fuerza específica).

    Para hacer uso del código, usted debe ser un asistente o conocer los fundamentos de la INS y KF. Muchos de los parámetros tienen que ser afinado para que el filtro funcione. Como Edward adecuadamente poner, estos chicos están haciendo esto para vivir.

    Al menos en el google galaxy nexus esta implementación predeterminada se utiliza y es reemplazado por Invense del sistema de propiedad.

Dejar respuesta

Please enter your comment!
Please enter your name here