test

Dependencies:   mbed

kalman.h

Committer:
siwakon
Date:
2016-12-05
Revision:
0:dd400e4fe461

File content as of revision 0:dd400e4fe461:

#ifndef _kalman_H
#define _kalman_H
// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
class kalman
{
public:
    kalman(void);
    double getAngle(double newAngle, double newRate, double dt);
    
    void setAngle(double newAngle);
    void setQangle(double newQ_angle);
    void setQgyroBias(double newQ_gyroBias);
    void setRangle(double newR_angle);
    
    double getRate(void);
    double getQangle(void);
    double getQbias(void);
    double getRangle(void);
    

private:
    double P[2][2];         //error covariance matrix
    double K[2];            //kalman gain
    double y;               //angle difference
    double S;               //estimation error

    double rate;            //rate in deg/s
    double angle;           //kalman angle
    double bias;            //kalman gyro bias

    double Q_angle;         //process noise variance for the angle of the accelerometer
    double Q_gyroBias;      //process noise variance for the gyroscope bias
    double R_angle;         //measurement noise variance 
};

#endif