Android 互补滤波器陀螺加速度计

我用互补滤波器来测量陀螺仪和加速度计的方位角。。。我从这个网站上得到：

过滤器的核心是：

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

我保存了结果并用matlab绘制了它们。。。但图中显示陀螺仪的方位是负的。。。但FuseDeReientation小于+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;
       }        

//RealGyro
if（gyroOrientationReal[0]<-0.5*Math.PI&&accMagOrientation[0]>0.0）{
gyroOrientationReal[0]=（浮点）（gyroOrientation[0]+2.0*数学PI）；
gyroOrientationReal[0]=（gyroOrientationReal[0]>Math.PI）？2.0*Math.PI:0；
}        

---

有时还可以，但我不知道如果我有负加速度数据和正陀螺仪数据怎么办？

当我尝试将手机旋转360度时。。我从matlab得到了这个图：

绿色表示陀螺仪，红色表示融合定向，蓝色表示加速度

• 为什么陀螺的时间有偏移？？原因是漂移？？情节正确吗
• 如何使用0-360周期而不是0_180和-180_0

---

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

如果你想要陀螺仪数据，你添加到互补滤波器的那部分是不必要的。所有陀螺仪数据都应该已经在

陀螺仪方位实

矢量中。无需进一步处理。实际陀螺仪数据不应受到

accMagOrientation

的影响（如果

accMagOrientation[0]>0.0

，您可以通过更改

gyroOrientationReal

来实现）

如果你想从[0,360]周期变为[0,180，-180,0]周期，你只需要从结果中减去180，就完成了

---

我希望我能帮你解决这个问题。

如果（fusedAzimuth<0）{fusedAzimuth=（360-Math.abs（fusedAzimuth））；}

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