filtering.c

一些经典的数字信号处理算法方面的c源代码。

{                        
       int i;
       double y;

       *(*p)-- = x;
       wrap(M, w, p);                          /* \(p\) now points to \(s\sb{M}\) */

       for (y=0, h+=M, i=M; i>=0; i--) {       /* \(h\) starts at \(h\sb{M}\) */
              y += (*h--) * (*(*p)--);
              wrap(M, w, p);
              }

       return y;
}



/* cfir2.c - FIR filter implemented with circular delay-line buffer */

void wrap2();

double cfir2(M, h, w, q, x)
double *h, *w, x;                                /* \(q\) = circular offset index */
int M, *q;                                       /* \(M\) = filter order */
{                        
       int i;
       double y;

       w[*q] = x;                                /* read input sample \(x\) */

       for (y=0, i=0; i<=M; i++) {               /* compute output sample \(y\) */
              y += (*h++) * w[(*q)++];
              wrap2(M, q);
              }

       (*q)--;                                   /* update circular delay line */
       wrap2(M, q);
       
       return y;
}


/* conv.c - convolution of x[n] with h[n], resulting in y[n] */

#include <stdlib.h>                       /* defines max( ) and min( ) */

void conv(M, h, L, x, y)
double *h, *x, *y;                        /* \(h,x,y\) = filter, input, output arrays */
int M, L;                                 /* \(M\) = filter order, \(L\) = input length */
{
       int n, m;

       for (n = 0; n < L+M; n++)
              for (y[n] = 0, m = max(0, n-L+1); m <= min(n, M); m++)
                     y[n] += h[m] * x[n-m];
}


/* csos.c - circular buffer implementation of a single SOS */

void wrap();

double csos(a, b, w, p, x)                      /* \(a,b,w\) are 3-dimensional */
double *a, *b, *w, **p, x;                      /* \(p\) is circular pointer to \(w\) */
{
       double y, s0;

       *(*p) = x;                               /* read input sample \(x\) */

       s0  = *(*p)++;           wrap(2, w, p);
       s0 -= a[1] * (*(*p)++);  wrap(2, w, p);
       s0 -= a[2] * (*(*p)++);  wrap(2, w, p);

       *(*p) = s0;                              /* \(p\) has wrapped around once */

       y  = b[0] * (*(*p)++);  wrap(2, w, p);
       y += b[1] * (*(*p)++);  wrap(2, w, p);
       y += b[2] * (*(*p));                     /* \(p\) now points to \(s\sb{2}\) */

       return y;
}


/* csos2.c - circular buffer implementation of a single SOS */

void wrap2();

double csos2(a, b, w, q, x)
double *a, *b, *w, x;                   /* \(a,b,w\) are 3-dimensional arrays */
int *q;                                 /* \(q\) is circular offset relative to \(w\) */
{
       double y;

       w[*q] = x - a[1] * w[(*q+1)%3] - a[2] * w[(*q+2)%3];

       y = b[0] * w[*q] + b[1] * w[(*q+1)%3] + b[2] * w[(*q+2)%3];

       (*q)--;  
       wrap2(2, q);

       return y;
}


/* delay.c - delay by D time samples */

void delay(D, w)                          /* \(w[0]\) = input, \(w[D]\) = output */
int D;
double *w;
{
       int i;

       for (i=D; i>=1; i--)               /* reverse-order updating */
              w[i] = w[i-1];

}

/* dir.c - IIR filtering in direct form */

double dir(M, a, L, b, w, v, x)           /* usage: y = dir(M, a, L, b, w, v, x); */
double *a, *b, *w, *v, x;                 /* \(v,w\) are internal states */
int M, L;                                 /* denominator and numerator orders */
{
       int i;

       v[0] = x;                          /* current input sample */
       w[0] = 0;                          /* current output to be computed */

       for (i=0; i<=L; i++)               /* numerator part */
              w[0] += b[i] * v[i];

       for (i=1; i<=M; i++)               /* denominator part */
              w[0] -= a[i] * w[i];

       for (i=L; i>=1; i--)               /* reverse-order updating of \(v\) */
              v[i] = v[i-1];

       for (i=M; i>=1; i--)               /* reverse-order updating of \(w\) */
              w[i] = w[i-1];

       return w[0];                       /* current output sample */
}



/* dir2.c - IIR filtering in direct form */

double dot();
void delay();

double dir2(M, a, L, b, w, v, x)          /* usage: y = dir2(M, a, L, b, w, v, x); */
double *a, *b, *w, *v, x;
int M, L;
{
       v[0] = x;                                 /* current input sample */
       w[0] = 0;                                 /* needed for dot(M,a,w) */

       w[0] = dot(L, b, v) - dot(M, a, w);       /* current output */

       delay(L, v);                              /* update input delay line */

       delay(M, w);                              /* update output delay line */

       return w[0];
}


/* fir.c - FIR filter in direct form */

double fir(M, h, w, x)                       /* Usage: y = fir(M, h, w, x); */
double *h, *w, x;                            /* \(h\) = filter, \(w\) = state, \(x\) = input sample */
int M;                                       /* \(M\) = filter order */
{                        
       int i;
       double y;                             /* output sample */

       w[0] = x;                             /* read current input sample \(x\) */

       for (y=0, i=0; i<=M; i++)
              y += h[i] * w[i];              /* compute current output sample \(y\) */

       for (i=M; i>=1; i--)                  /* update states for next call */
              w[i] = w[i-1];                 /* done in reverse order */

       return y;
}



/* fir2.c - FIR filter in direct form */

double dot();
void delay();

double fir2(M, h, w, x)                       /* Usage: y = fir2(M, h, w, x); */
double *h, *w, x;                             /* \(h\) = filter, \(w\) = state, \(x\) = input */
int M;                                        /* \(M\) = filter order */
{                        
       double y;

       w[0] = x;                              /* read input */

       y = dot(M, h, w);                      /* compute output */

       delay(M, w);                           /* update states */

       return y;
}



/* fir3.c - FIR filter emulating a DSP chip */

double fir3(M, h, w, x)
double *h, *w, x;
int M;
{                        
       int i;
       double y;

       w[0] = x;                                 /* read input */

       for (y=h[M]*w[M], i=M-1; i>=0; i--) {
              w[i+1] = w[i];                     /* data shift instruction */
              y += h[i] * w[i];                  /* MAC instruction */
              }

       return y;
}


/* sos.c - IIR filtering by single second order section */

double sos(a, b, w, x)                    /* \(a, b, w\) are 3-dimensional */
double *a, *b, *w, x;                     /* \(a[0]=1\) always */
{
       double y;

       w[0] = x - a[1] * w[1] - a[2] * w[2];
       y = b[0] * w[0] + b[1] * w[1] + b[2] * w[2];

       w[2] = w[1];
       w[1] = w[0];

       return y;
}


/* tap.c - i-th tap of circular delay-line buffer */

double tap(D, w, p, i)                    /* usage: si = tap(D, w, p, i); */
double *w, *p;                            /* \(p\) passed by value */
int D, i;                                 /* \(i=0,1,\dotsc, D\) */
{
       return w[(p - w + i) % (D + 1)];
}




/* tap2.c - i-th tap of circular delay-line buffer */

double tap2(D, w, q, i)                   /* usage: si = tap2(D, w, q, i); */
double *w;
int D, q, i;                              /* \(i=0,1,\dotsc,D\) */
{
       return w[(q + i) % (D + 1)];
}



/* wrap.c - circular wrap of pointer p, relative to array w */

void wrap(M, w, p)
double *w, **p;
int M;
{
       if (*p > w + M)  
              *p -= M + 1;          /* when \(*p=w+M+1\), it wraps around to \(*p=w\) */

       if (*p < w)  
              *p += M + 1;          /* when \(*p=w-1\), it wraps around to \(*p=w+M\) */
}


/* wrap2.c - circular wrap of pointer offset q, relative to array w */

void wrap2(M, q)
int M, *q;
{
       if (*q > M)  
              *q -= M + 1;          /* when \(*q=M+1\), it wraps around to \(*q=0\) */

       if (*q < 0)  
              *q += M + 1;          /* when \(*q=-1\), it wraps around to \(*q=M\) */
}