File content as of revision 1:6bc2c5d68446:

#include "kalman.h"
// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
kalman::kalman(void){
    P[0][0]     = 0;
    P[0][1]     = 0;
    P[1][0]     = 0;
    P[1][1]     = 0;
    
    angle       = 0;
    bias        = 0;
    
    Q_angle     = 0.001;
    Q_gyroBias  = 0.003;
    R_angle     = 0.03;     
}

double kalman::getAngle(double newAngle, double newRate, double dt){
    //predict the angle according to the gyroscope
    rate         = newRate - bias;
    angle        = dt * rate;
    //update the error covariance
    P[0][0]     += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
    P[0][1]     -= dt * P[1][1];
    P[1][0]     -= dt * P[1][1];
    P[1][1]     += dt * Q_gyroBias;
    //difference between the predicted angle and the accelerometer angle
    y            = newAngle - angle;
    //estimation error
    S            = P[0][0] + R_angle;
    //determine the kalman gain according to the error covariance and the distrust
    K[0]         = P[0][0]/S;
    K[1]         = P[1][0]/S;
    //adjust the angle according to the kalman gain and the difference with the measurement
    angle       += K[0] * y;
    bias        += K[1] * y;
    //update the error covariance
    P[0][0]     -= K[0] * P[0][0];
    P[0][1]     -= K[0] * P[0][1];
    P[1][0]     -= K[1] * P[0][0];
    P[1][1]     -= K[1] * P[0][1];

    return angle;
}
void kalman::setAngle(double newAngle) { angle = newAngle; };
void kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; };
void kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; };
void kalman::setRangle(double newR_angle) { R_angle = newR_angle; };

double kalman::getRate(void) { return rate; };
double kalman::getQangle(void) { return Q_angle; };
double kalman::getQbias(void) { return Q_gyroBias; };
double kalman::getRangle(void) { return R_angle; };