17 #ifndef IGNITION_MATH_FILTER_HH_ 18 #define IGNITION_MATH_FILTER_HH_ 23 #include <ignition/math/config.hh> 29 inline namespace IGNITION_MATH_VERSION_NAMESPACE
41 public:
virtual void Set(
const T &_val)
49 public:
virtual void Fc(
double _fc,
double _fs) = 0;
53 public:
virtual const T &
Value()
const 80 public:
virtual void Fc(
double _fc,
double _fs)
96 protected:
double a0 = 0;
99 protected:
double b1 = 0;
127 y0 = math::Quaterniond::Slerp(
a0,
y0, _x);
159 public:
BiQuad() =
default;
170 public:
void Fc(
double _fc,
double _fs)
172 this->
Fc(_fc, _fs, 0.5);
179 public:
void Fc(
double _fc,
double _fs,
double _q)
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;
187 this->
b1 = 2 * (k * k - 1.0) / kQuadDenom;
188 this->
b2 = (k * k - k / _q + 1.0) / kQuadDenom;
193 public:
virtual void Set(
const T &_val)
195 this->
y0 = this->
y1 = this->
y2 = this->
x1 = this->
x2 = _val;
203 this->
y0 = this->
a0 * _x +
204 this->
a1 * this->
x1 +
205 this->
a2 * this->
x2 -
206 this->
b1 * this->
y1 -
217 protected:
double a0 = 0,
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:179
T y2
Definition: Filter.hh:225
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:145
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:107
A one-pole DSP filter.
Definition: Filter.hh:66
T y1
Definition: Filter.hh:225
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:74
Vector3< double > Vector3d
Definition: Vector3.hh:756
A quaternion class.
Definition: Matrix3.hh:33
double a0
Input gain control coefficients.
Definition: Filter.hh:217
double b2
Definition: Filter.hh:222
double a1
Definition: Filter.hh:218
OnePoleVector3()
Constructor.
Definition: Filter.hh:137
virtual void Set(const T &_val)
Set the output of the filter.
Definition: Filter.hh:41
double b1
Definition: Filter.hh:221
virtual void Set(const T &_val)
Set the current filter's output.
Definition: Filter.hh:193
Filter base class.
Definition: Filter.hh:34
double b1
Gain of the feedback.
Definition: Filter.hh:99
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:241
One-pole quaternion filter.
Definition: Filter.hh:104
double a0
Input gain control.
Definition: Filter.hh:96
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:170
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
T x1
Gain of the feedback coefficients.
Definition: Filter.hh:225
Bi-quad filter base class.
Definition: Filter.hh:156
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:80
BiQuad vector3 filter.
Definition: Filter.hh:230
Quaternion< double > Quaterniond
Definition: Quaternion.hh:1081
double b0
Definition: Filter.hh:220
virtual const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:201
virtual const T & Value() const
Get the output of the filter.
Definition: Filter.hh:53
virtual ~Filter()
Destructor.
Definition: Filter.hh:37
T y0
Output.
Definition: Filter.hh:59
double a2
Definition: Filter.hh:219
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:115
One-pole vector3 filter.
Definition: Filter.hh:134
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:164
const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:89
BiQuadVector3()
Constructor.
Definition: Filter.hh:233
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:174
T x2
Definition: Filter.hh:225
OnePole()=default
Constructor.
BiQuad()=default
Constructor.
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition: Filter.hh:124