--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.py	Sun Oct 12 12:42:34 2014 +0200
@@ -0,0 +1,29 @@
+
+class KalmanFilter(object):
+    Q_angle = 0.001
+    Q_bias = 0.003
+    R_measure = 0.03
+
+    def __init__(self, angle):
+        self.angle = angle
+        self.bias = 0
+        self.P = [[0, 0], [0, 0]]
+
+    def get_and_update(self, angle, rate, dt):
+        new_rate = rate - self.bias
+        self.angle += dt * new_rate
+        P = self.P
+        P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + self.Q_angle)
+        P[0][1] -= dt * P[1][1]
+        P[1][0] -= dt * P[1][1]
+        P[1][1] += self.Q_bias * dt
+        S = P[0][0] + self.R_measure
+        K = (P[0][0] / S, P[1][0] / S)
+        y = angle - self.angle
+        self.angle += K[0] * y
+        self.bias += K[1] * y
+        P[0][0] -= K[0] * P[0][0]
+        P[0][1] -= K[0] * P[0][1]
+        P[1][0] -= K[1] * P[0][0]
+        P[1][1] -= K[1] * P[0][1]
+        return self.angle, new_rate