filterkit.c (7409B)
1 /********************************************************************** 2 3 resamplesubs.c 4 5 Real-time library interface by Dominic Mazzoni 6 7 Based on resample-1.7: 8 http://www-ccrma.stanford.edu/~jos/resample/ 9 10 Dual-licensed as LGPL and BSD; see README.md and LICENSE* files. 11 12 This file provides Kaiser-windowed low-pass filter support, 13 including a function to create the filter coefficients, and 14 two functions to apply the filter at a particular point. 15 16 **********************************************************************/ 17 18 /* Definitions */ 19 #include "resample_defs.h" 20 21 #include "filterkit.h" 22 23 #include <stdlib.h> 24 #include <string.h> 25 #include <stdio.h> 26 #include <math.h> 27 28 /* LpFilter() 29 * 30 * reference: "Digital Filters, 2nd edition" 31 * R.W. Hamming, pp. 178-179 32 * 33 * Izero() computes the 0th order modified bessel function of the first kind. 34 * (Needed to compute Kaiser window). 35 * 36 * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with 37 * the following characteristics: 38 * 39 * c[] = array in which to store computed coeffs 40 * frq = roll-off frequency of filter 41 * N = Half the window length in number of coeffs 42 * Beta = parameter of Kaiser window 43 * Num = number of coeffs before 1/frq 44 * 45 * Beta trades the rejection of the lowpass filter against the transition 46 * width from passband to stopband. Larger Beta means a slower 47 * transition and greater stopband rejection. See Rabiner and Gold 48 * (Theory and Application of DSP) under Kaiser windows for more about 49 * Beta. The following table from Rabiner and Gold gives some feel 50 * for the effect of Beta: 51 * 52 * All ripples in dB, width of transition band = D*N where N = window length 53 * 54 * BETA D PB RIP SB RIP 55 * 2.120 1.50 +-0.27 -30 56 * 3.384 2.23 0.0864 -40 57 * 4.538 2.93 0.0274 -50 58 * 5.658 3.62 0.00868 -60 59 * 6.764 4.32 0.00275 -70 60 * 7.865 5.0 0.000868 -80 61 * 8.960 5.7 0.000275 -90 62 * 10.056 6.4 0.000087 -100 63 */ 64 65 #define IzeroEPSILON 1E-21 /* Max error acceptable in Izero */ 66 67 static double Izero(double x) 68 { 69 double sum, u, halfx, temp; 70 int n; 71 72 sum = u = n = 1; 73 halfx = x/2.0; 74 do { 75 temp = halfx/(double)n; 76 n += 1; 77 temp *= temp; 78 u *= temp; 79 sum += u; 80 } while (u >= IzeroEPSILON*sum); 81 return(sum); 82 } 83 84 void lrsLpFilter(double c[], int N, double frq, double Beta, int Num) 85 { 86 double IBeta, temp, temp1, inm1; 87 int i; 88 89 /* Calculate ideal lowpass filter impulse response coefficients: */ 90 c[0] = 2.0*frq; 91 for (i=1; i<N; i++) { 92 temp = PI*(double)i/(double)Num; 93 c[i] = sin(2.0*temp*frq)/temp; /* Analog sinc function, cutoff = frq */ 94 } 95 96 /* 97 * Calculate and Apply Kaiser window to ideal lowpass filter. 98 * Note: last window value is IBeta which is NOT zero. 99 * You're supposed to really truncate the window here, not ramp 100 * it to zero. This helps reduce the first sidelobe. 101 */ 102 IBeta = 1.0/Izero(Beta); 103 inm1 = 1.0/((double)(N-1)); 104 for (i=1; i<N; i++) { 105 temp = (double)i * inm1; 106 temp1 = 1.0 - temp*temp; 107 temp1 = (temp1<0? 0: temp1); /* make sure it's not negative since 108 we're taking the square root - this 109 happens on Pentium 4's due to tiny 110 roundoff errors */ 111 c[i] *= Izero(Beta*sqrt(temp1)) * IBeta; 112 } 113 } 114 115 float lrsFilterUp(float Imp[], /* impulse response */ 116 float ImpD[], /* impulse response deltas */ 117 UWORD Nwing, /* len of one wing of filter */ 118 BOOL Interp, /* Interpolate coefs using deltas? */ 119 float *Xp, /* Current sample */ 120 double Ph, /* Phase */ 121 int Inc) /* increment (1 for right wing or -1 for left) */ 122 { 123 float *Hp, *Hdp = NULL, *End; 124 double a = 0; 125 float v, t; 126 127 Ph *= Npc; /* Npc is number of values per 1/delta in impulse response */ 128 129 v = 0.0; /* The output value */ 130 Hp = &Imp[(int)Ph]; 131 End = &Imp[Nwing]; 132 if (Interp) { 133 Hdp = &ImpD[(int)Ph]; 134 a = Ph - floor(Ph); /* fractional part of Phase */ 135 } 136 137 if (Inc == 1) /* If doing right wing... */ 138 { /* ...drop extra coeff, so when Ph is */ 139 End--; /* 0.5, we don't do too many mult's */ 140 if (Ph == 0) /* If the phase is zero... */ 141 { /* ...then we've already skipped the */ 142 Hp += Npc; /* first sample, so we must also */ 143 Hdp += Npc; /* skip ahead in Imp[] and ImpD[] */ 144 } 145 } 146 147 if (Interp) 148 while (Hp < End) { 149 t = *Hp; /* Get filter coeff */ 150 t += (*Hdp)*a; /* t is now interp'd filter coeff */ 151 Hdp += Npc; /* Filter coeff differences step */ 152 t *= *Xp; /* Mult coeff by input sample */ 153 v += t; /* The filter output */ 154 Hp += Npc; /* Filter coeff step */ 155 Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ 156 } 157 else 158 while (Hp < End) { 159 t = *Hp; /* Get filter coeff */ 160 t *= *Xp; /* Mult coeff by input sample */ 161 v += t; /* The filter output */ 162 Hp += Npc; /* Filter coeff step */ 163 Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ 164 } 165 166 return v; 167 } 168 169 float lrsFilterUD(float Imp[], /* impulse response */ 170 float ImpD[], /* impulse response deltas */ 171 UWORD Nwing, /* len of one wing of filter */ 172 BOOL Interp, /* Interpolate coefs using deltas? */ 173 float *Xp, /* Current sample */ 174 double Ph, /* Phase */ 175 int Inc, /* increment (1 for right wing or -1 for left) */ 176 double dhb) /* filter sampling period */ 177 { 178 float a; 179 float *Hp, *Hdp, *End; 180 float v, t; 181 double Ho; 182 183 v = 0.0; /* The output value */ 184 Ho = Ph*dhb; 185 End = &Imp[Nwing]; 186 if (Inc == 1) /* If doing right wing... */ 187 { /* ...drop extra coeff, so when Ph is */ 188 End--; /* 0.5, we don't do too many mult's */ 189 if (Ph == 0) /* If the phase is zero... */ 190 Ho += dhb; /* ...then we've already skipped the */ 191 } /* first sample, so we must also */ 192 /* skip ahead in Imp[] and ImpD[] */ 193 194 if (Interp) 195 while ((Hp = &Imp[(int)Ho]) < End) { 196 t = *Hp; /* Get IR sample */ 197 Hdp = &ImpD[(int)Ho]; /* get interp bits from diff table*/ 198 a = Ho - floor(Ho); /* a is logically between 0 and 1 */ 199 t += (*Hdp)*a; /* t is now interp'd filter coeff */ 200 t *= *Xp; /* Mult coeff by input sample */ 201 v += t; /* The filter output */ 202 Ho += dhb; /* IR step */ 203 Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ 204 } 205 else 206 while ((Hp = &Imp[(int)Ho]) < End) { 207 t = *Hp; /* Get IR sample */ 208 t *= *Xp; /* Mult coeff by input sample */ 209 v += t; /* The filter output */ 210 Ho += dhb; /* IR step */ 211 Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ 212 } 213 214 return v; 215 }