x_angle += dt * (gyro_angle - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;
y = acc_angle - x_angle;
S =P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
x_angle = kalman filter state estimate
now was that realy so hard?
gyro_angle = gyro input in rad per sec
acc_angle = accel input in rad
dt=time since last loop
Q_angle= 0.01
Q_gyro= 0.03
R_angle = 0.7
x_angle = final estimate