A combination of some frequently used filters
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 14:57:41 by
1.7.2