Kalman滤波各参数的一些调整

      接触kalman滤波已经很久了，现在也只是会用而已，对于里面计算公式的推理仍然是糊里糊涂。曾经也画过大概一周时间学习，最后还是放弃了。先给出kalman滤波的源程序，程序是我从一个坛子上下载的。网上很容易搜到。

 
static float dt=0.0044;        //传感器采样周期 不一定非要等于循环周期
float Q_acce=0.01,Q_gyro=0.005, R_angle=10;
float H_0=1; // 测量矩阵
//R_angle为测量误差  Q为预测误差
static float PP[2][2]={1,0,0,1}; //初始化协方差矩阵
float Pdot[4];
float angle_error;
float gyro_bias=0;
void Kalman_Filter_X(float accel,float gyro)    // range -1000  -2g
{
      
   floatPHt_0,PHt_1,E,K_0,K_1,Y_0,Y_1;
   angle_x+=(gyro-gyro_bias)*dt;  // 由角速度积分得到的角度  /***The ChangePoint***/
        Pdot[0]=Q_acce-PP[0][1]-PP[1][0];
        Pdot[1]=-PP[1][1];
        Pdot[2]=-PP[1][1];
        Pdot[3]=Q_gyro;
        
        PP[0][0]+=Pdot[0]*dt;
        PP[0][1]+=Pdot[1]*dt;
        PP[1][0]+=Pdot[2]*dt;
        PP[1][1]+=Pdot[3]*dt;
      
        PHt_0=H_0*PP[0][0];
        PHt_1=H_0*PP[1][0];
      
        E=R_angle+H_0*PHt_0; //估计偏差
      
        K_0=PHt_0/E;
        K_1=PHt_1/E;
      
        angle_error=accel-angle_x;
        angle_x+=K_0*angle_error; // 最优估计值
        gyro_bias+=K_1*angle_error;
        rotate_x=gyro-gyro_bias;// 最优估计值
        
        
       //后验估计，更新协方差矩阵
        Y_0=PHt_0;
        Y_1=H_0*PP[0][1];
        
        PP[0][0]-=K_0*Y_0;
        PP[0][1]-=K_0*Y_1;
        PP[1][0]-=K_1*Y_0;
        PP[1][1]-=K_1*Y_1;
}

Kalman滤波相对于互补滤波的其中一个优点就是他可自动估计出陀螺仪的零偏值并自动减去，所以使用kalman滤波不必非要把陀螺仪校准到静止时输出为零只要是一个固定的值就可以。

推荐大家调节kalman滤波参数的时候，使用一个串口示波器，同时打印出由加速度计算出的当前角度（A曲线）、单纯由陀螺仪积分得出的当前角度（B曲线）、kalman滤波融合后的角度（C曲线）。

观察着三条曲线，调节参数最终使得C曲线能够紧紧跟随A曲线，并且C曲线和B曲线保持上下平移关系（陀螺仪零偏已校准）如下图。先静态调节（不开驱动电机），在动态调节（开驱动电机）。

红线： A曲线  黄线：B曲线  蓝线  C曲线 

下面开始说各个参数如何调节

1  dt

即采样时间，传感器连续两次采样的时间间隔，这个Dt（积分时间）必须要非常精确，如Dt=2ms 5ms 10ms等，

可以用一个定时器去卡时间。dt的值理论上应该等于采样间隔即定时器设定的时间Dt。但是实际应用中发现

这个较好滤波效果的dt往往并不等于定时器设定的时间Dt。而是在其上下小幅的浮动。dt≈Dt 在这里在多说

一句，调节dt，从结果上来讲和调节陀螺仪的比例系数是一样的（在这里假定大家已经调节的好陀螺仪的比

例了，陀螺仪的比例值将单片机采到陀螺仪AD值转化为度/秒）。

调节方法是 C曲线跟随A曲线过冲， dt大了应减小

                    C 曲线跟随A曲线迟缓   dt小了 应加大

 

 

上图 dt 过小（黄线：C曲线 红线：A曲线）  

 上图 dt 过大（黄线：C曲线红线：A曲线）   

 

上图 dt=0.01  严重过冲 （黄线：C曲线 红线：A曲线） dt=0.007  过冲（黄线：C曲线 红线：A曲线）

上图 dt=0.006 过冲（黄线：C曲线 红线：A曲线）

上图dt=0.005  跟踪较好（黄线：C曲线 红线：A曲线）

 

2  Q_acce,Q_gyro

Q_acce,Q_gyro分别表示对加速度计和陀螺仪的信任度，Q_acce越大

C曲线对A曲线跟的越紧密，滤波后的噪声越大，Q_gyro越大C曲线对

B曲线跟的越紧密，要根据实际情况分析后调整。

上图 Q_gyro=0.3 可以看出 下降沿有过冲 （红线：C曲线 蓝线：A曲线）

Q_gyro=0.005 下降沿的过冲减小了 （红线：C曲线 蓝线：A曲线）

有时候大家在调参数的时候，怎么调节dt都达不到好的滤波效果，这时候调节下

Q_gyro和Q_acce 说不定会有意外的收获 ！

 

3  R_angle

R_angle对滤波效果的影响是在开驱动电机之后显现出来的。

如果静态调节是时波形很好，可是一开动电机后kalman滤波后的波形变得很乱，

这主要是由于电机对加速度计的干扰造成的，这时可以适当减小Q_acce

增大R_angle.  R_angle越大对电机造成的噪声越不敏感，即滤波后的数据跳动越小

但R_angle越大 kalman滤波输出收敛的越慢。如下图

（红线：C曲线 蓝线：A曲线）

上图 第一次连续晃动是的 Rangle 小于 第二次连续晃动时的Rangle 明显第二次晃动之后红线经过较长的时间才跟踪到蓝线。

不过一般在实际应用中并不会出现这样剧烈、大幅度的晃动，所以适当增大一点R_angle换来低的噪声的同时牺牲一点收敛速度还是值得的。

红色：A曲线  黄色：B曲线  蓝色: C曲线

以上内容是我观察总结所得，如有不对之处还请大家批评指正。