filters.h
Go to the documentation of this file.
00001 #ifndef RAZER_HYDRA_FILTERS_H
00002 #define RAZER_HYDRA_FILTERS_H
00003 
00004 #include <tf/tf.h>
00005 #include <ros/ros.h>
00006 
00007 template <class T>
00008 class Filter {
00009 public:
00010   virtual void setValue(const T& val) { y0 = val; }
00011   virtual void setFc(double Fc, double Fs) = 0;
00012   inline virtual const T& getValue() { return y0; }
00013   //inline const T& process(const T& x) = 0;
00014 
00015 protected:
00016   T y0;
00017 };
00018 
00019 template <class T>
00020 class OnePole : public Filter<T> { // http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/
00021 public:
00022   OnePole() : a0(0), b1(0) { }
00023 
00024   OnePole(double Fc, double Fs) : a0(0), b1(0) { setFc(Fc, Fs); }
00025 
00026   virtual void setFc(double Fc, double Fs)
00027   {
00028     b1 = exp(-2.0 * M_PI * Fc / Fs);
00029     a0 = 1.0 - b1;
00030   }
00031 
00032   inline const T& process(const T& x) {
00033     this->y0 = a0*x + b1*this->y0;
00034     return this->y0;
00035   }
00036 
00037 protected:
00038     double a0, b1;
00039 };
00040 
00041 class OnePoleQuaternion : public OnePole<tf::Quaternion> {
00042 public:
00043   OnePoleQuaternion() { setValue(tf::Quaternion(0,0,0,1)); }
00044   OnePoleQuaternion(double Fc, double Fs) : OnePole<tf::Quaternion>(Fc, Fs) { setValue(tf::Quaternion(0,0,0,1)); }
00045 
00046   inline const tf::Quaternion& process(const tf::Quaternion& x) {
00047     y0 = y0.slerp(x, a0);
00048     return y0;
00049   }
00050 };
00051 
00052 class OnePoleVector3 : public OnePole<tf::Vector3> {
00053 public:
00054   OnePoleVector3() { setValue(tf::Vector3(0,0,0)); }
00055   OnePoleVector3(double Fc, double Fs) : OnePole<tf::Vector3>(Fc, Fs) { setValue(tf::Vector3(0,0,0)); }
00056 
00057 };
00058 
00059 template <class T>
00060 class BiQuad : public Filter<T>
00061 { // http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/
00062 public:
00063   BiQuad()
00064     : a0(0), a1(0), a2(0), b0(0), b1(0), b2(0) { }
00065   BiQuad(double Fc, double Fs)
00066     : a0(0), a1(0), a2(0), b0(0), b1(0), b2(0) { setFc(Fc, Fs); }
00067 
00068   inline void setFc(double Fc, double Fs) { setFc( Fc, Fs, 0.5); }
00069 
00070   inline void setFc(double Fc, double Fs, double Q) {
00071     double K = tan(M_PI*Fc/Fs);
00072     double k_quad_denom = K*K + K/Q + 1.0;
00073     a0 = K*K/k_quad_denom;
00074     a1 = 2*a0;
00075     a2 = a0;
00076     b0 = 1.0;
00077     b1 = 2*(K*K - 1.0)/k_quad_denom;
00078     b2 = (K*K - K/Q + 1.0)/k_quad_denom;
00079   }
00080 
00081   virtual void setValue(const T& val) { this->y0 = y1 = y2 = x1 = x2 = val; }
00082 
00083   inline virtual const T& process(const T& x) {
00084     this->y0 = a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2;
00085     x2 = x1;
00086     x1 = x;
00087     y2 = y1;
00088     y1 = this->y0;
00089     return this->y0;
00090 
00091 //    w0 = x - b1*w1 - b2*w2;
00092 //    this->y0 = a0*w0 + a1*w1 + a2*w2;
00093 //    w2 = w1;
00094 //    w1 = w0;
00095 //    return this->y0;
00096   }
00097 
00098 protected:
00099     double a0, a1, a2, b0, b1, b2;
00100     T x1, x2, y1, y2;
00101     //T w0, w1, w2;
00102 };
00103 
00104 class BiQuadVector3 : public BiQuad<tf::Vector3> {
00105 public:
00106   BiQuadVector3() { setValue(tf::Vector3(0,0,0)); }
00107   BiQuadVector3(double Fc, double Fs) : BiQuad<tf::Vector3>(Fc, Fs) { setValue(tf::Vector3(0,0,0)); }
00108 };
00109 
00110 
00111 #endif // RAZER_HYDRA_FILTERS_H


razer_hydra
Author(s): Adam Leeper
autogenerated on Mon Oct 6 2014 04:01:06