【三明治开发玩转开源智能产品，期待和更多有相同兴趣的同学一起交流、讨论。也可进入技术干货_三明治开发社区的博客-CSDN博客 我们已分享了将近20款智能产品开源方案和多篇技术文档。】

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

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。

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

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

上期精彩回顾：浅谈GD32和STM32之前的区别