一阶互补滤波的应用

上一篇给大家介绍了卡尔曼滤波,今天再给大家讲讲一阶互补滤波。

一阶互补滤波的程序非常简单，如下

void First_order_Filter(float angle_m, float gyro_m)
{
   angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
}

使用的时候，我们只需要如下调用函数：

First_order_Filter(Accel_Angle,Gyro_X); 

其中

• Accel_Angle就是原始计算出的角度值，具体可以查看上篇卡尔曼滤波；

• Gyro_X就是X轴的角速度，直接从传感器读取，不过要注意单位转换，如图所示

对于MPU6050，我们若选择FS_SEL=3，即±2000度/秒的量程，则需要如下处理

Gyro_X=Gyro_X/16.4; 

式子中，

angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);

angle_m是根据传感器原始数据计算出的角度；

gyro_m是对应轴的角速度；

dt的值，根据我们采样周期来定。如果我们5ms采样一次，则dt=0.05。在单片机处理能力许可的情况下，我们建议采样频率适当大一些。

而gyro_m* dt，就是在dt时间内，设备转过的角度。

K1的值，根据我们实际需求来，如果希望动态响应快，增大灵敏度，K取值可以大一些；如果想减小传感器本身的系统误差，减少干扰抖动，增强平滑性，K取值可以小一点；这里我们令K1=0.02。

我们将原始数据和一阶互补滤波后的数据绘制出来，如图：

可以看到，加了一阶互补滤波后，数据非常平滑，也很吻合实际情况。

最后

以上就是开放饼干为你收集整理的一阶互补滤波的应用的全部内容，希望文章能够帮你解决一阶互补滤波的应用所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。