Chawisorn samrit / Mbed 2 deprecated testIMU2_copy2

kor bork wa cop koy ma

Dependencies:   mbed

Fork of testIMU2_copy2 by OX

kalman.h@3:ee0df78b0dd3, 2017-12-07 (annotated)

Committer:
csggreen
Date:
Thu Dec 07 08:04:06 2017 +0000
Revision:
3:ee0df78b0dd3
Parent:
0:77a7d1a1c6db 
copy koy ma

Who changed what in which revision?

UserRevisionLine numberNew contents of line
siwakon 0:77a7d1a1c6db 1 #ifndef _kalman_H
siwakon 0:77a7d1a1c6db 2 #define _kalman_H
siwakon 0:77a7d1a1c6db 3 // derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
siwakon 0:77a7d1a1c6db 4 class kalman
siwakon 0:77a7d1a1c6db 5 {
siwakon 0:77a7d1a1c6db 6 public:
siwakon 0:77a7d1a1c6db 7 kalman(void);
siwakon 0:77a7d1a1c6db 8 double getAngle(double newAngle, double newRate, double dt);
siwakon 0:77a7d1a1c6db 9
siwakon 0:77a7d1a1c6db 10 void setAngle(double newAngle);
siwakon 0:77a7d1a1c6db 11 void setQangle(double newQ_angle);
siwakon 0:77a7d1a1c6db 12 void setQgyroBias(double newQ_gyroBias);
siwakon 0:77a7d1a1c6db 13 void setRangle(double newR_angle);
siwakon 0:77a7d1a1c6db 14
siwakon 0:77a7d1a1c6db 15 double getRate(void);
siwakon 0:77a7d1a1c6db 16 double getQangle(void);
siwakon 0:77a7d1a1c6db 17 double getQbias(void);
siwakon 0:77a7d1a1c6db 18 double getRangle(void);
siwakon 0:77a7d1a1c6db 19
siwakon 0:77a7d1a1c6db 20
siwakon 0:77a7d1a1c6db 21 private:
siwakon 0:77a7d1a1c6db 22 double P[2][2]; //error covariance matrix
siwakon 0:77a7d1a1c6db 23 double K[2]; //kalman gain
siwakon 0:77a7d1a1c6db 24 double y; //angle difference
siwakon 0:77a7d1a1c6db 25 double S; //estimation error
siwakon 0:77a7d1a1c6db 26
siwakon 0:77a7d1a1c6db 27 double rate; //rate in deg/s
siwakon 0:77a7d1a1c6db 28 double angle; //kalman angle
siwakon 0:77a7d1a1c6db 29 double bias; //kalman gyro bias
siwakon 0:77a7d1a1c6db 30
siwakon 0:77a7d1a1c6db 31 double Q_angle; //process noise variance for the angle of the accelerometer
siwakon 0:77a7d1a1c6db 32 double Q_gyroBias; //process noise variance for the gyroscope bias
siwakon 0:77a7d1a1c6db 33 double R_angle; //measurement noise variance
siwakon 0:77a7d1a1c6db 34 };
siwakon 0:77a7d1a1c6db 35
siwakon 0:77a7d1a1c6db 36 #endif