OX / Mbed 2 deprecated class_IMU_V1

test

Dependencies:   mbed

kalman.cpp@0:dd400e4fe461, 2016-12-05 (annotated)

Committer:
siwakon
Date:
Mon Dec 05 14:47:17 2016 +0000
Revision:
0:dd400e4fe461
asdasdasd

Who changed what in which revision?

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