IMU filter
Fork of IMUfilter by
IMUfilter.h
- Committer:
- guqinchen
- Date:
- 2014-03-23
- Revision:
- 3:a2f4c5c123e6
- Parent:
- 2:fae851819c43
File content as of revision 3:a2f4c5c123e6:
#ifndef IMU_FILTER_H #define IMU_FILTER_H #include "mbed.h" #define PI 3.1415926536 class IMUfilter { public: /** * Constructor. * * Initializes filter variables. * * @param rate The rate at which the filter should be updated. * @param gyroscopeMeasurementError The error of the gyroscope in degrees * per second. This used to calculate a tuning constant for the filter. * Try changing this value if there are jittery readings, or they change * too much or too fast when rotating the IMU. */ IMUfilter(double rate, double gyroscopeMeasurementError); /** * Update the filter variables. * * @param w_x X-axis gyroscope reading in rad/s. * @param w_y Y-axis gyroscope reading in rad/s. * @param w_z Z-axis gyroscope reading in rad/s. * @param a_x X-axis accelerometer reading in m/s/s. * @param a_y Y-axis accelerometer reading in m/s/s. * @param a_z Z-axis accelerometer reading in m/s/s. */ void updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z); /** * Compute the Euler angles based on the current filter data. */ void computeEuler(void); /** * Get the current roll. * * @return The current roll angle in radians. */ double getRoll(void); /** * Get the current pitch. * * @return The current pitch angle in radians. */ double getPitch(void); /** * Get the current yaw. * * @return The current yaw angle in radians. */ double getYaw(void); /** * Reset the filter. */ void reset(void); private: int firstUpdate; //Quaternion orientation of earth frame relative to auxiliary frame. double AEq_1; double AEq_2; double AEq_3; double AEq_4; //Estimated orientation quaternion elements with initial conditions. double SEq_1; double SEq_2; double SEq_3; double SEq_4; //Sampling period double deltat; //Gyroscope measurement error (in degrees per second). double gyroMeasError; //Compute beta (filter tuning constant.. double beta; double phi; double theta; double psi; }; #endif /* IMU_FILTER_H */