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 #include <ignition/math/config.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE
30  {
33  template <class T>
34  class Filter
35  {
37  public: virtual ~Filter() {}
38 
41  public: virtual void Set(const T &_val)
42  {
43  y0 = _val;
44  }
45 
49  public: virtual void Fc(double _fc, double _fs) = 0;
50 
53  public: virtual const T &Value() const
54  {
55  return y0;
56  }
57 
59  protected: T y0{};
60  };
61 
65  template <class T>
66  class OnePole : public Filter<T>
67  {
69  public: OnePole() = default;
70 
74  public: OnePole(double _fc, double _fs)
75  {
76  this->Fc(_fc, _fs);
77  }
78 
79  // Documentation Inherited.
80  public: virtual void Fc(double _fc, double _fs)
81  {
82  b1 = exp(-2.0 * IGN_PI * _fc / _fs);
83  a0 = 1.0 - b1;
84  }
85 
89  public: const T& Process(const T &_x)
90  {
91  this->y0 = a0 * _x + b1 * this->y0;
92  return this->y0;
93  }
94 
96  protected: double a0 = 0;
97 
99  protected: double b1 = 0;
100  };
101 
104  class OnePoleQuaternion : public OnePole<math::Quaterniond>
105  {
108  {
109  this->Set(math::Quaterniond(1, 0, 0, 0));
110  }
111 
115  public: OnePoleQuaternion(double _fc, double _fs)
116  : OnePole<math::Quaterniond>(_fc, _fs)
117  {
118  this->Set(math::Quaterniond(1, 0, 0, 0));
119  }
120 
124  public: const math::Quaterniond& Process(
125  const math::Quaterniond &_x)
126  {
128  return y0;
129  }
130  };
131 
134  class OnePoleVector3 : public OnePole<math::Vector3d>
135  {
137  public: OnePoleVector3()
138  {
139  this->Set(math::Vector3d(0, 0, 0));
140  }
141 
145  public: OnePoleVector3(double _fc, double _fs)
146  : OnePole<math::Vector3d>(_fc, _fs)
147  {
148  this->Set(math::Vector3d(0, 0, 0));
149  }
150  };
151 
155  template <class T>
156  class BiQuad : public Filter<T>
157  {
159  public: BiQuad() = default;
160 
164  public: BiQuad(double _fc, double _fs)
165  {
166  this->Fc(_fc, _fs);
167  }
168 
169  // Documentation Inherited.
170  public: void Fc(double _fc, double _fs)
171  {
172  this->Fc(_fc, _fs, 0.5);
173  }
174 
179  public: void Fc(double _fc, double _fs, double _q)
180  {
181  double k = tan(IGN_PI * _fc / _fs);
182  double kQuadDenom = k * k + k / _q + 1.0;
183  this->a0 = k * k/ kQuadDenom;
184  this->a1 = 2 * this->a0;
185  this->a2 = this->a0;
186  this->b0 = 1.0;
187  this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
188  this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
189  }
190 
193  public: virtual void Set(const T &_val)
194  {
195  this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
196  }
197 
201  public: virtual const T& Process(const T &_x)
202  {
203  this->y0 = this->a0 * _x +
204  this->a1 * this->x1 +
205  this->a2 * this->x2 -
206  this->b1 * this->y1 -
207  this->b2 * this->y2;
208 
209  this->x2 = this->x1;
210  this->x1 = _x;
211  this->y2 = this->y1;
212  this->y1 = this->y0;
213  return this->y0;
214  }
215 
217  protected: double a0 = 0,
218  a1 = 0,
219  a2 = 0,
220  b0 = 0,
221  b1 = 0,
222  b2 = 0;
223 
225  protected: T x1{}, x2{}, y1{}, y2{};
226  };
227 
230  class BiQuadVector3 : public BiQuad<math::Vector3d>
231  {
233  public: BiQuadVector3()
234  {
235  this->Set(math::Vector3d(0, 0, 0));
236  }
237 
241  public: BiQuadVector3(double _fc, double _fs)
242  : BiQuad<math::Vector3d>(_fc, _fs)
243  {
244  this->Set(math::Vector3d(0, 0, 0));
245  }
246  };
247  }
248  }
249 }
250 
251 #endif
Helpers.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Filter::~Filter
virtual ~Filter()
Destructor.
Definition: Filter.hh:37
ignition
Definition: Angle.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Slerp
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: Quaternion.hh:868
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::a0
double a0
Input gain control coefficients.
Definition: Filter.hh:217
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::b2
double b2
Definition: Filter.hh:222
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::a1
double a1
Definition: Filter.hh:218
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole::b1
double b1
Gain of the feedback.
Definition: Filter.hh:99
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole::a0
double a0
Input gain control.
Definition: Filter.hh:96
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole::OnePole
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:74
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole
A one-pole DSP filter.
Definition: Filter.hh:67
Vector3.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleVector3
One-pole vector3 filter.
Definition: Filter.hh:135
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuadVector3
BiQuad vector3 filter.
Definition: Filter.hh:231
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion
A quaternion class.
Definition: Matrix3.hh:33
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleVector3::OnePoleVector3
OnePoleVector3()
Constructor.
Definition: Filter.hh:137
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleQuaternion::OnePoleQuaternion
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:115
Quaternion.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole::Fc
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:80
IGN_PI
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:174
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::Process
virtual const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:201
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Filter
Filter base class.
Definition: Filter.hh:35
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuadVector3::BiQuadVector3
BiQuadVector3()
Constructor.
Definition: Filter.hh:233
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Filter::Value
virtual const T & Value() const
Get the output of the filter.
Definition: Filter.hh:53
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad
Bi-quad filter base class.
Definition: Filter.hh:157
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Filter::Fc
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::Fc
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:179
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Filter::Set
virtual void Set(const T &_val)
Set the output of the filter.
Definition: Filter.hh:41
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::BiQuad
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:164
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleQuaternion::OnePoleQuaternion
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:107
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::Fc
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:170
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::x2
T x2
Definition: Filter.hh:225
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: Vector3.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Filter::y0
T y0
Output.
Definition: Filter.hh:59
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::a2
double a2
Definition: Filter.hh:219
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole::OnePole
OnePole()=default
Constructor.
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePole::Process
const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:89
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleVector3::OnePoleVector3
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:145
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::b1
double b1
Definition: Filter.hh:221
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::x1
T x1
Gain of the feedback coefficients.
Definition: Filter.hh:225
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleQuaternion
One-pole quaternion filter.
Definition: Filter.hh:105
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::OnePoleQuaternion::Process
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition: Filter.hh:124
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::y1
T y1
Definition: Filter.hh:225
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::y2
T y2
Definition: Filter.hh:225
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::Set
virtual void Set(const T &_val)
Set the current filter's output.
Definition: Filter.hh:193
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::BiQuad
BiQuad()=default
Constructor.
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuad::b0
double b0
Definition: Filter.hh:220
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::BiQuadVector3::BiQuadVector3
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:241