A combination of some frequently used filters
Embed:
(wiki syntax)
Show/hide line numbers
FILTER_LIB.h
00001 // 00002 #ifndef PI 00003 #define PI 3.1415926 00004 #endif 00005 // 00006 #ifndef FILTER_LIB_H 00007 #define FILTER_LIB_H 00008 // 00009 #include "IIR.h" 00010 #include <math.h> 00011 #include <vector> 00012 00013 using std::vector; 00014 00015 //--------------------LPF---------------------// 00016 class LPF{ // Low-pass filter 00017 public: 00018 float output; 00019 00020 LPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" 00021 float filter(float input); 00022 void reset(float input); 00023 00024 private: 00025 float Ts; 00026 float cutOff_freq_Hz; // Hz 00027 float alpha_Ts; 00028 float One_alpha_Ts; 00029 00030 // Flag 00031 bool Flag_Init; 00032 }; 00033 00034 //--------------------LPF_vector---------------------// 00035 class LPF_vector{ // Vectorized low-pass filter 00036 public: 00037 vector<float> output; 00038 00039 LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" 00040 vector<float> filter(const vector<float> &input); 00041 void reset(const vector<float> &input); 00042 00043 private: 00044 size_t n; 00045 float Ts; 00046 float cutOff_freq_Hz; // Hz 00047 float alpha_Ts; 00048 float One_alpha_Ts; 00049 00050 // Flag 00051 bool Flag_Init; 00052 00053 // 00054 vector<float> zeros; // Zero vector [0;0;0] 00055 }; 00056 00057 //--------------------LPF_nthOrderCritical---------------------// 00058 class LPF_nthOrderCritical{ // nth-order critical-damped Low-pass filter (all the poles are at the same place) 00059 public: 00060 float output; 00061 00062 LPF_nthOrderCritical(float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz" 00063 float filter(float input); 00064 void reset(float input); 00065 00066 private: 00067 float Ts; 00068 size_t order; 00069 float cutOff_freq_Hz; // Hz 00070 00071 // Layers of 1st-order LPF 00072 vector<LPF> filter_layers; 00073 00074 // Flag 00075 bool Flag_Init; 00076 }; 00077 00078 //--------------------LPF_vector_nthOrderCritical---------------------// 00079 class LPF_vector_nthOrderCritical{ // Vectorized nth-order critical-damped Low-pass filter (all the poles are at the same place) 00080 public: 00081 vector<float> output; 00082 00083 LPF_vector_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz" 00084 vector<float> filter(const vector<float> &input); 00085 void reset(const vector<float> &input); 00086 00087 private: 00088 size_t n; 00089 float Ts; 00090 size_t order; 00091 00092 float cutOff_freq_Hz; // Hz 00093 00094 // Flag 00095 bool Flag_Init; 00096 00097 // Layers of vectorized 1st-order LPF 00098 vector<LPF_vector> filter_layers; 00099 00100 // 00101 vector<float> zeros; // Zero vector [0;0;0] 00102 }; 00103 00104 00105 //--------------------HPF---------------------// 00106 class HPF{ // High-pass filter 00107 public: 00108 float output; 00109 00110 HPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" 00111 float filter(float input); 00112 void reset(float input); 00113 00114 private: 00115 float Ts; 00116 float cutOff_freq_Hz; // Hz 00117 // float alpha_Ts; 00118 // float One_alpha_Ts; 00119 00120 // Flag 00121 bool Flag_Init; 00122 00123 // 00124 LPF lpf; 00125 }; 00126 00127 //--------------------HPF_vector---------------------// 00128 class HPF_vector{ // Vectorized high-pass filter 00129 public: 00130 vector<float> output; 00131 00132 HPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz" 00133 vector<float> filter(const vector<float> &input); 00134 void reset(const vector<float> &input); 00135 00136 private: 00137 size_t n; 00138 float Ts; 00139 float cutOff_freq_Hz; // Hz 00140 // float alpha_Ts; 00141 // float One_alpha_Ts; 00142 00143 // Flag 00144 bool Flag_Init; 00145 00146 // 00147 LPF_vector lpf_v; 00148 }; 00149 00150 //--------------------HPF_nthOrderCritical---------------------// 00151 class HPF_nthOrderCritical{ // nth-order critical-damped High-pass filter (all the poles are at the same place) 00152 public: 00153 float output; 00154 00155 HPF_nthOrderCritical(float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz" 00156 float filter(float input); 00157 void reset(float input); 00158 00159 private: 00160 float Ts; 00161 size_t order; 00162 float cutOff_freq_Hz; // Hz 00163 00164 // Layers of 1st-order HPF 00165 vector<HPF> filter_layers; 00166 00167 // Flag 00168 bool Flag_Init; 00169 }; 00170 00171 00172 //--------------------HPF_vector_nthOrderCritical---------------------// 00173 class HPF_vector_nthOrderCritical{ // Vectorized nth-order critical-damped High-pass filter (all the poles are at the same place) 00174 public: 00175 vector<float> output; 00176 00177 HPF_vector_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz" 00178 vector<float> filter(const vector<float> &input); 00179 void reset(const vector<float> &input); 00180 00181 private: 00182 size_t n; 00183 float Ts; 00184 size_t order; 00185 00186 float cutOff_freq_Hz; // Hz 00187 00188 // Flag 00189 bool Flag_Init; 00190 00191 // Layers of vectorized 1st-order LPF 00192 vector<HPF_vector> filter_layers; 00193 00194 // 00195 vector<float> zeros; // Zero vector [0;0;0] 00196 }; 00197 00198 //--------------------HPF_vector_1minusLPF_nthOrderCritical---------------------// 00199 class HPF_vector_1minusLPF_nthOrderCritical{ // Vectorized nth-order critical-damped High-pass filter ( the version of (1 - nth-order LPF), all the poles are at the same place) 00200 public: 00201 vector<float> output; 00202 00203 HPF_vector_1minusLPF_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in); // cutOff_freq_Hz_in is in "Hz" 00204 vector<float> filter(const vector<float> &input); 00205 void reset(const vector<float> &input); 00206 00207 private: 00208 size_t n; 00209 float Ts; 00210 size_t order; 00211 00212 float cutOff_freq_Hz; // Hz 00213 00214 // Flag 00215 bool Flag_Init; 00216 00217 // Layers of vectorized 1st-order LPF 00218 vector<LPF_vector> filter_layers; 00219 00220 // 00221 vector<float> zeros; // Zero vector [0;0;0] 00222 }; 00223 00224 00225 //--------------------Derivative_appr---------------------// 00226 class Derivative_appr{ // Approximated Derivative, cut-off at 10% of sampling frequency 00227 public: 00228 float output; 00229 00230 Derivative_appr(float samplingTime); 00231 float filter(float input); 00232 void reset(float input); 00233 00234 private: 00235 float Ts; 00236 float cutOff_freq_Hz; // Hz 00237 00238 // Flag 00239 bool Flag_Init; 00240 00241 // 00242 IIR derivative_LPF2; 00243 }; 00244 00245 //--------------------Rate-saturation Filter---------------------// 00246 class RateSaturation_Filter{ // Rate-saturation Filter 00247 public: 00248 float output; 00249 float error; 00250 00251 RateSaturation_Filter(float samplingTime, float limit_rate_in); // limit_rate is in the unit of "value/s" 00252 float filter(float input); 00253 void reset(float input); 00254 00255 private: 00256 float Ts; 00257 float limit_rate; 00258 float limit_increment; 00259 00260 // Flag 00261 bool Flag_Init; 00262 }; 00263 00264 //-----------First-Order Kalman Filter--------// 00265 class FirstOrder_KalmanFilter{ // 1st-order Kalman filter 00266 public: 00267 00268 // Parameters 00269 float A; 00270 float B; 00271 float C; 00272 // 00273 float R; 00274 float Q; 00275 00276 // States 00277 float mu_est; 00278 float Sigma_est; 00279 // Kalman gain 00280 float K; 00281 00282 FirstOrder_KalmanFilter(float samplingTime, float A_in, float B_in, float C_in, float R_in, float Q_in, bool is_continuousTime); // If is_continuousTime -> continuous time system 00283 float filter(float u, float z); 00284 void reset(float z); 00285 00286 private: 00287 float Ts; 00288 00289 00290 // Flag 00291 bool Flag_Init; 00292 }; 00293 00294 //-----------------Saturation---------------// 00295 class Saturation{ // Saturation 00296 public: 00297 00298 // States 00299 float output; 00300 00301 Saturation(float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system 00302 float filter(float input); 00303 void reset(float input); 00304 00305 private: 00306 float Ts; 00307 00308 // 00309 float bound_up; 00310 float bound_low; 00311 00312 // Flag 00313 bool Flag_Init; 00314 }; 00315 00316 //-----------------Saturation_vector---------------// 00317 class Saturation_vector{ // Saturation 00318 public: 00319 00320 // States 00321 vector<float> output; 00322 00323 Saturation_vector(size_t dimension, float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system 00324 vector<float> filter(vector<float> input); 00325 void reset(vector<float> input); 00326 00327 private: 00328 size_t n; 00329 00330 // 00331 float bound_up; 00332 float bound_low; 00333 00334 // Flag 00335 bool Flag_Init; 00336 }; 00337 00338 #endif
Generated on Fri Jul 15 2022 14:57:41 by
1.7.2