Kalman.h

00001 #ifndef KALMAN_H
00002 #define KALMAN_H
00003 
00004 #include <stdlib.h>
00005 #include <math.h>
00006 
00007 class Kalman 
00008 {
00009 
00010     public:
00011         Kalman();
00012         ~Kalman(void);
00013         double getAngle(double newAngle, double newRate, double dt);
00014         void setAngle(double newAngle);
00015         double getRate();
00016         
00017         void setQangle(double newQ_angle);
00018         void setQbias(double newQ_bias);
00019         void setRmeasure(double newR_measure);
00020         
00021     private:
00022         /* Kalman filter variables */
00023         double Q_angle; // Process noise variance for the accelerometer
00024         double Q_bias; // Process noise variance for the gyro bias
00025         double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
00026         
00027         double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix
00028         double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
00029         double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
00030         
00031         double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
00032         double K[2]; // Kalman gain - This is a 2x1 matrix
00033         double y; // Angle difference - 1x1 matrix
00034         double S; // Estimate error - 1x1 matrix
00035 };
00036 
00037 #endif