如何使用磁力计为陀螺z轴添加辅助滤波器？

Question

我为陀螺仪X和陀螺仪Y值使用了互补滤波器：

float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;

但是，由于陀螺仪Z漂移，我正在使用磁力计来尝试消除它。

我有我的局部倾斜角和z轴磁力计读数，但是我不确定如何将其应用于互补滤波器。

我尝试猜测（我知道这无济于事）来尝试以下操作：

float angle_z = alpha*gyro_angle_z + ((1.0 -alpha)*accel_angle_y*InclinationAngle)/magZ;

但是，它只是在移动时增加陀螺仪z值，然后在静止时将其恢复为0。

Answer 1

使用Madgwick滤镜效果最佳。 链接可以在这里找到。 这很容易实现（如果使用正确的库并且读数非常准确。目前唯一的缺点是每次您要在新方向上使用设备时都必须校准磁力计）。