MadgwickFilter.h
Go to the documentation of this file.
1 
2 /*
3  * Copyright (C) 2010, CCNY Robotics Lab
4  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
5  *
6  * http://robotics.ccny.cuny.edu
7  *
8  * Based on implementation of Madgwick's IMU and AHRS algorithms.
9  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
10  *
11  *
12  * This program is free software: you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation, either version 3 of the License, or
15  * (at your option) any later version.
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU General Public License for more details.
21  *
22  * You should have received a copy of the GNU General Public License
23  * along with this program. If not, see <http://www.gnu.org/licenses/>.
24  */
25 
26 #ifndef CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_
27 #define CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_
28 
29 #include <rtabmap/core/IMUFilter.h>
30 #include <cmath>
31 
32 namespace rtabmap {
33 
34 class MadgwickFilter : public IMUFilter
35 {
36  public:
37 
38  MadgwickFilter(const ParametersMap & parameters = ParametersMap());
39  virtual ~MadgwickFilter(){}
40 
41  private:
42  // **** state variables
43  double q0, q1, q2, q3; // quaternion
44  float w_bx_, w_by_, w_bz_; //
46 
47  // **** paramaters
48  double gain_; // algorithm gain
49  double zeta_; // gyro drift bias gain
50 
51 public:
56  void setAlgorithmGain(double gain);
57 
61  void setDriftBiasGain(double zeta);
62 
63  virtual void parseParameters(const ParametersMap & parameters);
64  virtual IMUFilter::Type type() const {return IMUFilter::kMadgwick;}
65  virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const;
66  virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0);
67 private:
68 
69  // Update from accelerometer and gyroscope data.
70  // [gx, gy, gz]: Angular veloctiy, in rad / s.
71  // [ax, ay, az]: Normalized gravity vector.
72  // dt: time delta, in seconds.
73  void updateImpl(
74  double gx, double gy, double gz,
75  double ax, double ay, double az,
76  double dt);
77 };
78 
79 }
80 
81 
82 #endif /* CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_ */
void setAlgorithmGain(double gain)
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
virtual void parseParameters(const ParametersMap &parameters)
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const
void setDriftBiasGain(double zeta)
MadgwickFilter(const ParametersMap &parameters=ParametersMap())
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
virtual IMUFilter::Type type() const
void updateImpl(double gx, double gy, double gz, double ax, double ay, double az, double dt)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29