Filter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014-2016 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 
18 #ifndef _GAZEBO_FILTER_HH_
19 #define _GAZEBO_FILTER_HH_
20 
21 #include <gazebo/math/Vector3.hh>
23 
24 namespace gazebo
25 {
27 
29  namespace math
30  {
33 
36  template <class T>
38  {
40  public: virtual ~Filter() {}
41 
44  public: virtual void SetValue(const T &_val) { y0 = _val; }
45 
49  public: virtual void SetFc(double _fc, double _fs) = 0;
50 
53  public: inline virtual const T& GetValue() { return y0; }
54 
56  protected: T y0;
57  };
58 
62  template <class T>
63  class GZ_MATH_VISIBLE OnePole : public Filter<T>
64  {
66  public: OnePole() : a0(0), b1(0) {}
67 
71  public: OnePole(double _fc, double _fs)
72  : a0(0), b1(0)
73  {
74  this->SetFc(_fc, _fs);
75  }
76 
77  // Documentation Inherited.
78  public: virtual void SetFc(double _fc, double _fs)
79  {
80  b1 = exp(-2.0 * M_PI * _fc / _fs);
81  a0 = 1.0 - b1;
82  }
83 
87  public: inline const T& Process(const T &_x)
88  {
89  this->y0 = a0 * _x + b1 * this->y0;
90  return this->y0;
91  }
92 
94  protected: double a0;
95 
97  protected: double b1;
98  };
99 
102  class GZ_MATH_VISIBLE OnePoleQuaternion : public OnePole<math::Quaternion>
103  {
106  {
107  this->SetValue(math::Quaternion(1, 0, 0, 0));
108  }
109 
113  public: OnePoleQuaternion(double _fc, double _fs)
114  : OnePole<math::Quaternion>(_fc, _fs)
115  {
116  this->SetValue(math::Quaternion(1, 0, 0, 0));
117  }
118 
122  public: inline const math::Quaternion& Process(const math::Quaternion &_x)
123  {
124  y0 = math::Quaternion::Slerp(a0, y0, _x);
125  return y0;
126  }
127  };
128 
131  class GZ_MATH_VISIBLE OnePoleVector3 : public OnePole<math::Vector3>
132  {
134  public: OnePoleVector3()
135  {
136  this->SetValue(math::Vector3(0, 0, 0));
137  }
138 
142  public: OnePoleVector3(double _fc, double _fs)
143  : OnePole<math::Vector3>(_fc, _fs)
144  {
145  this->SetValue(math::Vector3(0, 0, 0));
146  }
147  };
148 
152  template <class T>
153  class GZ_MATH_VISIBLE BiQuad : public Filter<T>
154  {
156  public: BiQuad()
157  : a0(0), a1(0), a2(0), b0(0), b1(0), b2(0)
158  {
159  }
160 
164  public: BiQuad(double _fc, double _fs)
165  : a0(0), a1(0), a2(0), b0(0), b1(0), b2(0)
166  {
167  this->SetFc(_fc, _fs);
168  }
169 
170  // Documentation Inherited.
171  public: inline void SetFc(double _fc, double _fs)
172  {
173  this->SetFc(_fc, _fs, 0.5);
174  }
175 
180  public: inline void SetFc(double _fc, double _fs, double _q)
181  {
182  double k = tan(M_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 SetValue(const T &_val)
195  {
196  this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
197  }
198 
202  public: inline 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, a1, a2, b0, b1, b2;
219 
221  protected: T x1, x2, y1, y2;
222  };
223 
226  class GZ_MATH_VISIBLE BiQuadVector3 : public BiQuad<math::Vector3>
227  {
229  public: BiQuadVector3()
230  {
231  this->SetValue(math::Vector3(0, 0, 0));
232  }
233 
237  public: BiQuadVector3(double _fc, double _fs)
238  : BiQuad<math::Vector3>(_fc, _fs)
239  {
240  this->SetValue(math::Vector3(0, 0, 0));
241  }
242  };
243 
245  }
246 }
247 
248 #endif
OnePoleVector3()
Constructor.
Definition: Filter.hh:134
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:113
virtual const T & GetValue()
Get the output of the filter.
Definition: Filter.hh:53
BiQuad vector3 filter.
Definition: Filter.hh:226
static Quaternion Slerp(double _fT, const Quaternion &_rkP, const Quaternion &_rkQ, bool _shortestPath=false)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
One-pole quaternion filter.
Definition: Filter.hh:102
const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:87
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
T y0
Output.
Definition: Filter.hh:56
double b1
Gain of the feedback.
Definition: Filter.hh:97
double a0
Input gain control.
Definition: Filter.hh:94
virtual void SetFc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:78
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:142
Bi-quad filter base class.
Definition: Filter.hh:153
virtual void SetValue(const T &_val)
Set the current filter's output.
Definition: Filter.hh:194
OnePole()
Constructor.
Definition: Filter.hh:66
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:164
A one-pole DSP filter.
Definition: Filter.hh:63
#define GZ_MATH_VISIBLE
Definition: system.hh:134
BiQuadVector3()
Constructor.
Definition: Filter.hh:229
double b2
Definition: Filter.hh:218
virtual const T & process(const T &_x)
Update the filter's output.
Definition: Filter.hh:202
BiQuad()
Constructor.
Definition: Filter.hh:156
One-pole vector3 filter.
Definition: Filter.hh:131
T y2
Definition: Filter.hh:221
A quaternion class.
Definition: Quaternion.hh:42
void SetFc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:171
virtual void SetValue(const T &_val)
Set the output of the filter.
Definition: Filter.hh:44
const math::Quaternion & Process(const math::Quaternion &_x)
Update the filter's output.
Definition: Filter.hh:122
virtual ~Filter()
Destructor.
Definition: Filter.hh:40
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:71
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:105
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:237
Filter base class.
Definition: Filter.hh:37
void SetFc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:180