test
Dependencies: mbed
kalman.h
- Committer:
- siwakon
- Date:
- 2016-12-05
- Revision:
- 0:dd400e4fe461
File content as of revision 0:dd400e4fe461:
#ifndef _kalman_H #define _kalman_H // derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/ class kalman { public: kalman(void); double getAngle(double newAngle, double newRate, double dt); void setAngle(double newAngle); void setQangle(double newQ_angle); void setQgyroBias(double newQ_gyroBias); void setRangle(double newR_angle); double getRate(void); double getQangle(void); double getQbias(void); double getRangle(void); private: double P[2][2]; //error covariance matrix double K[2]; //kalman gain double y; //angle difference double S; //estimation error double rate; //rate in deg/s double angle; //kalman angle double bias; //kalman gyro bias double Q_angle; //process noise variance for the angle of the accelerometer double Q_gyroBias; //process noise variance for the gyroscope bias double R_angle; //measurement noise variance }; #endif