Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef IMU_FILTER_MADWICK_IMU_FILTER_H
00026 #define IMU_FILTER_MADWICK_IMU_FILTER_H
00027
00028 class ImuFilter
00029 {
00030 public:
00031
00032 ImuFilter();
00033 virtual ~ImuFilter();
00034
00035 private:
00036
00037 double gain_;
00038 double zeta_;
00039
00040
00041 double q0, q1, q2, q3;
00042 float w_bx_, w_by_, w_bz_;
00043
00044
00045
00046
00047
00048 static float invSqrt(float x)
00049 {
00050 float xhalf = 0.5f * x;
00051 union
00052 {
00053 float x;
00054 int i;
00055 } u;
00056 u.x = x;
00057 u.i = 0x5f3759df - (u.i >> 1);
00058
00059 u.x = u.x * (1.5f - xhalf * u.x * u.x);
00060 return u.x;
00061 }
00062
00063 public:
00064 void setAlgorithmGain(double gain)
00065 {
00066 gain_ = gain;
00067 }
00068
00069 void setDriftBiasGain(double zeta)
00070 {
00071 zeta_ = zeta;
00072 }
00073
00074 void getOrientation(double& q0, double& q1, double& q2, double& q3)
00075 {
00076 q0 = this->q0;
00077 q1 = this->q1;
00078 q2 = this->q2;
00079 q3 = this->q3;
00080 }
00081
00082 void setOrientation(double q0, double q1, double q2, double q3)
00083 {
00084 this->q0 = q0;
00085 this->q1 = q1;
00086 this->q2 = q2;
00087 this->q3 = q3;
00088
00089 w_bx_ = 0;
00090 w_by_ = 0;
00091 w_bz_ = 0;
00092 }
00093
00094 void madgwickAHRSupdate(float gx, float gy, float gz,
00095 float ax, float ay, float az,
00096 float mx, float my, float mz,
00097 float dt);
00098
00099 void madgwickAHRSupdateIMU(float gx, float gy, float gz,
00100 float ax, float ay, float az,
00101 float dt);
00102 };
00103
00104 #endif // IMU_FILTER_IMU_MADWICK_FILTER_H