滤波算法总结

一阶互补滤波

基本理解与对应公式

• 加速度计数据比较震荡，含有较多高频分量，陀螺仪积分得到角度值，但是时间长了之后会有零漂，因此需要对两者数据进行融合。相当于低通滤波器，滤去加速度计得到的角度的高频分量，同时去除陀螺仪的零漂。

示例代码

1. double CompFilter( double angleout , double newangle , double newgyro , double dt ,float tau )

2. {

3. float K;

4. //tau = 0.075;

5. K = tau / ( tau + dt );

6. angleout = K * (angleout + newgyro * dt) + (1-K) * newangle; //互补滤波加惯性滤波

7. return angleout;

8. }

* 输入 
* dt：两次采样时间间隔 
* tau：反应惯性滤波的系数 
* angleout：上次角度输出值 
* newangle：角速度计得到的角度值 
* newgyro：陀螺仪的角速度值 
* 输出 
* 互补滤波计算得到的角度值

优缺点

• 优点：有效去除加速度计数据的抖动
• 缺点：当惯性滤波对于过去的值有较大的权重时，即上式中K值很小时，系统会出现较大的之后效应，当K=0时，完全采用新的加速度计的值，得到的数据抖动较大。

kalman滤波

基本理解与对应公式

• 先验预测值 
  • X(k+1|k)=A X(k|k)+B U(k)
• 先验预测方差 
  • P(k+1|k)=A P(k|k) A’+Q(k)
• kalman增益矩阵的计算 
  • 首先计算量测预测值（这一步好像没有必要，可以省略。。） 
    • Z(k+1|k) = H X(k+1|k)
  • 增益矩阵 
    • Kg(k+1)= P(k+1|k) H’/ (H P(k+1|k) H’ + R(k+1))
• 后验预测值 
  • X(k+1|k+1)= X(k+1|k)+Kg(k+1) (Z(k+1)-H X(k+1|k))
• 后验预测方差 
  • P(k+1|k+1)=（I-Kg(k+1) H）P(k+1|k)

示例代码

1. //之前需要定义的变量

2. float Q_angle = 0.01; //0.001

3. float Q_gyro = 0.0003; //0.003

4. float R_angle = 0.01; //0.03

5. float x_bias = 0;

6. float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;

7. float y, S; float K_0, K_1;

8. double KalmanFilter(double angleout , double newAngle, double newRate,double dt)

9. {

10. angleout += dt * (newRate - x_bias);

11. P_00 += - dt * (P_10 + P_01) + Q_angle * dt; //在这里忽略了(dt)^2这个高阶项

12. P_01 += - dt * P_11;

13. P_10 += - dt * P_11;

14. P_11 += + Q_gyro * dt;

15. y = newAngle - angleout;

16. S = P_00 + R_angle;

17. K_0 = P_00 / S;

18. K_1 = P_10 / S;

19. angleout += K_0 * y;

20. x_bias += K_1 * y;

21. P_00 -= K_0 * P_00;

22. P_01 -= K_0 * P_01;

23. P_10 -= K_1 * P_00;

24. P_11 -= K_1 * P_01;

25. return angleout;

26. }

• 程序解释 
  • X(k+1|k) = [angleout , gyro]’，由上一次的角度值和测量得到的角速度组成的列向量
  • A = [1 dt ; 0 1] ， 2X2的数组
  • Q = [Q_angle , 0 ; 0 , Q_gyro] 2X2的协方差数组，表示过程噪声
  • R = R_angle：测量噪声
  • H = [1 0]’，一个列向量，表示测量参数的矩阵，大概是只需要观测1个角度值，因此只有一维数组即可
  • Z(k+1)，最近一次的测量值，此处是加速度计直接得到的角度

优缺点

• 优点：逐渐收敛
• 缺点：计算量太大

滤波算法总结_互补滤波会引起震荡吗为什么-CSDN博客