A combination of some frequently used filters

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers FILTER_LIB.h Source File

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