非常好的滤波方法（卡尔曼滤波）

Kalman滤波
static float gyro,acceler,Vref=2.048,fangdabeishu=24.6;
static float acc,gg,acce;

static float angle, angle_dot;   
static const float Q_angle = 0.001, Q_gyro = 0.003, R_angle = 0.5, dt = 0.006;
static float PP[2][2] = {{1,0},{0,1}};
static float Pdot[4]  = {0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
static float gyro,acceler;

void Kalman_Filter(float angle_m,float gyro_m)  
{
   angle += (gyro_m-q_bias)*dt;

   Pdot[0] = Q_angle-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;

   angle_err = angle_m-angle;

   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;
   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;
   q_bias += K_1*angle_err;
   angle_dot = (gyro_m-q_bias);
}