Gazebo Math
API Reference
8.1.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
src
gz-math
include
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::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
{
106
public
:
OnePoleQuaternion
()
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
123
public
:
const
math::Quaterniond
&
Process
(
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
{
136
public
:
OnePoleVector3
()
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
{
232
public
:
BiQuadVector3
()
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_