android - instalar - giroscopio para celular

Usando el giroscopio de Android en lugar de acelerómetro. Encuentro muchos pedazos y piezas, pero no hay código completo

Question

El video de Sensor Fusion 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 solo usa acelerómetro y brújula. También uso un filtro de Kalman en los 3 valores de orientación, pero eso es demasiado código para mostrar aquí. En última instancia, esto funciona bien, pero el resultado es demasiado nervioso o demasiado lento dependiendo de lo que haga con los resultados y de cuán bajo sea el factor de filtrado.

/** 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);
}

Intenté averiguar cómo agregar datos de giroscopio, pero no lo estoy haciendo bien. El documento de Google en http://developer.android.com/reference/android/hardware/SensorEvent.html muestra algunos códigos para obtener una matriz delta de los datos del giroscopio. La idea parece ser que yo accionaría los filtros del acelerómetro y los sensores magnéticos para que estuvieran realmente estables. Eso haría un seguimiento de la orientación a largo plazo.

Luego, mantendría un historial de las matrices N delta más recientes del giroscopio. Cada vez que obtenía una nueva, dejaba la más antigua y las multiplicaba todas juntas para obtener una matriz final que multiplicaría contra la matriz estable devuelta por el acelerómetro y los sensores magnéticos.

Esto no parece funcionar. O, al menos, mi implementación no funciona. El resultado es mucho más nervioso que el acelerómetro. Aumentar el tamaño de la historia del giroscopio en realidad aumenta la vibración, lo que me hace pensar que no estoy calculando los valores correctos del 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 contiene mis cambios del primer bloque de código que agrega el giroscopio a la mezcla.

Específicamente, el factor de filtrado para la aceleración se hace más pequeño (lo que hace que el valor sea más estable). La clase MatrixHistory mantiene un registro de los últimos 100 valores deltaRotationMatrix del giroscopio que se calculan en el método gyroscopeSensorChanged.

He visto muchas preguntas en este sitio sobre este tema. Me han ayudado a llegar a este punto, pero no puedo averiguar qué hacer a continuación. Realmente deseo que el chico de Sensor Fusion haya publicado algún código en alguna parte. Obviamente lo tenía todo arreglado.

Answer 1

A la pregunta de dónde encontrar el código completo, aquí hay una implementación predeterminada en el jelly bean de Android: https://android.googlesource.com/platform/frameworks/base/+/jb-release/services/sensorservice/ Comience por verificar la fusión. cpp / h. Utiliza parámetros de Rodrigues modificados (cercanos a los ángulos de Euler) en lugar de cuaterniones. Además de la orientación, el filtro de Kalman estima la deriva del giro. Para actualizaciones de medición usa magnetómetro y, un poco incorrectamente, aceleración (fuerza específica).

Para utilizar el código, debe ser un asistente o conocer los conceptos básicos de INS y KF. Muchos parámetros deben ajustarse para que el filtro funcione. Como Edward expresó adecuadamente, estos chicos están haciendo esto para vivir.

Al menos en la galaxia nexus de google, esta implementación predeterminada no se utiliza y está anulada por el sistema propietario de Invense.

Answer 2

Bueno, +1 para ti incluso por saber lo que es un filtro de Kalman. Si lo desea, editaré esta publicación y le daré el código que escribí hace un par de años para hacer lo que está tratando de hacer.

Pero primero, te diré por qué no lo necesitas.

Las implementaciones modernas de la pila de sensores de Android utilizan Sensor Fusion , como Stan mencionó anteriormente. Esto solo significa que todos los datos disponibles (accel, mag, gyro) se recopilan en un algoritmo, y luego todas las salidas se vuelven a leer en forma de sensores de Android.

Edit: Me topé con este excelente Google Tech Talk sobre el tema: Sensor Fusion en dispositivos Android: una revolución en el procesamiento de movimiento . Bien vale la pena los 45 minutos para verlo si te interesa el tema.

En esencia, Sensor Fusion es una caja negra. He buscado en el código fuente de la implementación de Android, y es un gran filtro de Kalman escrito en C ++. Hay bastante buen código allí, y mucho más sofisticado que cualquier filtro que haya escrito, y probablemente más sofisticado que lo que estás escribiendo. Recuerda, estos chicos están haciendo esto para vivir.

También sé que al menos un fabricante de chipset tiene su propia implementación de fusión de sensores. El fabricante del dispositivo elige entre la implementación de Android y del proveedor en función de sus propios criterios.

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

De todos modos, todo se reduce a que la fusión de sensores incorporada en su dispositivo probablemente sea superior a cualquier cosa que usted o yo podamos improvisar. Entonces, lo que realmente quieres hacer es acceder a eso.

En Android, hay sensores tanto físicos como virtuales. Los sensores virtuales son los que se sintetizan a partir de los sensores físicos disponibles. El ejemplo más conocido es TYPE_ORIENTATION, que toma un acelerómetro y un magnetómetro y crea una salida de balanceo / inclinación / rumbo. (Por cierto, no debes 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 del acelerómetro con el efecto de movimiento filtrado TYPE_LINEAR_ACCELERATION es el acelerómetro con el componente de gravedad filtrado.

Estos dos sensores virtuales se sintetizan a través de una combinación de entrada de acelerómetro y entrada de giro.

Otro sensor notable es TYPE_ROTATION_VECTOR, que es un Quaternion sintetizado a partir de acelerómetro, magnetómetro y giroscopio. Representa la orientación tridimensional completa del dispositivo con los efectos de la aceleración lineal filtrada.

Sin embargo, los Cuaterniones son un poco abstractos para la mayoría de las personas, y como de todas formas está trabajando con transformaciones 3D, su mejor enfoque es combinar TYPE_GRAVITY y TYPE_MAGNETIC_FIELD a través de SensorManager.getRotationMatrix ().

Un punto más: si está trabajando con un dispositivo que ejecuta una versión anterior de Android, debe detectar que no está recibiendo eventos TYPE_GRAVITY y usar TYPE_ACCELEROMETER en su lugar. Teóricamente, este sería un lugar para usar su propio filtro kalman, pero si su dispositivo no tiene la fusión de sensores incorporada, probablemente tampoco tenga giroscopios.

De todos modos, aquí hay un 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 tiene una biblioteca de Quaternion a mano, es probable que sea más simple recibir TYPE_ROTATION_VECTOR y convertirlo en una matriz.