互补滤波器陀螺加速度计

Posted 2023-03-05

【中文标题】互补滤波器陀螺加速度计

【英文标题】：Complementary filter Gyro accelerometer

【发布时间】：2015-01-06 08:01:27

【问题描述】：

我正在为陀螺仪和加速度计使用互补滤波器，仅用于方位角...我从这个网站得到它：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);

我想将此与具有漂移的真实陀螺仪数据进行比较... 为此，我使用了 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 并且加速度方向略大于 +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;
               

有时没关系，但我不知道如果我有负加速度数据和正陀螺数据该怎么办？

【参考方案1】：

当我尝试将手机旋转 360 度时......我从 matlab 得到了这个图： http://www.uppic.com/uploads/14205374821.jpg

绿色单独用于陀螺仪，红色用于融合方向，蓝色用于加速。

为什么陀螺的时间有偏移？？原因是漂移？？剧情正确吗？ 如何使用 0-360 句点代替 0_180 和 -180_0？

【讨论】：

尝试 0-360 周期 if (fusedAzimuth

【参考方案2】：

您发布的图片的链接不起作用，所以我无法从 matlab 中检查绘图。 陀螺漂移不会给数据添加任何时间偏移。可能偏移是由一些内部延迟（不可能）引起的，或者更可能是由算法中的延迟引起的。我不能确切地说，因为我看不到时间偏移有多大。

如果您只需要陀螺仪数据，则添加到互补过滤器中的部分是不必要的。所有陀螺仪数据应该已经在gyroOrientationReal 向量中。无需进一步处理。真正的陀螺仪数据不应受到accMagOrientation 的影响（您可以通过更改gyroOrientationReal if accMagOrientation[0] > 0.0 来影响。

如果您想从 [0, 360] 周期变为 [0, 180, -180, 0] 周期，您只需从结果中减去 180，即可完成。

希望能帮到你。