Question

我正在尝试实现一个互补滤波器，以减少漂移和噪声对SparkFun LSM9DS1 IMU的影响。目的是实时获得设备旋转的可靠表示（欧拉角）。在几个网站上了解了这种过滤器之后，我尝试编写一些代码来实现此目的，但是由于我似乎找不到有效的解决方案，因此肯定缺少某些内容。

我首先定义时间步长DT，该时间步长将用于集成陀螺仪角速度以找到三个轴中每个轴的角位置：

#define SAMPLE_RATE 952
#define DT 1/SAMPLE_RATE // 1.050420168 ms

然后，我通过将陀螺仪相对于时间的角速度积分来计算角度。我不完全确定这是否是正确的方法，并且会非常感谢您的帮助：

void calcGyroAngles()
{
    gyroAngleX += rateX*DT;
    gyroAngleY += rateY*DT;
    gyroAngleZ += rateZ*DT;
}

使用atan2函数从加速度计获取角度数据（处理该函数的功能已在LSM9DS1 Arduino库中提供），因此该函数定义了滤波器：

void runFilter()
{
    // Work out roll, pitch and yaw from accelerometer data
    calcAttitude(imu.ax, imu.ay, imu.az, -imu.my, -imu.mx, imu.mz);
    calcGyroAngles();

    // 98% of the gyro signal is fused with 2% of the accelerometer signal
    x_filtered = 0.98*(gyroAngleX) + 0.02*roll;
    y_filtered = 0.98*(gyroAngleY) + 0.02*pitch;
    z_filtered = 0.98*(gyroAngleZ) + 0.02*yaw;

    printf("%f, %f, %f \n", x_filtered, y_filtered, z_filtered);
}

由于某种原因，我从中得到的都是无意义的值，这些值会随着时间的推移不断增加，并且与我要寻找的角度不符。有人可以帮我了解如何使互补过滤器在C ++中正常工作吗？

使用C ++的IMU互补过滤器？

0 个答案: