wilson.c

speech signal process tools

/*	

 This material contains proprietary software of Entropic Processing, Inc.
 Any reproduction, distribution, or publication without the the prior	   
 written permission of Entropic Processing, Inc. is strictly prohibited.
 Any public distribution of copies of this work authorized in writing by
 Entropic Processing, Inc. must bear the notice			
       
	Copyright 1986, Entropic Proccessing, Inc
	(C) 1985, Entropic Processing, Inc.
         */

#ifdef SCCS
static char *sccsid = "@(#)wilson.c	1.2 4/28/86";
#endif
	

/*			WILSON				*/
/*		John's improved version			*/
/*	C subroutine that corresponds to newils.f	*/


#include <stdio.h>
#define MAXSIZ 800
#define FILTSZ ((MAXSIZ*(MAXSIZ+1))/2)
#define ZERO 0.0
#define ONE 1.0e0
extern debug_level;

wilson(c,size,th_init,init_siz,theta,rfl_coef,eps)
double *c,*th_init,*theta,*rfl_coef,eps;
int size,init_siz;

/*
    FACTORIZATION OF THE COVARIANCE OF A PURE MOVING AVERAGE PROCESS
(new algorithm modified by John P. Burg that avoids 
     the matrix inversion)

Ref : " Factorization of the covariance generating function
	of a pure moving average process "
	G. Wilson ; SIAM J. Numerical Analysis
	Vol 6, No 1, March 1969

The c vector represents the coefficients of the covariance 
	generating function of a pure M.A process.The
	polynomial is symmetric : c(k)=c(-k) 0<=k<=M
The Theta vector represents the coefficients of the moving 
	average process 

       Find theta , knowing c  such that c(Z)=theta(Z)*theta(1/Z)
M is the order of the filter
q is the previous estimation of theta and is used to compute
	theta: the new estimation
filt is the vector containing the coeffients of all the 
prediction error filters of order less than or equal to M.

rlf_coef[0] returns with the value of gain of the inverse filter
*/

/*
  WARNING 1 :
the c vector is assumed to correspond to actual correlation
values and is thus not being tested here in this respect.
*/

/*
  WARNING 2 :
The resulting polynomial has all its zeros OUTSIDE the unit
circle and is thus a minimum phase filter if one
considers it as a polynomial in Z.
Otherwise one just has to flip the coefficients around : i.e.
	theta(k)<->theta(M-k)

the iteration stops when the normalized standard deviation
  between q and theta as given by the stddev subroutine is
 	  less than epsilon.
*/

{
    double  tmp, q[MAXSIZ], stdv, R[MAXSIZ + MAXSIZ], S[MAXSIZ];
    double  filt[FILTSZ];
    int     iter, M, tM, sigM, i;
    double  sqrt (), stddev ();


/*       INITIALIZATION    */

    if (size > MAXSIZ)
	faterr ("wilson", "the dimension of the filter is too big", 1002);
    else if (size <= 0)
	faterr ("wilson", "the dimension of the filter is negative", 1003);
    else if (size == 1) {	/* trivial case */
	fprintf (stderr, " wilson : trivial case; size = 1");
	if (c[0] <= 0.0)
	    faterr ("wilson", " c[0] is negative ", 1004);
	else {
	    theta[0] = sqrt (c[0]);
	    rfl_coef[0] = ONE / c[0];
	}
	return (0);
    }

    M = size-1;			/* M is the order of the polynomial */
    tM = M + M;
    sigM = M * (M + 1) / 2;

    if (init_siz == 0) {	/*  use default initialization  */
    /* printf("default initialization      "); */
	for (i = 1, tmp = ZERO; i <= M; i++) {
	    tmp += c[i];
	    q[i] = ZERO;
	}
	q[0] = c[0] + tmp + tmp;
	if (q[0] <= ZERO) {
	    fprintf (stderr, "GASP : q[0]<0   q[0] = %lg \n", q[0]);
	    fprintf (stderr, " try again ! \n");
	    return (4);
	}
	q[0] = sqrt (q[0]);
    /* printf(" q[0] = %lg \n",q[0]); */
/*     this is an optimal initial value for theta according to Wilson */
    }
    else			/* use the array th_init of size init_siz 
				*/
	shiftar (q, th_init, init_siz);
/*
printf(" wilson alg initial guess\n");
for(i=0 ; i<=M ; i++)
	printf(" q[%d] = %lg \n",i,q[i]);
*/


/*       MAIN LOOP    */

    stdv = 0.0;
    for (iter = 1;; iter++) {
	/*
	if (debug_level >= 2)
	    printf("wilson: iteration %d   stdv= %g\n",iter,stdv);
	*/
	if (q[0] <= ZERO) {
	    fprintf (stderr, "GASP : q[0]<0   q[0] = %lg \n", q[0]);
	    fprintf (stderr, " try again ! \n");
	    return (2);
	}
	if (baclev (q, rfl_coef, M, R, tM, filt, sigM) != 0)
	    return (1);
	compuS (R, tM, c, S, M);
	mkthta (S, q, theta, M);

	stdv = stddev (theta, q, M);
	if (stdv < eps)
	    break;

	shiftar (q, theta, size);
    }
/*
printf("last standard deviation on theta : %g   after %d iterations\n",stdv,iter);
*/
    check_wils (q, theta, M);
    stdv = stddev (c, q, M);
/*
printf(" standard deviation on check : %g\n",stdv);
*/
    return (0);
}

/*      -------------------------------------------------------    */

baclev(q,rflcof,m,r,tm,fil,sm)
double  q[],rflcof[],r[],fil[];
int  m,tm,sm;

/*
"Backwards" Levinson algorithm :    
         	Given a minimum phase filter q of order m, compute    
    	the autocorrelation values R out to    
    	lag tm(=2m)corresponding to the inverse filter.    
    	fil contains all the coefficients of the prediction error     
    	filters in increasing order.    
At the same time, get the reflection coefficients rflcof;
The gain of the filter is returned as r[0]=rflcof[0]
*/

{
register int  k,km1,find,ff,fb,j;
double  gain,cof,oocsq,temp;


/*
the coefficients of the successive prediction error filters are 
    	stored, in the array fil, in the following way : 
    	- in increasing order of 'order of the filter'    
    	- for each filter, in increasing order of 'coefficient order'
    	  the coefficient of order 0 is never stored since it is    
    		always equal to 1.    
    	  i.e. fil(1) is the coefficient of the filter of order 1    
    		the second order prediction error filter is equal to   
    		1+ fil(2).Z + fil(3).Z    
    	find is the running indice for fil.    
*/

/*        INITIALIZATION    */

/*  printf("baclev\n");  */
if (tm != m+m)
	faterr("baclev","tm != 2m",1101);
if (sm != m*(m+1)/2)
	faterr("baclev","sm != m*(m+1)/2",1102);

/* normalize the filter so that the coefficient of order 0
		is equal to 1 */
if (q[0] == 0.0)
	{
		fprintf(stderr," baclev init -> q[0] = 0.0  \n");
		fprintf(stderr," try again ! ");
		return(3);
	}
gain =  ONE / q[0];
for ( k=1,find=sm-m+1 ; k<=m ; k++,find++)
	fil[find]=q[k]*gain;
gain = gain*gain;

/*       MAIN LOOP    */

for ( k=m,fb=sm,find=sm-m ; k>=2 ; k--)
	{
	km1=k-1;
	ff=find+1;
	cof=fil[fb];
	if (cof >= 1.0 || cof <= -1.0)
		{
		fprintf(stderr," | refl coeff(%d) = %g | >= 1.0 \n",k,cof);
		fprintf(stderr," try again ! \n");
		return(1);
		}
	fb=fb-1;
	rflcof[k]=cof;
	oocsq=ONE/(ONE-cof*cof);
	gain *= oocsq;
	/*
	if (fb != k*(k+1)/2-1 || fb != find+km1) 
		printf("tu t''es plante dans ff mon gars\n");
	if (ff  !=  k*km1/2+1 || ff != fb-km1+1) 
		printf("tu t''es plante dans fb mon gars\n");
	*/
/*
     - We are here computing the filter of order (k-1). The coefficients
    	are computed in decreasing order, i.e. from j=k-1 to 1
    	find is the indice of the coefficient of order j in the
	filter of order k
    	fb coefficient j , filter of order k
    	ff coefficient k-j, filter of order k    
*/
	for (j=km1 ; j>=1 ; j--,find--,fb--,ff++)
		fil[find]= oocsq*(fil[fb]-cof*fil[ff]);
	}

cof=fil[1];
if (cof >= 1.0 || cof <= -1.0)
	{
		fprintf(stderr," | refl coeff(1) = %g | >= 1.0 \n",cof);
		fprintf(stderr," try again ! \n");
		return(1);
	}
fb--;
rflcof[1]=cof;
oocsq=ONE/(ONE-cof*cof);
gain *= oocsq;
r[0]=gain;
rflcof[0] = gain;

for (k=1,find=1 ; k<=m ; k++)
	{
	for (j=k-1,temp=ZERO ; j>=0 ; j--,find++)
		temp -= fil[find]*r[j];
	r[k]=temp;
	}

for (k=m+1 ; k<=tm ; k++)
	{
	for (j=k-m,temp=ZERO,find=sm ; j<=k-1 ; j++,find--)
		temp -= fil[find]*r[j];

	r[k]=temp;
	}
/*
for k>M r(k) is the convolution of r(j) k-m<=j<=k-1 with the
    	  Mth order filter since the process is considered to be of
    	  order M.
*/

return(0);
}

/*     -----------------------------------------------------------    */

compuS(r,tm,c,s,m)
double  r[],c[],s[];
register int  m;

/*
s represents the product of the 2 polynomials c and r 
the 3 polynomials are symmetric (i.e. the coefficients
    	  of the negative powers of Z are identical to those of
    	  the positive powers)
       r is an infinite polynomial, but since c is of order m, we
    	only need the powers of r up to order 2m(tm)    
       we then take the positive powers of s(Z)+1 => we take    
    	(s(0)+1)/2 instead of s(0)    
*/

{
    register int    k, j, kpj, kmj;
    double  tmp;

/*  printf("compuS\n");  */
/*    	tmp=ONE;    */
    tmp = 0.5e0 * (ONE + c[0] * r[0]);
    for (j = 1; j <= m; j++)
	tmp += c[j] * r[j];
/*    	s[0]=0.5e0*tmp;    */
    s[0] = tmp;

    for (k = 1; k <= m; k++) {
	tmp = c[0] * r[k];

	for (j = 1, kmj = k - 1, kpj = k + 1; j <= m; j++, kmj--, kpj++)
	    tmp += c[j] * (r[abs (kmj)] + r[kpj]);
	s[k] = tmp;
    }

}

/*      ------------------------------------------------------------  */

mkthta(s,q,t,m)
double  s[],q[],t[];
register int  m;

/*      s,q,t are 3 polynomials of order m t=s*q    */

{
register int  k,j;
double  tmp;

/* printf("mkthta\n");  */
for (k=0 ; k<=m ; k++)
	{
	for (j=0,tmp=ZERO ; j<=k ; j++)
		tmp += s[j]*q[k-j];
	t[k]=tmp;
	}

}

/*      ---------------------------------------------------------   */

check_wils(c,theta,m)
double *theta,*c;
int m;

/*
Check the result of the wilson algorithm
c(Z) = theta(Z) * theta(1/Z)
both polynomials are of order m
*/

{
register int j,k,p;
register double tmp;

/* printf(" check_wils \n");  */

for(k=0 ; k<=m ; k++)
	{
	p= m-k;
	tmp=0.0;
	for(j=0 ; j<=p ; j++)
		tmp+=theta[j]*theta[k+j];
	c[k]=tmp;
	}

return(0);
}

/*    -------------------------------------------------------------   */

#include <stdio.h>
faterr(sub,messag,n)
char *sub,*messag;
int n;
{
/*    Subroutine for fatal error messages */

fprintf(stderr,"			FATAL ERROR in : %s \n",sub);
fprintf(stderr,"%s\n",messag);
exit(n);
}

/*    ---------------------------------------------------------    */

shiftar(u,v,n)
double *u,*v;
int n;

/*
SHIFT ARRAY v into array u  (elements 0 to (n-1) included)
*/

{
while(n-- > 0)
	*(u++) = *(v++);

return(0);
}

/*     -----------------------------------------------------------    */
double stddev(u,v,n)
double *u,*v;
int n;

{
register int  i;
double tmp,e,denom,w,sqrt();

/*
printf("\n");
printf(" stddev \n");
*/

for (i=0,tmp=0.0,denom=0.0; i<=n ; i++)
	{
	w=u[i];
	e=w - v[i];
	tmp += e*e;
	denom += w*w;
	}
denom *= n;
tmp /= denom;

return(sqrt(tmp));
}

/*     -----------------------------------------------------------    */