OX / Mbed 2 deprecated IMU_oo

new

Dependencies:   mbed

kalman.cpp@1:6bc2c5d68446, 2016-12-04 (annotated)

Committer:
siwakon
Date:
Sun Dec 04 09:19:53 2016 +0000
Revision:
1:6bc2c5d68446
Parent:
0:77a7d1a1c6db 
weewfd

Who changed what in which revision?

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