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
00014
00015 protected:
00016 T y0;
00017 };
00018
00019 template <class T>
00020 class OnePole : public Filter<T> {
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 {
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
00092
00093
00094
00095
00096 }
00097
00098 protected:
00099 double a0, a1, a2, b0, b1, b2;
00100 T x1, x2, y1, y2;
00101
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