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 IGNITION_MATH_FILTER_HH_
18 #define IGNITION_MATH_FILTER_HH_
19 
20 #include <ignition/math/Helpers.hh>
21 #include <ignition/math/Vector3.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
30  template <class T>
31  class Filter
32  {
34  public: virtual ~Filter() {}
35 
38  public: virtual void Set(const T &_val)
39  {
40  y0 = _val;
41  }
42 
46  public: virtual void Fc(double _fc, double _fs) = 0;
47 
50  public: virtual const T &Value() const
51  {
52  return y0;
53  }
54 
56  protected: T y0{};
57  };
58 
62  template <class T>
63  class OnePole : public Filter<T>
64  {
66  public: OnePole() = default;
67 
71  public: OnePole(double _fc, double _fs)
72  {
73  this->Fc(_fc, _fs);
74  }
75 
76  // Documentation Inherited.
77  public: virtual void Fc(double _fc, double _fs)
78  {
79  b1 = exp(-2.0 * IGN_PI * _fc / _fs);
80  a0 = 1.0 - b1;
81  }
82 
86  public: const T& Process(const T &_x)
87  {
88  this->y0 = a0 * _x + b1 * this->y0;
89  return this->y0;
90  }
91 
93  protected: double a0 = 0;
94 
96  protected: double b1 = 0;
97  };
98 
101  class OnePoleQuaternion : public OnePole<math::Quaterniond>
102  {
105  {
106  this->Set(math::Quaterniond(1, 0, 0, 0));
107  }
108 
112  public: OnePoleQuaternion(double _fc, double _fs)
113  : OnePole<math::Quaterniond>(_fc, _fs)
114  {
115  this->Set(math::Quaterniond(1, 0, 0, 0));
116  }
117 
121  public: const math::Quaterniond& Process(
122  const math::Quaterniond &_x)
123  {
124  y0 = math::Quaterniond::Slerp(a0, y0, _x);
125  return y0;
126  }
127  };
128 
131  class OnePoleVector3 : public OnePole<math::Vector3d>
132  {
134  public: OnePoleVector3()
135  {
136  this->Set(math::Vector3d(0, 0, 0));
137  }
138 
142  public: OnePoleVector3(double _fc, double _fs)
143  : OnePole<math::Vector3d>(_fc, _fs)
144  {
145  this->Set(math::Vector3d(0, 0, 0));
146  }
147  };
148 
152  template <class T>
153  class BiQuad : public Filter<T>
154  {
156  public: BiQuad() = default;
157 
161  public: BiQuad(double _fc, double _fs)
162  {
163  this->Fc(_fc, _fs);
164  }
165 
166  // Documentation Inherited.
167  public: void Fc(double _fc, double _fs)
168  {
169  this->Fc(_fc, _fs, 0.5);
170  }
171 
176  public: void Fc(double _fc, double _fs, double _q)
177  {
178  double k = tan(IGN_PI * _fc / _fs);
179  double kQuadDenom = k * k + k / _q + 1.0;
180  this->a0 = k * k/ kQuadDenom;
181  this->a1 = 2 * this->a0;
182  this->a2 = this->a0;
183  this->b0 = 1.0;
184  this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
185  this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
186  }
187 
190  public: virtual void Set(const T &_val)
191  {
192  this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
193  }
194 
198  public: virtual const T& Process(const T &_x)
199  {
200  this->y0 = this->a0 * _x +
201  this->a1 * this->x1 +
202  this->a2 * this->x2 -
203  this->b1 * this->y1 -
204  this->b2 * this->y2;
205 
206  this->x2 = this->x1;
207  this->x1 = _x;
208  this->y2 = this->y1;
209  this->y1 = this->y0;
210  return this->y0;
211  }
212 
214  protected: double a0 = 0,
215  a1 = 0,
216  a2 = 0,
217  b0 = 0,
218  b1 = 0,
219  b2 = 0;
220 
222  protected: T x1{}, x2{}, y1{}, y2{};
223  };
224 
227  class BiQuadVector3 : public BiQuad<math::Vector3d>
228  {
230  public: BiQuadVector3()
231  {
232  this->Set(math::Vector3d(0, 0, 0));
233  }
234 
238  public: BiQuadVector3(double _fc, double _fs)
239  : BiQuad<math::Vector3d>(_fc, _fs)
240  {
241  this->Set(math::Vector3d(0, 0, 0));
242  }
243  };
244  }
245 }
246 
247 #endif
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:167
Bi-quad filter base class.
Definition: Filter.hh:153
virtual void Set(const T &_val)
Set the current filter&#39;s output.
Definition: Filter.hh:190
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:161
BiQuad vector3 filter.
Definition: Filter.hh:227
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:77
virtual const T & Value() const
Get the output of the filter.
Definition: Filter.hh:50
T y0
Output.
Definition: Filter.hh:56
const T & Process(const T &_x)
Update the filter&#39;s output.
Definition: Filter.hh:86
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:104
virtual ~Filter()
Destructor.
Definition: Filter.hh:34
static Quaternion< double > Slerp(double _fT, const Quaternion< double > &_rkP, const Quaternion< double > &_rkQ, bool _shortestPath=false)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
Definition: Quaternion.hh:855
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:176
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:238
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:142
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:71
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:112
OnePoleVector3()
Constructor.
Definition: Filter.hh:134
One-pole quaternion filter.
Definition: Filter.hh:101
virtual void Set(const T &_val)
Set the output of the filter.
Definition: Filter.hh:38
Filter base class.
Definition: Filter.hh:31
Definition: Angle.hh:38
A one-pole DSP filter.
Definition: Filter.hh:63
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:173
BiQuadVector3()
Constructor.
Definition: Filter.hh:230
One-pole vector3 filter.
Definition: Filter.hh:131
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter&#39;s output.
Definition: Filter.hh:121
virtual const T & Process(const T &_x)
Update the filter&#39;s output.
Definition: Filter.hh:198