This is filter.c in view mode; [Download] [Up]
#include <stdio.h>
#include <math.h>
#define LP 1
#define HP 2
#define BP 3
#define BS 4
#define BUTT 1
#define CHEB 2
#define INVCH 3
#define ELLIP 4
#define MAXPOLES 30
#define invcosh(x) log( (x) + sqrt((x)*(x) - 1) )
#define invsinh(x) log( (x) + sqrt((x)*(x) + 1) )
#define order(x) ( (double) ( (int) (x) ) == (x) ? (int) (x) : (int) ((x) + 1) )
#define sgn(x) ( (x) < 0 ? -1 : 1)
#define PI 3.1415927
#define BIG 999999999
struct cmplx_num { /* The Complex number structure */
double real;
double imag;
};
#define sqr(X) ( X )*( X )
#define cset(X, Y, Z) X.real = Y; X.imag = Z
#define cmpl(X) X.imag = - X.imag
#define cadd(X, Y, Z) Z.real = X.real + Y.real;\
Z.imag = X.imag + Y.imag
#define csub(X, Y, Z) Z.real = X.real - Y.real;\
Z.imag = X.imag - Y.imag
#define cmul(X, Y, Z) { \
struct cmplx_num cmplx_tmp; \
cmplx_tmp.real = X.real * Y.real - \
X.imag * Y.imag ; \
cmplx_tmp.imag = X.real * Y.imag + \
X.imag * Y.real ; \
Z.real = cmplx_tmp.real; \
Z.imag = cmplx_tmp.imag; \
}
#define cdiv(X, Y, Z) { \
struct cmplx_num cmplx_tmp; \
double den; \
den = sqr( Y.real ) + sqr( Y.imag ); \
cmplx_tmp.real = X.real * Y.real + \
X.imag * Y.imag ; \
cmplx_tmp.imag = X.imag * Y.real - \
X.real * Y.imag ; \
Z.imag = cmplx_tmp.imag / den; \
Z.real = cmplx_tmp.real / den; \
}
#define cmag(X) sqrt( sqr(X.real) + sqr(X.imag) )
main()
{
int type, design;
double fp, fs, f1, f2, f3, f4;
double wp, ws, w1, w2, w3, w4, wx, dw;
double Op, Os, O1, O2, O3, O4;
double ap, as;
double ap1, as1;
double M, eps, a;
double phi[MAXPOLES], start_angle, angle_incr;
double r, theta;
int n, i;
struct cmplx_num pole[MAXPOLES*2], zero[MAXPOLES*2];
struct cmplx_num tmp1, tmp2;
struct cmplx_num k, knum, kden;
fprintf(stderr, "Filter Design\n\n");
/* get filter type */
do
{
fprintf(stderr, "1) Low Pass\n");
fprintf(stderr, "2) High Pass\n");
fprintf(stderr, "3) Band Pass\n");
fprintf(stderr, "4) Band Stop\n\n");
fprintf(stderr, "Enter filter type: ");
scanf("%d", &type);
} while (type < 1 || type > 4);
/* get filter design type */
do
{
fprintf(stderr, "\n1) Butterworth\n");
fprintf(stderr, "2) Chebyshev\n");
fprintf(stderr, "3) Inverse Chebyshev\n");
fprintf(stderr, "4) Elliptical\n\n");
fprintf(stderr, "Enter filter design type: ");
scanf("%d", &design);
} while (design < 1 || design > 4);
/* get cutoff frequencies */
if (type == LP || type == HP)
{
fprintf(stderr, "\nEnter fp, fs: ");
scanf("%lf,%lf", &fp, &fs);
wp = 2*PI * fp;
ws = 2*PI * fs;
}
else
{
fprintf(stderr, "\nEnter f1, f2, f3, f4: ");
scanf("%lf,%lf,%lf,%lf", &f1, &f2, &f3, &f4);
w1 = 2*PI * f1;
w2 = 2*PI * f2;
w3 = 2*PI * f3;
w4 = 2*PI * f4;
}
/* get attenuation */
fprintf(stderr, "\nEnter ap, as: ");
scanf("%lf,%lf", &ap, &as);
printf("\nTodd Day's Filter Design Program\n");
printf( "--------------------------------\n");
printf("Designing a");
switch(design)
{
case BUTT:
printf(" Butterworth");
break;
case CHEB:
printf(" Chebyshev");
break;
case INVCH:
printf("n Inverse Chebyshev");
break;
case ELLIP:
printf("n Elliptical");
break;
}
switch(type)
{
case LP:
printf(" Low Pass");
break;
case HP:
printf(" High Pass");
break;
case BP:
printf(" Band Pass");
break;
case BS:
printf(" Band Stop");
break;
}
printf("filter\n");
switch(type)
{
case LP:
case HP:
printf("fp = %f, fs = %f", fp, fs);
break;
case BP:
case BS:
printf("f1 = %f, f2 = %f, f3 = %f, f4 = %f",
f1, f2, f3, f4);
break;
}
printf("\nap = %f, as = %f", ap, as);
/* these value are used a lot */
ap1 = pow( (double) 10, ap / (double) 10);
as1 = pow( (double) 10, as / (double) 10);
/* find "special pt" wx */
switch (type)
{
case LP:
case HP:
switch (design)
{
case BUTT:
wx = sqrt(wp * ws);
break;
case CHEB:
wx = wp;
break;
case INVCH:
wx = ws;
break;
case ELLIP:
wx = sqrt(wp * ws);
break;
}
break;
case BP:
case BS:
wx = sqrt(w1 * w4);
break;
}
printf("\n\nwx = %f\n", wx);
/* find Op, Os for LP baseband */
switch (type)
{
case LP:
Op = wp / wx;
Os = ws / wx;
break;
case HP:
Op = wx / wp;
Os = wx / ws;
break;
case BP:
Os = w4 - w1;
O2 = fabs( (w2*w2 - wx*wx) / w2);
O3 = fabs( (w3*w3 - wx*wx) / w3);
if (O2 > O3)
Op = O2;
else
Op = O3;
break;
case BS:
Op = 1 / (w4 - w1);
O2 = fabs( w2 / (w2*w2 - wx*wx) );
O3 = fabs( w3 / (w3*w3 - wx*wx) );
if (O2 < O3)
Os = O2;
else
Os = O3;
break;
}
printf("\nOp = %f, Os = %f\n", Op, Os);
/* now, filter is in LP' form */
/* but first, we must find order (M) */
/* also, find epsilon and a */
switch (design)
{
case BUTT:
M = log10( (as1 - 1) / (ap1 - 1) ) /
( 2 * log10(Os/Op) );
n = order(M);
break;
case CHEB:
M = invcosh( sqrt((as1 - 1) / (ap1 - 1)) ) /
( invcosh(Os/Op) );
eps = sqrt(ap1 - 1);
n = order(M);
a = 1/(double)n * invsinh( 1/eps );
break;
case INVCH:
M = invcosh( sqrt((as1 - 1) / (ap1 - 1)) ) /
( invcosh(Os/Op) );
eps = 1 / sqrt(as1 - 1 );
n = order(M);
a = 1/(double)n * invsinh( 1/eps );
break;
case ELLIP:
printf("\nSorry, not Implemented Yet!\n");
exit(1);
}
if (n > MAXPOLES)
{
printf("\nSorry, order too high (%d)!\n", n);
exit(1);
}
printf("\nOrder = %d, %f\n", n, M);
printf("eps = %f, a = %f\n", eps, a);
/* find dw for BP, BS filters */
if (type == BP || type == BS)
{
switch (design)
{
case BUTT:
{
double Oop, Oos;
Oop = wp / pow(ap1 - 1, 1 / (2*n));
Oos = ws / pow(as1 - 1, 1 / (2*n));
dw = sqrt(Oop * Oos);
}
case CHEB:
dw = Op;
break;
case INVCH:
dw = Os;
break;
case ELLIP:
dw = sqrt(Op * Os);
break;
}
if (type == BS)
dw = 1 / dw;
}
printf("\ndw = %f\n", dw);
/* find Butterworth angles */
angle_incr = PI / n;
start_angle = PI/2 - angle_incr/2;
printf("\nButterworth angles (degrees)\n");
for (i = 0; i < n; i++)
{
phi[i] = start_angle - i*angle_incr;
printf("%f\n", phi[i] / PI * 180);
}
/* now find pole and zero positions */
if (design == BUTT)
{
for (i = 0; i < n; i++)
{
pole[i].real = -wx * cos(phi[i]);
pole[i].imag = wx * sin(phi[i]);
zero[i].real = 0;
zero[i].imag = BIG;
}
}
else if (design == CHEB || design == INVCH)
{
for (i = 0; i < n; i++)
{ /* find the poles */
pole[i].real = -cos(phi[i]) * sinh(a);
pole[i].imag = sin(phi[i]) * cosh(a);
if (design == INVCH)
{ /* invert the poles */
invert(&pole[i]);
}
}
/* calculate the zeros */
if (design == CHEB)
{
for (i = 0; i < n; i++)
{
zero[i].real = 0;
zero[i].imag = BIG;
}
}
else if (design == INVCH)
{
for (i = 1; i < n; i += 2) /* 1,3,5... */
{
zero[i].real = 0;
zero[i-1].real = 0;
zero[i].imag = 1 / cos(i*PI / (2*n) );
zero[i-1].imag = -zero[i].imag;
}
if (n % 2) /* if n odd,fake it for calc purposes */
{
zero[n-1].real = 0;
zero[n-1].imag = BIG;
}
}
}
printf("\nLP Baseband Poles\n");
for (i = 0; i < n; i++)
printf("%g %g\n", pole[i].real, pole[i].imag);
printf("\nLP Baseband Zeroes\n");
for (i = 0; i < n; i++)
printf("%g %g\n", zero[i].real, zero[i].imag);
/* frequency transform */
if (type == LP)
{ /* s = S * wx */
for (i = 0; i < n; i++)
{
pole[i].real *= wx;
pole[i].imag *= wx;
if (design == INVCH || design == ELLIP)
{
zero[i].real *= wx;
zero[i].imag *= wx;
}
}
}
else if (type == HP)
{ /* s = wx / S */
for (i = 0; i < n; i++)
{
invert(&pole[i]);
pole[i].real *= wx;
pole[i].imag *= wx;
if (design == INVCH || design == ELLIP)
{
invert(&zero[i]);
zero[i].real *= wx;
zero[i].imag *= wx;
}
}
}
else if (type == BP)
{
for (i = 0; i < n; i++)
{
bpconvert(pole, dw, wx, i, n);
bpconvert(zero, dw, wx, i, n);
}
}
else if (type == BS)
{
for (i = 0; i < n; i++)
{
bsconvert(pole, dw, wx, i, n);
bsconvert(zero, dw, wx, i, n);
}
}
if (type == BP || type == BS)
n *= 2;
/* find k */
knum.real = 1;
knum.imag = 0;
kden.real = 1;
kden.imag = 0;
for (i = 0; i < n; i++)
{
cmul(knum, zero[i], knum);
cmul(kden, pole[i], kden);
}
cdiv(kden, knum, k);
printf("\n\nK multiplier\n");
printf("%g %g\n", k.real, k.imag);
printf("\nPoles\n");
for (i = 0; i < n; i++)
printf("%g %g\n", pole[i].real, pole[i].imag);
printf("\nZeros\n");
for (i = 0; i < n; i++)
printf("%g %g\n", zero[i].real, zero[i].imag);
}
invert(x)
struct cmplx_num *x;
{
double r, theta;
r = 1 / sqrt( sqr(x->real) + sqr(x->imag) );
if (x->real == 0)
{
if (x->imag < 0)
theta = 3*PI/2;
else
theta = PI/2;
}
else
theta = atan2(x->imag, x->real);
x->real = r * cos(theta);
x->imag = r * sin(theta);
}
imsqrt(x)
struct cmplx_num *x;
{
double r, theta;
r = sqrt( sqrt( sqr(x->real) + sqr(x->imag) ) );
if (x->real == 0)
{
if (x->imag < 0)
theta = 3*PI/2;
else
theta = PI/2;
}
else
{
theta = atan2(x->imag, x->real);
if (theta < 0)
theta += (2*PI);
}
theta /= 2;
x->real = r * cos(theta);
x->imag = r * sin(theta);
}
bpconvert(x, dw, wx, i, n)
struct cmplx_num x[];
double dw, wx;
int i, n;
{
struct cmplx_num tmp1, tmp2;
/* s^2 - dw*S*s + wx^2 = 0 */
/* -b */
tmp1.real = tmp2.real = dw * x[i].real;
tmp1.imag = tmp2.imag = dw * x[i].imag;
/* b*b */
cmul(tmp2, tmp2, tmp2);
/* b*b - 4*a*c */
tmp2.real -= (4*wx*wx);
/* sqrt (b*b - 4*a*c) */
imsqrt(&tmp2);
/* div by 2*a */
tmp1.real /= 2;
tmp1.imag /= 2;
tmp2.real /= 2;
tmp2.imag /= 2;
/* +/- */
x[i].real = tmp1.real + tmp2.real;
x[i].imag = tmp1.imag + tmp2.imag;
x[i+n].real = tmp1.real - tmp2.real;
x[i+n].imag = tmp1.imag - tmp2.imag;
}
bsconvert(x, dw, wx, i, n)
struct cmplx_num x[];
double dw, wx;
int i, n;
{
struct cmplx_num tmp1, tmp2, tmp3;
/* S*s^2 - dw*s + S*wx^2 = 0 */
/* -b */
tmp1.real = tmp2.real = dw;
tmp1.imag = tmp2.imag = 0;
/* b*b */
tmp2.real *= tmp2.real;
/* 4*a*c */
tmp3.real = x[i].real;
tmp3.imag = x[i].imag;
cmul(tmp3, tmp3, tmp3);
tmp3.real *= (4*wx*wx);
tmp3.imag *= (4*wx*wx);
/* b*b - 4*a*c */
tmp2.real -= tmp3.real;
tmp2.imag -= tmp3.imag;
/* sqrt (b*b - 4*a*c) */
imsqrt(&tmp2);
/* div by 2*a */
tmp3.real = 2 * x[i].real;
tmp3.imag = 2 * x[i].imag;
cdiv(tmp1, tmp3, tmp1);
cdiv(tmp2, tmp3, tmp2);
/* +/- */
x[i].real = tmp1.real + tmp2.real;
x[i].imag = tmp1.imag + tmp2.imag;
x[i+n].real = tmp1.real - tmp2.real;
x[i+n].imag = tmp1.imag - tmp2.imag;
}
These are the contents of the former NiCE NeXT User Group NeXTSTEP/OpenStep software archive, currently hosted by Netfuture.ch.