El filtro que yo uso es este:
//---------------------------------constantes y variables para el filtro kalman---------------------------------------------
#define dt 0.05//[s]/constante del tiempo de muestreo
#define Qaccel 0.001
#define Qgyro 0.003
#define R 0.6
#define SoA 0.002//sensibilidad del giroscopio 2 mV/°/s
static float Vbias=0,angle=0,gyro=0,accel=0;//gyro-> lectura del giroscopio; accel->lectura del acelerometro
float PK[2][2]={{1,0},{0,1}};
//-----------------------------FILTRO KALMAN--------------------------
void kalman_update()//funcion de actualizacion del filtro kalman
{
float K[2];
float ang_err=0;//angulo de error
ang_err=accel-angle;
K[0]=PK[0][0]/(PK[0][0]+R);
K[1]=PK[1][0]/(PK[0][0]+R);
PK[0][0]-=PK[0][0]*K[0];
PK[0][1]-=PK[0][1]*K[0];
PK[1][0]-=PK[0][0]*K[1];
PK[1][1]-=PK[0][1]*K[1];
angle+=K[0]*ang_err;
Vbias+=K[1]*ang_err;
}
void kalman_prediction()//funcion de prediccion del filtro kalman
{
float Pdot[2];
Pdot[0]=-PK[0][1]-PK[1][0];
Pdot[1]=-PK[1][1];
gyro-=Vbias;
angle+=gyro*dt;
PK[0][0]+=Qaccel+Pdot[0]*dt+PK[1][1]*dt*dt;
PK[0][1]+=Pdot[1]*dt;
PK[1][0]+=Pdot[1]*dt;
PK[1][1]+=Qgyro;
//angle=accel;
}
//-------------------------------------------------------------------