17#ifndef IGNITION_MATH_FILTER_HH_
18#define IGNITION_MATH_FILTER_HH_
23#include <ignition/math/config.hh>
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);
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,
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition Helpers.hh:174
BiQuad vector3 filter.
Definition Filter.hh:231
BiQuadVector3()
Constructor.
Definition Filter.hh:233
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition Filter.hh:241
Bi-quad filter base class.
Definition Filter.hh:157
T y2
Definition Filter.hh:225
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition Filter.hh:179
BiQuad()=default
Constructor.
double a1
Definition Filter.hh:218
T x1
Gain of the feedback coefficients.
Definition Filter.hh:225
double a2
Definition Filter.hh:219
BiQuad(double _fc, double _fs)
Constructor.
Definition Filter.hh:164
double a0
Input gain control coefficients.
Definition Filter.hh:217
T y1
Definition Filter.hh:225
double b0
Definition Filter.hh:220
virtual void Set(const T &_val)
Set the current filter's output.
Definition Filter.hh:193
double b2
Definition Filter.hh:222
T x2
Definition Filter.hh:225
double b1
Definition Filter.hh:221
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition Filter.hh:170
virtual const T & Process(const T &_x)
Update the filter's output.
Definition Filter.hh:201
Filter base class.
Definition Filter.hh:35
virtual void Set(const T &_val)
Set the output of the filter.
Definition Filter.hh:41
virtual const T & Value() const
Get the output of the filter.
Definition Filter.hh:53
virtual ~Filter()
Destructor.
Definition Filter.hh:37
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
T y0
Output.
Definition Filter.hh:59
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition Filter.hh:124
OnePoleQuaternion()
Constructor.
Definition Filter.hh:107
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition Filter.hh:115
OnePoleVector3()
Constructor.
Definition Filter.hh:137
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition Filter.hh:145
OnePole()=default
Constructor.
OnePole(double _fc, double _fs)
Constructor.
Definition Filter.hh:74
double a0
Input gain control.
Definition Filter.hh:96
const T & Process(const T &_x)
Update the filter's output.
Definition Filter.hh:89
double b1
Gain of the feedback.
Definition Filter.hh:99
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition Filter.hh:80
Vector3< double > Vector3d
Definition Vector3.hh:756
Quaternion< double > Quaterniond
Definition Quaternion.hh:1081
Math classes and function useful in robot applications.
Definition Angle.hh:42