Gazebo Math

API Reference

8.1.0
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 GZ_MATH_FILTER_HH_
18#define GZ_MATH_FILTER_HH_
19
20#include <gz/math/Helpers.hh>
21#include <gz/math/Vector3.hh>
22#include <gz/math/Quaternion.hh>
23#include <gz/math/config.hh>
24
25namespace gz::math
26{
27 // Inline bracket to help doxygen filtering.
28 inline namespace GZ_MATH_VERSION_NAMESPACE {
29 //
32 template <class T>
33 class Filter
34 {
36 public: virtual ~Filter() {}
37
40 public: virtual void Set(const T &_val)
41 {
42 y0 = _val;
43 }
44
48 public: virtual void Fc(double _fc, double _fs) = 0;
49
52 public: virtual const T &Value() const
53 {
54 return y0;
55 }
56
58 protected: T y0{};
59 };
60
64 template <class T>
65 class OnePole : public Filter<T>
66 {
68 public: OnePole() = default;
69
73 public: OnePole(double _fc, double _fs)
74 {
75 this->Fc(_fc, _fs);
76 }
77
78 // Documentation Inherited.
79 public: virtual void Fc(double _fc, double _fs) override
80 {
81 b1 = exp(-2.0 * GZ_PI * _fc / _fs);
82 a0 = 1.0 - b1;
83 }
84
88 public: const T& Process(const T &_x)
89 {
90 this->y0 = a0 * _x + b1 * this->y0;
91 return this->y0;
92 }
93
95 protected: double a0 = 0;
96
98 protected: double b1 = 0;
99 };
100
103 class OnePoleQuaternion : public OnePole<math::Quaterniond>
104 {
107 {
108 this->Set(math::Quaterniond(1, 0, 0, 0));
109 }
110
114 public: OnePoleQuaternion(double _fc, double _fs)
115 : OnePole<math::Quaterniond>(_fc, _fs)
116 {
117 this->Set(math::Quaterniond(1, 0, 0, 0));
118 }
119
124 const math::Quaterniond &_x)
125 {
126 y0 = math::Quaterniond::Slerp(a0, y0, _x);
127 return y0;
128 }
129 };
130
133 class OnePoleVector3 : public OnePole<math::Vector3d>
134 {
137 {
138 this->Set(math::Vector3d(0, 0, 0));
139 }
140
144 public: OnePoleVector3(double _fc, double _fs)
145 : OnePole<math::Vector3d>(_fc, _fs)
146 {
147 this->Set(math::Vector3d(0, 0, 0));
148 }
149 };
150
154 template <class T>
155 class BiQuad : public Filter<T>
156 {
158 public: BiQuad() = default;
159
163 public: BiQuad(double _fc, double _fs)
164 {
165 this->Fc(_fc, _fs);
166 }
167
168 // Documentation Inherited.
169 public: void Fc(double _fc, double _fs) override
170 {
171 this->Fc(_fc, _fs, 0.5);
172 }
173
178 public: void Fc(double _fc, double _fs, double _q)
179 {
180 double k = tan(GZ_PI * _fc / _fs);
181 double kQuadDenom = k * k + k / _q + 1.0;
182 this->a0 = k * k/ kQuadDenom;
183 this->a1 = 2 * this->a0;
184 this->a2 = this->a0;
185 this->b0 = 1.0;
186 this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
187 this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
188 }
189
192 public: virtual void Set(const T &_val) override
193 {
194 this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
195 }
196
200 public: virtual const T& Process(const T &_x)
201 {
202 this->y0 = this->a0 * _x +
203 this->a1 * this->x1 +
204 this->a2 * this->x2 -
205 this->b1 * this->y1 -
206 this->b2 * this->y2;
207
208 this->x2 = this->x1;
209 this->x1 = _x;
210 this->y2 = this->y1;
211 this->y1 = this->y0;
212 return this->y0;
213 }
214
216 protected: double a0 = 0,
217 a1 = 0,
218 a2 = 0,
219 b0 = 0,
220 b1 = 0,
221 b2 = 0;
222
224 protected: T x1{}, x2{}, y1{}, y2{};
225 };
226
229 class BiQuadVector3 : public BiQuad<math::Vector3d>
230 {
233 {
234 this->Set(math::Vector3d(0, 0, 0));
235 }
236
240 public: BiQuadVector3(double _fc, double _fs)
241 : BiQuad<math::Vector3d>(_fc, _fs)
242 {
243 this->Set(math::Vector3d(0, 0, 0));
244 }
245 };
246 } // namespace GZ_MATH_VERSION_NAMESPACE
247} // namespace gz::math
248#endif // GZ_MATH_FILTER_HH_