互補濾波器陀螺加速度計

Question

我正在為陀螺儀和加速度計使用輔助濾波器，僅用於方位角...我已經從以下站點獲得了它： http : //www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/1 /

過濾器的核心是：

         /*
         * 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;
            Log.d("test","gyro Is Negative");
        }
        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;
            Log.d("test","accel Is Negative");

        }
        else {
            fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
        }
        gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
        System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);

我想將其與具有漂移的Real陀螺儀數據進行比較...為此，我使用了gyroOreintationReal ...並添加了一些代碼來保存gyroOreintation，如下所示：

 if(initState) {
        float[] initMatrix = new float[9];
        initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
        float[] test = new float[3];
        SensorManager.getOrientation(initMatrix, test);
        gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
        gyroMatrixReal = matrixMultiplication(gyroMatrixReal, 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];
    float[] deltaVectorReal = new float[4];

    if(timestamp != 0) {
        final float dT = (event.timestamp - timestamp) * NS2S;


    System.arraycopy(event.values, 0, gyro, 0, 3);
    System.arraycopy(event.values, 0, gyroReal, 0, 3);


    getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
    getRotationVectorFromGyro(gyroReal, deltaVectorReal, 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];
    float[] deltaMatrixReal = new float[9];

    SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
    SensorManager.getRotationMatrixFromVector(deltaMatrixReal, deltaVectorReal);


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


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

我保存了結果並用matlab對其進行了繪制...但是該圖顯示了陀螺儀的方向為負...但是fusedOreientation小於+150而accel方向略大於+150 ...

我該如何解決這個問題？ 我在補充過濾器的核心添加一些代碼：

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

有時候沒關系，但是我不知道加速度數據為負而陀螺數據為正怎么辦？

Answer 1

當我嘗試將手機旋轉360度時。.我從matlab獲得此圖： http : //www.uppic.com/uploads/14205374821.jpg

綠色的代表陀螺儀，紅色的代表融合方向，藍色的代表加速度。

• 為什么陀螺儀的時間抵消了？ 原因是漂移？ 情節是正確的嗎？
• 如何使用0-360周期而不是0_180和-180_0？

Answer 2

您發布的圖片的鏈接不起作用，因此我無法從matlab中查看繪圖。 陀螺儀漂移不會向數據添加任何時間偏移。 偏移可能是由某些內部延遲（不太可能）引起的，或更可能是由算法中的延遲引起的。 我無法確切說出時間偏移量，因為我看不到。

如果您只需要陀螺儀數據，則無需添加到互補濾波器中。 所有陀螺儀數據都應該已經在gyroOrientationReal向量中。 無需進一步處理。 實際的陀螺儀數據不應受accMagOrientation影響（如果accMagOrientation[0] > 0.0 ，則可以通過更改gyroOrientationReal accMagOrientation[0] > 0.0 。

如果您要從[0，360]周期變為[0，180，-180，0]周期，則只需要從結果中減去180，就可以完成。

希望我能幫到你。