Leo Veenstra / kalman

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

kalman.h@1:e84e9e89e445, 2014-02-21 (annotated)

Committer:
SED9008
Date:
Fri Feb 21 13:14:11 2014 +0000
Revision:
1:e84e9e89e445
Parent:
0:8472cce76dbd 
// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SED9008 0:8472cce76dbd 1 #ifndef _kalman_H
SED9008 0:8472cce76dbd 2 #define _kalman_H
SED9008 1:e84e9e89e445 3 // derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
SED9008 0:8472cce76dbd 4 class kalman
SED9008 0:8472cce76dbd 5 {
SED9008 0:8472cce76dbd 6 public:
SED9008 0:8472cce76dbd 7 kalman(void);
SED9008 0:8472cce76dbd 8 double getAngle(double newAngle, double newRate, double dt);
SED9008 0:8472cce76dbd 9
SED9008 0:8472cce76dbd 10 void setAngle(double newAngle);
SED9008 0:8472cce76dbd 11 void setQangle(double newQ_angle);
SED9008 0:8472cce76dbd 12 void setQgyroBias(double newQ_gyroBias);
SED9008 0:8472cce76dbd 13 void setRangle(double newR_angle);
SED9008 0:8472cce76dbd 14
SED9008 0:8472cce76dbd 15 double getRate(void);
SED9008 0:8472cce76dbd 16 double getQangle(void);
SED9008 0:8472cce76dbd 17 double getQbias(void);
SED9008 0:8472cce76dbd 18 double getRangle(void);
SED9008 0:8472cce76dbd 19
SED9008 0:8472cce76dbd 20
SED9008 0:8472cce76dbd 21 private:
SED9008 0:8472cce76dbd 22 double P[2][2]; //error covariance matrix
SED9008 0:8472cce76dbd 23 double K[2]; //kalman gain
SED9008 0:8472cce76dbd 24 double y; //angle difference
SED9008 0:8472cce76dbd 25 double S; //estimation error
SED9008 0:8472cce76dbd 26
SED9008 0:8472cce76dbd 27 double rate; //rate in deg/s
SED9008 0:8472cce76dbd 28 double angle; //kalman angle
SED9008 0:8472cce76dbd 29 double bias; //kalman gyro bias
SED9008 0:8472cce76dbd 30
SED9008 0:8472cce76dbd 31 double Q_angle; //process noise variance for the angle of the accelerometer
SED9008 0:8472cce76dbd 32 double Q_gyroBias; //process noise variance for the gyroscope bias
SED9008 0:8472cce76dbd 33 double R_angle; //measurement noise variance
SED9008 0:8472cce76dbd 34 };
SED9008 0:8472cce76dbd 35
SED9008 0:8472cce76dbd 36 #endif