Gazebo Math

API Reference

7.4.0
gz/math/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
26{
27 namespace math
28 {
29 // Inline bracket to help doxygen filtering.
30 inline namespace GZ_MATH_VERSION_NAMESPACE {
31 //
34 template <class T>
35 class Filter
36 {
38 public: virtual ~Filter() {}
39
42 public: virtual void Set(const T &_val)
43 {
44 y0 = _val;
45 }
46
50 public: virtual void Fc(double _fc, double _fs) = 0;
51
54 public: virtual const T &Value() const
55 {
56 return y0;
57 }
58
60 protected: T y0{};
61 };
62
66 template <class T>
67 class OnePole : public Filter<T>
68 {
70 public: OnePole() = default;
71
75 public: OnePole(double _fc, double _fs)
76 {
77 this->Fc(_fc, _fs);
78 }
79
80 // Documentation Inherited.
81 public: virtual void Fc(double _fc, double _fs) override
82 {
83 b1 = exp(-2.0 * GZ_PI * _fc / _fs);
84 a0 = 1.0 - b1;
85 }
86
90 public: const T& Process(const T &_x)
91 {
92 this->y0 = a0 * _x + b1 * this->y0;
93 return this->y0;
94 }
95
97 protected: double a0 = 0;
98
100 protected: double b1 = 0;
101 };
102
105 class OnePoleQuaternion : public OnePole<math::Quaterniond>
106 {
109 {
110 this->Set(math::Quaterniond(1, 0, 0, 0));
111 }
112
116 public: OnePoleQuaternion(double _fc, double _fs)
117 : OnePole<math::Quaterniond>(_fc, _fs)
118 {
119 this->Set(math::Quaterniond(1, 0, 0, 0));
120 }
121
126 const math::Quaterniond &_x)
127 {
128 y0 = math::Quaterniond::Slerp(a0, y0, _x);
129 return y0;
130 }
131 };
132
135 class OnePoleVector3 : public OnePole<math::Vector3d>
136 {
139 {
140 this->Set(math::Vector3d(0, 0, 0));
141 }
142
146 public: OnePoleVector3(double _fc, double _fs)
147 : OnePole<math::Vector3d>(_fc, _fs)
148 {
149 this->Set(math::Vector3d(0, 0, 0));
150 }
151 };
152
156 template <class T>
157 class BiQuad : public Filter<T>
158 {
160 public: BiQuad() = default;
161
165 public: BiQuad(double _fc, double _fs)
166 {
167 this->Fc(_fc, _fs);
168 }
169
170 // Documentation Inherited.
171 public: void Fc(double _fc, double _fs) override
172 {
173 this->Fc(_fc, _fs, 0.5);
174 }
175
180 public: void Fc(double _fc, double _fs, double _q)
181 {
182 double k = tan(GZ_PI * _fc / _fs);
183 double kQuadDenom = k * k + k / _q + 1.0;
184 this->a0 = k * k/ kQuadDenom;
185 this->a1 = 2 * this->a0;
186 this->a2 = this->a0;
187 this->b0 = 1.0;
188 this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
189 this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
190 }
191
194 public: virtual void Set(const T &_val) override
195 {
196 this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
197 }
198
202 public: virtual const T& Process(const T &_x)
203 {
204 this->y0 = this->a0 * _x +
205 this->a1 * this->x1 +
206 this->a2 * this->x2 -
207 this->b1 * this->y1 -
208 this->b2 * this->y2;
209
210 this->x2 = this->x1;
211 this->x1 = _x;
212 this->y2 = this->y1;
213 this->y1 = this->y0;
214 return this->y0;
215 }
216
218 protected: double a0 = 0,
219 a1 = 0,
220 a2 = 0,
221 b0 = 0,
222 b1 = 0,
223 b2 = 0;
224
226 protected: T x1{}, x2{}, y1{}, y2{};
227 };
228
231 class BiQuadVector3 : public BiQuad<math::Vector3d>
232 {
235 {
236 this->Set(math::Vector3d(0, 0, 0));
237 }
238
242 public: BiQuadVector3(double _fc, double _fs)
243 : BiQuad<math::Vector3d>(_fc, _fs)
244 {
245 this->Set(math::Vector3d(0, 0, 0));
246 }
247 };
248 }
249 }
250}
251
252#endif