A combination of some frequently used filters

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers FILTER_LIB.cpp Source File

FILTER_LIB.cpp

00001 #include "FILTER_LIB.h"
00002 
00003 //--------------------LPF---------------------//
00004 // Low-pass filter
00005 LPF::LPF(float samplingTime, float cutOff_freq_Hz_in){
00006     //
00007     Ts = samplingTime;
00008     cutOff_freq_Hz = cutOff_freq_Hz_in;
00009 
00010     // alpha_Ts = (2*PI)*cutOff_freq_Hz*Ts;
00011     alpha_Ts = 1.0 - exp(-(2.0*PI)*cutOff_freq_Hz*Ts); // The discrete system's pole
00012 
00013     One_alpha_Ts = 1.0 - alpha_Ts;
00014     output = 0.0;
00015 
00016     //
00017     Flag_Init = false;
00018 }
00019 float LPF::filter(float input){
00020     // Initialization
00021     if (!Flag_Init){
00022         reset(input);
00023         Flag_Init = true;
00024         return output;
00025     }
00026     // output = One_alpha_Ts*output + alpha_Ts*input;
00027     output += alpha_Ts*(input - output);
00028     return output;
00029 }
00030 void LPF::reset(float input){
00031     // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
00032     output = input;
00033     return;
00034 }
00035 
00036 //
00037 
00038 //--------------------LPF_vector---------------------//
00039 // Vectorized low-pass filter
00040 LPF_vector::LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in){
00041     //
00042     n = dimension;
00043     Ts = samplingTime;
00044     cutOff_freq_Hz = cutOff_freq_Hz_in;
00045 
00046     // alpha_Ts = (2*PI)*cutOff_freq_Hz*Ts;
00047     alpha_Ts = 1.0 - exp(-(2.0*PI)*cutOff_freq_Hz*Ts); // The discrete system's pole
00048 
00049     One_alpha_Ts = 1.0 - alpha_Ts;
00050     //
00051     zeros.assign(n, 0.0);
00052     //
00053     output = zeros;
00054     //
00055     Flag_Init = false;
00056 }
00057 vector<float> LPF_vector::filter(const vector<float> &input){
00058     // Initialization
00059     if (!Flag_Init){
00060         reset(input);
00061         Flag_Init = true;
00062         return output;
00063     }
00064     //
00065     static vector<float>::iterator it_output;
00066     static vector<float>::iterator it_input;
00067     //
00068     it_output = output.begin();
00069     it_input = vector<float>(input).begin();
00070     for (size_t i = 0; i < n; ++i){
00071         // output = One_alpha_Ts*output + alpha_Ts*input;
00072         *it_output += alpha_Ts*(*it_input - *it_output);
00073         //
00074         it_output++;
00075         it_input++;
00076     }
00077     //
00078     /*
00079     for (size_t i = 0; i < n; ++i){
00080         // output = One_alpha_Ts*output + alpha_Ts*input;
00081         output[i] += alpha_Ts*(input[i] - output[i]);
00082     }
00083     */
00084     return output;
00085 }
00086 void LPF_vector::reset(const vector<float> &input){
00087     // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
00088     output = input;
00089     return;
00090 }
00091 
00092 //--------------------LPF_nthOrderCritical---------------------//
00093 // nth-order critical-damped Low-pass filter (all the poles are at the same place)
00094 LPF_nthOrderCritical::LPF_nthOrderCritical(float samplingTime, float cutOff_freq_Hz_in, size_t order_in):
00095         filter_layers(order_in, LPF(samplingTime,cutOff_freq_Hz_in))
00096 {
00097     //
00098     Ts = samplingTime;
00099     order = order_in;
00100     cutOff_freq_Hz = cutOff_freq_Hz_in;
00101 
00102     output = 0.0;
00103 
00104     //
00105     Flag_Init = false;
00106 }
00107 float LPF_nthOrderCritical::filter(float input){
00108     // Initialization
00109     if (!Flag_Init){
00110         reset(input);
00111         Flag_Init = true;
00112         return output;
00113     }
00114 
00115     // 0th-order, simply by-pass the input
00116     if (order == 0){
00117         output = input;
00118         return output;
00119     }
00120 
00121     // The first layer, i = 0
00122     filter_layers[0].filter(input);
00123     // The rest, i = 1 ~ n-1
00124     for (size_t i = 1; i < order; ++i){
00125         filter_layers[i].filter(filter_layers[i-1].output);
00126     }
00127 
00128     // Output the last layer's output
00129     output = filter_layers[order-1].output;
00130     return output;
00131 }
00132 void LPF_nthOrderCritical::reset(float input){
00133     // Reset all layers
00134     for (size_t i = 0; i < order; ++i){
00135         filter_layers[i].reset(input);
00136     }
00137     //
00138     output = input;
00139     return;
00140 }
00141 
00142 //--------------------LPF_vector_nthOrderCritical---------------------//
00143 // Vectorized nth-order critical-damped Low-pass filter (all the poles are at the same place)
00144 LPF_vector_nthOrderCritical::LPF_vector_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in):
00145         filter_layers(order_in, LPF_vector(dimension, samplingTime, cutOff_freq_Hz_in))
00146 {
00147     //
00148     n = dimension;
00149     Ts = samplingTime;
00150     order = order_in;
00151 
00152     cutOff_freq_Hz = cutOff_freq_Hz_in;
00153 
00154     //
00155     zeros.assign(n, 0.0);
00156     //
00157     output = zeros;
00158     //
00159     Flag_Init = false;
00160 }
00161 vector<float> LPF_vector_nthOrderCritical::filter(const vector<float> &input){
00162     // Initialization
00163     if (!Flag_Init){
00164         reset(input);
00165         Flag_Init = true;
00166         return output;
00167     }
00168 
00169     // 0th-order, simply by-pass the input
00170     if (order == 0){
00171         output = input;
00172         return output;
00173     }
00174 
00175     // The first layer, i = 0
00176     filter_layers[0].filter(input);
00177     // The rest, i = 1 ~ n-1
00178     for (size_t i = 1; i < order; ++i){
00179         filter_layers[i].filter(filter_layers[i-1].output);
00180     }
00181 
00182     // Output the last layer's output
00183     output = filter_layers[order-1].output;
00184     return output;
00185 }
00186 void LPF_vector_nthOrderCritical::reset(const vector<float> &input){
00187     // Reset all layers
00188     for (size_t i = 0; i < order; ++i){
00189         filter_layers[i].reset(input);
00190     }
00191     //
00192     output = input;
00193     return;
00194 }
00195 
00196 
00197 //--------------------HPF---------------------//
00198 // High-pass filter
00199 HPF::HPF(float samplingTime, float cutOff_freq_Hz_in):
00200             lpf(samplingTime,cutOff_freq_Hz_in)
00201 {
00202     //
00203     Ts = samplingTime;
00204     cutOff_freq_Hz = cutOff_freq_Hz_in;
00205     // alpha_Ts = (2*PI)*cutOff_freq_Hz*Ts;
00206     // One_alpha_Ts = 1.0 - alpha_Ts;
00207     output = 0.0;
00208 
00209     //
00210     Flag_Init = false;
00211 }
00212 float HPF::filter(float input){
00213     // Initialization
00214     if (!Flag_Init){
00215         reset(input);
00216         Flag_Init = true;
00217         return output; // output = 0.0
00218     }
00219 
00220     output = input - lpf.filter(input); // hpf = (1 - lpf)*x
00221     return output;
00222 }
00223 void HPF::reset(float input){
00224     lpf.reset(input);
00225     output = 0.0;
00226     return;
00227 }
00228 //--------------------HPF_vector---------------------//
00229 // Vectorized how-pass filter
00230 HPF_vector::HPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in):
00231             lpf_v(dimension,samplingTime,cutOff_freq_Hz_in)
00232 {
00233     //
00234     n = dimension;
00235     Ts = samplingTime;
00236     cutOff_freq_Hz = cutOff_freq_Hz_in;
00237     // alpha_Ts = (2*PI)*cutOff_freq_Hz*Ts;
00238     // One_alpha_Ts = 1.0 - alpha_Ts;
00239     output.assign(n,0.0);
00240 
00241     //
00242     Flag_Init = false;
00243 }
00244 vector<float> HPF_vector::filter(const vector<float> &input){
00245     // Initialization
00246     if (!Flag_Init){
00247         reset(input);
00248         Flag_Init = true;
00249         return output; // output = zeros_n
00250     }
00251     //
00252     lpf_v.filter(input);
00253     //
00254     /*
00255     // hpf = (1 - lpf)*x
00256     for (size_t i = 0; i < n; ++i){
00257         output[i] = input[i] - lpf_v.output[i];
00258     }
00259     */
00260 
00261     // hpf = (1 - lpf)*x
00262     static vector<float>::iterator it_output;
00263     static vector<float>::iterator it_input;
00264     static vector<float>::iterator it_lpf_v;
00265     //
00266     it_output = output.begin();
00267     it_input = vector<float>(input).begin();
00268     it_lpf_v = lpf_v.output.begin();
00269     for (size_t i = 0; i < n; ++i){
00270         *it_output = *it_input - *it_lpf_v;
00271         //
00272         it_output++;
00273         it_input++;
00274         it_lpf_v++;
00275     }
00276     return output;
00277 }
00278 void HPF_vector::reset(const vector<float> &input){
00279     lpf_v.reset(input);
00280     output.assign(n,0.0);
00281     return;
00282 }
00283 
00284 //--------------------HPF_nthOrderCritical---------------------//
00285 // nth-order critical-damped High-pass filter (all the poles are at the same place)
00286 HPF_nthOrderCritical::HPF_nthOrderCritical(float samplingTime, float cutOff_freq_Hz_in, size_t order_in):
00287         filter_layers(order_in, HPF(samplingTime,cutOff_freq_Hz_in))
00288 {
00289     //
00290     Ts = samplingTime;
00291     order = order_in;
00292     cutOff_freq_Hz = cutOff_freq_Hz_in;
00293 
00294     output = 0.0;
00295 
00296     //
00297     Flag_Init = false;
00298 }
00299 float HPF_nthOrderCritical::filter(float input){
00300     // Initialization
00301     if (!Flag_Init){
00302         reset(input);
00303         Flag_Init = true;
00304         return output;
00305     }
00306 
00307     // 0th-order, simply by-pass the input
00308     if (order == 0){
00309         output = input;
00310         return output;
00311     }
00312 
00313     // The first layer, i = 0
00314     filter_layers[0].filter(input);
00315     // The rest, i = 1 ~ n-1
00316     for (size_t i = 1; i < order; ++i){
00317         filter_layers[i].filter(filter_layers[i-1].output);
00318     }
00319 
00320     // Output the last layer's output
00321     output = filter_layers[order-1].output;
00322     return output;
00323 }
00324 void HPF_nthOrderCritical::reset(float input){
00325     // Reset all layers
00326     for (size_t i = 0; i < order; ++i){
00327         filter_layers[i].reset(input);
00328     }
00329     //
00330     output = 0.0;
00331     return;
00332 }
00333 
00334 //--------------------HPF_vector_nthOrderCritical---------------------//
00335 // Vectorized nth-order critical-damped High-pass filter (all the poles are at the same place)
00336 HPF_vector_nthOrderCritical::HPF_vector_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in):
00337         filter_layers(order_in, HPF_vector(dimension, samplingTime, cutOff_freq_Hz_in))
00338 {
00339     //
00340     n = dimension;
00341     Ts = samplingTime;
00342     order = order_in;
00343 
00344     cutOff_freq_Hz = cutOff_freq_Hz_in;
00345 
00346     //
00347     zeros.assign(n, 0.0);
00348     //
00349     output = zeros;
00350     //
00351     Flag_Init = false;
00352 }
00353 vector<float> HPF_vector_nthOrderCritical::filter(const vector<float> &input){
00354     // Initialization
00355     if (!Flag_Init){
00356         reset(input);
00357         Flag_Init = true;
00358         return output;
00359     }
00360 
00361     // 0th-order, simply by-pass the input
00362     if (order == 0){
00363         output = input;
00364         return output;
00365     }
00366 
00367     // The first layer, i = 0
00368     filter_layers[0].filter(input);
00369     // The rest, i = 1 ~ n-1
00370     for (size_t i = 1; i < order; ++i){
00371         filter_layers[i].filter(filter_layers[i-1].output);
00372     }
00373 
00374     // Output the last layer's output
00375     output = filter_layers[order-1].output;
00376     return output;
00377 }
00378 void HPF_vector_nthOrderCritical::reset(const vector<float> &input){
00379     // Reset all layers
00380     for (size_t i = 0; i < order; ++i){
00381         filter_layers[i].reset(input);
00382     }
00383     //
00384     output.assign(n,0.0);
00385     return;
00386 }
00387 
00388 //--------------------HPF_vector_1minusLPF_nthOrderCritical---------------------//
00389 // Vectorized nth-order critical-damped High-pass filter ( the version of (1 - nth-order LPF), all the poles are at the same place)
00390 HPF_vector_1minusLPF_nthOrderCritical::HPF_vector_1minusLPF_nthOrderCritical(size_t dimension, float samplingTime, float cutOff_freq_Hz_in, size_t order_in):
00391         filter_layers(order_in, LPF_vector(dimension, samplingTime, cutOff_freq_Hz_in))
00392 {
00393     //
00394     n = dimension;
00395     Ts = samplingTime;
00396     order = order_in;
00397 
00398     cutOff_freq_Hz = cutOff_freq_Hz_in;
00399 
00400     //
00401     zeros.assign(n, 0.0);
00402     //
00403     output = zeros;
00404     //
00405     Flag_Init = false;
00406 }
00407 vector<float> HPF_vector_1minusLPF_nthOrderCritical::filter(const vector<float> &input){
00408     // Initialization
00409     if (!Flag_Init){
00410         reset(input);
00411         Flag_Init = true;
00412         return output;
00413     }
00414 
00415     // 0th-order, simply by-pass the input
00416     if (order == 0){
00417         output = input;
00418         return output;
00419     }
00420 
00421     // The first layer, i = 0
00422     filter_layers[0].filter(input);
00423     // The rest, i = 1 ~ n-1
00424     for (size_t i = 1; i < order; ++i){
00425         filter_layers[i].filter(filter_layers[i-1].output);
00426     }
00427 
00428     // Output the last layer's output
00429     // output = filter_layers[order-1].output;
00430 
00431     // hpf = (1 - lpf)*x
00432     static vector<float>::iterator it_output;
00433     static vector<float>::iterator it_input;
00434     static vector<float>::iterator it_lastFilter;
00435     //
00436     it_output = output.begin();
00437     it_input = vector<float>(input).begin();
00438     it_lastFilter = filter_layers[order-1].output.begin();
00439     for (size_t i = 0; i < n; ++i){
00440         *it_output = *it_input - *it_lastFilter;
00441         //
00442         it_output++;
00443         it_input++;
00444         it_lastFilter++;
00445     }
00446     return output;
00447 }
00448 void HPF_vector_1minusLPF_nthOrderCritical::reset(const vector<float> &input){
00449     // Reset all layers
00450     for (size_t i = 0; i < order; ++i){
00451         filter_layers[i].reset(input);
00452     }
00453     //
00454     output.assign(n,0.0);
00455     return;
00456 }
00457 
00458 
00459 
00460 
00461 //--------------------Derivative_appr---------------------//
00462 // Approximated Derivative, cut-off at 10% of sampling frequency
00463 Derivative_appr::Derivative_appr(float samplingTime):
00464         // derivative_LPF2(2,2) // 2nd-order LPF
00465         // derivative_LPF2(4,4) // 4th-order LPF
00466         derivative_LPF2(6,6) // 6th-order LPF
00467 {
00468     //
00469     Ts = samplingTime;
00470     cutOff_freq_Hz = 0.1*Ts;
00471 
00472     /*
00473     // 10% of Fs
00474     float num[] = {114.296712320901,    0., -114.296712320901};
00475     float den[] = {1.,  -1.04377110555725,  0.272364530199049};
00476     float gain = 1.0;
00477     */
00478     /*
00479     // 20% of Fs
00480     float num[] = {297.790611663702,    0., -297.790611663702};
00481     float den[] = {1.,  -0.456521819619850, 0.0521030429472546};
00482     float gain = 1.0;
00483     */
00484 
00485     // 20% of Fs, 4th-order LPF
00486     float num[] = {44.3396241975210,    88.6792483950420,   0., -88.6792483950420,  -44.3396241975210};
00487     float den[] = {1.,  -0.913043639239699, 0.312618257683527,  -0.0475723519480234,    0.00271472708436336};
00488     float gain = 1.0;
00489 
00490 
00491     /*
00492     // 20% of Fs, 6th-order LPF
00493     float num[] = {6.60196190535925,    26.4078476214370,   33.0098095267962,   0., -33.0098095267962,  -26.4078476214370,  -6.60196190535925};
00494     float den[] = {1.,  -1.36956545885955,  0.781545644208822,  -0.237861759740118, 0.0407209062654518, -0.00371799644497471,   0.000141445541866579};
00495     float gain = 1.0;
00496     */
00497     //
00498     derivative_LPF2.Assign_parameters(num,den,gain);
00499     //
00500     output = 0.0;
00501 
00502     //
00503     Flag_Init = false;
00504 }
00505 float Derivative_appr::filter(float input){
00506     // Initialization
00507     if (!Flag_Init){
00508         reset(input);
00509         Flag_Init = true;
00510         return output; // output = 0.0
00511     }
00512     //
00513     output = derivative_LPF2.Iterate_once(input);
00514     return output;
00515 }
00516 void Derivative_appr::reset(float input){
00517     //
00518     /*
00519     for (size_t i = 0; i < 3; ++i){
00520         derivative_LPF2.Iterate_once(0.0);
00521     }
00522     */
00523     derivative_LPF2.Reset(input);
00524     output = 0.0;
00525     return;
00526 }
00527 
00528 //--------------------Rate-saturation Filter---------------------//
00529 // Rate-saturation Filter
00530 //
00531 //          _______
00532 //         /
00533 //        /
00534 //       /
00535 //  ____/
00536 //
00537 RateSaturation_Filter::RateSaturation_Filter(float samplingTime, float limit_rate_in){ // limit_rate is in the unit of "value/s"
00538     //
00539     Ts = samplingTime;
00540     limit_rate = limit_rate_in;
00541     limit_increment = limit_rate*Ts;
00542     output = 0.0;
00543 
00544     //
00545     Flag_Init = false;
00546 }
00547 float RateSaturation_Filter::filter(float input){
00548     // Initialization
00549     if (!Flag_Init){
00550         reset(input);
00551         Flag_Init = true;
00552         return output;
00553     }
00554     error = input - output;
00555     if (error > limit_increment){
00556         error = limit_increment;
00557     }else if(error < -limit_increment){
00558         error = -limit_increment;
00559     }
00560     output += error;
00561     return output;
00562 }
00563 void RateSaturation_Filter::reset(float input){
00564     // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
00565     output = input;
00566     return;
00567 }
00568 
00569 //-----------First-Order Kalman Filter--------//
00570 // FirstOrder_KalmanFilter
00571 FirstOrder_KalmanFilter::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
00572     //
00573     Ts = samplingTime;
00574 
00575     // Parameters
00576     if(is_continuousTime){
00577         A = 1.0 + Ts*A_in;
00578         B = Ts*B_in;
00579     }else{
00580         A = A_in;
00581         B = B_in;
00582     }
00583     //
00584     C = C_in;
00585     //
00586     R = R_in;
00587     Q = Q_in;
00588 
00589 
00590     //
00591     mu_est = 0.0;
00592     Sigma_est = 100000; // No a-priori knoledge
00593 
00594     // Kalman gain
00595     K = 0;
00596 
00597     //
00598     Flag_Init = false;
00599 }
00600 float FirstOrder_KalmanFilter::filter(float u, float z){
00601     // Initialization
00602     if (!Flag_Init){
00603         reset(z);
00604         Flag_Init = true;
00605         return mu_est;
00606     }
00607 
00608     // Prediction
00609     mu_est  = A*mu_est + B*u;
00610     Sigma_est = A*Sigma_est*A + R;
00611     // Update
00612     K = Sigma_est*C/(C*Sigma_est*C + Q);
00613     mu_est += K*(z - C*mu_est);
00614     Sigma_est -= K*C*Sigma_est;
00615     //
00616 
00617     return mu_est;
00618 }
00619 void FirstOrder_KalmanFilter::reset(float z){
00620     //
00621     mu_est = z;
00622     Sigma_est = 100000; // No a-priori knoledge
00623 
00624     // Kalman gain
00625     K = 0;
00626     return;
00627 }
00628 
00629 //-----------Saturation--------//
00630 // Saturation
00631 Saturation::Saturation(float bound_up_in, float bound_low_in){ // If is_continuousTime -> continuous time system
00632 
00633     //
00634     bound_up = bound_up_in;
00635     bound_low = bound_low_in;
00636 
00637     //
00638     output = 0.0;
00639 
00640 
00641     Flag_Init = false;
00642 }
00643 float Saturation::filter(float input){
00644     // Initialization
00645     if (!Flag_Init){
00646         reset(input);
00647         Flag_Init = true;
00648         return output;
00649     }
00650 
00651     // Saturation
00652     if (input > bound_up){
00653         output = bound_up;
00654     }else if (input < bound_low){
00655         output = bound_low;
00656     }else{
00657         output = input;
00658     }
00659 
00660     return output;
00661 }
00662 void Saturation::reset(float input){
00663     //
00664     // Saturation
00665     if (input > bound_up){
00666         output = bound_up;
00667     }else if (input < bound_low){
00668         output = bound_low;
00669     }else{
00670         output = input;
00671     }
00672 
00673     return;
00674 }
00675 
00676 //-----------Saturation_vector--------//
00677 // Saturation_vector
00678 Saturation_vector::Saturation_vector(size_t dimension, float bound_up_in, float bound_low_in){ // If is_continuousTime -> continuous time system
00679     //
00680     n = dimension;
00681 
00682     //
00683     bound_up = bound_up_in;
00684     bound_low = bound_low_in;
00685 
00686     //
00687     output.assign(n, 0.0);
00688 
00689 
00690     Flag_Init = false;
00691 }
00692 vector<float> Saturation_vector::filter(vector<float> input){
00693     // Initialization
00694     if (!Flag_Init){
00695         reset(input);
00696         Flag_Init = true;
00697         return output;
00698     }
00699 
00700     // Saturation
00701     for (size_t i = 0; i < n; ++i){
00702         //
00703         if (input[i] > bound_up){
00704             output[i] = bound_up;
00705         }else if (input[i] < bound_low){
00706             output[i] = bound_low;
00707         }else{
00708             output[i] = input[i];
00709         }
00710         //
00711     }
00712 
00713 
00714     return output;
00715 }
00716 void Saturation_vector::reset(vector<float> input){
00717     //
00718     // Saturation
00719     for (size_t i = 0; i < n; ++i){
00720         //
00721         if (input[i] > bound_up){
00722             output[i] = bound_up;
00723         }else if (input[i] < bound_low){
00724             output[i] = bound_low;
00725         }else{
00726             output[i] = input[i];
00727         }
00728         //
00729     }
00730 
00731     return;
00732 }