firfilter.cpp (10896B)
1 /*============================================================================= 2 // 3 // This software has been released under the terms of the GNU Public 4 // license. See http://www.gnu.org/copyleft/gpl.html for details. 5 // 6 // Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au 7 // 8 //============================================================================= 9 */ 10 11 /* Design and implementation of different types of digital filters 12 13 */ 14 #include "firfilter.hpp" 15 16 #include <algorithm> 17 #include <cmath> 18 19 #if 0 20 /* Add new data to circular queue designed to be used with a FIR 21 filter. xq is the circular queue, in pointing at the new sample, xi 22 current index for xq and n the length of the filter. xq must be n*2 23 long. 24 */ 25 #define updateq(n,xi,xq,in)\ 26 xq[xi]=(xq)[(xi)+(n)]=*(in);\ 27 xi=(++(xi))&((n)-1); 28 #endif 29 30 31 /****************************************************************************** 32 * FIR filter implementations 33 ******************************************************************************/ 34 35 /* C implementation of FIR filter y=w*x 36 37 n number of filter taps, where mod(n,4)==0 38 w filter taps 39 x input signal must be a circular buffer which is indexed backwards 40 */ 41 /* 42 TfirFilter::_ftype_t TfirFilter::fir(register unsigned int n, _ftype_t* w, _ftype_t* x) 43 { 44 register _ftype_t y; // Output 45 y = 0.0; 46 do{ 47 n--; 48 y+=w[n]*x[n]; 49 }while(n != 0); 50 return y; 51 } 52 */ 53 /* 54 // Boxcar 55 // 56 // n window length 57 // w buffer for the window parameters 58 */ 59 void TfirFilter::boxcar(int n, _ftype_t* w) 60 { 61 int i; 62 // Calculate window coefficients 63 for (i=0 ; i<n ; i++) 64 w[i] = 1.0; 65 } 66 67 68 /* 69 // Triang a.k.a Bartlett 70 // 71 // | (N-1)| 72 // 2 * |k - -----| 73 // | 2 | 74 // w = 1.0 - --------------- 75 // N+1 76 // n window length 77 // w buffer for the window parameters 78 */ 79 void TfirFilter::triang(int n, _ftype_t* w) 80 { 81 _ftype_t k1 = (_ftype_t)(n & 1); 82 _ftype_t k2 = 1/((_ftype_t)n + k1); 83 int end = (n + 1) >> 1; 84 int i; 85 86 // Calculate window coefficients 87 for (i=0 ; i<end ; i++) 88 w[i] = w[n-i-1] = _ftype_t((2.0*((_ftype_t)(i+1))-(1.0-k1))*k2); 89 } 90 91 92 /* 93 // Hanning 94 // 2*pi*k 95 // w = 0.5 - 0.5*cos(------), where 0 < k <= N 96 // N+1 97 // n window length 98 // w buffer for the window parameters 99 */ 100 void TfirFilter::hanning(int n, _ftype_t* w) 101 { 102 int i; 103 _ftype_t k = _ftype_t(2*M_PI/((_ftype_t)(n+1))); // 2*pi/(N+1) 104 105 // Calculate window coefficients 106 for (i=0; i<n; i++) 107 *w++ = _ftype_t(0.5*(1.0 - cos(k*(_ftype_t)(i+1)))); 108 } 109 110 /* 111 // Hamming 112 // 2*pi*k 113 // w(k) = 0.54 - 0.46*cos(------), where 0 <= k < N 114 // N-1 115 // 116 // n window length 117 // w buffer for the window parameters 118 */ 119 void TfirFilter::hamming(int n,_ftype_t* w) 120 { 121 int i; 122 _ftype_t k = _ftype_t(2*M_PI/((_ftype_t)(n-1))); // 2*pi/(N-1) 123 124 // Calculate window coefficients 125 for (i=0; i<n; i++) 126 *w++ = _ftype_t(0.54 - 0.46*cos(k*(_ftype_t)i)); 127 } 128 129 /* 130 // Blackman 131 // 2*pi*k 4*pi*k 132 // w(k) = 0.42 - 0.5*cos(------) + 0.08*cos(------), where 0 <= k < N 133 // N-1 N-1 134 // 135 // n window length 136 // w buffer for the window parameters 137 */ 138 void TfirFilter::blackman(int n,_ftype_t* w) 139 { 140 int i; 141 _ftype_t k1 = _ftype_t(2*M_PI/((_ftype_t)(n-1))); // 2*pi/(N-1) 142 _ftype_t k2 = 2*k1; // 4*pi/(N-1) 143 144 // Calculate window coefficients 145 for (i=0; i<n; i++) 146 *w++ = _ftype_t(0.42 - 0.50*cos(k1*(_ftype_t)i) + 0.08*cos(k2*(_ftype_t)i)); 147 } 148 149 /* 150 // Flattop 151 // 2*pi*k 4*pi*k 152 // w(k) = 0.2810638602 - 0.5208971735*cos(------) + 0.1980389663*cos(------), where 0 <= k < N 153 // N-1 N-1 154 // 155 // n window length 156 // w buffer for the window parameters 157 */ 158 void TfirFilter::flattop(int n,_ftype_t* w) 159 { 160 int i; 161 _ftype_t k1 = _ftype_t(2*M_PI/((_ftype_t)(n-1))); // 2*pi/(N-1) 162 _ftype_t k2 = 2*k1; // 4*pi/(N-1) 163 164 // Calculate window coefficients 165 for (i=0; i<n; i++) 166 *w++ = _ftype_t(0.2810638602 - 0.5208971735*cos(k1*(_ftype_t)i) + 0.1980389663*cos(k2*(_ftype_t)i)); 167 } 168 169 /* Computes the 0th order modified Bessel function of the first kind. 170 // (Needed to compute Kaiser window) 171 // 172 // y = sum( (x/(2*n))^2 ) 173 // n 174 */ 175 176 TfirFilter::_ftype_t TfirFilter::besselizero(_ftype_t x) 177 { 178 static const _ftype_t BIZ_EPSILON=_ftype_t(1E-21); // Max error acceptable 179 _ftype_t temp; 180 _ftype_t sum = 1.0f; 181 _ftype_t u = 1.0f; 182 _ftype_t halfx = x/2.0f; 183 int n = 1; 184 185 do { 186 temp = halfx/(_ftype_t)n; 187 u *=temp * temp; 188 sum += u; 189 n++; 190 } while (u >= BIZ_EPSILON * sum); 191 return(sum); 192 } 193 194 /* 195 // Kaiser 196 // 197 // n window length 198 // w buffer for the window parameters 199 // b beta parameter of Kaiser window, Beta >= 1 200 // 201 // Beta trades the rejection of the low pass filter against the 202 // transition width from passband to stop band. Larger Beta means a 203 // slower transition and greater stop band rejection. See Rabiner and 204 // Gold (Theory and Application of DSP) under Kaiser windows for more 205 // about Beta. The following table from Rabiner and Gold gives some 206 // feel for the effect of Beta: 207 // 208 // All ripples in dB, width of transition band = D*N where N = window 209 // length 210 // 211 // BETA D PB RIP SB RIP 212 // 2.120 1.50 +-0.27 -30 213 // 3.384 2.23 0.0864 -40 214 // 4.538 2.93 0.0274 -50 215 // 5.658 3.62 0.00868 -60 216 // 6.764 4.32 0.00275 -70 217 // 7.865 5.0 0.000868 -80 218 // 8.960 5.7 0.000275 -90 219 // 10.056 6.4 0.000087 -100 220 */ 221 void TfirFilter::kaiser(int n, _ftype_t* w, _ftype_t b) 222 { 223 _ftype_t tmp; 224 _ftype_t k1 = 1.0f/besselizero(b); 225 int k2 = 1 - (n & 1); 226 int end = (n + 1) >> 1; 227 int i; 228 229 // Calculate window coefficients 230 for (i=0 ; i<end ; i++){ 231 tmp = (_ftype_t)(2*i + k2) / ((_ftype_t)n - 1.0f); 232 w[end-(1&(!k2))+i] = w[end-1-i] = k1 * besselizero(_ftype_t(b*sqrt(1.0 - tmp*tmp))); 233 } 234 } 235 236 237 /****************************************************************************** 238 * FIR filter design 239 ******************************************************************************/ 240 241 /* Design FIR filter using the Window method 242 243 n filter length must be odd for HP and BS filters 244 w buffer for the filter taps (must be n long) 245 fc cutoff frequencies (1 for LP and HP, 2 for BP and BS) 246 0 < fc < 1 where 1 <=> Fs/2 247 flags window and filter type as defined in filter.h 248 variables are ored together: i.e. LP|HAMMING will give a 249 low pass filter designed using a hamming window 250 opt beta constant used only when designing using kaiser windows 251 252 returns 0 if OK, -1 if fail 253 */ 254 TfirFilter::_ftype_t* TfirFilter::design_fir(unsigned int *n, _ftype_t* fc, Type type, Window window, _ftype_t opt) 255 { 256 unsigned int o = *n & 1; // Indicator for odd filter length 257 unsigned int end = ((*n + 1) >> 1) - o; // Loop end 258 unsigned int i; // Loop index 259 260 _ftype_t k1 = 2 * _ftype_t(M_PI); // 2*pi*fc1 261 _ftype_t k2 = 0.5f * (_ftype_t)(1 - o);// Constant used if the filter has even length 262 _ftype_t k3; // 2*pi*fc2 Constant used in BP and BS design 263 _ftype_t g = 0.0f; // Gain 264 _ftype_t t1,t2,t3; // Temporary variables 265 _ftype_t fc1,fc2; // Cutoff frequencies 266 267 // Sanity check 268 if(*n==0) return NULL; 269 fc[0]=std::clamp(fc[0],_ftype_t(0.001),_ftype_t(1)); 270 271 if (!o && (type==Type::BANDSTOP || type==Type::HIGHPASS)) 272 (*n)++; 273 auto w = new _ftype_t[*n]; 274 275 // Get window coefficients 276 switch(window){ 277 case(Window::BOX): 278 boxcar(*n,w); break; 279 case(Window::TRIANGLE): 280 triang(*n,w); break; 281 case(Window::HAMMING): 282 hamming(*n,w); break; 283 case(Window::HANNING): 284 hanning(*n,w); break; 285 case(Window::BLACKMAN): 286 blackman(*n,w); break; 287 case(Window::FLATTOP): 288 flattop(*n,w); break; 289 case(Window::KAISER): 290 kaiser(*n,w,opt); break; 291 default: 292 { 293 delete []w; 294 return NULL; 295 } 296 } 297 298 if(type==Type::LOWPASS || type==Type::HIGHPASS){ 299 fc1=*fc; 300 // Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2 301 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25f; 302 k1 *= fc1; 303 304 if(type==Type::LOWPASS){ // Low pass filter 305 306 // If the filter length is odd, there is one point which is exactly 307 // in the middle. The value at this point is 2*fCutoff*sin(x)/x, 308 // where x is zero. To make sure nothing strange happens, we set this 309 // value separately. 310 if (o){ 311 w[end] = fc1 * w[end] * 2.0f; 312 g=w[end]; 313 } 314 315 // Create filter 316 for (i=0 ; i<end ; i++){ 317 t1 = (_ftype_t)(i+1) - k2; 318 w[end-i-1] = w[*n-end+i] = _ftype_t(w[end-i-1] * sin(k1 * t1)/(M_PI * t1)); // Sinc 319 g += 2*w[end-i-1]; // Total gain in filter 320 } 321 } 322 else{ // High pass filter 323 //if (!o) // High pass filters must have odd length 324 // return -1; 325 w[end] = _ftype_t(1.0 - (fc1 * w[end] * 2.0)); 326 g= w[end]; 327 328 // Create filter 329 for (i=0 ; i<end ; i++){ 330 t1 = (_ftype_t)(i+1); 331 w[end-i-1] = w[*n-end+i] = _ftype_t(-1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1)); // Sinc 332 g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter 333 } 334 } 335 } 336 337 if(type==Type::BANDPASS || type==Type::BANDSTOP){ 338 fc1=fc[0]; 339 fc2=std::clamp(fc[1],_ftype_t(0.001),_ftype_t(1)); 340 // Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2 341 fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25f; 342 fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25f; 343 k3 = k1 * fc2; // 2*pi*fc2 344 k1 *= fc1; // 2*pi*fc1 345 346 if(type==Type::BANDPASS){ // Band pass 347 // Calculate center tap 348 if (o){ 349 g=w[end]*(fc1+fc2); 350 w[end] = (fc2 - fc1) * w[end] * 2.0f; 351 } 352 353 // Create filter 354 for (i=0 ; i<end ; i++){ 355 t1 = (_ftype_t)(i+1) - k2; 356 t2 = _ftype_t(sin(k3 * t1)/(M_PI * t1)); // Sinc fc2 357 t3 = _ftype_t(sin(k1 * t1)/(M_PI * t1)); // Sinc fc1 358 g += w[end-i-1] * (t3 + t2); // Total gain in filter 359 w[end-i-1] = w[*n-end+i] = w[end-i-1] * (t2 - t3); 360 } 361 } 362 else{ // Band stop 363 //if (!o) // Band stop filters must have odd length 364 // return -1; 365 w[end] = _ftype_t(1.0 - (fc2 - fc1) * w[end] * 2.0); 366 g= w[end]; 367 368 // Create filter 369 for (i=0 ; i<end ; i++){ 370 t1 = (_ftype_t)(i+1); 371 t2 = _ftype_t(sin(k1 * t1)/(M_PI * t1)); // Sinc fc1 372 t3 = _ftype_t(sin(k3 * t1)/(M_PI * t1)); // Sinc fc2 373 w[end-i-1] = w[*n-end+i] = w[end-i-1] * (t2 - t3); 374 g += 2*w[end-i-1]; // Total gain in filter 375 } 376 } 377 } 378 379 // Normalize gain 380 g=1/g; 381 for (i=0; i<*n; i++) 382 w[i] *= g; 383 384 return w; 385 }