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 #include <imu_filter_madgwick/world_frame.h>
00029 #include <iostream>
00030 #include <cmath>
00031
00032 class ImuFilter
00033 {
00034 public:
00035
00036 ImuFilter();
00037 virtual ~ImuFilter();
00038
00039 private:
00040
00041 double gain_;
00042 double zeta_;
00043 WorldFrame::WorldFrame world_frame_;
00044
00045
00046 double q0, q1, q2, q3;
00047 float w_bx_, w_by_, w_bz_;
00048
00049 public:
00050 void setAlgorithmGain(double gain)
00051 {
00052 gain_ = gain;
00053 }
00054
00055 void setDriftBiasGain(double zeta)
00056 {
00057 zeta_ = zeta;
00058 }
00059
00060 void setWorldFrame(WorldFrame::WorldFrame frame)
00061 {
00062 world_frame_ = frame;
00063 }
00064
00065 void getOrientation(double& q0, double& q1, double& q2, double& q3)
00066 {
00067 q0 = this->q0;
00068 q1 = this->q1;
00069 q2 = this->q2;
00070 q3 = this->q3;
00071
00072
00073
00074
00075 double recipNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00076 q0 *= recipNorm;
00077 q1 *= recipNorm;
00078 q2 *= recipNorm;
00079 q3 *= recipNorm;
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