天工造物局

看到有人说互补滤波跟卡尔曼滤波效果差不多，我是不相信的。不过单我自己说不行，要有结果，后来我就用下面的代码进行测试，结果如下图
<ignore_js_op> 
结果：果然两滤波结果几乎相同。黑色曲线是互补滤波结果，黄色曲线是卡尔曼滤波结果。两者非常相似，所以需要把线条加粗才行

1. //每10ms执行一次
2.     if((millis() - time) >= 10)
3.     {
4.         digitalWrite(14, HIGH);//用于测试程序运行时间
5.            
6.         time = millis();
7.            
8.         //读传感器数据
9.         mpu_get_data();
10.            
11.         digitalWrite(13, HIGH);
13.         //互补滤波
14.         angle_hb = (0.98) * (angle_hb - mpu_data.y_gyro_f * 0.01) + (0.02)*(mpu_data.x_accel_f);
16.         //卡尔曼滤波
17.         Kalman_Filter(mpu_data.x_accel_f, -mpu_data.y_gyro_f);
19.         digitalWrite(13, LOW);   
20.       
21.         //串口发送数据
22.         Serial3.print(-mpu_data.y_gyro_f);
23.         Serial3.print(",");
24.         Serial3.print(mpu_data.x_accel_f);
25.         Serial3.print(",");
26.         Serial3.print(angle_hb);
27.         Serial3.print(",");
28.         Serial3.print(angle);
29.         Serial3.println("");
30.            
31.         digitalWrite(14, LOW);
32.     }

复制代码