18 #ifndef IGNITION_MATH_FILTER_HH_ 19 #define IGNITION_MATH_FILTER_HH_ 39 public:
virtual void Set(
const T &_val)
47 public:
virtual void Fc(
double _fc,
double _fs) = 0;
51 public:
virtual const T &
Value()
const 78 public:
virtual void Fc(
double _fc,
double _fs)
80 b1 = exp(-2.0 *
IGN_PI * _fc / _fs);
89 this->
y0 = a0 * _x + b1 * this->
y0;
94 protected:
double a0 = 0;
97 protected:
double b1 = 0;
157 public:
BiQuad() =
default;
168 public:
void Fc(
double _fc,
double _fs)
170 this->
Fc(_fc, _fs, 0.5);
177 public:
void Fc(
double _fc,
double _fs,
double _q)
179 double k = tan(
IGN_PI * _fc / _fs);
180 double kQuadDenom = k * k + k / _q + 1.0;
181 this->a0 = k * k/ kQuadDenom;
182 this->a1 = 2 * this->a0;
185 this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
186 this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
191 public:
virtual void Set(
const T &_val)
193 this->
y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
201 this->
y0 = this->a0 * _x +
202 this->a1 * this->x1 +
203 this->a2 * this->x2 -
204 this->b1 * this->y1 -
215 protected:
double a0 = 0,
223 protected: T x1{}, x2{}, y1{}, y2{};
void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:168
Bi-quad filter base class.
Definition: Filter.hh:154
virtual void Set(const T &_val)
Set the current filter's output.
Definition: Filter.hh:191
BiQuad(double _fc, double _fs)
Constructor.
Definition: Filter.hh:162
BiQuad vector3 filter.
Definition: Filter.hh:228
virtual void Fc(double _fc, double _fs)
Set the cutoff frequency and sample rate.
Definition: Filter.hh:78
T y0
Output.
Definition: Filter.hh:57
const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:87
OnePoleQuaternion()
Constructor.
Definition: Filter.hh:105
virtual const T & Value() const
Get the output of the filter.
Definition: Filter.hh:51
virtual ~Filter()
Destructor.
Definition: Filter.hh:35
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:832
void Fc(double _fc, double _fs, double _q)
Set the cutoff frequency, sample rate and Q coefficient.
Definition: Filter.hh:177
BiQuadVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:239
OnePoleVector3(double _fc, double _fs)
Constructor.
Definition: Filter.hh:143
OnePole(double _fc, double _fs)
Constructor.
Definition: Filter.hh:72
OnePoleQuaternion(double _fc, double _fs)
Constructor.
Definition: Filter.hh:113
OnePoleVector3()
Constructor.
Definition: Filter.hh:135
One-pole quaternion filter.
Definition: Filter.hh:102
virtual void Set(const T &_val)
Set the output of the filter.
Definition: Filter.hh:39
Filter base class.
Definition: Filter.hh:32
Definition: AffineException.hh:30
A one-pole DSP filter.
Definition: Filter.hh:64
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:139
BiQuadVector3()
Constructor.
Definition: Filter.hh:231
One-pole vector3 filter.
Definition: Filter.hh:132
const math::Quaterniond & Process(const math::Quaterniond &_x)
Update the filter's output.
Definition: Filter.hh:122
virtual const T & Process(const T &_x)
Update the filter's output.
Definition: Filter.hh:199