File content as of revision 0:c15430f1895f:

#include "kalman.h"

// Kalman filter module
 // Shamelessly ripped from http://forum.arduino.cc/index.php/topic,8652.0.html

 float Q_angle  =  0.001;
 float Q_gyro   =  0.003;
 float R_angle  =  0.03; 

 float x_angle = 0;
 float x_bias = 0;
 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;      
 float y, S;
 float K_0, K_1;


 float kalmanCalculate(float newAngle, float newRate, float  dt) 
 {
  // dt = float(looptime)/1000;                                  
   x_angle += dt * (newRate - 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 = newAngle - 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;
   
   return x_angle;
 }