Leo Veenstra / kalman

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

kalman.cpp@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 #include "kalman.h"
SED9008 1:e84e9e89e445 2 // derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
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; };