Qinchen Gu / IMUfilter

IMU filter

Dependents:   P4_IMU

Fork of IMUfilter by Abe Takumi

IMUfilter.h@2:fae851819c43, 2013-07-05 (annotated)

Committer:
YSB
Date:
Fri Jul 05 04:17:19 2013 +0000
Revision:
2:fae851819c43
Parent:
1:8a920397b510
Child:
3:a2f4c5c123e6
IMUfilter

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
YSB 2:fae851819c43 7 #define g0 9.812865328//Gravity at Earth's surface in m/s/s
YSB 2:fae851819c43 8 #define SAMPLES 4//Number of samples to average.
YSB 2:fae851819c43 9 #define CALIBRATION_SAMPLES 64//128//Number of samples to be averaged for a null bias calculation//during calibration.
YSB 2:fae851819c43 10 #define toDegrees(x) (x * 57.2957795)//Convert from radians to degrees.
YSB 2:fae851819c43 11 #define toRadians(x) (x * 0.01745329252)//Convert from degrees to radians.
YSB 2:fae851819c43 12 #define GYROSCOPE_GAIN (1 / 14.375)//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
YSB 2:fae851819c43 13 #define ACCELEROMETER_GAIN (0.004 * g0)//Full scale resolution on the ADXL345 is 4mg/LSB.
YSB 2:fae851819c43 14 #define GYRO_RATE 0.005//Sampling gyroscope at 200Hz.
YSB 2:fae851819c43 15 #define ACC_RATE 0.005//Sampling accelerometer at 200Hz.
YSB 2:fae851819c43 16 #define FILTER_RATE 0.1//Updating filter at 40Hz.
aberk 0:976ab2e4e4bd 17
aberk 0:976ab2e4e4bd 18 class IMUfilter {
aberk 0:976ab2e4e4bd 19
aberk 0:976ab2e4e4bd 20 public:
aberk 0:976ab2e4e4bd 21
aberk 0:976ab2e4e4bd 22 /**
aberk 0:976ab2e4e4bd 23 * Constructor.
aberk 0:976ab2e4e4bd 24 *
aberk 0:976ab2e4e4bd 25 * Initializes filter variables.
aberk 0:976ab2e4e4bd 26 *
aberk 0:976ab2e4e4bd 27 * @param rate The rate at which the filter should be updated.
aberk 0:976ab2e4e4bd 28 * @param gyroscopeMeasurementError The error of the gyroscope in degrees
aberk 0:976ab2e4e4bd 29 * per second. This used to calculate a tuning constant for the filter.
aberk 0:976ab2e4e4bd 30 * Try changing this value if there are jittery readings, or they change
aberk 0:976ab2e4e4bd 31 * too much or too fast when rotating the IMU.
aberk 0:976ab2e4e4bd 32 */
aberk 0:976ab2e4e4bd 33 IMUfilter(double rate, double gyroscopeMeasurementError);
aberk 0:976ab2e4e4bd 34
aberk 0:976ab2e4e4bd 35 /**
aberk 0:976ab2e4e4bd 36 * Update the filter variables.
aberk 0:976ab2e4e4bd 37 *
aberk 0:976ab2e4e4bd 38 * @param w_x X-axis gyroscope reading in rad/s.
aberk 0:976ab2e4e4bd 39 * @param w_y Y-axis gyroscope reading in rad/s.
aberk 0:976ab2e4e4bd 40 * @param w_z Z-axis gyroscope reading in rad/s.
aberk 0:976ab2e4e4bd 41 * @param a_x X-axis accelerometer reading in m/s/s.
aberk 0:976ab2e4e4bd 42 * @param a_y Y-axis accelerometer reading in m/s/s.
aberk 0:976ab2e4e4bd 43 * @param a_z Z-axis accelerometer reading in m/s/s.
aberk 0:976ab2e4e4bd 44 */
aberk 0:976ab2e4e4bd 45 void updateFilter(double w_x, double w_y, double w_z,
aberk 0:976ab2e4e4bd 46 double a_x, double a_y, double a_z);
aberk 0:976ab2e4e4bd 47
aberk 0:976ab2e4e4bd 48 /**
aberk 0:976ab2e4e4bd 49 * Compute the Euler angles based on the current filter data.
aberk 0:976ab2e4e4bd 50 */
aberk 0:976ab2e4e4bd 51 void computeEuler(void);
aberk 0:976ab2e4e4bd 52
aberk 0:976ab2e4e4bd 53 /**
aberk 0:976ab2e4e4bd 54 * Get the current roll.
aberk 0:976ab2e4e4bd 55 *
aberk 0:976ab2e4e4bd 56 * @return The current roll angle in radians.
aberk 0:976ab2e4e4bd 57 */
aberk 0:976ab2e4e4bd 58 double getRoll(void);
aberk 0:976ab2e4e4bd 59
aberk 0:976ab2e4e4bd 60 /**
aberk 0:976ab2e4e4bd 61 * Get the current pitch.
aberk 0:976ab2e4e4bd 62 *
aberk 0:976ab2e4e4bd 63 * @return The current pitch angle in radians.
aberk 0:976ab2e4e4bd 64 */
aberk 0:976ab2e4e4bd 65 double getPitch(void);
aberk 0:976ab2e4e4bd 66
aberk 0:976ab2e4e4bd 67 /**
aberk 0:976ab2e4e4bd 68 * Get the current yaw.
aberk 0:976ab2e4e4bd 69 *
aberk 0:976ab2e4e4bd 70 * @return The current yaw angle in radians.
aberk 0:976ab2e4e4bd 71 */
aberk 0:976ab2e4e4bd 72 double getYaw(void);
aberk 0:976ab2e4e4bd 73
aberk 1:8a920397b510 74 /**
aberk 1:8a920397b510 75 * Reset the filter.
aberk 1:8a920397b510 76 */
aberk 1:8a920397b510 77 void reset(void);
aberk 1:8a920397b510 78
aberk 0:976ab2e4e4bd 79 private:
aberk 0:976ab2e4e4bd 80
aberk 0:976ab2e4e4bd 81 int firstUpdate;
aberk 0:976ab2e4e4bd 82
aberk 0:976ab2e4e4bd 83 //Quaternion orientation of earth frame relative to auxiliary frame.
aberk 0:976ab2e4e4bd 84 double AEq_1;
aberk 0:976ab2e4e4bd 85 double AEq_2;
aberk 0:976ab2e4e4bd 86 double AEq_3;
aberk 0:976ab2e4e4bd 87 double AEq_4;
aberk 0:976ab2e4e4bd 88
aberk 0:976ab2e4e4bd 89 //Estimated orientation quaternion elements with initial conditions.
aberk 0:976ab2e4e4bd 90 double SEq_1;
aberk 0:976ab2e4e4bd 91 double SEq_2;
aberk 0:976ab2e4e4bd 92 double SEq_3;
aberk 0:976ab2e4e4bd 93 double SEq_4;
aberk 0:976ab2e4e4bd 94
aberk 0:976ab2e4e4bd 95 //Sampling period
aberk 0:976ab2e4e4bd 96 double deltat;
aberk 0:976ab2e4e4bd 97
aberk 0:976ab2e4e4bd 98 //Gyroscope measurement error (in degrees per second).
aberk 0:976ab2e4e4bd 99 double gyroMeasError;
aberk 0:976ab2e4e4bd 100
aberk 0:976ab2e4e4bd 101 //Compute beta (filter tuning constant..
aberk 0:976ab2e4e4bd 102 double beta;
aberk 0:976ab2e4e4bd 103
aberk 0:976ab2e4e4bd 104 double phi;
aberk 0:976ab2e4e4bd 105 double theta;
aberk 0:976ab2e4e4bd 106 double psi;
aberk 0:976ab2e4e4bd 107
aberk 0:976ab2e4e4bd 108 };
aberk 0:976ab2e4e4bd 109
aberk 0:976ab2e4e4bd 110 #endif /* IMU_FILTER_H */