Libbarrett
1.2.4
|
00001 /* 00002 * first_order_filter-inl.h 00003 * 00004 * Created on: Apr 1, 2010 00005 * Author: dc 00006 */ 00007 00008 00009 #include <string> 00010 00011 00012 namespace barrett { 00013 namespace math { 00014 00015 00016 template<typename T, typename MathTraits> 00017 FirstOrderFilter<T, MathTraits>::FirstOrderFilter(double timeStep) : 00018 a(MT::zero()), b(MT::zero()), c(MT::zero()), T_s(timeStep), 00019 c1(MT::zero()), c2(MT::zero()), c3(MT::zero()), 00020 y_0(MT::zero()), y_1(MT::zero()), x_0(MT::zero()), x_1(MT::zero()) 00021 { 00022 } 00023 00024 template<typename T, typename MathTraits> 00025 FirstOrderFilter<T, MathTraits>::FirstOrderFilter(const libconfig::Setting& setting) : 00026 a(MT::zero()), b(MT::zero()), c(MT::zero()), T_s(0.0), 00027 c1(MT::zero()), c2(MT::zero()), c3(MT::zero()), 00028 y_0(MT::zero()), y_1(MT::zero()), x_0(MT::zero()), x_1(MT::zero()) 00029 { 00030 setFromConfig(setting); 00031 } 00032 00033 template<typename T, typename MathTraits> 00034 inline void FirstOrderFilter<T, MathTraits>::setSamplePeriod(double timeStep) 00035 { 00036 T_s = timeStep; 00037 updateCoefficients(); 00038 } 00039 00040 template<typename T, typename MathTraits> 00041 void FirstOrderFilter<T, MathTraits>::setFromConfig(const libconfig::Setting& setting) 00042 { 00043 // TODO(dc): test! 00044 T omega_p(MT::zero()); 00045 if (setting.exists("omega_p")) { 00046 omega_p = setting["omega_p"]; 00047 } 00048 T omega_z(MT::zero()); 00049 if (setting.exists("omega_z")) { 00050 omega_z = setting["omega_z"]; 00051 } 00052 T a(MT::zero()); 00053 if (setting.exists("a")) { 00054 a = setting["a"]; 00055 } 00056 T b(MT::zero()); 00057 if (setting.exists("b")) { 00058 b = setting["b"]; 00059 } 00060 T c(MT::zero()); 00061 if (setting.exists("c")) { 00062 c = setting["c"]; 00063 } 00064 00065 if (setting.exists("type")) { 00066 std::string type = setting["type"]; 00067 switch (type[0]) { 00068 case 'l': // low_pass 00069 if (setting.exists("dc_gain")) { 00070 setLowPass(omega_p, T(setting["dc_gain"])); 00071 } else { 00072 setLowPass(omega_p); 00073 } 00074 break; 00075 case 'h': // high_pass 00076 if (setting.exists("hf_gain")) { 00077 setHighPass(omega_p, T(setting["hf_gain"])); 00078 } else { 00079 setHighPass(omega_p); 00080 } 00081 break; 00082 case 'z': // zpk 00083 if (setting.exists("dc_gain")) { 00084 setZPK(omega_z, omega_p, T(setting["dc_gain"])); 00085 } else { 00086 setZPK(omega_z, omega_p); 00087 } 00088 break; 00089 case 'i': // integrator 00090 if (setting.exists("gain")) { 00091 setIntegrator(T(setting["gain"])); 00092 } else { 00093 setIntegrator(); 00094 } 00095 break; 00096 case 'p': // parameters 00097 setParameters(a, b, c); 00098 break; 00099 } 00100 } 00101 } 00102 00103 template<typename T, typename MathTraits> 00104 void FirstOrderFilter<T, MathTraits>::setLowPass(const T& omega_p, const T& dcGain) 00105 { 00106 a = MT::zero(); 00107 b = MT::mult(dcGain, omega_p); 00108 c = omega_p; 00109 00110 updateCoefficients(); 00111 } 00112 00113 template<typename T, typename MathTraits> 00114 void FirstOrderFilter<T, MathTraits>::setHighPass(const T& omega_p, const T& hfGain) 00115 { 00116 a = hfGain; 00117 b = MT::zero(); 00118 c = omega_p; 00119 00120 updateCoefficients(); 00121 } 00122 00123 template<typename T, typename MathTraits> 00124 void FirstOrderFilter<T, MathTraits>::setZPK(const T& omega_z, const T& omega_p, const T& dcGain) 00125 { 00126 a = MT::div(MT::mult(dcGain, omega_p), omega_z); 00127 b = MT::mult(dcGain, omega_p); 00128 c = omega_p; 00129 00130 updateCoefficients(); 00131 } 00132 00133 template<typename T, typename MathTraits> 00134 void FirstOrderFilter<T, MathTraits>::setIntegrator(const T& gain) 00135 { 00136 a = MT::zero(); 00137 b = gain; 00138 c = MT::zero(); 00139 00140 updateCoefficients(); 00141 } 00142 00143 template<typename T, typename MathTraits> 00144 void FirstOrderFilter<T, MathTraits>::setParameters(const T& a_, const T& b_, const T& c_) 00145 { 00146 a = a_; 00147 b = b_; 00148 c = c_; 00149 00150 updateCoefficients(); 00151 } 00152 00153 00154 // filter parameters: 00155 // a*s + b 00156 // H(s) = --------- 00157 // s + c 00158 // 00159 // difference equation: 00160 // 1 a + b*T_s a 00161 // y[n] = ----------- * y[n-1] + ----------- * x[n] - ----------- * x[n-1] 00162 // 1 + c*T_s 1 + c*T_s 1 + c*T_s 00163 template<typename T, typename MathTraits> 00164 inline void FirstOrderFilter<T, MathTraits>::updateCoefficients() 00165 { 00166 T den = MT::add(1.0, MT::mult(c,T_s)); 00167 00168 c1 = MT::div(1.0, den); 00169 c2 = MT::div(MT::add(a, MT::mult(b, T_s)), den); 00170 c3 = MT::div(a, den); 00171 } 00172 00173 template<typename T, typename MathTraits> 00174 inline const T& FirstOrderFilter<T, MathTraits>::eval(const T& x_0) 00175 { 00176 // y_0 = c1*y_1 + c2*x_0 - c3*x_1; 00177 y_0 = MT::sub(MT::add(MT::mult(c1,y_1), MT::mult(c2,x_0)), MT::mult(c3,x_1)); 00178 00179 y_1 = y_0; 00180 x_1 = x_0; 00181 00182 return y_0; 00183 } 00184 00185 00186 } 00187 }