genburg.c

speech signal process tools

	and siginv*S*siginv=M is hermitian, so
	trace[M*Pi]=trace[Pi*M]=trace[(M*Pi)']=trace[M*Pi]complex cojugate,Q.E.D

	Bi=trace[M*Pi], M=siginv*S*siginv=Ms+iMa
	Ms is real and symmetric, iMa is imaginary and anti-symmetric
	Bi=trace[Ms*Pi] i=0 to Ni-1
	Bi=trace[iMa*iPi] i=Ni to tNi-1
	iPi is the anti-symmetrized Pi, with an i added
	*/

	/* make S*siginv in sigtemp */
	cdmmult(S,iS,siginv,isiginv,sigtemp,isigtemp,sig1temp,isig1temp,qp);

	/* make siginv*S*siginv in sig1temp */
	cdmmult(siginv,isiginv,sigtemp,isigtemp,sig1temp,isig1temp,sig2temp,isig2temp,qp);

	for(i=0;i<fNi;i++)
		{
		make(Pi,i);
		Bt=spdmtrace(sig1temp,Pi,qp);
		Bi[i]=Bt;
		}
	for(i=fNi;i<ftNi;i++)
		{
		make(Pi,i-fNi+1);
		antisymmetrize(Pi,qp);
		Bt=spdmtrace(isig1temp,Pi,qp);
		Bi[i]= -Bt;/* -1 because of product of two i's */
		}
	return;
}
antisymmetrize(Pi,n)/* make the symmetric matric Pi antisymmetric, ones in upper triangle */
register short *Pi;
register int n;
{
	register int i,j,iM,jM;

	for(i=iM=0;i<n;i++,iM+=n)
		{
		for(j=i+1,jM=iM+n;j<n;j++,jM+=n)
			{
			if(*(Pi+iM+j))
				*(Pi+jM+i)= -1;
			}
		}
	return;
}
doRnew(Ainv,Bi)/* make Ainv*Bi */
register double Ainv[],Bi[];
{
	register int i,j,iM,fNi,ftNi;
	register double Rt,*s,iRt;
	double dot();

	fNi=Ni;
	ftNi=tNi;
	for(i=0,s=Ainv;i<fNi;i++,s+=ftNi)
		{
		Rt=dot(s,1,Bi,1,ftNi);
		if(anderson && (initflag==2 || initflag==3 || it_cnt>1) && rflag)
			Rt+=R0[i];
		Rnew[i]=Rt;
		}
	if(cflag)
		{
		iRnew[0]=0;
		for(i=fNi,iM=fNi*ftNi;i<ftNi;i++,iM+=ftNi)
			{
			iRt=0;
			for(j=0;j<tNi;j++)
				iRt+=Ainv[iM+j]*Bi[j];
			iRnew[i-Ni+1]=iRt;
			}
		}
	return;
}
makesc(Pi,s,qp)/* make single channel basis matrix */
register short Pi[];
register int s,qp;
{
	register int i,del,lim;
	register short *p,*pm,value;

	lim=qpxqp;
	value=0;
	for(i=0,p=Pi;i<lim;i++,p++)
		*p=value;
	lim=qp-s;
	value=1;
	del=qp+1;
	for(i=0,p=Pi+s,pm=Pi+s*qp;i<lim;i++,p+=del,pm+=del)
		{
		*p=value;
		*pm=value;
		}
	/*
	fprintf(stderr,"\n");
	for(i=0;i<qp;i++)
		{
		for(j=0;j<qp;j++)
			fprintf(stderr,"%d ",*(Pi+i*qp+j));
		fprintf(stderr,"\n");
		}
	*/
	return;
}
make(Pi,s)
register short Pi[];
register int s;
{
	int i,j,k,l,P,a,b,c,kp,lp;
	register int m,n;

	if(scflag)
		{
		makesc(Pi,s,qp);
		return;
		}
	if(mdflag)
		{
		a=Ptaba[s]; b=Ptabb[s];
		for(k=0,kp=0;k<qdim;k++,kp+=pdim)
			{
			for(l=k,lp=kp;l<qdim;l++,lp+=pdim)
				{
				for(i=0;i<pdim;i++)
					{
					j=0;
					if(l==k)j=i;
					for(;j<pdim;j++)
						{
						m=kp+i;
						n=lp+j;
						P=0;
						if( (a == (l-k)) && (b == (j-i))) P=1;
						Pi[m*qp+n]=Pi[n*qp+m]=P;
						}
					}
				}
			}
		return;
		}
	if(mcflag)
		{
		a=Ptaba[s];
		b=Ptabb[s];
		c=Ptabc[s];
		for(k=0,kp=0;k<qdim;k++,kp+=pdim)
			{
			for(l=k,lp=kp;l<qdim;l++,lp+=pdim)
				{
				for(i=0;i<pdim;i++)
					{
					j=0;
					if(l==k)j=i;
					for(;j<pdim;j++)
						{
						m=kp+i;
						n=lp+j;
						P=0;
						if((a == (l-k)) && (b == (j-i)))
							{
							if(b==0 && i==c)P=1;
							if(b!=0) P=1;
							}
						Pi[m*qp+n]=Pi[n*qp+m]=P;
						}
					}
				}
			}
		return;
		}
	fprintf(stderr,"trouble in make().\n");
	exit(1);
}
doPtab()
{
	int i,a,b,j,isum,k,l;

	if(scflag || mdflag)
		{
		i=0;
		a=0;
		for(b=0;b<pdim;b++)
			{
			Ptaba[i]=a;
			Ptabb[i]=b;
			i++;
			}
		for(a=1;a<qdim;a++)
			{
			for(b= -(pdim-1);b<pdim;b++)
				{
				Ptaba[i]=a;
				Ptabb[i]=b;
				i++;
				}
			}
		}
	if(mcflag)
		{
		i=0;
		a=0;
		b=0;
		for(j=0;j<pdim;j++)
			{
			Ptaba[i]=a;
			Ptabb[i]=b;
			Ptabc[i]=j;
			i++;
			}
		for(b=1;b<pdim;b++)
			{
			Ptaba[i]=a;
			Ptabb[i]=b;
			i++;
			}
		for(a=1;a<qdim;a++)
			{
			for(b= -(pdim-1);b<pdim;b++)
				{
				if(b==0)
					{
					for(j=0;j<pdim;j++)
						{
						Ptaba[i]=a;
						Ptabb[i]=b;
						Ptabc[i]=j;
						i++;
						}
					}
				else
					{
					Ptaba[i]=a;
					Ptabb[i]=b;
					i++;
					}
				}
			}
		}
	if(i!=Ni) { fprintf(stderr,"Ptab not ok\n");exit(1); }

	for(i=0;i<Ni;i++)
		{
		make(Pi,i);
		for(j=i;j<Ni;j++)
			{
			make(Pj,j);
			isum=0;
			for(k=0;k<qp;k++)
				{
				for(l=0;l<qp;l++)
					{
					if(!Pi[k*qp+l])continue;
					if(!Pj[l*qp+k])continue;
					isum++;
					}
				}
			if(isum!=0 && i!=j) { fprintf(stderr,"pij!=0\n");exit(1); }
			if(i!=j)continue;
			if(isum==0) { fprintf(stderr,"Pii=0\n"); }
			Pii[i]=isum;
			}
		}
	return;
}
makesigma(R,iR,sigma,isigma,Pi)
register double R[],iR[],sigma[],isigma[];
register short Pi[];
{
	register int i,m,n,nM,mM;
	register double Rt,iRt;

	dmclear(sigma,qp);
	if(cflag)
		dmclear(isigma,qp);

	for(i=0;i<Ni;i++)
		{
		make(Pi,i);
		Rt=R[i];
		if(cflag)iRt=iR[i];
		for(m=0,mM=0;m<qp;m++,mM+=qp)
			{
			for(n=m;n<qp;n++)
				{
				if(!Pi[mM+n])continue;
				sigma[mM+n]+=Rt;
				if(cflag)
					isigma[mM+n]+=iRt;/* iPi one's in upper triangle */
				}
			}
		}
	for(m=0,mM=0;m<qp;m++,mM+=qp)
		{
		for(n=m+1,nM=mM+qp;n<qp;n++,nM+=qp)
			{
			sigma[nM+m]=sigma[mM+n];
			if(cflag)
				isigma[nM+m]= -isigma[mM+n];
			}
		}
	return;
}
gb_free()
{
	freedp(&R);
	freedp(&R0);
	freedp(&Rnew);
	freedp(&del);
	freedp(&siginv);
	freedp(&sigtemp);
	freedp(&sig1temp);
	freedp(&sig2temp);
	freesp(&Pii);
	freesp(&Pi);
	freesp(&Pj);
	freesp(&Ptaba);
	freesp(&Ptabb);
	freesp(&Ptabc);
	freedp(&Aij);
	freedp(&Ainv);
	freedp(&M);
	freedp(&Bi);
	freedp(&tBi);
	freedp(&ttBi);
	freedp(&tttBi);
	if(cflag)
		{
		freedp(&iR);
		freedp(&iR0);
		freedp(&iRnew);
		freedp(&idel);
		freedp(&isiginv);
		freedp(&isigtemp);
		freedp(&isig1temp);
		freedp(&isig2temp);
		}
	return;
}
static do_alloc()
{
	if(allocsp(&Pii,Ni))return(1);
	if(allocsp(&Pi,qpxqp))return(1);
	if(allocsp(&Pj,qpxqp))return(1);
	if(allocsp(&Ptaba,Ni))return(1);
	if(allocsp(&Ptabb,Ni))return(1);
	if(mcflag)
		{ if(allocsp(&Ptabc,Ni))return(1); }
	if(allocdp(&R,Ni))return(1);
	if(allocdp(&R0,Ni))return(1);
	if(allocdp(&Rnew,Ni))return(1);
	if(allocdp(&del,Ni))return(1);
	if(allocdp(&siginv,qpxqp))return(1);
	if(allocdp(&Aij,tNi*tNi))return(1);
	if(allocdp(&Ainv,tNi*tNi))return(1);
	if(allocdp(&M,tNi*tNi))return(1);
	if(allocdp(&Bi,tNi))return(1);
	if(allocdp(&tBi,tNi))return(1);
	if(allocdp(&ttBi,tNi))return(1);
	if(allocdp(&tttBi,tNi))return(1);
	if(allocdp(&sigtemp,qpxqp))return(1);
	if(allocdp(&sig1temp,qpxqp))return(1);
	if(allocdp(&sig2temp,qpxqp))return(1);

	if(cflag)
		{
		if(allocdp(&iR,Ni))return(1);
		if(allocdp(&iR0,Ni))return(1);
		if(allocdp(&iRnew,Ni))return(1);
		if(allocdp(&idel,Ni))return(1);
		if(allocdp(&isiginv,qpxqp))return(1);
		if(allocdp(&isigtemp,qpxqp))return(1);
		if(allocdp(&isig1temp,qpxqp))return(1);
		if(allocdp(&isig2temp,qpxqp))return(1);
		}
	return(0);
}
setdel()/* set del vector for interpolation */
{
	if(anderson && (initflag==2 || initflag==3 || it_cnt>1) && rflag)
		dvmove(Rnew,del,Ni);
	else
		{
		dvsub(Rnew,R0,del,Ni);
		if(cflag)
			dvsub(iRnew,iR0,idel,Ni);
		}
	return;
}
interp()	/* long interpolation, by tenths of interval  */
{
	double Fact,lim;
	register double fact,ssfact,sfact;
	double dointerppoint();

	setdel();/* setup del vector */
	sfact=0;
	for(Fact=.1;sfact==0 && Fact>small;Fact/=10)
		{
		lim=10.1*Fact;
		for(fact=0;fact<=lim && !((fact+Fact)==fact);fact+=Fact)
			{
			/* check interpolated point for positive
			definitness and save in R0, and save in
			R if it has a lower measure */

			if((ssfact=dointerppoint(fact)))
				sfact=ssfact;
			}
		}
	if(sfact)update(sfact);/* update R0 and monitor if moflag */
	return(sfact?0:1);/* return indication of sucess or not */
}
update(sfact)/* maybe update R0 and monitor */
double sfact;
{
	R_R0();/* save best R in R0 */
	inv_det(R0,iR0,siginv,isiginv,&R0_det);/* get inverse and determinant */

	if(moflag)
		{
		fprintf(stderr,"maxfact=%g\n",sfact);
		fprintf(stderr,"R(%g)=\n",sfact);
		dvprintf(R,Ni);
		if(cflag)dvprintf(iR,Ni);
		fprintf(stderr,"ln|R(%g)|=%g\n",sfact,R_det);
		fprintf(stderr,"d(S,R(%g))=%.15g\n\n",sfact,R_measure);
		}
	return;
}
double
dointerppoint(fact)/* do interpolation point for fact */
register double fact;
{
	register int rtflag;

	spmult(R0,fact,del,Rnew,Ni);/* form Rnew[]=R0[]+fact*del[] */
	if(cflag)
		spmult(iR0,fact,idel,iRnew,Ni);

	rtflag=inv_det(Rnew,iRnew,siginv,isiginv,&Rnew_det);
		 /* 0 Rnew pos-def
		    1 Rnew singular, Rnew_det=0
		    2 Rnew non-pos-def, Rnew_det=0 */
	Rnew_measure=domeasure(siginv,isiginv,Rnew_det);

	if(moflag)
		{
		fprintf(stderr,"%g %.10g ",fact,Rnew_measure);
		if(rtflag==0)fprintf(stderr,"\n");
		if(rtflag==1)fprintf(stderr,"singular or negative def\n");
		if(rtflag==2)fprintf(stderr,"npd\n");
		}
	if((Rnew_measure<R_measure) && rtflag==0)
		{
		Rnew_R();/* update R[] with Rnew[] */
		return(fact);/* sucessful point */
		}
	else
		return((double)0);/* not a sucessful point */
}
Rnew_R()/* update R[] with Rnew[] */
{
	R_measure=Rnew_measure;
	R_det=Rnew_det;
	dvmove(Rnew,R,Ni);
	if(cflag)
		dvmove(iRnew,iR,Ni);
	return;
}
Rnew_R0()/* move Rnew[] to R0[] */
{
	R0_det=Rnew_det;
	R0_measure=Rnew_measure;
	dvmove(Rnew,R0,Ni);
	if(cflag)
		dvmove(iRnew,iR0,Ni);
	return;
}
R_R0()/* move R[] vector to R0[] vector */
{
	R0_det=R_det;
	R0_measure=R_measure;
	dvmove(R,R0,Ni);
	if(cflag)
		dvmove(iR,iR0,Ni);
	return;
}
iinterp()	/* interpolation by 1/2 */
{
	register double ssfact,sfact,Fact;
	double dointerppoint();

	setdel();/* setup del vector */
	sfact=0;
	for(Fact=.5;sfact==0 && Fact>small;Fact/=2)
		{
		if((ssfact=dointerppoint(Fact)))
			sfact=ssfact;
		}
	if(sfact)update(sfact);/* update R0 and maybe monitor */
	return(sfact?0:1);
}
spmult(p1,f,p2,p3,n)/* form the vector p3=p1+f*p2 */
register double *p1,f,*p2,*p3;
register int n;
{
	register int i;

	for(i=0;i<n;i++,p1++,p2++,p3++)
		*p3= *p1 + f*(*p2);
	return;
}
exR(sigma,isigma,R,iR,qp,Ni)
/* extract R from sigma */
register int qp,Ni;
register double sigma[],isigma[],R[],iR[];
{
	register int i;
	int n,m,nM,mM;
	register double temp,trace;
	double spdmtrace();

	for(i=0;i<Ni;i++)
		{
		make(Pi,i);
		trace=spdmtrace(sigma,Pi,qp);
		temp=trace/Pii[i];
		R[i]=temp;
		if(cflag)
			{
			for(m=0,mM=0;m<qp;m++,mM+=qp)
				{
				for(n=m+1,nM=mM+qp;n<qp;n++,nM+=qp)
					Pi[nM+m]= -Pi[mM+n];
				}
			trace=spdmtrace(isigma,Pi,qp);
			temp= -trace/Pii[i];
			iR[i]=temp;
			}
		}
	return;
}
static set_up()
{
	if(scflag)
		{
		pdim=1;
		Ni=qdim;
		qp=qdim;
		tNi=Ni;
		if(cflag)
			tNi= 2*Ni-1;
		}
	if(mdflag)
		{
		qp=qdim*pdim;
		Ni= 2*qp-qdim-pdim+1;
		tNi=Ni;
		if(cflag)
			tNi= 2*Ni-1;
		}
	if(mcflag)
		{
		pdim=2;
		qp=qdim*pdim;
		Ni= 2*qp-qdim-pdim+1;
		Ni+=qdim;	/* not general */
		tNi=Ni;
		if(cflag)
			tNi= 2*Ni-2;
		}
	qpxqp=qp*qp;
	return;
}