filtering.c

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

/* blockcon.c - block convolution by overlap-add method */

void conv();

void blockcon(M, h, L, x, y, ytemp)
double *h, *x, *y, *ytemp;                    /* ytemp is tail of previous block */
int M, L;                                     /* \(M\) = filter order, \(L\) = block size */
{
    int i;

    conv(M, h, L, x, y);                      /* compute output block y */

    for (i=0; i<M; i++) {
        y[i] += ytemp[i];                     /* add tail of previous block */
        ytemp[i] = y[i+L];                    /* update tail for next call */
        }
}

/* can.c - IIR filtering in canonical form */

double can(M, a, L, b, w, x)              /* usage: y = can(M, a, L, b, w, x); */
double *a, *b, *w, x;                     /* \(w\) = internal state vector */
int M, L;                                 /* denominator and numerator orders */
{
       int K, i;
       double y = 0;

       K = (L <= M) ? M : L;              /* \(K=\max(M,L)\) */

       w[0] = x;                          /* current input sample */

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

       for (i=0; i<=L; i++)               /* output adder */
              y += b[i] * w[i];

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

       return y;                          /* current output sample */
}


/* can2.c - IIR filtering in canonical form */

double dot();
void delay();

double can2(M, a, L, b, w, x)             /* usage: y = can2(M, a, L, b, w, x); */
double *a, *b, *w, x;
int M, L;
{
       int K;
       double y;

       K = (L <= M) ? M : L;                     /* \(K=\max(M,L)\) */

       w[0] = 0;                                 /* needed for dot(M,a,w) */

       w[0] = x - dot(M, a, w);                  /* input adder */

       y = dot(L, b, w);                         /* output adder */

       delay(K, w);                              /* update delay line */

       return y;                                 /* current output sample */
}


/* can3.c - IIR filtering in canonical form, emulating a DSP chip */

double can3(M, a, b, w, x)                /* usage: y = can3(M, a, b, w, x); */
double *a, *b, *w, x;                     /* \(w\) = internal state vector */
int M;                                    /* \(a,b\) have common order \(M\) */
{
       int i;
       double y;

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

       for (i=1; i<=M; i++)                      /* forward order */
              w[0] -= a[i] * w[i];               /* MAC instruction */

       y = b[M] * w[M];

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

       return y;                                 /* output sample */
}


/* cas2can.c - cascade to canonical */

#include <stdlib.h>                                     /* declares calloc */

void conv();

void cas2can(K, A, a)                                   /* \(a\) is \((2K+1)\)-dimensional */
double **A, *a;                                         /* \(A\) is \(Kx3\) matrix */
int K;                                                  /* \(K\) = no. of sections */
{
       int i,j;
       double *d;

       d = (double *) calloc(2*K+1, sizeof(double));

       a[0] = 1;                                        /* initialize */

       for(i=0; i<K; i++) {
              conv(2, A[i], 2*i+1, a, d);               /* \(d = a[i] \ast a\) */
              for(j=0; j<2*i+3; j++)                    /* \(a = d\) */
                     a[j] = d[j];
              }
}


/* cas.c - IIR filtering in cascade of second-order sections */

double sos();                                    /* single second-order section */

double cas(K, A, B, W, x)
int K;
double **A, **B, **W, x;                         /* \(A,B,W\) are \(Kx3\) matrices */
{
       int i;
       double y;

       y = x;                                    /* initial input to first SOS */

       for (i=0; i<K; i++)
              y = sos(A[i], B[i], W[i], y);      /* output of \(i\)th section */

       return y;                                 /* final output from last SOS */
}



/* ccan.c - circular buffer implementation of canonical realization */

void wrap();                              /* defined in \ref{hardware.sec} */

double ccan(M, a, b, w, p, x)             /* usage: y=ccan(M, a, b, w, &p, x); */
double *a, *b, *w, **p, x;                /* \(p\) = circular pointer to buffer \(w\) */
int M;                                    /* \(a,b\) have common order \(M\) */
{
       int i;
       double y = 0, s0;

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

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

       for (a++, i=1; i<=M; i++) {              /* start with \(a\) incremented to \(a\sb{1}\) */
              s0 -= (*a++) * (*(*p)++);
              wrap(M, w, p);
              }

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

       for (i=0; i<=M; i++) {                   /* numerator part */
              y += (*b++) * (*(*p)++);
              wrap(M, w, p);                    /* upon exit, \(p\) has wrapped */
              }                                 /* around once again */

       (*p)--;                                  /* update circular delay line */
       wrap(M, w, p);

       return y;                                /* output sample */
}



/* ccan2.c - circular buffer implementation of canonical realization */

void wrap2();                        /* defined in Section \ref{hardware.sec} */

double ccan2(M, a, b, w, q, x)
double *a, *b, *w, x;                /* q = circular pointer offset index */
int M, *q;                           /* a,b have common order M */
{
       int i;
       double y = 0;

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

       for (i=1; i<=M; i++)
              w[*q] -= a[i] * w[(*q+i)%(M+1)];

       for (i=0; i<=M; i++)
              y += b[i] * w[(*q+i)%(M+1)];

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

       return y;                                /* output sample */
}


/* ccas.c - circular buffer implementation of cascade realization */

double csos();                          /* circular-buffer version of single SOS */

double ccas(K, A, B, W, P, x)
int K;
double **A, **B, **W, **P, x;           /* \(P\) = array of circular pointers */
{
       int i;
       double y;

       y = x;

       for (i=0; i<K; i++)
              y = csos(A[i], B[i], W[i], P+i, y);        /* note, \(P+i\) = &\(P[i]\) */

       return y;
}

/* ccas2.c - circular buffer implementation of cascade realization */

double csos2();                      /* circular-buffer version of single SOS */

double ccas2(K, A, B, W, Q, x)
int K, *Q;                           /* \(Q\) = array of circular pointer offsets */
double **A, **B, **W, x;       
{
       int i;
       double y;

       y = x;

       for (i=0; i<K; i++)
              y = csos2(A[i], B[i], W[i], Q+i, y);       /* note, \(Q+i\) = &\(Q[i]\) */

       return y;
}



/* cdelay.c - circular buffer implementation of D-fold delay */

void wrap();

void cdelay(D, w, p)
int D;
double *w, **p;
{
       (*p)--;                      /* decrement pointer and wrap modulo-\((D+1)\) */
       wrap(D, w, p);               /* when \(*p=w-1\), it wraps around to \(*p=w+D\) */
}



/* cdelay2.c - circular buffer implementation of D-fold delay */

void wrap2();

void cdelay2(D, q)
int D, *q;
{
       (*q)--;                      /* decrement offset and wrap modulo-\((D+1)\) */
       wrap2(D, q);                 /* when \(*q=-1\), it wraps around to \(*q=D\) */
}


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

void wrap();

double cfir(M, h, w, p, x)
double *h, *w, **p, x;                         /* \(p\) = circular pointer to \(w\) */
int M;                                         /* \(M\) = filter order */
{                        
       int i;
       double y;

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

       for (y=0, i=0; i<=M; i++) {             /* compute output sample \(y\) */
              y += (*h++) * (*(*p)++);
              wrap(M, w, p);
              }

       (*p)--;                                 /* update circular delay line */
       wrap(M, w, p);

       return y;
}


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

void wrap();

double cfir1(M, h, w, p, x)
double *h, *w, **p, x;
int M;