Gazebo Math
API Reference
7.4.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
include
gz
math
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
25
namespace
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
{
108
public
:
OnePoleQuaternion
()
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
125
public
:
const
math::Quaterniond
&
Process
(
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
{
138
public
:
OnePoleVector3
()
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
{
234
public
:
BiQuadVector3
()
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