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_ */
void updateBiases(double ax, double ay, double az, double wx, double wy, double wz)
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
void invertQuaternion(double q0, double q1, double q2, double q3, double &q0_inv, double &q1_inv, double &q2_inv, double &q3_inv)
static const double kAccelerationThreshold
virtual IMUFilter::Type type() const
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3)
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const
void getMeasurement(double ax, double ay, double az, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas)
virtual void parseParameters(const ParametersMap &parameters)
static const double kDeltaAngularVelocityThreshold
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)
void setDoAdaptiveGain(bool do_adaptive_gain)
bool checkState(double ax, double ay, double az, double wx, double wy, double wz) const
bool setBiasAlpha(double bias_alpha)
void scaleQuaternion(double gain, double &dq0, double &dq1, double &dq2, double &dq3)
void normalizeVector(double &x, double &y, double &z)
virtual void updateImpl(double gx, double gy, double gz, double ax, double ay, double az, double dt)
ComplementaryFilter(const ParametersMap &parameters=ParametersMap())
void rotateVectorByQuaternion(double x, double y, double z, double q0, double q1, double q2, double q3, double &vx, double &vy, double &vz)
double getAdaptiveGain(double alpha, double ax, double ay, double az)
void setDoBiasEstimation(bool do_bias_estimation)
void getPrediction(double wx, double wy, double wz, double dt, double &q0_pred, double &q1_pred, double &q2_pred, double &q3_pred) const
void getAccCorrection(double ax, double ay, double az, double p0, double p1, double p2, double p3, double &dq0, double &dq1, double &dq2, double &dq3)
static const double kAngularVelocityThreshold


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