Diff: kalman.cpp

Revision:

0:77a7d1a1c6db

diff -r 000000000000 -r 77a7d1a1c6db kalman.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.cpp	Sat Dec 03 18:21:47 2016 +0000
@@ -0,0 +1,52 @@
+#include "kalman.h"
+// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
+kalman::kalman(void){
+    P[0][0]     = 0;
+    P[0][1]     = 0;
+    P[1][0]     = 0;
+    P[1][1]     = 0;
+    
+    angle       = 0;
+    bias        = 0;
+    
+    Q_angle     = 0.001;
+    Q_gyroBias  = 0.003;
+    R_angle     = 0.03;     
+}
+
+double kalman::getAngle(double newAngle, double newRate, double dt){
+    //predict the angle according to the gyroscope
+    rate         = newRate - bias;
+    angle        = dt * rate;
+    //update the error covariance
+    P[0][0]     += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
+    P[0][1]     -= dt * P[1][1];
+    P[1][0]     -= dt * P[1][1];
+    P[1][1]     += dt * Q_gyroBias;
+    //difference between the predicted angle and the accelerometer angle
+    y            = newAngle - angle;
+    //estimation error
+    S            = P[0][0] + R_angle;
+    //determine the kalman gain according to the error covariance and the distrust
+    K[0]         = P[0][0]/S;
+    K[1]         = P[1][0]/S;
+    //adjust the angle according to the kalman gain and the difference with the measurement
+    angle       += K[0] * y;
+    bias        += K[1] * y;
+    //update the error covariance
+    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 angle;
+}
+void kalman::setAngle(double newAngle) { angle = newAngle; };
+void kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; };
+void kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; };
+void kalman::setRangle(double newR_angle) { R_angle = newR_angle; };
+
+double kalman::getRate(void) { return rate; };
+double kalman::getQangle(void) { return Q_angle; };
+double kalman::getQbias(void) { return Q_gyroBias; };
+double kalman::getRangle(void) { return R_angle; };
\ No newline at end of file