Problèmes de gyroscope avec l'orientation de l'appareil

Question

J'obtiens des données de tangage et de roulis à partir du gyroscope de mon appareil en utilisant ce tutoriel : http://www.thousand-thoughts.com/2012/03/Android-sensor-fusion-tutorial/

Tous les relevés sont extrêmement précis (un filtre est appliqué au code dans le tutoriel pour éliminer la dérive du gyroscope). Malheureusement, le code ne fonctionne que lorsque mon appareil est placé à plat sur une surface parallèle au sol. La position la plus idéale pour que mon application fonctionne serait que le haut de l'appareil soit dirigé vers le haut (c'est-à-dire que l'appareil soit perpendiculaire au sol, l'écran faisant face à l'utilisateur). Chaque fois que j'oriente mon appareil dans cette position, les valeurs de tangage passent à +90 degrés (comme prévu). Ce que j'aimerais faire, c'est définir cette position comme le point à 0 degré (ou position initiale) de mon appareil, de sorte que les valeurs d'inclinaison soient de 0 degré lorsque mon appareil est orienté verticalement (en mode portrait) avec l'écran face à l'utilisateur.

J'ai demandé à l'auteur du tutoriel de m'aider sur ce point et il m'a répondu :

"Si vous voulez avoir la position verticale comme position initiale, vous devrez faire pivoter votre cadre de référence en conséquence. La méthode la plus simple consiste à faire pivoter la matrice de rotation résultante de -90 degrés autour de l'axe des X. Mais il faut faire attention à quel moment de l'algorithme appliquer cette rotation. Mais vous devez faire attention à quel moment de l'algorithme vous devez appliquer cette rotation. N'oubliez jamais que les rotations ne sont pas des opérations commutatives. Pour être plus précis sur ce point, il faudrait que je revoie à nouveau le code, puisque je n'ai pas travaillé avec depuis un moment maintenant."

Je suis vraiment très confus et je ne sais pas comment faire pivoter mon cadre de référence. Je suppose que le résultat final est que je n'ai aucune idée de la manière de faire pivoter la matrice de -90 degrés autour de l'axe des x. Si quelqu'un pouvait m'aider sur ce point, ce serait fantastique. Voici mon code au cas où quelqu'un voudrait s'y référer :

public class AttitudeDisplayIndicator extends SherlockActivity implements SensorEventListener {

private SensorManager mSensorManager = null;

// angular speeds from gyro
private float[] gyro = new float[3];

// rotation matrix from gyro data
private float[] gyroMatrix = new float[9];

// orientation angles from gyro matrix
private float[] gyroOrientation = new float[3];

// magnetic field vector
private float[] magnet = new float[3];

// accelerometer vector
private float[] accel = new float[3];

// orientation angles from accel and magnet
private float[] accMagOrientation = new float[3];

// final orientation angles from sensor fusion
private float[] fusedOrientation = new float[3];

// accelerometer and magnetometer based rotation matrix
private float[] rotationMatrix = new float[9];

public static final float EPSILON = 0.000000001f;
private static final float NS2S = 1.0f / 1000000000.0f;
private float timestamp;
private boolean initState = true;

public static final int TIME_CONSTANT = 30;
public static final float FILTER_COEFFICIENT = 0.98f;
private Timer fuseTimer = new Timer();

// The following members are only for displaying the sensor output.
public Handler mHandler;
DecimalFormat d = new DecimalFormat("#.##");

//ADI background image.
private ImageView adiBackground;

//ADI axes.
private ImageView adiAxes;

//ADI frame.
private ImageView adiFrame;

//Layout.
private RelativeLayout layout;

//Pitch and Roll TextViews.
private TextView pitchAngleText;
private TextView bankAngleText;

//Instantaneous output values from sensors as the device moves.
public static double pitch;
public static double roll;

//Matrix for rotating the ADI (roll).
Matrix mMatrix = new Matrix();

@Override
public void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    setContentView(R.layout.activity_attitude_display_indicator);

    gyroOrientation[0] = 0.0f;
    gyroOrientation[1] = 0.0f;
    gyroOrientation[2] = 0.0f;

    // initialise gyroMatrix with identity matrix
    gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f;
    gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f;
    gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f;

    // get sensorManager and initialise sensor listeners
    mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
    initListeners();

    // wait for one second until gyroscope and magnetometer/accelerometer
    // data is initialised then scedule the complementary filter task
    fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(),
                                  1000, TIME_CONSTANT);

    mHandler = new Handler();
    adiBackground = (ImageView) findViewById(R.id.adi_background);
    adiFrame = (ImageView) findViewById(R.id.adi_frame);
    adiAxes = (ImageView) findViewById(R.id.adi_axes);

    layout = (RelativeLayout) findViewById(R.id.adi_layout);
    new Color();
    layout.setBackgroundColor(Color.rgb(150, 150, 150));

    pitchAngleText = (TextView) findViewById(R.id.pitch_angle_text);
    bankAngleText = (TextView) findViewById(R.id.bank_angle_text);

}

// This function registers sensor listeners for the accelerometer, magnetometer and gyroscope.
public void initListeners(){
    mSensorManager.registerListener(this,
        mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
        SensorManager.SENSOR_DELAY_FASTEST);

    mSensorManager.registerListener(this,
        mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
        SensorManager.SENSOR_DELAY_FASTEST);

    mSensorManager.registerListener(this,
        mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
        SensorManager.SENSOR_DELAY_FASTEST);
}

//@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {    

}

//@Override
public void onSensorChanged(SensorEvent event) {
    switch(event.sensor.getType()) {
    case Sensor.TYPE_ACCELEROMETER:
        // copy new accelerometer data into accel array and calculate orientation
        System.arraycopy(event.values, 0, accel, 0, 3);
        calculateAccMagOrientation();
        break;

    case Sensor.TYPE_GYROSCOPE:
        // process gyro data
        gyroFunction(event);
        break;

    case Sensor.TYPE_MAGNETIC_FIELD:
        // copy new magnetometer data into magnet array
        System.arraycopy(event.values, 0, magnet, 0, 3);
        break;
    }
}

// calculates orientation angles from accelerometer and magnetometer output
    public void calculateAccMagOrientation() {
        if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
            SensorManager.getOrientation(rotationMatrix, accMagOrientation);
        }
    }

    // This function is borrowed from the Android reference
    // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
    // It calculates a rotation vector from the gyroscope angular speed values.
    private void getRotationVectorFromGyro(float[] gyroValues,
            float[] deltaRotationVector,
            float timeFactor)
    {
        float[] normValues = new float[3];

        // Calculate the angular speed of the sample
        float omegaMagnitude =
        (float)Math.sqrt(gyroValues[0] * gyroValues[0] +
        gyroValues[1] * gyroValues[1] +
        gyroValues[2] * gyroValues[2]);

        // Normalize the rotation vector if it's big enough to get the axis
        if(omegaMagnitude > EPSILON) {
        normValues[0] = gyroValues[0] / omegaMagnitude;
        normValues[1] = gyroValues[1] / omegaMagnitude;
        normValues[2] = gyroValues[2] / 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 * timeFactor;
        float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
        float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
        deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
        deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
        deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
        deltaRotationVector[3] = cosThetaOverTwo;
    }

    // This function performs the integration of the gyroscope data.
    // It writes the gyroscope based orientation into gyroOrientation.
    public void gyroFunction(SensorEvent event) {
        // don't start until first accelerometer/magnetometer orientation has been acquired
        if (accMagOrientation == null)
            return;

        // initialisation of the gyroscope based rotation matrix
        if(initState) {
            float[] initMatrix = new float[9];
            initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
            float[] test = new float[3];
            SensorManager.getOrientation(initMatrix, test);
            gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
            initState = false;
        }

        // copy the new gyro values into the gyro array
        // convert the raw gyro data into a rotation vector
        float[] deltaVector = new float[4];
        if(timestamp != 0) {
            final float dT = (event.timestamp - timestamp) * NS2S;
        System.arraycopy(event.values, 0, gyro, 0, 3);
        getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
        }

        // measurement done, save current time for next interval
        timestamp = event.timestamp;

        // convert rotation vector into rotation matrix
        float[] deltaMatrix = new float[9];
        SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);

        // apply the new rotation interval on the gyroscope based rotation matrix
        gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);

        // get the gyroscope based orientation from the rotation matrix
        SensorManager.getOrientation(gyroMatrix, gyroOrientation);
    }

    private float[] getRotationMatrixFromOrientation(float[] o) {
        float[] xM = new float[9];
        float[] yM = new float[9];
        float[] zM = new float[9];

        float sinX = (float)Math.sin(o[1]);
        float cosX = (float)Math.cos(o[1]);
        float sinY = (float)Math.sin(o[2]);
        float cosY = (float)Math.cos(o[2]);
        float sinZ = (float)Math.sin(o[0]);
        float cosZ = (float)Math.cos(o[0]);

        // rotation about x-axis (pitch)
        xM[0] = 1.0f; xM[1] = 0.0f; xM[2] = 0.0f;
        xM[3] = 0.0f; xM[4] = cosX; xM[5] = sinX;
        xM[6] = 0.0f; xM[7] = -sinX; xM[8] = cosX;

        // rotation about y-axis (roll)
        yM[0] = cosY; yM[1] = 0.0f; yM[2] = sinY;
        yM[3] = 0.0f; yM[4] = 1.0f; yM[5] = 0.0f;
        yM[6] = -sinY; yM[7] = 0.0f; yM[8] = cosY;

        // rotation about z-axis (azimuth)
        zM[0] = cosZ; zM[1] = sinZ; zM[2] = 0.0f;
        zM[3] = -sinZ; zM[4] = cosZ; zM[5] = 0.0f;
        zM[6] = 0.0f; zM[7] = 0.0f; zM[8] = 1.0f;

        // rotation order is y, x, z (roll, pitch, azimuth)
        float[] resultMatrix = matrixMultiplication(xM, yM);
        resultMatrix = matrixMultiplication(zM, resultMatrix);
        return resultMatrix;
    }

    private float[] matrixMultiplication(float[] A, float[] B) {
        float[] result = new float[9];

        result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
        result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
        result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

        result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
        result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
        result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

        result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
        result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
        result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

        return result;
    }

    class calculateFusedOrientationTask extends TimerTask {
        public void run() {
            float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;

            /*
             * Fix for 179° <--> -179° transition problem:
             * Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.
             * If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360° from the result
             * if it is greater than 180°. This stabilizes the output in positive-to-negative-transition cases.
             */

            // azimuth
            if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
                fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]);
                fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
            }
            else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
                fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI));
                fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0;
            }
            else {
                fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
            }

            // pitch
            if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) {
                fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[1]);
                fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
            }
            else if (accMagOrientation[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) {
                fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * (accMagOrientation[1] + 2.0 * Math.PI));
                fusedOrientation[1] -= (fusedOrientation[1] > Math.PI)? 2.0 * Math.PI : 0;
            }
            else {
                fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1];
            }

            // roll
            if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) {
                fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[2]);
                fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
            }
            else if (accMagOrientation[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) {
                fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * (accMagOrientation[2] + 2.0 * Math.PI));
                fusedOrientation[2] -= (fusedOrientation[2] > Math.PI)? 2.0 * Math.PI : 0;
            }
            else {
                fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2];
            }

            // overwrite gyro matrix and orientation with fused orientation
            // to comensate gyro drift
            gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
            System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);

            // update sensor output in GUI
            mHandler.post(updateOrientationDisplayTask);
        }
    }

Merci d'avance pour votre aide !

Answer 1

La théorie...

Je ne suis pas vraiment sûr du format dans lequel votre matrice de "cadre de référence" est représentée, mais en général, les rotations sont effectuées avec une multiplication de matrice.

En gros, vous prenez votre "matrice de cadre de référence" et la multipliez par une matrice de rotation de 90 degrés.

Une telle matrice peut être trouvée sur Wikipedia : Matrices de rotation tridimensionnelles

Puisque votre angle est de 90 degrés, vos sinus et cosinus se résument à des 1 ou des 0 que vous pouvez insérer directement dans la matrice au lieu de calculer les sinus et les cosinus. Par exemple, une matrice qui effectuerait une rotation de 90 degrés dans le sens inverse des aiguilles d'une montre autour de l'axe des x ressemblerait à ceci :

1   0   0
0   0   1
0  -1   0

Veuillez également noter que les matrices de ce type opèrent sur des vecteurs de lignes de coordonnées x y z. Ainsi, par exemple, si vous avez un point dans l'espace situé à (2,5,7) et que vous souhaitez le faire pivoter en utilisant la matrice ci-dessus, vous devrez effectuer l'opération suivante :

|2 5 7| |1  0  0|
        |0  0  1|
        |0 -1  0|

Ce qui donne [2 -7 5]

...appliqué à votre code

J'ai jeté un coup d'œil rapide à votre code et il semble que la modification que vous devez apporter concerne la sortie de calculateAccMagOrientation() car elle est utilisée pour initialiser l'orientation du dispositif.

1: public void calculateAccMagOrientation() {
2:     if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
3:         SensorManager.getOrientation(rotationMatrix, accMagOrientation);
4:     }
5: }

À la ligne 2 de l'extrait ci-dessus, vous obtenez votre rotationMatrix initiale. Essayez de multiplier rotationMatrix par une matrice de rotation de 90 degrés réalisée à la main avant d'appeler getOrientation à la ligne 3. Je pense que cela réalignera efficacement votre orientation de référence :

public void calculateAccMagOrientation() {
    if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
        rotationMatrix = matrixMultiplication(rotationMatrix, my90DegRotationMatrix);
        SensorManager.getOrientation(rotationMatrix, accMagOrientation);
    }
}

Veuillez noter que, selon la façon dont les angles fonctionnent dans Android, vous devrez peut-être utiliser une matrice de rotation de 90 degrés dans le sens des aiguilles d'une montre plutôt que dans le sens inverse.

Solution alternative

Je viens de penser que vous pourriez peut-être aussi simplement soustraire 90 du résultat final de la hauteur avant de l'afficher ?