All Classes Namespaces Files Functions Variables Typedefs Friends Macros Pages
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 
18 #ifndef _IGNITION_FILTER_HH_
19 #define _IGNITION_FILTER_HH_
20 
21 #include <ignition/math/Helpers.hh>
22 #include <ignition/math/Vector3.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
31  template <class T>
32  class Filter
33  {
35  public: virtual ~Filter() {}
36 
39  public: virtual void Set(const T &_val)
40  {
41  y0 = _val;
42  }
43 
47  public: virtual void Fc(double _fc, double _fs) = 0;
48 
51  public: virtual const T &Value() const
52  {
53  return y0;
54  }
55 
57  protected: T y0{};
58  };
59 
63  template <class T>
64  class OnePole : public Filter<T>
65  {
67  public: OnePole() = default;
68 
72  public: OnePole(double _fc, double _fs)
73  {
74  this->Fc(_fc, _fs);
75  }
76 
77  // Documentation Inherited.
78  public: virtual void Fc(double _fc, double _fs)
79  {
80  b1 = exp(-2.0 * IGN_PI * _fc / _fs);
81  a0 = 1.0 - b1;
82  }
83 
87  public: 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 = 0;
95 
97  protected: double b1 = 0;
98  };
99 
102  class OnePoleQuaternion : public OnePole<math::Quaterniond>
103  {
106  {
107  this->Set(math::Quaterniond(1, 0, 0, 0));
108  }
109 
113  public: OnePoleQuaternion(double _fc, double _fs)
114  : OnePole<math::Quaterniond>(_fc, _fs)
115  {
116  this->Set(math::Quaterniond(1, 0, 0, 0));
117  }
118 
122  public: const math::Quaterniond& Process(
123  const math::Quaterniond &_x)
124  {
126  return y0;
127  }
128  };
129 
132  class OnePoleVector3 : public OnePole<math::Vector3d>
133  {
135  public: OnePoleVector3()
136  {
137  this->Set(math::Vector3d(0, 0, 0));
138  }
139 
143  public: OnePoleVector3(double _fc, double _fs)
144  : OnePole<math::Vector3d>(_fc, _fs)
145  {
146  this->Set(math::Vector3d(0, 0, 0));
147  }
148  };
149 
153  template <class T>
154  class BiQuad : public Filter<T>
155  {
157  public: BiQuad() = default;
158 
162  public: BiQuad(double _fc, double _fs)
163  {
164  this->Fc(_fc, _fs);
165  }
166 
167  // Documentation Inherited.
168  public: void Fc(double _fc, double _fs)
169  {
170  this->Fc(_fc, _fs, 0.5);
171  }
172 
177  public: void Fc(double _fc, double _fs, double _q)
178  {
179  double k = tan(IGN_PI * _fc / _fs);
180  double kQuadDenom = k * k + k / _q + 1.0;
181  this->a0 = k * k/ kQuadDenom;
182  this->a1 = 2 * this->a0;
183  this->a2 = this->a0;
184  this->b0 = 1.0;
185  this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
186  this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
187  }
188 
191  public: virtual void Set(const T &_val)
192  {
193  this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
194  }
195 
199  public: virtual const T& Process(const T &_x)
200  {
201  this->y0 = this->a0 * _x +
202  this->a1 * this->x1 +
203  this->a2 * this->x2 -
204  this->b1 * this->y1 -
205  this->b2 * this->y2;
206 
207  this->x2 = this->x1;
208  this->x1 = _x;
209  this->y2 = this->y1;
210  this->y1 = this->y0;
211  return this->y0;
212  }
213 
215  protected: double a0 = 0,
216  a1 = 0,
217  a2 = 0,
218  b0 = 0,
219  b1 = 0,
220  b2 = 0;
221 
223  protected: T x1{}, x2{}, y1{}, y2{};
224  };
225 
228  class BiQuadVector3 : public BiQuad<math::Vector3d>
229  {
231  public: BiQuadVector3()
232  {
233  this->Set(math::Vector3d(0, 0, 0));
234  }
235 
239  public: BiQuadVector3(double _fc, double _fs)
240  : BiQuad<math::Vector3d>(_fc, _fs)
241  {
242  this->Set(math::Vector3d(0, 0, 0));
243  }
244  };
245  }
246 }
247 
248 #endif
double b2
Definition: Filter.hh:220
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:168
T x1
Gain of the feedback coefficients.
Definition: Filter.hh:223
Bi-quad filter base class.
Definition: Filter.hh:154
virtual void Set(const T &_val)
Set the current filter's output.
Definition: Filter.hh:191
BiQuad()=default
Constructor.
double b1
Definition: Filter.hh:219
T y1
Definition: Filter.hh:223
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:162
BiQuad vector3 filter.
Definition: Filter.hh:228
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:78
T y0
Output.
Definition: Filter.hh:57
const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:87
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:105
double a0
Input gain control coefficients.
Definition: Filter.hh:215
virtual const T & Value() const
Get the output of the filter.
Definition: Filter.hh:51
T x2
Definition: Filter.hh:223
virtual ~Filter()
Destructor.
Definition: Filter.hh:35
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:688
double a2
Definition: Filter.hh:217
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:177
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:239
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:143
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:72
double a1
Definition: Filter.hh:216
double b0
Definition: Filter.hh:218
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:113
OnePole()=default
Constructor.
OnePoleVector3()
Constructor.
Definition: Filter.hh:135
T y2
Definition: Filter.hh:223
double b1
Gain of the feedback.
Definition: Filter.hh:97
One-pole quaternion filter.
Definition: Filter.hh:102
virtual void Set(const T &_val)
Set the output of the filter.
Definition: Filter.hh:39
Filter base class.
Definition: Filter.hh:32
A one-pole DSP filter.
Definition: Filter.hh:64
A quaternion class.
Definition: Quaternion.hh:31
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:59
BiQuadVector3()
Constructor.
Definition: Filter.hh:231
One-pole vector3 filter.
Definition: Filter.hh:132
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition: Filter.hh:122
virtual const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:199
double a0
Input gain control.
Definition: Filter.hh:94