kalman.cpp

00001 #include "kalman.h"
00002 // derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
00003 kalman::kalman(void){
00004     P[0][0]     = 0;
00005     P[0][1]     = 0;
00006     P[1][0]     = 0;
00007     P[1][1]     = 0;
00008     
00009     angle       = 0;
00010     bias        = 0;
00011     
00012     Q_angle     = 0.001;
00013     Q_gyroBias  = 0.003;
00014     R_angle     = 0.03;     
00015 }
00016 
00017 double kalman::getAngle(double newAngle, double newRate, double dt){
00018     //predict the angle according to the gyroscope
00019     rate         = newRate - bias;
00020     angle        = dt * rate;
00021     //update the error covariance
00022     P[0][0]     += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
00023     P[0][1]     -= dt * P[1][1];
00024     P[1][0]     -= dt * P[1][1];
00025     P[1][1]     += dt * Q_gyroBias;
00026     //difference between the predicted angle and the accelerometer angle
00027     y            = newAngle - angle;
00028     //estimation error
00029     S            = P[0][0] + R_angle;
00030     //determine the kalman gain according to the error covariance and the distrust
00031     K[0]         = P[0][0]/S;
00032     K[1]         = P[1][0]/S;
00033     //adjust the angle according to the kalman gain and the difference with the measurement
00034     angle       += K[0] * y;
00035     bias        += K[1] * y;
00036     //update the error covariance
00037     P[0][0]     -= K[0] * P[0][0];
00038     P[0][1]     -= K[0] * P[0][1];
00039     P[1][0]     -= K[1] * P[0][0];
00040     P[1][1]     -= K[1] * P[0][1];
00041 
00042     return angle;
00043 }
00044 void kalman::setAngle(double newAngle) { angle = newAngle; };
00045 void kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; };
00046 void kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; };
00047 void kalman::setRangle(double newR_angle) { R_angle = newR_angle; };
00048 
00049 double kalman::getRate(void) { return rate; };
00050 double kalman::getQangle(void) { return Q_angle; };
00051 double kalman::getQbias(void) { return Q_gyroBias; };
00052 double kalman::getRangle(void) { return R_angle; };