instf.c

dsp AD公司ADSP21的代码,里面有FFT FIR IIR EQULIZER G722_21F 等可以在项目中直接应用的代码.此代码的来源是ADI公司自己出版的书籍,此书在美国购得

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "rtdspc.h"

/* LMS Instantaneous Frequency Estimation Program 7-31-93 PME */

#define L 127       /* filter order, L+1 coefficients */
#define LMAX 200    /* max filter order, L+1 coefficients */
#define NEST 100    /* estimate decimation ratio in output */

/* FFT length must be a power of 2 */
#define FFT_LENGTH 1024
#define M 10           /* must be log2(FFT_LENGTH) */

/* set convergence parameter */
    float mu = 0.01;

void main()
{
    float lms(float,float,float *,int,float,float);
    static float b[LMAX];
    static COMPLEX samp[FFT_LENGTH];
    static float   mag[FFT_LENGTH];
    float x,d,tempflt,p1,p2;
    int i,j,k;

/* scale based on L */
    mu = 2.0*mu/(L+1);

    d = x = 0.0;
    for(;;) {
        for(i = 0 ; i < NEST ; i++) {
/* add noise to input for this example */
            x = getinput() + 100.0*gaussian();
            lms(x,d,b,L,mu,0.01);
/* delay d one sample */
            d = x;
        }

/* copy L+1 coefficients */
        for(i = 0 ; i <= L ; i++) {
            samp[i].real = b[i];
            samp[i].imag = 0.0;
        }

/* zero pad */
        for( ; i < FFT_LENGTH ; i++) {
            samp[i].real = 0.0;
            samp[i].imag = 0.0;
        }
    
        fft(samp,M);

        for(j = 0 ; j < FFT_LENGTH/2 ; j++) {
            tempflt  = samp[j].real * samp[j].real;
            tempflt += samp[j].imag * samp[j].imag;
            mag[j] = tempflt;
        }
/* find the biggest magnitude spectral bin and output */
        tempflt = mag[0];
        i=0;
        for(j = 1 ; j < FFT_LENGTH/2 ; j++) {
            if(mag[j] > tempflt) {
                tempflt = mag[j];
                i=j;
            }
        }
/* interpolate the peak loacation */
        if(i == 0) {
            p1 = p2 = 0.0;
        }
        else {
            p1 = mag[i] - mag[i-1];
            p2 = mag[i] - mag[i+1];
        }
        sendout(((float)i + 0.5*((p1-p2)/(p1+p2+1e-30)))/FFT_LENGTH);
    }
}

/*
      function lms(x,d,b,l,mu,alpha)

Implements NLMS Algorithm b(k+1)=b(k)+2*mu*e*x(k)/((l+1)*sig)

x      = input data
d      = desired signal
b[0:l] = Adaptive coefficients of lth order fir filter
l      = order of filter (> 1)
mu     = Convergence parameter (0.0 to 1.0)
alpha  = Forgetting factor   sig(k)=alpha*(x(k)**2)+(1-alpha)*sig(k-1)
         (>= 0.0 and < 1.0)

returns the filter output
*/

float lms(float x,float d,float *b,int l,
                  float mu,float alpha)
{
    int ll;
    float e,mu_e,lms_const,y;
    static float px[251];      /* max L = 250 */
    static float sigma = 1.41421; /* start at 2 and update internally */

    px[0]=x;

/* calculate filter output */
    y=b[0]*px[0];
    for(ll = 1 ; ll <= l ; ll++)
        y=y+b[ll]*px[ll];

/* error signal */
    e=d-y;

/* update sigma */
    sigma=alpha*(px[0]*px[0])+(1-alpha)*sigma;
    mu_e=mu*e/sigma;

/* update coefficients */
    for(ll = 0 ; ll <= l ; ll++)
        b[ll]=b[ll]+mu_e*px[ll];
/* update history */
    for(ll = l ; ll >= 1 ; ll--)
        px[ll]=px[ll-1];

    return(y);
}