float K1 =0.02;
float angle, angle_dot;        
float Q_angle=0.001;// 1y3ìÔëéùμÄD-·½2î
float Q_gyro=0.003;//0.03 1y3ìÔëéùμÄD-·½2î 1y3ìÔëéùμÄD-·½2îÎaò»¸öò»DDá½áD¾ØÕó
float R_angle=0.5;// 2aá¿ÔëéùμÄD-·½2î ¼è2aá¿Æ«2î
float dt=0.005;//                 
char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/**************************************************************************
oˉêy1|Äü￡o¼òò׿¨¶ûÂüÂË2¨
èë¿ú2Îêy￡o¼óËù¶è¡￠½ÇËù¶è
·μ»Ø  Öμ￡oÎT
×÷    Õß￡oMini Balance
**************************************************************************/
void Kalman_Filter(float Accel,float Gyro)                //¼óËù¶è    ½ÇËù¶è
{
        angle+=(Gyro - Q_bias) * dt; //ÏèÑé1à¼Æ
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îμÄÎ￠·Ö

        Pdot[1]=-PP[1][1];
        Pdot[2]=-PP[1][1];
        Pdot[3]=Q_gyro;
        PP[0][0] += Pdot[0] * dt;   // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îÎ￠·ÖμÄ»y·Ö
        PP[0][1] += Pdot[1] * dt;   // =ÏèÑé1à¼ÆÎó2îD-·½2î
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
               
        Angle_err = Accel - angle;        //zk-ÏèÑé1à¼Æ
       
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
       
        E = R_angle + C_0 * PCt_0;
       
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
       
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];

        PP[0][0] -= K_0 * t_0;                 //oóÑé1à¼ÆÎó2îD-·½2î
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
               
        angle        += K_0 * Angle_err;         //oóÑé1à¼Æ
        Q_bias        += K_1 * Angle_err;         //oóÑé1à¼Æ
        angle_dot   = Gyro - Q_bias;         //êä3öÖμ(oóÑé1à¼Æ)μÄÎ￠·Ö=½ÇËù¶è
}