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_ */
rtabmap::MadgwickFilter::reset
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
Definition: MadgwickFilter.cpp:287
rtabmap::MadgwickFilter::getOrientation
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const
Definition: MadgwickFilter.cpp:270
rtabmap::MadgwickFilter::q1
double q1
Definition: MadgwickFilter.h:43
rtabmap::MadgwickFilter::type
virtual IMUFilter::Type type() const
Definition: MadgwickFilter.h:64
rtabmap::MadgwickFilter::q3
double q3
Definition: MadgwickFilter.h:43
rtabmap::MadgwickFilter::q2
double q2
Definition: MadgwickFilter.h:43
rtabmap::MadgwickFilter::w_bz_
float w_bz_
Definition: MadgwickFilter.h:44
rtabmap::MadgwickFilter::updateImpl
void updateImpl(double gx, double gy, double gz, double ax, double ay, double az, double dt)
Definition: MadgwickFilter.cpp:301
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::MadgwickFilter::w_by_
float w_by_
Definition: MadgwickFilter.h:44
rtabmap::IMUFilter::kMadgwick
@ kMadgwick
Definition: IMUFilter.h:41
rtabmap::IMUFilter::Type
Type
Definition: IMUFilter.h:40
rtabmap::MadgwickFilter::gain_
double gain_
Definition: MadgwickFilter.h:48
rtabmap::MadgwickFilter::zeta_
double zeta_
Definition: MadgwickFilter.h:49
rtabmap::IMUFilter
Definition: IMUFilter.h:37
rtabmap::MadgwickFilter::w_bx_
float w_bx_
Definition: MadgwickFilter.h:44
rtabmap::MadgwickFilter::~MadgwickFilter
virtual ~MadgwickFilter()
Definition: MadgwickFilter.h:39
rtabmap::MadgwickFilter::initialized_
bool initialized_
Definition: MadgwickFilter.h:45
rtabmap::MadgwickFilter
Definition: MadgwickFilter.h:34
IMUFilter.h
rtabmap::MadgwickFilter::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: MadgwickFilter.cpp:247
rtabmap::MadgwickFilter::q0
double q0
Definition: MadgwickFilter.h:43
rtabmap::MadgwickFilter::setDriftBiasGain
void setDriftBiasGain(double zeta)
Definition: MadgwickFilter.cpp:265
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::MadgwickFilter::setAlgorithmGain
void setAlgorithmGain(double gain)
Definition: MadgwickFilter.cpp:257
rtabmap::MadgwickFilter::MadgwickFilter
MadgwickFilter(const ParametersMap &parameters=ParametersMap())
Definition: MadgwickFilter.cpp:236


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11