Kalman.h

00001 #ifndef _Kalman_h
00002 #define _Kalman_h
00003 
00004 class Kalman {
00005 public:
00006     Kalman() {
00007         /* We will set the variables like so, these can also be tuned by the user */
00008         Q_angle = 0.001;
00009         Q_bias = 0.003;
00010         R_measure = 0.03;
00011 
00012         angle = 0; // Reset the angle
00013         bias = 0; // Reset bias
00014 
00015         P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so
00016         P[0][1] = 0;
00017         P[1][0] = 0;
00018         P[1][1] = 0;
00019     };
00020     // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
00021     double getAngle(double newAngle, double newRate, double dt) {
00022 
00023         // Discrete Kalman filter time update equations - Time Update ("Predict")
00024         // Update xhat - Project the state ahead
00025         /* Step 1 */
00026         rate = newRate - bias;
00027         angle += dt * rate;
00028 
00029         // Update estimation error covariance - Project the error covariance ahead
00030         /* Step 2 */
00031         P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
00032         P[0][1] -= dt * P[1][1];
00033         P[1][0] -= dt * P[1][1];
00034         P[1][1] += Q_bias * dt;
00035 
00036         // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
00037         // Calculate Kalman gain - Compute the Kalman gain
00038         /* Step 4 */
00039         S = P[0][0] + R_measure;
00040         /* Step 5 */
00041         K[0] = P[0][0] / S;
00042         K[1] = P[1][0] / S;
00043 
00044         // Calculate angle and bias - Update estimate with measurement zk (newAngle)
00045         /* Step 3 */
00046         y = newAngle - angle;
00047         /* Step 6 */
00048         angle += K[0] * y;
00049         bias += K[1] * y;
00050 
00051         // Calculate estimation error covariance - Update the error covariance
00052         /* Step 7 */
00053         P[0][0] -= K[0] * P[0][0];
00054         P[0][1] -= K[0] * P[0][1];
00055         P[1][0] -= K[1] * P[0][0];
00056         P[1][1] -= K[1] * P[0][1];
00057 
00058         return angle;
00059     };
00060     void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
00061     double getRate() { return rate; }; // Return the unbiased rate
00062 
00063     /* These are used to tune the Kalman filter */
00064     void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
00065     void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
00066     void setRmeasure(double newR_measure) { R_measure = newR_measure; };
00067 
00068     double getQangle() { return Q_angle; };
00069     double getQbias() { return Q_bias; };
00070     double getRmeasure() { return R_measure; };
00071 
00072 private:
00073     /* Kalman filter variables */
00074     double Q_angle; // Process noise variance for the accelerometer
00075     double Q_bias; // Process noise variance for the gyro bias
00076     double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
00077 
00078     double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
00079     double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
00080     double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
00081 
00082     double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
00083     double K[2]; // Kalman gain - This is a 2x1 vector
00084     double y; // Angle difference
00085     double S; // Estimate error
00086 };
00087 
00088 #endif
00089