Loading...
Searching...
No Matches
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
23#include <ignition/math/config.hh>
24
25namespace 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
125 const math::Quaterniond &_x)
126 {
128 return y0;
129 }
130 };
131
134 class OnePoleVector3 : public OnePole<math::Vector3d>
135 {
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 {
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
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition Helpers.hh:174
BiQuad vector3 filter.
Definition Filter.hh:231
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition Filter.hh:241
Bi-quad filter base class.
Definition Filter.hh:157
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition Filter.hh:179
T x1
Gain of the feedback coefficients.
Definition Filter.hh:225
BiQuad(double _fc, double _fs)
Constructor.
Definition Filter.hh:164
double a0
Input gain control coefficients.
Definition Filter.hh:217
virtual void Set(const T &_val)
Set the current filter's output.
Definition Filter.hh:193
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition Filter.hh:170
virtual const T & Process(const T &_x)
Update the filter's output.
Definition Filter.hh:201
Filter base class.
Definition Filter.hh:35
virtual void Set(const T &_val)
Set the output of the filter.
Definition Filter.hh:41
virtual const T & Value() const
Get the output of the filter.
Definition Filter.hh:53
virtual ~Filter()
Destructor.
Definition Filter.hh:37
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
One-pole quaternion filter.
Definition Filter.hh:105
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition Filter.hh:124
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition Filter.hh:115
One-pole vector3 filter.
Definition Filter.hh:135
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition Filter.hh:145
A one-pole DSP filter.
Definition Filter.hh:67
OnePole(double _fc, double _fs)
Constructor.
Definition Filter.hh:74
double a0
Input gain control.
Definition Filter.hh:96
const T & Process(const T &_x)
Update the filter's output.
Definition Filter.hh:89
double b1
Gain of the feedback.
Definition Filter.hh:99
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition Filter.hh:80
A quaternion class.
Definition Quaternion.hh:38
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
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
Definition Angle.hh:40