卡尔曼滤波器之我见(完善中...)

　　最近事情有点多,忙的有点上火,必需要权衡多个方面.下面结合这个网页中的代码修改的卡尔曼算法,便于多个角度的卡尔曼滤波,程序如下:

定义 的结构体

struct GyroKalman
{    
     float angle,angle_dot,val ;         //外部需要引用的变量
     float P[2][2];                                            
     float Pdot[4];
     float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
     //const int temp=5;    结构体中不能有const
};

滤波算法:

//-------------------------------------------------------
////-------------------------------------------------------
//-------------------------------------------------------

void init_GyroDKalman(struct GyroKalman *G)
{
        G->P[0][0] = 1;
        G->P[0][1] = 0;
        G->P[1][0] = 0;
        G->P[1][1] = 1;
        G->Pdot[0] = 0;
        G->Pdot[1] = 0;
        G->Pdot[2] = 0;
        G->Pdot[3] = 0;
        G->q_bias=0;
      G->angle_err=0; 
        G->PCt_0=0; 
        G->PCt_1=0; 
      G->E=0;
        G->K_0=0;
        G->K_1=0; 
        G->t_0=0;
        G->t_1=0;
}



//注意：dt的取值为kalman滤波器采样时间;
static const float dt=0.005;
static const char C_0 = 1;
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,struct GyroKalman *gyro_m)            //gyro_m:gyro_measure
{                                                    //传递的参数为弧度    

/* X(K|K-1)=AX(K-1|K-1)+BU(K) 公式 1 */
gyro_m->angle+=(gyro_m->val-gyro_m->q_bias) * dt; /*单位 弧度,弧度/秒 */
/* P(K|K-1)=AP(K-1|K-1)A`+Q 公式 2 对协方差求导,进行线性近似*/
gyro_m->Pdot[0]=Q_angle - gyro_m->P[0][1] - gyro_m->P[1][0];
gyro_m->Pdot[1]=- gyro_m->P[1][1];
gyro_m->Pdot[2]=- gyro_m->P[1][1];
gyro_m->Pdot[3]=Q_gyro;
gyro_m->P[0][0] += gyro_m->Pdot[0] * dt;
gyro_m->P[0][1] += gyro_m->Pdot[1] * dt;
gyro_m->P[1][0] += gyro_m->Pdot[2] * dt;
gyro_m->P[1][1] += gyro_m->Pdot[3] * dt;
gyro_m->angle_err = angle_m - gyro_m->angle;
gyro_m->PCt_0 = C_0 * gyro_m->P[0][0];
gyro_m->PCt_1 = C_0 * gyro_m->P[1][0];
gyro_m->E = R_angle + C_0 * gyro_m->PCt_0; /*E=HP(K|(K-1))H'+R */
gyro_m->K_0 = gyro_m->PCt_0 / gyro_m->E; /*Kg(k)=P(K|(K-1)H'/(HP(K|(K-1))H'+R) 公式 4 */
gyro_m->K_1 = gyro_m->PCt_1 / gyro_m->E;
gyro_m->t_0 = gyro_m->PCt_0;
gyro_m->t_1 = C_0 * gyro_m->P[0][1];
gyro_m->P[0][0] -= gyro_m->K_0 * gyro_m->t_0; //P(K|K)=(I-Kg(K)H)*P(K|(K-1)) 公式 5
gyro_m->P[0][1] -= gyro_m->K_0 * gyro_m->t_1;
gyro_m->P[1][0] -= gyro_m->K_1 * gyro_m->t_0;
gyro_m->P[1][1] -= gyro_m->K_1 * gyro_m->t_1;
/*后验估计 X(k|k)=X(K|K-1)+Kg(k)(Z(k)-HX(K|K-1)) 公式 3 ,Z(K)是K 时刻的测量值也就是代码中的 angle_m */
gyro_m->angle += gyro_m->K_0 * gyro_m->angle_err; 

gyro_m->q_bias+= gyro_m->K_1 * gyro_m->angle_err;

gyro_m->angle_dot = gyro_m->val-gyro_m->q_bias; //这里angle_dot与angle在PID控制中会起到作用

        
}

下面进行一下,简单的移动平均滤波,卡尔曼,互补滤波三种滤波方法. 红为 单陀螺仪 积分数据,黄为卡尔曼滤波,蓝为 互补滤波

有振动情况下,不晃动四轴:

 

由振动不晃动到静止不晃动:

振动,晃动

最终:

总结上面这些曲线,得出结论:卡尔曼在 0漂拟制上与互补滤波相当 ,在转动效果上与 单陀螺仪测量的数据相当,不易受振动的影响.

上位机用的自己写的软件,直观观察卡尔曼是首选的滤波算法,但是目前来看只适合单轴,我本想应用到四轴上来,目前还没有找到较

好的方法与四元数结合起来.

最左为滑动平均滤波,已经不知道漂到哪里去了,中间是卡尔曼滤波,反应较快,最右则是互补滤波,反应有点迟缓,易受振动影响.

博客为本人所写,转载请表明出处,卡尔曼滤波器之我见(完善中...)