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

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

定义的结构体

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;