ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: Kalman.h
- Revision:
- 34:eaea0ae92dfa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman.h Sun Apr 24 17:15:20 2016 +0000 @@ -0,0 +1,36 @@ +#ifndef _Kalman_h_ +#define _Kalman_h_ + +class Kalman { +public: + Kalman(); + + // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds + float getAngle(float newAngle, float newRate, float dt); + + void setAngle(float angle); // Used to set angle, this should be set as the starting angle + float getRate(); // Return the unbiased rate + + /* These are used to tune the Kalman filter */ + void setQangle(float Q_angle); + void setQbias(float Q_bias); + void setRmeasure(float R_measure); + + float getQangle(); + float getQbias(); + float getRmeasure(); + +private: + /* Kalman filter variables */ + float Q_angle; // Process noise variance for the accelerometer + float Q_bias; // Process noise variance for the gyro bias + float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise + + float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector + float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector + float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate + + float P[2][2]; // Error covariance matrix - This is a 2x2 matrix +}; + +#endif \ No newline at end of file