OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

kalman.cpp@1:d8ce226c8c2e, 2016-12-06 (annotated)

Committer:
arthicha
Date:
Tue Dec 06 06:11:54 2016 +0000
Revision:
1:d8ce226c8c2e
;UUpdate;

Who changed what in which revision?

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