ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Kalman.h@34:eaea0ae92dfa, 2016-04-24 (annotated)
- Committer:
- ivo_david_michelle
- Date:
- Sun Apr 24 17:15:20 2016 +0000
- Revision:
- 34:eaea0ae92dfa
testing with kalman
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ivo_david_michelle | 34:eaea0ae92dfa | 1 | #ifndef _Kalman_h_ |
ivo_david_michelle | 34:eaea0ae92dfa | 2 | #define _Kalman_h_ |
ivo_david_michelle | 34:eaea0ae92dfa | 3 | |
ivo_david_michelle | 34:eaea0ae92dfa | 4 | class Kalman { |
ivo_david_michelle | 34:eaea0ae92dfa | 5 | public: |
ivo_david_michelle | 34:eaea0ae92dfa | 6 | Kalman(); |
ivo_david_michelle | 34:eaea0ae92dfa | 7 | |
ivo_david_michelle | 34:eaea0ae92dfa | 8 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds |
ivo_david_michelle | 34:eaea0ae92dfa | 9 | float getAngle(float newAngle, float newRate, float dt); |
ivo_david_michelle | 34:eaea0ae92dfa | 10 | |
ivo_david_michelle | 34:eaea0ae92dfa | 11 | void setAngle(float angle); // Used to set angle, this should be set as the starting angle |
ivo_david_michelle | 34:eaea0ae92dfa | 12 | float getRate(); // Return the unbiased rate |
ivo_david_michelle | 34:eaea0ae92dfa | 13 | |
ivo_david_michelle | 34:eaea0ae92dfa | 14 | /* These are used to tune the Kalman filter */ |
ivo_david_michelle | 34:eaea0ae92dfa | 15 | void setQangle(float Q_angle); |
ivo_david_michelle | 34:eaea0ae92dfa | 16 | void setQbias(float Q_bias); |
ivo_david_michelle | 34:eaea0ae92dfa | 17 | void setRmeasure(float R_measure); |
ivo_david_michelle | 34:eaea0ae92dfa | 18 | |
ivo_david_michelle | 34:eaea0ae92dfa | 19 | float getQangle(); |
ivo_david_michelle | 34:eaea0ae92dfa | 20 | float getQbias(); |
ivo_david_michelle | 34:eaea0ae92dfa | 21 | float getRmeasure(); |
ivo_david_michelle | 34:eaea0ae92dfa | 22 | |
ivo_david_michelle | 34:eaea0ae92dfa | 23 | private: |
ivo_david_michelle | 34:eaea0ae92dfa | 24 | /* Kalman filter variables */ |
ivo_david_michelle | 34:eaea0ae92dfa | 25 | float Q_angle; // Process noise variance for the accelerometer |
ivo_david_michelle | 34:eaea0ae92dfa | 26 | float Q_bias; // Process noise variance for the gyro bias |
ivo_david_michelle | 34:eaea0ae92dfa | 27 | float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise |
ivo_david_michelle | 34:eaea0ae92dfa | 28 | |
ivo_david_michelle | 34:eaea0ae92dfa | 29 | float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector |
ivo_david_michelle | 34:eaea0ae92dfa | 30 | float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector |
ivo_david_michelle | 34:eaea0ae92dfa | 31 | float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate |
ivo_david_michelle | 34:eaea0ae92dfa | 32 | |
ivo_david_michelle | 34:eaea0ae92dfa | 33 | float P[2][2]; // Error covariance matrix - This is a 2x2 matrix |
ivo_david_michelle | 34:eaea0ae92dfa | 34 | }; |
ivo_david_michelle | 34:eaea0ae92dfa | 35 | |
ivo_david_michelle | 34:eaea0ae92dfa | 36 | #endif |