imu_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010, CCNY Robotics Lab
3  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
4  *
5  * http://robotics.ccny.cuny.edu
6  *
7  * Based on implementation of Madgwick's IMU and AHRS algorithms.
8  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, either version 3 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program. If not, see <http://www.gnu.org/licenses/>.
23  */
24 
25 #ifndef IMU_FILTER_MADWICK_IMU_FILTER_H
26 #define IMU_FILTER_MADWICK_IMU_FILTER_H
27 
29 #include <iostream>
30 #include <cmath>
31 
32 class ImuFilter
33 {
34  public:
35 
36  ImuFilter();
37  virtual ~ImuFilter();
38 
39  private:
40  // **** paramaters
41  double gain_; // algorithm gain
42  double zeta_; // gyro drift bias gain
44 
45  // **** state variables
46  double q0, q1, q2, q3; // quaternion
47  float w_bx_, w_by_, w_bz_; //
48 
49 public:
50  void setAlgorithmGain(double gain)
51  {
52  gain_ = gain;
53  }
54 
55  void setDriftBiasGain(double zeta)
56  {
57  zeta_ = zeta;
58  }
59 
61  {
62  world_frame_ = frame;
63  }
64 
65  void getOrientation(double& q0, double& q1, double& q2, double& q3)
66  {
67  q0 = this->q0;
68  q1 = this->q1;
69  q2 = this->q2;
70  q3 = this->q3;
71 
72  // perform precise normalization of the output, using 1/sqrt()
73  // instead of the fast invSqrt() approximation. Without this,
74  // TF2 complains that the quaternion is not normalized.
75  double recipNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
76  q0 *= recipNorm;
77  q1 *= recipNorm;
78  q2 *= recipNorm;
79  q3 *= recipNorm;
80  }
81 
82  void setOrientation(double q0, double q1, double q2, double q3)
83  {
84  this->q0 = q0;
85  this->q1 = q1;
86  this->q2 = q2;
87  this->q3 = q3;
88 
89  w_bx_ = 0;
90  w_by_ = 0;
91  w_bz_ = 0;
92  }
93 
94  void madgwickAHRSupdate(float gx, float gy, float gz,
95  float ax, float ay, float az,
96  float mx, float my, float mz,
97  float dt);
98 
99  void madgwickAHRSupdateIMU(float gx, float gy, float gz,
100  float ax, float ay, float az,
101  float dt);
102 
103  void getGravity(float& rx, float& ry, float& rz,
104  float gravity = 9.80665);
105 };
106 
107 #endif // IMU_FILTER_IMU_MADWICK_FILTER_H
double q3
Definition: imu_filter.h:46
double q2
Definition: imu_filter.h:46
void setOrientation(double q0, double q1, double q2, double q3)
Definition: imu_filter.h:82
void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
Definition: imu_filter.cpp:175
void getGravity(float &rx, float &ry, float &rz, float gravity=9.80665)
Definition: imu_filter.cpp:313
void getOrientation(double &q0, double &q1, double &q2, double &q3)
Definition: imu_filter.h:65
void setAlgorithmGain(double gain)
Definition: imu_filter.h:50
double q0
Definition: imu_filter.h:46
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt)
Definition: imu_filter.cpp:258
virtual ~ImuFilter()
Definition: imu_filter.cpp:171
void setWorldFrame(WorldFrame::WorldFrame frame)
Definition: imu_filter.h:60
float w_bx_
Definition: imu_filter.h:47
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
float w_by_
Definition: imu_filter.h:47
double gain_
Definition: imu_filter.h:41
void setDriftBiasGain(double zeta)
Definition: imu_filter.h:55
double q1
Definition: imu_filter.h:46
float w_bz_
Definition: imu_filter.h:47
double zeta_
Definition: imu_filter.h:42
WorldFrame::WorldFrame world_frame_
Definition: imu_filter.h:43


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00