Ive made a kalman filter function based on Tom Pyckes tutorial. Read the tutorial first, it will be easier to understand the function. The function lets you use the kalmanfilter on multiple axis. It works great, but youre gonna have to tune it to your application and IMU's(EDIT: to tune the filter you just change the values Sz and Sw. in the code you see Sz as 0.01 and Sw as 0.0000001 and 0.0000003(se Toms tutorial for explanation of these)).

//Kalman

double x[2][2];

double xv[2][2];

double p[2][4];

double pv[2][4];

double k[2][2];

double Inn[2];

double s[2];

double Sz = 0.01; // was 17.2 for dt =10. Sz is lower the higher delta time.

double KalmanFilter(int a, double y, double u){

x[a][1] = u+x[a][1]-dt*x[a][2];

x[a][2] = x[a][2];

Inn[a] = y-x[a][1];

s[a] = p[a][1]+Sz;

k[a][1] = (p[a][1]-dt*p[a][3])/s[a];

k[a][2] = p[a][3]/s[a];

xv[a][1] = x[a][1]+k[a][1]*(y-x[a][1]);

xv[a][2] = x[a][2]+k[a][2]*(y-x[a][1]);

x[a][1] = xv[a][1];

x[a][2] = xv[a][2];

pv[a][1] = 0.0000003+p[a][1]-k[a][1]*p[a][1]+dt*k[a][1]*p[a][2]-dt*p[a][3]-dt*(p[a][2]-dt*p[a][4]);

pv[a][2] = p[a][2]-k[a][1]*p[a][2]-dt*p[a][4];

pv[a][3] = dt*k[a][2]*p[a][2]+p[a][3]-dt*p[a][4]-k[a][2]*p[a][1];

pv[a][4] = 0.00000001-k[a][2]*p[a][2]+p[a][4];

p[a][1] = pv[a][1];

p[a][2] = pv[a][2];

p[a][3] = pv[a][3];

p[a][4] = pv[a][4];

return x[a][1];

}

on top, you see the variables declared as field. you might declare them inside the function frame but make sure to make then "volatile". Kalman filter is a statistical optimal estimator so you need the previous values to make new ones.

a is the axis number(if you only have one axis, it would be "a=0")

y is the angle from the accelerometers

u is the gyro rate

In addtition to these you need a cycle time variable which Ive called dt.

Feel free to use it out of the box.