ComplementaryFilter.h
Go to the documentation of this file.
1 /*
2  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
3  @section LICENSE
4  Copyright (c) 2015, City University of New York
5  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
6  All rights reserved.
7  Redistribution and use in source and binary forms, with or without
8  modification, are permitted provided that the following conditions are met:
9  1. Redistributions of source code must retain the above copyright
10  notice, this list of conditions and the following disclaimer.
11  2. Redistributions in binary form must reproduce the above copyright
12  notice, this list of conditions and the following disclaimer in the
13  documentation and/or other materials provided with the distribution.
14  3. Neither the name of the City College of New York nor the
15  names of its contributors may be used to endorse or promote products
16  derived from this software without specific prior written permission.
17  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20  DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
21  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28 
29 
30 #ifndef CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_
31 #define CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_
32 
33 #include <rtabmap/core/IMUFilter.h>
34 
35 namespace rtabmap {
36 
38 {
39  public:
40  ComplementaryFilter(const ParametersMap & parameters = ParametersMap());
41  virtual ~ComplementaryFilter() {}
42 
43  bool setGainAcc(double gain);
44  double getGainAcc() const;
45 
46  bool setBiasAlpha(double bias_alpha);
47  double getBiasAlpha() const;
48 
49  // When the filter is in the steady state, bias estimation will occur (if the
50  // parameter is enabled).
51  bool getSteadyState() const;
52 
53  void setDoBiasEstimation(bool do_bias_estimation);
54  bool getDoBiasEstimation() const;
55 
56  void setDoAdaptiveGain(bool do_adaptive_gain);
57  bool getDoAdaptiveGain() const;
58 
59  double getAngularVelocityBiasX() const;
60  double getAngularVelocityBiasY() const;
61  double getAngularVelocityBiasZ() const;
62 
63  virtual void parseParameters(const ParametersMap & parameters);
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 
68  // Update from accelerometer and gyroscope data.
69  // [gx, gy, gz]: Angular veloctiy, in rad / s.
70  // [ax, ay, az]: Normalized gravity vector.
71  // dt: time delta, in seconds.
72  virtual void updateImpl(
73  double gx, double gy, double gz,
74  double ax, double ay, double az,
75  double dt);
76 
77  private:
78  static const double kGravity;
79  static const double gamma_;
80  // Bias estimation steady state thresholds
81  static const double kAngularVelocityThreshold;
82  static const double kAccelerationThreshold;
83  static const double kDeltaAngularVelocityThreshold;
84 
85  // Gain parameter for the complementary filter, belongs in [0, 1].
86  double gain_acc_;
87 
88  // Bias estimation gain parameter, belongs in [0, 1].
89  double bias_alpha_;
90 
91  // Parameter whether to do bias estimation or not.
93 
94  // Parameter whether to do adaptive gain or not.
96 
99 
100  // The orientation as a Hamilton quaternion (q0 is the scalar). Represents
101  // the orientation of the fixed frame wrt the body frame.
102  double q0_, q1_, q2_, q3_;
103 
104  // Bias in angular velocities;
106 
107  // Bias in angular velocities;
109 
110  void updateBiases(double ax, double ay, double az,
111  double wx, double wy, double wz);
112 
113  bool checkState(double ax, double ay, double az,
114  double wx, double wy, double wz) const;
115 
116  void getPrediction(
117  double wx, double wy, double wz, double dt,
118  double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const;
119 
120  void getMeasurement(
121  double ax, double ay, double az,
122  double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
123 
124  void getMeasurement(
125  double ax, double ay, double az,
126  double mx, double my, double mz,
127  double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
128 
129  void getAccCorrection(
130  double ax, double ay, double az,
131  double p0, double p1, double p2, double p3,
132  double& dq0, double& dq1, double& dq2, double& dq3);
133 
134  double getAdaptiveGain(double alpha, double ax, double ay, double az);
135 };
136 
137 // Utility math functions:
138 
139 void normalizeVector(double& x, double& y, double& z);
140 
141 void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3);
142 
143 void scaleQuaternion(double gain,
144  double& dq0, double& dq1, double& dq2, double& dq3);
145 
146 void invertQuaternion(
147  double q0, double q1, double q2, double q3,
148  double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv);
149 
150 void quaternionMultiplication(double p0, double p1, double p2, double p3,
151  double q0, double q1, double q2, double q3,
152  double& r0, double& r1, double& r2, double& r3);
153 
154 void rotateVectorByQuaternion(double x, double y, double z,
155  double q0, double q1, double q2, double q3,
156 double& vx, double& vy, double& vz);
157 
158 }
159 
160 
161 #endif /* CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_ */
rtabmap::ComplementaryFilter::~ComplementaryFilter
virtual ~ComplementaryFilter()
Definition: ComplementaryFilter.h:41
rtabmap::ComplementaryFilter::setBiasAlpha
bool setBiasAlpha(double bias_alpha)
Definition: ComplementaryFilter.cpp:111
rtabmap::ComplementaryFilter::getAccCorrection
void getAccCorrection(double ax, double ay, double az, double p0, double p1, double p2, double p3, double &dq0, double &dq1, double &dq2, double &dq3)
Definition: ComplementaryFilter.cpp:354
rtabmap::rotateVectorByQuaternion
void rotateVectorByQuaternion(double x, double y, double z, double q0, double q1, double q2, double q3, double &vx, double &vy, double &vz)
Definition: ComplementaryFilter.cpp:470
rtabmap::ComplementaryFilter::getGainAcc
double getGainAcc() const
Definition: ComplementaryFilter.cpp:101
rtabmap::ComplementaryFilter::q1_
double q1_
Definition: ComplementaryFilter.h:102
rtabmap::ComplementaryFilter::kAccelerationThreshold
static const double kAccelerationThreshold
Definition: ComplementaryFilter.h:82
rtabmap::ComplementaryFilter::setGainAcc
bool setGainAcc(double gain)
Definition: ComplementaryFilter.cpp:90
rtabmap::ComplementaryFilter::wx_prev_
double wx_prev_
Definition: ComplementaryFilter.h:105
rtabmap::ComplementaryFilter::wx_bias_
double wx_bias_
Definition: ComplementaryFilter.h:108
rtabmap::ComplementaryFilter::getOrientation
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const
Definition: ComplementaryFilter.cpp:376
rtabmap::ComplementaryFilter::initialized_
bool initialized_
Definition: ComplementaryFilter.h:97
rtabmap::ComplementaryFilter::setDoAdaptiveGain
void setDoAdaptiveGain(bool do_adaptive_gain)
Definition: ComplementaryFilter.cpp:80
rtabmap::invertQuaternion
void invertQuaternion(double q0, double q1, double q2, double q3, double &q0_inv, double &q1_inv, double &q2_inv, double &q3_inv)
Definition: ComplementaryFilter.cpp:420
rtabmap::ComplementaryFilter::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: ComplementaryFilter.cpp:62
rtabmap::ComplementaryFilter
Definition: ComplementaryFilter.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::ComplementaryFilter::updateBiases
void updateBiases(double ax, double ay, double az, double wx, double wy, double wz)
Definition: ComplementaryFilter.cpp:237
rtabmap::ComplementaryFilter::wz_prev_
double wz_prev_
Definition: ComplementaryFilter.h:105
rtabmap::IMUFilter::Type
Type
Definition: IMUFilter.h:40
rtabmap::ComplementaryFilter::wz_bias_
double wz_bias_
Definition: ComplementaryFilter.h:108
rtabmap::ComplementaryFilter::getPrediction
void getPrediction(double wx, double wy, double wz, double dt, double &q0_pred, double &q1_pred, double &q2_pred, double &q3_pred) const
Definition: ComplementaryFilter.cpp:254
rtabmap::ComplementaryFilter::getSteadyState
bool getSteadyState() const
Definition: ComplementaryFilter.cpp:106
rtabmap::ComplementaryFilter::getAngularVelocityBiasY
double getAngularVelocityBiasY() const
Definition: ComplementaryFilter.cpp:149
rtabmap::ComplementaryFilter::q2_
double q2_
Definition: ComplementaryFilter.h:102
rtabmap::IMUFilter
Definition: IMUFilter.h:37
rtabmap::normalizeQuaternion
void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3)
Definition: ComplementaryFilter.cpp:411
rtabmap::ComplementaryFilter::q3_
double q3_
Definition: ComplementaryFilter.h:102
rtabmap::ComplementaryFilter::getAdaptiveGain
double getAdaptiveGain(double alpha, double ax, double ay, double az)
Definition: ComplementaryFilter.cpp:383
rtabmap::ComplementaryFilter::gain_acc_
double gain_acc_
Definition: ComplementaryFilter.h:86
rtabmap::IMUFilter::kComplementaryFilter
@ kComplementaryFilter
Definition: IMUFilter.h:42
rtabmap::ComplementaryFilter::do_bias_estimation_
bool do_bias_estimation_
Definition: ComplementaryFilter.h:92
rtabmap::ComplementaryFilter::kAngularVelocityThreshold
static const double kAngularVelocityThreshold
Definition: ComplementaryFilter.h:81
rtabmap::ComplementaryFilter::type
virtual IMUFilter::Type type() const
Definition: ComplementaryFilter.h:64
rtabmap::normalizeVector
void normalizeVector(double &x, double &y, double &z)
Definition: ComplementaryFilter.cpp:402
rtabmap::ComplementaryFilter::do_adaptive_gain_
bool do_adaptive_gain_
Definition: ComplementaryFilter.h:95
rtabmap::ComplementaryFilter::getDoAdaptiveGain
bool getDoAdaptiveGain() const
Definition: ComplementaryFilter.cpp:85
rtabmap::ComplementaryFilter::steady_state_
bool steady_state_
Definition: ComplementaryFilter.h:98
rtabmap::ComplementaryFilter::updateImpl
virtual void updateImpl(double gx, double gy, double gz, double ax, double ay, double az, double dt)
Definition: ComplementaryFilter.cpp:159
rtabmap::ComplementaryFilter::kGravity
static const double kGravity
Definition: ComplementaryFilter.h:78
rtabmap::quaternionMultiplication
void quaternionMultiplication(double p0, double p1, double p2, double p3, double q0, double q1, double q2, double q3, double &r0, double &r1, double &r2, double &r3)
Definition: ComplementaryFilter.cpp:458
rtabmap::ComplementaryFilter::getDoBiasEstimation
bool getDoBiasEstimation() const
Definition: ComplementaryFilter.cpp:75
rtabmap::scaleQuaternion
void scaleQuaternion(double gain, double &dq0, double &dq1, double &dq2, double &dq3)
Definition: ComplementaryFilter.cpp:431
rtabmap::ComplementaryFilter::q0_
double q0_
Definition: ComplementaryFilter.h:102
IMUFilter.h
rtabmap::ComplementaryFilter::reset
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
Definition: ComplementaryFilter.cpp:127
rtabmap::ComplementaryFilter::getBiasAlpha
double getBiasAlpha() const
Definition: ComplementaryFilter.cpp:122
rtabmap::ComplementaryFilter::bias_alpha_
double bias_alpha_
Definition: ComplementaryFilter.h:89
rtabmap::ComplementaryFilter::wy_prev_
double wy_prev_
Definition: ComplementaryFilter.h:105
rtabmap::ComplementaryFilter::wy_bias_
double wy_bias_
Definition: ComplementaryFilter.h:108
rtabmap::ComplementaryFilter::checkState
bool checkState(double ax, double ay, double az, double wx, double wy, double wz) const
Definition: ComplementaryFilter.cpp:217
rtabmap::ComplementaryFilter::getMeasurement
void getMeasurement(double ax, double ay, double az, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas)
Definition: ComplementaryFilter.cpp:326
rtabmap::ComplementaryFilter::ComplementaryFilter
ComplementaryFilter(const ParametersMap &parameters=ParametersMap())
Definition: ComplementaryFilter.cpp:47
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::ComplementaryFilter::getAngularVelocityBiasX
double getAngularVelocityBiasX() const
Definition: ComplementaryFilter.cpp:144
rtabmap::ComplementaryFilter::kDeltaAngularVelocityThreshold
static const double kDeltaAngularVelocityThreshold
Definition: ComplementaryFilter.h:83
rtabmap::ComplementaryFilter::gamma_
static const double gamma_
Definition: ComplementaryFilter.h:79
rtabmap::ComplementaryFilter::setDoBiasEstimation
void setDoBiasEstimation(bool do_bias_estimation)
Definition: ComplementaryFilter.cpp:70
rtabmap::ComplementaryFilter::getAngularVelocityBiasZ
double getAngularVelocityBiasZ() const
Definition: ComplementaryFilter.cpp:154


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