IMU filter
Fork of IMUfilter by
IMUfilter.h@3:a2f4c5c123e6, 2014-03-23 (annotated)
- Committer:
- guqinchen
- Date:
- Sun Mar 23 22:06:54 2014 +0000
- Revision:
- 3:a2f4c5c123e6
- Parent:
- 2:fae851819c43
Project Specific Changes
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aberk | 0:976ab2e4e4bd | 1 | #ifndef IMU_FILTER_H |
aberk | 0:976ab2e4e4bd | 2 | #define IMU_FILTER_H |
aberk | 0:976ab2e4e4bd | 3 | |
aberk | 0:976ab2e4e4bd | 4 | #include "mbed.h" |
aberk | 0:976ab2e4e4bd | 5 | |
aberk | 0:976ab2e4e4bd | 6 | #define PI 3.1415926536 |
aberk | 0:976ab2e4e4bd | 7 | |
aberk | 0:976ab2e4e4bd | 8 | class IMUfilter { |
aberk | 0:976ab2e4e4bd | 9 | |
aberk | 0:976ab2e4e4bd | 10 | public: |
aberk | 0:976ab2e4e4bd | 11 | |
aberk | 0:976ab2e4e4bd | 12 | /** |
aberk | 0:976ab2e4e4bd | 13 | * Constructor. |
aberk | 0:976ab2e4e4bd | 14 | * |
aberk | 0:976ab2e4e4bd | 15 | * Initializes filter variables. |
aberk | 0:976ab2e4e4bd | 16 | * |
aberk | 0:976ab2e4e4bd | 17 | * @param rate The rate at which the filter should be updated. |
aberk | 0:976ab2e4e4bd | 18 | * @param gyroscopeMeasurementError The error of the gyroscope in degrees |
aberk | 0:976ab2e4e4bd | 19 | * per second. This used to calculate a tuning constant for the filter. |
aberk | 0:976ab2e4e4bd | 20 | * Try changing this value if there are jittery readings, or they change |
aberk | 0:976ab2e4e4bd | 21 | * too much or too fast when rotating the IMU. |
aberk | 0:976ab2e4e4bd | 22 | */ |
aberk | 0:976ab2e4e4bd | 23 | IMUfilter(double rate, double gyroscopeMeasurementError); |
aberk | 0:976ab2e4e4bd | 24 | |
aberk | 0:976ab2e4e4bd | 25 | /** |
aberk | 0:976ab2e4e4bd | 26 | * Update the filter variables. |
aberk | 0:976ab2e4e4bd | 27 | * |
aberk | 0:976ab2e4e4bd | 28 | * @param w_x X-axis gyroscope reading in rad/s. |
aberk | 0:976ab2e4e4bd | 29 | * @param w_y Y-axis gyroscope reading in rad/s. |
aberk | 0:976ab2e4e4bd | 30 | * @param w_z Z-axis gyroscope reading in rad/s. |
aberk | 0:976ab2e4e4bd | 31 | * @param a_x X-axis accelerometer reading in m/s/s. |
aberk | 0:976ab2e4e4bd | 32 | * @param a_y Y-axis accelerometer reading in m/s/s. |
aberk | 0:976ab2e4e4bd | 33 | * @param a_z Z-axis accelerometer reading in m/s/s. |
aberk | 0:976ab2e4e4bd | 34 | */ |
aberk | 0:976ab2e4e4bd | 35 | void updateFilter(double w_x, double w_y, double w_z, |
aberk | 0:976ab2e4e4bd | 36 | double a_x, double a_y, double a_z); |
aberk | 0:976ab2e4e4bd | 37 | |
aberk | 0:976ab2e4e4bd | 38 | /** |
aberk | 0:976ab2e4e4bd | 39 | * Compute the Euler angles based on the current filter data. |
aberk | 0:976ab2e4e4bd | 40 | */ |
aberk | 0:976ab2e4e4bd | 41 | void computeEuler(void); |
aberk | 0:976ab2e4e4bd | 42 | |
aberk | 0:976ab2e4e4bd | 43 | /** |
aberk | 0:976ab2e4e4bd | 44 | * Get the current roll. |
aberk | 0:976ab2e4e4bd | 45 | * |
aberk | 0:976ab2e4e4bd | 46 | * @return The current roll angle in radians. |
aberk | 0:976ab2e4e4bd | 47 | */ |
aberk | 0:976ab2e4e4bd | 48 | double getRoll(void); |
aberk | 0:976ab2e4e4bd | 49 | |
aberk | 0:976ab2e4e4bd | 50 | /** |
aberk | 0:976ab2e4e4bd | 51 | * Get the current pitch. |
aberk | 0:976ab2e4e4bd | 52 | * |
aberk | 0:976ab2e4e4bd | 53 | * @return The current pitch angle in radians. |
aberk | 0:976ab2e4e4bd | 54 | */ |
aberk | 0:976ab2e4e4bd | 55 | double getPitch(void); |
aberk | 0:976ab2e4e4bd | 56 | |
aberk | 0:976ab2e4e4bd | 57 | /** |
aberk | 0:976ab2e4e4bd | 58 | * Get the current yaw. |
aberk | 0:976ab2e4e4bd | 59 | * |
aberk | 0:976ab2e4e4bd | 60 | * @return The current yaw angle in radians. |
aberk | 0:976ab2e4e4bd | 61 | */ |
aberk | 0:976ab2e4e4bd | 62 | double getYaw(void); |
aberk | 0:976ab2e4e4bd | 63 | |
aberk | 1:8a920397b510 | 64 | /** |
aberk | 1:8a920397b510 | 65 | * Reset the filter. |
aberk | 1:8a920397b510 | 66 | */ |
aberk | 1:8a920397b510 | 67 | void reset(void); |
aberk | 1:8a920397b510 | 68 | |
aberk | 0:976ab2e4e4bd | 69 | private: |
aberk | 0:976ab2e4e4bd | 70 | |
aberk | 0:976ab2e4e4bd | 71 | int firstUpdate; |
aberk | 0:976ab2e4e4bd | 72 | |
aberk | 0:976ab2e4e4bd | 73 | //Quaternion orientation of earth frame relative to auxiliary frame. |
aberk | 0:976ab2e4e4bd | 74 | double AEq_1; |
aberk | 0:976ab2e4e4bd | 75 | double AEq_2; |
aberk | 0:976ab2e4e4bd | 76 | double AEq_3; |
aberk | 0:976ab2e4e4bd | 77 | double AEq_4; |
aberk | 0:976ab2e4e4bd | 78 | |
aberk | 0:976ab2e4e4bd | 79 | //Estimated orientation quaternion elements with initial conditions. |
aberk | 0:976ab2e4e4bd | 80 | double SEq_1; |
aberk | 0:976ab2e4e4bd | 81 | double SEq_2; |
aberk | 0:976ab2e4e4bd | 82 | double SEq_3; |
aberk | 0:976ab2e4e4bd | 83 | double SEq_4; |
aberk | 0:976ab2e4e4bd | 84 | |
aberk | 0:976ab2e4e4bd | 85 | //Sampling period |
aberk | 0:976ab2e4e4bd | 86 | double deltat; |
aberk | 0:976ab2e4e4bd | 87 | |
aberk | 0:976ab2e4e4bd | 88 | //Gyroscope measurement error (in degrees per second). |
aberk | 0:976ab2e4e4bd | 89 | double gyroMeasError; |
aberk | 0:976ab2e4e4bd | 90 | |
aberk | 0:976ab2e4e4bd | 91 | //Compute beta (filter tuning constant.. |
aberk | 0:976ab2e4e4bd | 92 | double beta; |
aberk | 0:976ab2e4e4bd | 93 | |
aberk | 0:976ab2e4e4bd | 94 | double phi; |
aberk | 0:976ab2e4e4bd | 95 | double theta; |
aberk | 0:976ab2e4e4bd | 96 | double psi; |
aberk | 0:976ab2e4e4bd | 97 | |
aberk | 0:976ab2e4e4bd | 98 | }; |
aberk | 0:976ab2e4e4bd | 99 | |
aberk | 0:976ab2e4e4bd | 100 | #endif /* IMU_FILTER_H */ |