滤波算法总结

一阶互补滤波

基本理解与对应公式

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

示例代码

double CompFilter( double angleout , double newangle , double newgyro , double dt ,float tau )
{
    float K;
    //tau = 0.075;
    K = tau / ( tau + dt );
    angleout = K * (angleout + newgyro * dt) + (1-K) * newangle; //互补滤波加惯性滤波
    return angleout;
}

* 输入
* 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)

示例代码

//之前需要定义的变量
float Q_angle  =  0.01; //0.001 
float Q_gyro   =  0.0003;  //0.003 
float R_angle  =  0.01;  //0.03
float x_bias = 0; 
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; 
float  y, S; float K_0, K_1;
double KalmanFilter(double angleout , double newAngle, double newRate,double dt)
{ 
    angleout += dt * (newRate - x_bias); 
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt; //在这里忽略了(dt)^2这个高阶项
    P_01 +=  - dt * P_11; 
    P_10 +=  - dt * P_11; 
    P_11 +=  + Q_gyro * dt;  
    y = newAngle - angleout; 
    S = P_00 + R_angle; 
    K_0 = P_00 / S; 
    K_1 = P_10 / S; 
    angleout +=  K_0 * y; 
    x_bias  +=  K_1 * y; 
    P_00 -= K_0 * P_00; 
    P_01 -= K_0 * P_01; 
    P_10 -= K_1 * P_00; 
    P_11 -= K_1 * P_01;  
    return angleout; 
}

• 程序解释
  
  • 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)，最近一次的测量值，此处是加速度计直接得到的角度

优缺点

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