Diff: kalman.h

Revision:

1:d8ce226c8c2e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Tue Dec 06 06:11:54 2016 +0000
@@ -0,0 +1,36 @@
+#ifndef _kalman_H
+#define _kalman_H
+// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
+class kalman
+{
+public:
+    kalman(void);
+    double getAngle(double newAngle, double newRate, double dt);
+    
+    void setAngle(double newAngle);
+    void setQangle(double newQ_angle);
+    void setQgyroBias(double newQ_gyroBias);
+    void setRangle(double newR_angle);
+    
+    double getRate(void);
+    double getQangle(void);
+    double getQbias(void);
+    double getRangle(void);
+    
+
+private:
+    double P[2][2];         //error covariance matrix
+    double K[2];            //kalman gain
+    double y;               //angle difference
+    double S;               //estimation error
+
+    double rate;            //rate in deg/s
+    double angle;           //kalman angle
+    double bias;            //kalman gyro bias
+
+    double Q_angle;         //process noise variance for the angle of the accelerometer
+    double Q_gyroBias;      //process noise variance for the gyroscope bias
+    double R_angle;         //measurement noise variance 
+};
+
+#endif
\ No newline at end of file