source: SH_SHM/trunk/util/butfreq.c @ 16

Revision 16, 12.4 KB checked in by marcus, 14 years ago (diff)

r1 | svn | 2007-12-13 11:10:29 +0100 (Do, 13 Dez 2007) | 2 lines

Initial import

Line 
1
2/* file BUTFREQ.C
3 *      =========
4 *
5 * version 1, 24-Jul-92
6 *
7 * creates Butterworth filter file
8 * same as but_freq, but different user interface
9 * K. Stammler, 24-Jul-92
10 */
11
12
13/*
14 *
15 *  SeismicHandler, seismic analysis software
16 *  Copyright (C) 1996,  Klaus Stammler, Federal Institute for Geosciences
17 *                                       and Natural Resources (BGR), Germany
18 *
19 *  This program is free software; you can redistribute it and/or modify
20 *  it under the terms of the GNU General Public License as published by
21 *  the Free Software Foundation; either version 2 of the License, or
22 *  (at your option) any later version.
23 *
24 *  This program is distributed in the hope that it will be useful,
25 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
26 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
27 *  GNU General Public License for more details.
28 *
29 *  You should have received a copy of the GNU General Public License
30 *  along with this program; if not, write to the Free Software
31 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
32 *
33 */
34
35/*
36 *     The transfer function of all Butterworth filter of order N
37 *     consists of factors of the form:
38 *        F(s,k,N)  =  1  /  ( T(s) - E(k,N) ) ;   k = 1,..,N
39 *
40 *        H(s)  =  F(s,1,N) * F(s,2,N) * .. * F(s,N,N)
41 *
42 *     where
43 *
44 *        E(k,N)  =  i * exp( i*pi*(2k-1) / (2*N) ) ;  k = 1,..,N
45 *
46 *     and
47 *
48 *        T(s)  =  s / wc                       lowpass
49 *        T(s)  =  wc / s                       highpass
50 *        T(s)  =  (s*s + wl*wu) / (s*(wu-wl))  bandpass
51 *        T(s)  =  (s*(wu-wl)) / (s*s + wl*wu)  bandstop
52 *
53 *     The program computes poles p and zeroes n of the transfer function
54 *     using the equations
55 *
56 *        T(p)  =  E(k,N)
57 *        denominator( T(n) ) = 0
58 *
59 *     From these result the final equations
60 *
61 *     N poles of LP and HP
62 *        p  =  wc * E(k,N)
63 *
64 *     2N poles of BP and BS
65 *        p  =  1/2 * ( E(k,N)*(wu-wl) +- sqrt( radicant ) )
66 *           where  radicant  =  E(k,N)**2 * (wu-wl)**2 - 4*wu*wl
67 *
68 *     N zeroes of HP and BP
69 *        n  =  0
70 *
71 *     2N zeroes of BS
72 *        n  =  +-i * sqrt( wu*wl )
73 *
74 *     LP has no zeroes
75 *
76 *     The computed poles and zeroes are stored in a filter file.
77 *
78 *     The normalisation constant G results in
79 *
80 *        LP:  G = wc**N
81 *        HP:  G = 1
82 *        BP:  G = (wu - wl)**N
83 *        BS:  G = 1
84 */
85
86
87#include <stdio.h>
88#include <string.h>
89#include <math.h>
90#include BASECNST
91#ifdef BC_STDLIB_EX
92#include <stdlib.h>
93#endif /* BC_STDLIB_EX */
94#include BC_SYSBASE
95#include BC_CPAR
96
97
98/* global type */
99typedef struct {
100        float    re;    /* real part */
101        float    im;    /* imaginary part */
102} COMPLEX;
103
104
105/* prototypes of local routines */
106void read_freq( char str[], char inp_kind, float *freq );
107void get_hp_lp_poles( float wc, int n, int max_poles, COMPLEX pole[],
108        int *no_of_poles );
109void get_bp_bs_poles( float wl, float wu, int n, int max_poles,
110        COMPLEX pole[], int *no_of_poles );
111void ep( int k, int n, COMPLEX *res );
112void ep2( int k, int n, COMPLEX *res );
113float intpow( float base, int exp );
114
115
116int main( int argc, char *argv[] )
117{
118
119        /* constant */
120#       define MAX_POLES 50
121                /* maximum number of poles */
122
123        /* local variables */
124        char     f_type[BC_LINELTH+1]; /* type of filter: LP,HP,BP,BS */
125        char     inp_kind;             /* kind of input: w, f, t */
126        COMPLEX  pole[MAX_POLES];      /* poles of transfer function */
127        int      no_of_poles;          /* number of poles */
128        float    wu, wl;               /* upper and lower corner freq. */
129        int      order;                /* order of filter */
130        COMPLEX  zero[MAX_POLES];      /* zeroes of transfer function */
131        int      no_of_zeroes;         /* number of zeroes */
132        int      i;                    /* counter */
133        float    sq;                   /* scratch */
134        float    norm_const;           /* normalisation */
135        char     filename[BC_FILELTH+1];/* Name of filter file */
136        char     info1[BC_LINELTH+1];  /* first info line */
137        char     info2[BC_LINELTH+1];  /* second info line */
138        FILE     *fp;                  /* filter file pointer */
139
140
141        /* executable code */
142
143        pa_init( argc, argv );
144        if  (pa_pnumber() < 3)  {
145                printf( "*** Usage: butfreq <ftype> <itype> <c1> [<c2>] ***\n" );
146                printf( "    Qualifiers: -o=<order> -f=<filterfile>\n" );
147                return 0;
148        } /*endif*/
149
150        strncpy( f_type, pa_pvalue(2), 1 );
151        inp_kind = Cap( *f_type );
152        strncpy( f_type, pa_pvalue(1), 2 );
153        f_type[0] = Cap( f_type[0] );
154        f_type[1] = Cap( f_type[1] );
155        f_type[2] = '\0';
156
157   if  (strcmp(f_type,"HP") == 0 || strcmp(f_type,"LP") == 0)  {
158                read_freq( pa_pvalue(3), inp_kind, &wl );
159                sprintf( info2, "! corner frequency %e Hz", wl/(2.*BC_PI) );
160        } else if  (strcmp(f_type,"BP") == 0 || strcmp(f_type,"BS") == 0)  {
161                read_freq( pa_pvalue(3), inp_kind, &wl );
162                if  (pa_pnumber() != 4)  {
163                        printf( "*** no upper frequency bound specified ***\n" );
164                        return 0;
165                } /*endif*/
166                read_freq( pa_pvalue(4), inp_kind, &wu );
167                if  (wu < wl)  {  /* swap wu,wl */
168                        sq = wu;
169                        wu = wl;
170                        wl = sq;
171                } /*endif*/
172                sprintf( info2, "! lower bound %e Hz,  upper bound %e Hz",
173                        wl/(2.0*BC_PI), wu/(2.0*BC_PI) );
174        } /*endif*/
175
176        /* get order & output filter file */
177        if  (pa_qspecified("-o"))  {
178                sscanf( pa_qvalue("-o"), "%d", &order );
179        } else {
180                order = 3;
181        } /*endif*/
182        if  (pa_qspecified("-f"))  {
183                strncpy( filename, pa_qvalue("-f"), BC_LINELTH );
184        } else {
185                strcpy( filename, "filter" );
186        } /*endif*/
187
188        /* computation of poles */
189        if  (strcmp(f_type,"HP") == 0)  {
190                get_hp_lp_poles( wl, order, MAX_POLES, pole, &no_of_poles );
191                sprintf( info1, "! Butterworth Highpass of order %d", order );
192        } else if  (strcmp(f_type,"LP") == 0)  {
193                get_hp_lp_poles( wl, order, MAX_POLES, pole, &no_of_poles );
194                sprintf( info1, "! Butterworth Lowpass of order %d", order );
195        } else if  (strcmp(f_type,"BP") == 0)  {
196                get_bp_bs_poles( wl, wu, order, MAX_POLES, pole, &no_of_poles );
197                sprintf( info1, "! Butterworth Bandpass of order %d", order );
198        } else if  (strcmp(f_type,"BS") == 0)  {
199                get_bp_bs_poles( wl, wu, order, MAX_POLES, pole, &no_of_poles );
200                sprintf( info1, "! Butterworth Bandstop of order %d", order );
201        } else {
202                printf( "*** wrong filter type ***\n" );
203                return 0;
204        } /*endif*/
205
206        /* computation of zeroes and normalisation */
207        for  (i=0; i<MAX_POLES; i++)  {
208                zero[i].re = 0.0;
209                zero[i].im = 0.0;
210        } /*endfor*/
211        if  (strcmp(f_type,"HP") == 0)  {
212                no_of_zeroes = order;  /* n zeroes at s=0 */
213                norm_const = 1.;
214        } else if  (strcmp(f_type,"LP") == 0)  {
215                no_of_zeroes = 0;      /* no zeroes */
216                norm_const = intpow( wl, order );
217        } else if  (strcmp(f_type,"BP") == 0)  {
218                no_of_zeroes = order;  /* n zeroes at s=0 */
219                norm_const = intpow( wu-wl, order );
220        } else if  (strcmp(f_type,"BS") == 0)  {
221                no_of_zeroes = 2*order;
222                sq = sqrt( wl*wu );
223                for  (i=0; i<order; i++)  {
224                        zero[2*i].re = 0.;
225                        zero[2*i].im = sq;
226                        zero[2*i+1].re = 0.;
227                        zero[2*i+1].im = -sq;
228                } /*endfor*/
229                norm_const = 1.;
230        } /*endif*/
231
232        /* write filter file */
233        strcat( filename, ".FLF" );
234        fp = fopen( filename, "w" );
235        fprintf( fp, "%s\n", info1 );
236        fprintf( fp, "%s\n", info2 );
237        fprintf( fp, "%ld\n", 1357913578L );  /* magic longword */
238        fprintf( fp, "%d\n", 1 );             /* storage ID */
239        fprintf( fp, "%e\n", norm_const );    /* normalisation */
240        fprintf( fp, "%d\n", no_of_zeroes );
241        for  (i=0; i<no_of_zeroes; i++)
242                fprintf( fp, "(%e,%e)\n", zero[i].re, zero[i].im );
243        fprintf( fp, "%d\n", no_of_poles );
244        for  (i=0; i<no_of_poles; i++)
245                fprintf( fp, "(%e,%e)\n", pole[i].re, pole[i].im );
246        fclose( fp );
247
248        return 0;
249
250} /* end of main */
251
252
253
254/*---------------------------------------------------------------------------*/
255
256
257
258void read_freq( char str[], char inp_kind, float *freq )
259
260/* reads in frequency from string "str".  Output is circular frequency
261 *
262 * parameters of routine
263 * char       inp_kind;       input; determines kind of input
264 * float      freq;           output; circular frequency
265 */
266{
267        /* executable code */
268
269        sscanf( str, "%f", freq );
270        if  (inp_kind == 'T')  {
271                *freq = 2.*BC_PI / *freq;
272        } else if  (inp_kind == 'F')  {
273                *freq = 2.*BC_PI*(*freq);
274        } else if  (inp_kind != 'W')  {
275                printf( "*** wrong input type ***\n" );
276                exit( 0 );
277        } /*endif*/
278
279} /* end of read_freq */
280
281
282
283/*--------------------------------------------------------------------------*/
284
285
286
287void get_hp_lp_poles( float wc, int n, int max_poles, COMPLEX pole[],
288        int *no_of_poles )
289
290/* computes poles of butterworth low/highpass of order n
291 *
292 * parameters of routine
293 * float      wc;          input; corner frequency
294 * int        n;           input; order of filter
295 * int        max_poles;   input; maximum length of pole array
296 * COMPLEX    pole[]       output; poles
297 * int        no_of_poles; output; number of poles
298 */
299{
300        /* local variables */
301        int      k;            /* counter */
302
303        /* executable code */
304
305        if  (n > max_poles)  {
306                printf( "*** too many poles ***\n" );
307                exit( 0 );
308        } /*endif*/
309        *no_of_poles = n;
310
311        for  (k=0; k<n; k++)  {
312                ep( k+1, n, pole+k ); /* pole(k) = wc * ep(k,n) */
313                pole[k].re *= wc;
314                pole[k].im *= wc;
315        } /*endfor*/
316
317} /* end of get_hp_lp_poles */
318
319
320
321/*--------------------------------------------------------------------------*/
322
323
324
325void get_bp_bs_poles( float wl, float wu, int n, int max_poles,
326        COMPLEX pole[], int *no_of_poles )
327
328/* computes poles of bandpass/stop filter of order n
329 *
330 * parameters of routine
331 * float      wl;          input; lower corner frequency
332 * float      wu;          input; upper corner frequency
333 * int        n;           input; order
334 * int        max_poles;   input; max length of pole array
335 * COMPLEX    pole[];      output; poles
336 * int        no_of_poles; output; number of poles
337 */
338{
339        /* local variables */
340        int      k;       /* counter */
341        float    w_dist;  /* difference of corner frequencies */
342        float    w_dist2; /* square of w_dist */
343        float    wlwu4;   /* 4 * wl * wu */
344        COMPLEX  root;    /* scratch */
345        float    r, phi;  /* scratch */
346
347        /* executable code */
348
349        *no_of_poles = 2*n;
350        if  (*no_of_poles > max_poles)  {
351                printf( "*** too many poles ***\n" );
352                exit( 0 );
353        } /*endif*/
354
355        w_dist = wu - wl;
356        w_dist2 = w_dist * w_dist;
357        wlwu4 = 4. * wu * wl;
358
359        for  (k=0; k<n; k++)  {
360                /* root = cmplx(w_dist2,0.) * ep2(k,n) - cmplx(wlwu4,0.) */
361                ep2( k+1, n, &root );
362                root.re *= w_dist2;
363                root.im *= w_dist2;
364                root.re -= wlwu4;
365                /* root = csqrt( root ) */
366                r = sqrt( root.re*root.re + root.im*root.im );
367                phi = atan2( root.im, root.re );
368                r = sqrt( r );
369                phi /= 2.0;
370                root.re = r * cos( phi );
371                root.im = r * sin( phi );
372                /* pole(2*k-1) = 0.5 * (ep(k,n)*cmplx(w_dist,0.) + root) */
373                ep( k+1, n, pole+2*k );
374                pole[2*k].re *= w_dist;
375                pole[2*k].im *= w_dist;
376                pole[2*k].re += root.re;
377                pole[2*k].im += root.im;
378                pole[2*k].re *= 0.5;
379                pole[2*k].im *= 0.5;
380                /* pole(2*k) = 0.5 * (ep(k,n)*cmplx(w_dist,0.) - root) */
381                ep( k+1, n, pole+2*k+1 );
382                pole[2*k+1].re *= w_dist;
383                pole[2*k+1].im *= w_dist;
384                pole[2*k+1].re -= root.re;
385                pole[2*k+1].im -= root.im;
386                pole[2*k+1].re *= 0.5;
387                pole[2*k+1].im *= 0.5;
388        } /*endfor*/
389
390} /* end of get_bp_bs_poles */
391
392
393
394/*--------------------------------------------------------------------------*/
395
396
397
398void ep( int k, int n, COMPLEX *res )
399
400/* returns E(k,N) from above formula in "res"
401 *
402 * parameters of routine
403 * int        k       input; 1 <= k <= n
404 * int        n       input; order
405 */
406{
407        /* local variables */
408        float    arc;     /* angle */
409
410        /* executable code */
411
412        /* degrees = (90./float(n)) * float(2*k-1) */
413        arc = BC_PI*0.5/(float)n * (float)(2*k-1);
414        res->re = -sin( arc );
415        res->im = cos( arc );
416
417} /* end of ep */
418
419
420
421/*--------------------------------------------------------------------------*/
422
423
424
425void ep2( int k, int n, COMPLEX *res )
426
427/* returns E(k,N)**2 from above formula in "res"
428 *
429 * parameters of routine
430 * int        k       input; 1 <= k <= n
431 * int        n       input; order
432 */
433{
434        /* local variables */
435        float    arc;      /* angle */
436
437        /* executable code */
438
439        arc = BC_PI/(float)n * (float)(2*k-1);
440        res->re = -cos( arc );
441        res->im = -sin( arc );
442
443} /* end of ep2 */
444
445
446
447/*--------------------------------------------------------------------------*/
448
449
450
451float intpow( float base, int exp )
452
453/* computes integer power of "base"
454 *
455 * parameters of routine
456 * float      base       input; base
457 * int        exp        input; exponent
458 */
459{
460        /* local variables */
461        int      i;         /* counter */
462        float    ret;       /* return value */
463
464        /* executable code */
465
466        ret = 1.;
467        for  (i=1; i<=exp; i++)
468                ret *= base;
469
470        return ret;
471
472} /* end of intpow */
473
474
475
476/*--------------------------------------------------------------------------*/
Note: See TracBrowser for help on using the repository browser.