Diff: kalman.cpp

Revision:

0:c15430f1895f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,39 @@
+#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;
+ }