Qinchen Gu / IMUfilter

IMU filter

Dependents:   P4_IMU

Fork of IMUfilter by Abe Takumi

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?

UserRevisionLine numberNew 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 */