http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
kalman.cpp@0:8472cce76dbd, 2014-02-21 (annotated)
- Committer:
- SED9008
- Date:
- Fri Feb 21 13:12:16 2014 +0000
- Revision:
- 0:8472cce76dbd
- Child:
- 1:e84e9e89e445
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 | #include "kalman.h" |
SED9008 | 0:8472cce76dbd | 2 | |
SED9008 | 0:8472cce76dbd | 3 | kalman::kalman(void){ |
SED9008 | 0:8472cce76dbd | 4 | P[0][0] = 0; |
SED9008 | 0:8472cce76dbd | 5 | P[0][1] = 0; |
SED9008 | 0:8472cce76dbd | 6 | P[1][0] = 0; |
SED9008 | 0:8472cce76dbd | 7 | P[1][1] = 0; |
SED9008 | 0:8472cce76dbd | 8 | |
SED9008 | 0:8472cce76dbd | 9 | angle = 0; |
SED9008 | 0:8472cce76dbd | 10 | bias = 0; |
SED9008 | 0:8472cce76dbd | 11 | |
SED9008 | 0:8472cce76dbd | 12 | Q_angle = 0.001; |
SED9008 | 0:8472cce76dbd | 13 | Q_gyroBias = 0.003; |
SED9008 | 0:8472cce76dbd | 14 | R_angle = 0.03; |
SED9008 | 0:8472cce76dbd | 15 | } |
SED9008 | 0:8472cce76dbd | 16 | |
SED9008 | 0:8472cce76dbd | 17 | double kalman::getAngle(double newAngle, double newRate, double dt){ |
SED9008 | 0:8472cce76dbd | 18 | //predict the angle according to the gyroscope |
SED9008 | 0:8472cce76dbd | 19 | rate = newRate - bias; |
SED9008 | 0:8472cce76dbd | 20 | angle = dt * rate; |
SED9008 | 0:8472cce76dbd | 21 | //update the error covariance |
SED9008 | 0:8472cce76dbd | 22 | P[0][0] += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle); |
SED9008 | 0:8472cce76dbd | 23 | P[0][1] -= dt * P[1][1]; |
SED9008 | 0:8472cce76dbd | 24 | P[1][0] -= dt * P[1][1]; |
SED9008 | 0:8472cce76dbd | 25 | P[1][1] += dt * Q_gyroBias; |
SED9008 | 0:8472cce76dbd | 26 | //difference between the predicted angle and the accelerometer angle |
SED9008 | 0:8472cce76dbd | 27 | y = newAngle - angle; |
SED9008 | 0:8472cce76dbd | 28 | //estimation error |
SED9008 | 0:8472cce76dbd | 29 | S = P[0][0] + R_angle; |
SED9008 | 0:8472cce76dbd | 30 | //determine the kalman gain according to the error covariance and the distrust |
SED9008 | 0:8472cce76dbd | 31 | K[0] = P[0][0]/S; |
SED9008 | 0:8472cce76dbd | 32 | K[1] = P[1][0]/S; |
SED9008 | 0:8472cce76dbd | 33 | //adjust the angle according to the kalman gain and the difference with the measurement |
SED9008 | 0:8472cce76dbd | 34 | angle += K[0] * y; |
SED9008 | 0:8472cce76dbd | 35 | bias += K[1] * y; |
SED9008 | 0:8472cce76dbd | 36 | //update the error covariance |
SED9008 | 0:8472cce76dbd | 37 | P[0][0] -= K[0] * P[0][0]; |
SED9008 | 0:8472cce76dbd | 38 | P[0][1] -= K[0] * P[0][1]; |
SED9008 | 0:8472cce76dbd | 39 | P[1][0] -= K[1] * P[0][0]; |
SED9008 | 0:8472cce76dbd | 40 | P[1][1] -= K[1] * P[0][1]; |
SED9008 | 0:8472cce76dbd | 41 | |
SED9008 | 0:8472cce76dbd | 42 | return angle; |
SED9008 | 0:8472cce76dbd | 43 | } |
SED9008 | 0:8472cce76dbd | 44 | void kalman::setAngle(double newAngle) { angle = newAngle; }; |
SED9008 | 0:8472cce76dbd | 45 | void kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; }; |
SED9008 | 0:8472cce76dbd | 46 | void kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; }; |
SED9008 | 0:8472cce76dbd | 47 | void kalman::setRangle(double newR_angle) { R_angle = newR_angle; }; |
SED9008 | 0:8472cce76dbd | 48 | |
SED9008 | 0:8472cce76dbd | 49 | double kalman::getRate(void) { return rate; }; |
SED9008 | 0:8472cce76dbd | 50 | double kalman::getQangle(void) { return Q_angle; }; |
SED9008 | 0:8472cce76dbd | 51 | double kalman::getQbias(void) { return Q_gyroBias; }; |
SED9008 | 0:8472cce76dbd | 52 | double kalman::getRangle(void) { return R_angle; }; |