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?
User | Revision | Line number | New 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 |