17 #ifndef IGNITION_MATH_FILTER_HH_ 18 #define IGNITION_MATH_FILTER_HH_ 38 public:
virtual void Set(
const T &_val)
46 public:
virtual void Fc(
double _fc,
double _fs) = 0;
50 public:
virtual const T &
Value()
const 77 public:
virtual void Fc(
double _fc,
double _fs)
79 b1 = exp(-2.0 *
IGN_PI * _fc / _fs);
88 this->
y0 = a0 * _x + b1 * this->
y0;
93 protected:
double a0 = 0;
96 protected:
double b1 = 0;
156 public:
BiQuad() =
default;
167 public:
void Fc(
double _fc,
double _fs)
169 this->
Fc(_fc, _fs, 0.5);
176 public:
void Fc(
double _fc,
double _fs,
double _q)
178 double k = tan(
IGN_PI * _fc / _fs);
179 double kQuadDenom = k * k + k / _q + 1.0;
180 this->a0 = k * k/ kQuadDenom;
181 this->a1 = 2 * this->a0;
184 this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
185 this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
190 public:
virtual void Set(
const T &_val)
192 this->
y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
200 this->
y0 = this->a0 * _x +
201 this->a1 * this->x1 +
202 this->a2 * this->x2 -
203 this->b1 * this->y1 -
214 protected:
double a0 = 0,
222 protected: T x1{}, x2{}, y1{}, y2{};
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:167
Bi-quad filter base class.
Definition: Filter.hh:153
virtual void Set(const T &_val)
Set the current filter's output.
Definition: Filter.hh:190
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:161
BiQuad vector3 filter.
Definition: Filter.hh:227
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:77
virtual const T & Value() const
Get the output of the filter.
Definition: Filter.hh:50
T y0
Output.
Definition: Filter.hh:56
const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:86
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:104
virtual ~Filter()
Destructor.
Definition: Filter.hh:34
static Quaternion< double > Slerp(double _fT, const Quaternion< double > &_rkP, const Quaternion< double > &_rkQ, bool _shortestPath=false)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
Definition: Quaternion.hh:855
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:176
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:238
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:142
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:71
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:112
OnePoleVector3()
Constructor.
Definition: Filter.hh:134
One-pole quaternion filter.
Definition: Filter.hh:101
virtual void Set(const T &_val)
Set the output of the filter.
Definition: Filter.hh:38
Filter base class.
Definition: Filter.hh:31
A one-pole DSP filter.
Definition: Filter.hh:63
virtual void Fc(double _fc, double _fs)=0
Set the cutoff frequency and sample rate.
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:173
BiQuadVector3()
Constructor.
Definition: Filter.hh:230
One-pole vector3 filter.
Definition: Filter.hh:131
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition: Filter.hh:121
virtual const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:198