卡尔曼滤波器（一）

    float gyrox = -(gx - gxo) / GyroRatio * dt; //x轴角速度  
    float gyroy = -(gy - gyo) / GyroRatio * dt; //y轴角速度  
    float gyroz = -(gz - gzo) / GyroRatio * dt; //z轴角速度  
    agx += gyrox;                             //x轴角速度积分  
    agy += gyroy;                             //x轴角速度积分  
    agz += gyroz;

/* kalman start */
    Sx = 0;
    Rx = 0;
    Sy = 0;
    Ry = 0;
    Sz = 0;
    Rz = 0;
for (int i = 1; i < 10; i++) {                
         //测量值平均值运算 ->加速度平均值  
        a_x[i - 1] = a_x[i];
        Sx += a_x[i];
        a_y[i - 1] = a_y[i];
        Sy += a_y[i];
        a_z[i - 1] = a_z[i];
        Sz += a_z[i];
    }
    a_x[9] = aax;
    Sx += aax;
    Sx /= 10;                                 //x轴加速度平均值  
    a_y[9] = aay;
    Sy += aay;
    Sy /= 10;                                 //y轴加速度平均值  
    a_z[9] = aaz;
    Sz += aaz;
    Sz /= 10;                                //z轴加速度平均值  
    
//计算方差
    for (int i = 0; i < 10; i++) {
        Rx += sq(a_x[i] - Sx);
        Ry += sq(a_y[i] - Sy);
        Rz += sq(a_z[i] - Sz);
    }

    Rx = Rx / 9;                              //方差  
    Ry = Ry / 9;
    Rz = Rz / 9;

    Px = Px + 0.0025;                         // 0.0025在下面有说明...  
    Kx = Px / (Px + Rx);                      //计算卡尔曼增益  
    agx = agx + Kx * (aax - agx);             //陀螺仪角度与加速度计速度叠加  
    Px = (1 - Kx) * Px;                       //更新p值  

    Py = Py + 0.0025;
    Ky = Py / (Py + Ry);
    agy = agy + Ky * (aay - agy);
    Py = (1 - Ky) * Py;

    Pz = Pz + 0.0025;
    Kz = Pz / (Pz + Rz);
    agz = agz + Kz * (aaz - agz);
    Pz = (1 - Kz) * Pz;