Gazebo Math

API Reference

6.15.1
gz/math/Filter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_MATH_FILTER_HH_
18 #define GZ_MATH_FILTER_HH_
19 
20 #include <gz/math/Helpers.hh>
21 #include <gz/math/Vector3.hh>
22 #include <gz/math/Quaternion.hh>
23 #include <gz/math/config.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
29  // Inline bracket to help doxygen filtering.
30  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
31  //
34  template <class T>
35  class Filter
36  {
38  public: virtual ~Filter() {}
39 
42  public: virtual void Set(const T &_val)
43  {
44  y0 = _val;
45  }
46 
50  public: virtual void Fc(double _fc, double _fs) = 0;
51 
54  public: virtual const T &Value() const
55  {
56  return y0;
57  }
58 
60  protected: T y0{};
61  };
62 
66  template <class T>
67  class OnePole : public Filter<T>
68  {
70  public: OnePole() = default;
71 
75  public: OnePole(double _fc, double _fs)
76  {
77  this->Fc(_fc, _fs);
78  }
79 
80  // Documentation Inherited.
81  public: virtual void Fc(double _fc, double _fs) override
82  {
83  b1 = exp(-2.0 * IGN_PI * _fc / _fs);
84  a0 = 1.0 - b1;
85  }
86 
90  public: const T& Process(const T &_x)
91  {
92  this->y0 = a0 * _x + b1 * this->y0;
93  return this->y0;
94  }
95 
97  protected: double a0 = 0;
98 
100  protected: double b1 = 0;
101  };
102 
105  class OnePoleQuaternion : public OnePole<math::Quaterniond>
106  {
109  {
110  this->Set(math::Quaterniond(1, 0, 0, 0));
111  }
112 
116  public: OnePoleQuaternion(double _fc, double _fs)
117  : OnePole<math::Quaterniond>(_fc, _fs)
118  {
119  this->Set(math::Quaterniond(1, 0, 0, 0));
120  }
121 
125  public: const math::Quaterniond& Process(
126  const math::Quaterniond &_x)
127  {
128  y0 = math::Quaterniond::Slerp(a0, y0, _x);
129  return y0;
130  }
131  };
132 
135  class OnePoleVector3 : public OnePole<math::Vector3d>
136  {
138  public: OnePoleVector3()
139  {
140  this->Set(math::Vector3d(0, 0, 0));
141  }
142 
146  public: OnePoleVector3(double _fc, double _fs)
147  : OnePole<math::Vector3d>(_fc, _fs)
148  {
149  this->Set(math::Vector3d(0, 0, 0));
150  }
151  };
152 
156  template <class T>
157  class BiQuad : public Filter<T>
158  {
160  public: BiQuad() = default;
161 
165  public: BiQuad(double _fc, double _fs)
166  {
167  this->Fc(_fc, _fs);
168  }
169 
170  // Documentation Inherited.
171  public: void Fc(double _fc, double _fs) override
172  {
173  this->Fc(_fc, _fs, 0.5);
174  }
175 
180  public: void Fc(double _fc, double _fs, double _q)
181  {
182  double k = tan(IGN_PI * _fc / _fs);
183  double kQuadDenom = k * k + k / _q + 1.0;
184  this->a0 = k * k/ kQuadDenom;
185  this->a1 = 2 * this->a0;
186  this->a2 = this->a0;
187  this->b0 = 1.0;
188  this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
189  this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
190  }
191 
194  public: virtual void Set(const T &_val) override
195  {
196  this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
197  }
198 
202  public: virtual const T& Process(const T &_x)
203  {
204  this->y0 = this->a0 * _x +
205  this->a1 * this->x1 +
206  this->a2 * this->x2 -
207  this->b1 * this->y1 -
208  this->b2 * this->y2;
209 
210  this->x2 = this->x1;
211  this->x1 = _x;
212  this->y2 = this->y1;
213  this->y1 = this->y0;
214  return this->y0;
215  }
216 
218  protected: double a0 = 0,
219  a1 = 0,
220  a2 = 0,
221  b0 = 0,
222  b1 = 0,
223  b2 = 0;
224 
226  protected: T x1{}, x2{}, y1{}, y2{};
227  };
228 
231  class BiQuadVector3 : public BiQuad<math::Vector3d>
232  {
234  public: BiQuadVector3()
235  {
236  this->Set(math::Vector3d(0, 0, 0));
237  }
238 
242  public: BiQuadVector3(double _fc, double _fs)
243  : BiQuad<math::Vector3d>(_fc, _fs)
244  {
245  this->Set(math::Vector3d(0, 0, 0));
246  }
247  };
248  }
249  }
250 }
251 
252 #endif
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: gz/math/Filter.hh:146
BiQuadVector3()
Constructor.
Definition: gz/math/Filter.hh:234
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: gz/math/Filter.hh:116
virtual ~Filter()
Destructor.
Definition: gz/math/Filter.hh:38
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
T y0
Output.
Definition: gz/math/Filter.hh:60
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4. This was put here for Windows support.
Definition: gz/math/Helpers.hh:184
T y1
Definition: gz/math/Filter.hh:226
A one-pole DSP filter.
Definition: gz/math/Filter.hh:67
double a0
Input gain control.
Definition: gz/math/Filter.hh:97
double a2
Definition: gz/math/Filter.hh:220
virtual const T & Process(const T &_x)
Update the filter's output.
Definition: gz/math/Filter.hh:202
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: gz/math/Filter.hh:242
A quaternion class.
Definition: gz/math/Matrix3.hh:35
T x2
Definition: gz/math/Filter.hh:226
BiQuad()=default
Constructor.
One-pole quaternion filter.
Definition: gz/math/Filter.hh:105
static Quaternion< T > Slerp(T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkQ, bool _shortestPath=false)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
Definition: gz/math/Quaternion.hh:885
OnePole(double _fc, double _fs)
Constructor.
Definition: gz/math/Filter.hh:75
virtual const T & Value() const
Get the output of the filter.
Definition: gz/math/Filter.hh:54
double b1
Gain of the feedback.
Definition: gz/math/Filter.hh:100
const T & Process(const T &_x)
Update the filter's output.
Definition: gz/math/Filter.hh:90
OnePoleQuaternion()
Constructor.
Definition: gz/math/Filter.hh:108
T y2
Definition: gz/math/Filter.hh:226
double a1
Definition: gz/math/Filter.hh:219
Bi-quad filter base class.
Definition: gz/math/Filter.hh:157
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: gz/math/Filter.hh:180
double b2
Definition: gz/math/Filter.hh:223
Filter base class.
Definition: gz/math/Filter.hh:35
OnePoleVector3()
Constructor.
Definition: gz/math/Filter.hh:138
T x1
Gain of the feedback coefficients.
Definition: gz/math/Filter.hh:226
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition: gz/math/Filter.hh:125
double b0
Definition: gz/math/Filter.hh:221
BiQuad(double _fc, double _fs)
Constructor.
Definition: gz/math/Filter.hh:165
void Fc(double _fc, double _fs) override
Set the cutoff frequency and sample rate.
Definition: gz/math/Filter.hh:171
BiQuad vector3 filter.
Definition: gz/math/Filter.hh:231
double b1
Definition: gz/math/Filter.hh:222
virtual void Set(const T &_val) override
Set the current filter's output.
Definition: gz/math/Filter.hh:194
virtual void Set(const T &_val)
Set the output of the filter.
Definition: gz/math/Filter.hh:42
One-pole vector3 filter.
Definition: gz/math/Filter.hh:135
OnePole()=default
Constructor.
virtual void Fc(double _fc, double _fs) override
Set the cutoff frequency and sample rate.
Definition: gz/math/Filter.hh:81
double a0
Input gain control coefficients.
Definition: gz/math/Filter.hh:218