kalman.h

00001 #ifndef _kalman_H
00002 #define _kalman_H
00003 // derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
00004 class kalman
00005 {
00006 public:
00007     kalman(void);
00008     double getAngle(double newAngle, double newRate, double dt);
00009     
00010     void setAngle(double newAngle);
00011     void setQangle(double newQ_angle);
00012     void setQgyroBias(double newQ_gyroBias);
00013     void setRangle(double newR_angle);
00014     
00015     double getRate(void);
00016     double getQangle(void);
00017     double getQbias(void);
00018     double getRangle(void);
00019     
00020 
00021 private:
00022     double P[2][2];         //error covariance matrix
00023     double K[2];            //kalman gain
00024     double y;               //angle difference
00025     double S;               //estimation error
00026 
00027     double rate;            //rate in deg/s
00028     double angle;           //kalman angle
00029     double bias;            //kalman gyro bias
00030 
00031     double Q_angle;         //process noise variance for the angle of the accelerometer
00032     double Q_gyroBias;      //process noise variance for the gyroscope bias
00033     double R_angle;         //measurement noise variance 
00034 };
00035 
00036 #endif