lpcfloat.c

speech signal process tools

		s -= a[j] * r[i-j];
	}
	k[i] = ( s - r[i+1] )/e;
	a[i] = k[i];
	for ( j=0; j<=i; j++){
		b[j] = a[j];
	}
	for ( j=0; j<i; j++){
		a[j] += k[i] * b[i-j-1];
	}
	e *= ( 1. - (k[i] * k[i]) );
    }
    *ex = e;
}

a_to_aca ( a, b, c, p )
double *a, *b, *c;
register int p;
/*  Compute the autocorrelations of the p LP coefficients in a. 
 *  (a[0] is assumed to be = 1 and not explicitely accessed.)
 *  The magnitude of a is returned in c.
 *  2* the other autocorrelation coefficients are returned in b.
 */
{
    double		s;
    register short	i, j, pm;
	for ( s=1., i = 0; i < p; i++ ){
		s += (a[i] * a[i]);
	}
	*c = s;
	pm = p-1;
	for ( i = 0; i < p; i++){
		s = a[i];
		for ( j = 0; j < (pm-i); j++){
			s += (a[j] * a[j+i+1]);
		}
		b[i] = 2. * s;
	}

}

double itakura ( p, b, c, r, gain )
register double *b, *c, *r, *gain;
register int p;
/* Compute the Itakura LPC distance between the model represented
 * by the signal autocorrelation (r) and its residual (gain) and
 * the model represented by an LPC autocorrelation (c, b).
 * Both models are of order p.
 * r is assumed normalized and r[0]=1 is not explicitely accessed.
 * Values returned by the function are >= 1.
 */
 {
     double s;
     register int i;
     for( s= *c, i=0; i< p; i++){
	s += r[i] * b[i];
    }
    return (s/ *gain);
}

lpc(lpc_ord,lpc_stabl,wsize,data,lpca,ar,lpck,normerr,rms,preemp,type)
     int lpc_ord, wsize, type;
     double lpc_stabl, *lpca, *ar, *lpck, *normerr, *rms, preemp;
     short *data;
{
  static double *dwind=NULL;
  static int nwind=0;
  double rho[MAXORDER+1], k[MAXORDER], a[MAXORDER+1],*r,*kp,*ap,en,er;
  double wfact = 1.0;

  if((wsize <= 0) || (!data) || (lpc_ord > MAXORDER)) return(FALSE);
  
  if(nwind != wsize) {
    if(dwind) dwind = (double*)realloc(dwind,wsize*sizeof(double));
    else dwind = (double*)malloc(wsize*sizeof(double));
    if(!dwind) {
      printf("Can't allocate scratch memory in lpc()\n");
      return(FALSE);
    }
    nwind = wsize;
  }
  
  w_window(data, dwind, wsize, preemp, type);
  if(!(r = ar)) r = rho;
  if(!(kp = lpck)) kp = k;
  if(!(ap = lpca)) ap = a;
  autoc( wsize, dwind, lpc_ord, r, &en );
  if(lpc_stabl > 1.0) { /* add a little to the diagonal for stability */
    int i;
    double ffact;
    ffact =1.0/(1.0 + exp((-lpc_stabl/20.0) * log(10.0)));
    for(i=1; i <= lpc_ord; i++) rho[i] = ffact * r[i];
    *rho = *r;
    r = rho;
    if(ar)
      for(i=0;i<=lpc_ord; i++) ar[i] = r[i]; /* copy out for possible use later */
  }
  durbin ( r, kp, &ap[1], lpc_ord, &er);

/* After talkin with David T., I decided to take the following correction
out -- if people window, the resulting output (spectrum and power) should
correspond to the windowed data, and we shouldn't try to back-correct
to un-windowed data. */

/*  switch(type) {		/* rms correction for window */
/*   case 0:
    wfact = 1.0;		/* rectangular */
/*    break;
  case 1:
    wfact = .630397;		/* Hamming */
/*    break;
  case 2:
    wfact = .443149;		/* (.5 - .5*cos)^4 */
/*    break;
  case 3:
    wfact = .612372;		/* Hanning */
/*    break;
  }
*/

  *ap = 1.0;
  if(rms) *rms = en/wfact;
  if(normerr) *normerr = er;
  return(TRUE);
}

/* convert reflection (PARCOR) coefficients to areas */
dreflar(c,a,n) double *c,*a; int n;{
/*	refl to area			*/
	register double *pa,*pa1,*pc,*pcl;
	pa = a + 1;	pa1 = a;
	*a = 1.;
	for(pc=c,pcl=c+ n;pc<pcl;pc++)
	*pa++ = *pa1++ * (1+*pc)/(1-*pc);
	}

/* covariance LPC analysis; originally from Markel and Gray */
/* (a translation from the fortran) */

w_covar(xx,m,n,istrt,y,alpha,r0,preemp,w_type)
     double *y, *alpha, *r0, preemp;
     short *xx;
     int *m,n,istrt;
     int w_type;
{
  static double *x=NULL;
  static int nold = 0, ofile = -1;
  static mold = 0;
  static double *b = NULL, *beta = NULL, *grc = NULL, *cc = NULL, gam,s;
 int ibeg, ibeg1, ibeg2, ibegm2, ibegmp, np0, ibegm1, msq, np, np1, mf, jp, ip,
     mp, i, j, minc, n1, n2, n3, npb, msub, mm1, isub, m2;
  int mnew = 0;

  if((n+1) > nold) {
    if(x) free(x);
    x = NULL;
    if(!(x = (double*)calloc((n+1), sizeof(double)))) {
      printf("Allocation failure in w_covar()\n");
      return(FALSE);
    }
    nold = n+1;
  }

  if(*m > mold) {
    if(b) free(b); if(beta) free(beta); if (grc) free(grc); if (cc) free(cc);
    b = beta = grc = cc = NULL;
    mnew = *m;
    
    if(!((b = (double*)malloc(sizeof(double)*((mnew+1)*(mnew+1)/2))) && 
       (beta = (double*)malloc(sizeof(double)*(mnew+3)))  &&
       (grc = (double*)malloc(sizeof(double)*(mnew+3)))  &&
       (cc = (double*)malloc(sizeof(double)*(mnew+3)))))   {
      printf("Allocation failure in w_covar()\n");
      return(FALSE);
    }
    mold = mnew;
  }

  w_window(xx, x, n, preemp, w_type);  

 ibeg = istrt - 1;
 ibeg1 = ibeg + 1;
 mp = *m + 1;
 ibegm1 = ibeg - 1;
 ibeg2 = ibeg + 2;
 ibegmp = ibeg + mp;
 i = *m;
 msq = ( i + i*i)/2;
 for (i=1; i <= msq; i++) b[i] = 0.0;
 *alpha = 0.0;
 cc[1] = 0.0;
 cc[2] = 0.0;
 for(np=mp; np <= n; np++) {
   np1 = np + ibegm1;
   np0 = np + ibeg;
   *alpha += x[np0] * x[np0];
   cc[1] += x[np0] * x[np1];
   cc[2] += x[np1] * x[np1];
 }
 *r0 = *alpha;
 b[1] = 1.0;
 beta[1] = cc[2];
 grc[1] = -cc[1]/cc[2];
 y[0] = 1.0;
 y[1] = grc[1];
 *alpha += grc[1]*cc[1];
 if( *m <= 1) return(FALSE);		/* need to correct indices?? */
 mf = *m;
 for( minc = 2; minc <= mf; minc++) {
   for(j=1; j <= minc; j++) {
     jp = minc + 2 - j;
     n1 = ibeg1 + mp - jp;
     n2 = ibeg1 + n - minc;
     n3 = ibeg2 + n - jp;
     cc[jp] = cc[jp - 1] + x[ibegmp-minc]*x[n1] - x[n2]*x[n3];
   }
   cc[1] = 0.0;
   for(np = mp; np <= n; np++) {
     npb = np + ibeg;
     cc[1] += x[npb-minc]*x[npb];
   }
   msub = (minc*minc - minc)/2;
   mm1 = minc - 1;
   b[msub+minc] = 1.0;
   for(ip=1; ip <= mm1; ip++) {
     isub = (ip*ip - ip)/2;
     if(beta[ip] <= 0.0) {
       *m = minc-1;
       return(TRUE);
     }
     gam = 0.0;
     for(j=1; j <= ip; j++)
       gam += cc[j+1]*b[isub+j];
     gam /= beta[ip];
     for(jp=1; jp <= ip; jp++)
       b[msub+jp] -= gam*b[isub+jp];
   }
   beta[minc] = 0.0;
   for(j=1; j <= minc; j++)
     beta[minc] += cc[j+1]*b[msub+j];
   if(beta[minc] <= 0.0) {
     *m = minc-1;
     return(TRUE);
   }
   s = 0.0;
   for(ip=1; ip <= minc; ip++)
     s += cc[ip]*y[ip-1];
   grc[minc] = -s/beta[minc];
   for(ip=1; ip < minc; ip++) {
     m2 = msub+ip;
     y[ip] += grc[minc]*b[m2];
   }
   y[minc] = grc[minc];
   s = grc[minc]*grc[minc]*beta[minc];
   *alpha -= s;
   if(*alpha <= 0.0) {
     if(minc < *m) *m = minc;
     return(TRUE);
   }
 }
}

/* Same as above, but returns alpha as a function of order. */
covar2(xx,m,n,istrt,y,alpha,r0,preemp)
     double *y, *alpha, *r0, preemp;
     short *xx;
     int *m,n,istrt;
{
  static double *x=NULL;
  static int nold = 0;
  double b[513],beta[33],grc[33],cc[33],gam,s;
  int ibeg, ibeg1, ibeg2,ibegm2, ibegmp, np0, ibegm1, msq, np, np1, mf, jp, ip,
     mp, i, j, minc, n1, n2, n3, npb, msub, mm1, isub, m2;

  if((n+1) > nold) {
    if(x) free(x);
    x = NULL;
    if(!(x = (double*)malloc(sizeof(double)*(n+1)))) {
      printf("Allocation failure in covar2()\n");
      return(FALSE);
    }
    nold = n+1;
  }

 for(i=1; i <= n; i++) x[i] = (double)xx[i] - preemp * xx[i-1];
 ibeg = istrt - 1;
 ibeg1 = ibeg + 1;
 mp = *m + 1;
 ibegm1 = ibeg - 1;
 ibeg2 = ibeg + 2;
 ibegmp = ibeg + mp;
 i = *m;
 msq = ( i + i*i)/2;
 for (i=1; i <= msq; i++) b[i] = 0.0;
 *alpha = 0.0;
 cc[1] = 0.0;
 cc[2] = 0.0;
 for(np=mp; np <= n; np++) {
   np1 = np + ibegm1;
   np0 = np + ibeg;
   *alpha += x[np0] * x[np0];
   cc[1] += x[np0] * x[np1];
   cc[2] += x[np1] * x[np1];
 }
 *r0 = *alpha;
 b[1] = 1.0;
 beta[1] = cc[2];
 grc[1] = -cc[1]/cc[2];
 y[0] = 1.0;
 y[1] = grc[1];
 *alpha += grc[1]*cc[1];
 if( *m <= 1) return;		/* need to correct indices?? */
 mf = *m;
 for( minc = 2; minc <= mf; minc++) {
   for(j=1; j <= minc; j++) {
     jp = minc + 2 - j;
     n1 = ibeg1 + mp - jp;
     n2 = ibeg1 + n - minc;
     n3 = ibeg2 + n - jp;
     cc[jp] = cc[jp - 1] + x[ibegmp-minc]*x[n1] - x[n2]*x[n3];
   }
   cc[1] = 0.0;
   for(np = mp; np <= n; np++) {
     npb = np + ibeg;
     cc[1] += x[npb-minc]*x[npb];
   }
   msub = (minc*minc - minc)/2;
   mm1 = minc - 1;
   b[msub+minc] = 1.0;
   for(ip=1; ip <= mm1; ip++) {
     isub = (ip*ip - ip)/2;
     if(beta[ip] <= 0.0) {
       *m = minc-1;
       return;
     }
     gam = 0.0;
     for(j=1; j <= ip; j++)
       gam += cc[j+1]*b[isub+j];
     gam /= beta[ip];
     for(jp=1; jp <= ip; jp++)
       b[msub+jp] -= gam*b[isub+jp];
   }
   beta[minc] = 0.0;
   for(j=1; j <= minc; j++)
     beta[minc] += cc[j+1]*b[msub+j];
   if(beta[minc] <= 0.0) {
     *m = minc-1;
     return;
   }
   s = 0.0;
   for(ip=1; ip <= minc; ip++)
     s += cc[ip]*y[ip-1];
   grc[minc] = -s/beta[minc];
   for(ip=1; ip < minc; ip++) {
     m2 = msub+ip;
     y[ip] += grc[minc]*b[m2];
   }
   y[minc] = grc[minc];
   s = grc[minc]*grc[minc]*beta[minc];
   alpha[minc-1] = alpha[minc-2] - s;
   if(alpha[minc-1] <= 0.0) {
     if(minc < *m) *m = minc;
     return;
   }
 }
}
   
	 
/*      ----------------------------------------------------------      */
/* Find the roots of the LPC denominator polynomial and convert the z-plane
	zeros to equivalent resonant frequencies and bandwidths.	*/
/* The complex poles are then ordered by frequency.  */
formant(lpc_order,s_freq,lpca,n_form,freq,band,init)
int	lpc_order, /* order of the LP model */
	*n_form,   /* number of COMPLEX roots of the LPC polynomial */
	init; 	   /* preset to true if no root candidates are available */
double	s_freq,    /* the sampling frequency of the speech waveform data */
	*lpca, 	   /* linear predictor coefficients */
	*freq,     /* returned array of candidate formant frequencies */
	*band;     /* returned array of candidate formant bandwidths */
{
  double  x, flo, pi2t, theta;
  static double  rr[MAXORDER], ri[MAXORDER];
  int	i,ii,iscomp1,iscomp2,fc,swit;

  if(init){ /* set up starting points for the root search near unit circle */
    x = M_PI/(lpc_order + 1);
    for(i=0;i<=lpc_order;i++){
      flo = lpc_order - i;
      rr[i] = 2.0 * cos((flo + 0.5) * x);
      ri[i] = 2.0 * sin((flo + 0.5) * x);
    }
  }
  if(! lbpoly(lpca,lpc_order,rr,ri)){ /* find the roots of the LPC polynomial */
    *n_form = 0;		/* was there a problem in the root finder? */
    return(FALSE);
  }

  pi2t = M_PI * 2.0 /s_freq;

  /* convert the z-plane locations to frequencies and bandwidths */
  for(fc=0, ii=0; ii < lpc_order; ii++){
    if((rr[ii] != 0.0)||(ri[ii] != 0.0)){
      theta = atan2(ri[ii],rr[ii]);
      freq[fc] = fabs(theta / pi2t);
      if((band[fc] = 0.5 * s_freq *
	  log(((rr[ii] * rr[ii]) + (ri[ii] * ri[ii])))/M_PI) < 0.0)
	band[fc] = -band[fc];
      fc++;			/* Count the number of real and complex poles. */

      if((rr[ii] == rr[ii+1])&&(ri[ii] == -ri[ii+1]) /* complex pole? */
	 && (ri[ii] != 0.0)) ii++; /* if so, don't duplicate */
    }
  }


  /* Now order the complex poles by frequency.  Always place the (uninteresting)
     real poles at the end of the arrays. 	*/
  theta = s_freq/2.0;		/* temporarily hold the folding frequency. */
  for(i=0; i < fc -1; i++){	/* order the poles by frequency (bubble) */
    for(ii=0; ii < fc -1 -i; ii++){
      /* Force the real poles to the end of the list. */
      iscomp1 = (freq[ii] > 1.0) && (freq[ii] < theta);
      iscomp2 = (freq[ii+1] > 1.0) && (freq[ii+1] < theta);
      swit = (freq[ii] > freq[ii+1]) && iscomp2 ;
      if(swit || (iscomp2 && ! iscomp1)){
	flo = band[ii+1];
	band[ii+1] = band[ii];
	band[ii] = flo;
	flo = freq[ii+1];
	freq[ii+1] = freq[ii];
	freq[ii] = flo;
      }
    }
  }
  /* Now count the complex poles as formant candidates. */
  for(i=0, theta = theta - 1.0, ii=0 ; i < fc; i++)
    if( (freq[i] > 1.0) && (freq[i] < theta) ) ii++;
  *n_form = ii;

  return(TRUE);
}



/*-----------------------------------------------------------------------*/
log_mag(x,y,z,n)
			/* z <- (10 * log10(x^2 + y^2))  for n elements */
double	*x, *y, *z;
int	n;
{
register double	*xp, *yp, *zp, t1, t2, ssq;

    if(x && y && z && n) {
        for(xp=x+n, yp=y+n, zp=z+n; zp > z;) {
	    t1 = *--xp;
	    t2 = *--yp;
	    ssq = (t1*t1)+(t2*t2);
	    *--zp = (ssq > 0.0)? 10.0 * log10(ssq) : -200.0;
	}
	return(TRUE);
    } else {
	return(FALSE);
    }
}

/*-----------------------------------------------------------------------*/
flog_mag(x,y,z,n)
			/* z <- (10 * log10(x^2 + y^2))  for n elements */
float	*x, *y, *z;
int	n;
{
register float	*xp, *yp, *zp, t1, t2, ssq;

    if(x && y && z && n) {
        for(xp=x+n, yp=y+n, zp=z+n; zp > z;) {
	    t1 = *--xp;
	    t2 = *--yp;
	    ssq = (t1*t1)+(t2*t2);
	    *--zp = (ssq > 0.0)? 10.0 * log10((double)ssq) : -200.0;
	}
	return(TRUE);
    } else {
	return(FALSE);
    }
}

#ifdef USE_OLD_FFT
#include	"thetable.c"	/*  table of sines and cosines */
/*-----------------------------------------------------------------------*/
fft ( l, x, y )
int l;
double *x, *y;
/* Compute the discrete Fourier transform of the 2**l complex sequence
 * in x (real) and y (imaginary).  The DFT is computed in place and the
 * Fourier coefficients are returned in x and y.
 */
{
register double	c, s,  t1, t2;
register int	j1, j2, li, lix, i;
int	np, lmx, lo, lixnp, lm, j, nv2, k, im, jm;