fletcher.c

这是数字信号处理方面的一些源码

/*******************************************************************************
Fletcher-Powell算法是用变尺度法搜索函数极值的一种算法，它的出发点Newton算法，因此它又称为拟Newton法。她与Newton法的区别在于它无需计算多维函数的二次偏导矩阵-Hessin矩阵，同时它利用Hessin矩阵的优点，因此这种算法不但计算量小，而且收敛速度快，可以说它是
目前极值搜索算法中最好的一种。
Fletcher_Powell算法源程序如下：
*******************************************************************************/
#include "memory.c"
int NUM;
int K;
int N;
int FNUM;
double *Coeff=NULL;
double *fp=NULL;
double *amp=NULL;
double Cq=1.0;
COMPLEX com;
void dfp(double *p,int n,double gtol,int *iter,double *fret,double (*func)(double *),void (*dfunc)(double *,double *))
{
	void lnsrch(int n,double *xold,double fold,double *g,double *p,double *x,double *f,double stpmax,int *check,double (*func)(double *));
	int check,i,its,j;
	double den,fac,fad,fae,fp,stpmax,sum=0.0,sumdg,sumxi,temp,test;
	double *dg,*g,*hdg,**hessin,*pnew,*xi;
	dg=vector(n);
	g=vector(n);
	hdg=vector(n);
	pnew=vector(n);
	xi=vector(n);
	hessin=matrix(n,n);
	fp=(*func)(p);
	(*dfunc)(p,g);
	for(i=0;i<n;i++)
	{
	for(j=0;j<n;j++) hessin[i][j]=0.0;
	hessin[i][i]=1.0;
	xi[i]=-g[i];
	sum+=p[i]*p[i];
	}
	stpmax=STPMX*FMAX(sqrt(sum),(double)n);
	for(its=1;its<=ITMAX;its++)
	{
	*iter=its;
	lnsrch(n,p,fp,g,xi,pnew,fret,stpmax,&check,func);
	fp=*fret;
	for(i=0;i<n;i++)
	{
	 xi[i]=pnew[i]-p[i];
	 p[i]=pnew[i];
	 }
	 test=0.0;
	 for(i=0;i<n;i++)
	 {
	 temp=fabs(xi[i])/FMAX(fabs(p[i]),1.0);
	 if(temp>test) test=temp;
	 }
	 if(test<TOLX) {
	 free(xi);
	 free(pnew);
	 free_matrix(hessin,n);
	 free(hdg);
	 free(g);
	 free(dg);
	 return;
	 }
	 for(i=0;i<n;i++) dg[i]=g[i];
	 (*dfunc)(p,g);
	 den=FMAX(*fret,1.0);
	 for(i=0;i<n;i++) {
	 temp=fabs(g[i])*FMAX(fabs(p[i]),1.0)/den;
	 if(temp>test) test=temp;}
	 if(test<gtol)
	 {
	   free(xi);
	   free(pnew);
	   free_matrix(hessin,n);
	   free(hdg);
	   free(g);
	   free(dg);
	   return;
	  }
	 for(i=0;i<n;i++)
	 {
	 hdg[i]=0.0;
	 for(j=0;j<n;j++) hdg[i]+=hessin[i][j]*dg[j];
	 }
	 fac=fae=sumdg=sumxi=0.0;
	 for(i=0;i<n;i++)
	 {
	 fac+=dg[i]*xi[i];
	 fae+=dg[i]*hdg[i];
	 sumdg+=SQR(dg[i]);
	 sumxi+=SQR(xi[i]);
	 }
	 if(fac*fac>EPS*sumdg*sumxi) {
	 fac=1.0/fac;fad=1.0/fae;
	 for(i=0;i<n;i++) dg[i]=fac*xi[i]-fad*hdg[i];
	 for(i=0;i<n;i++)
	 {
	 hessin[i][j]+=fac*xi[i]*xi[j]-fad*hdg[i]*hdg[j]+fae*dg[i]*dg[j];
	 }
	 }
	 for(i=0;i<n;i++)
	 {
	 xi[i]=0.0;
	 for(j=0;j<n;j++) xi[i]-=hessin[i][j]*g[j];
	 }
	 }
	 free(pnew);
	 free_matrix(hessin,n);
	 free(hdg);
	 free(g);
	 free(dg);
	 free(xi);
	 }

void lnsrch(int n,double *xold,double fold,double *g,double *p,double *x,double *f,double stpmax,int *check,double (*func)(double *))
{
	int i;
	double a,alam,alam2,alamin,b,disc,f2,fold2,rhs1,rhs2,slope,sum,temp,test,tmplam;
	double val;
	*check=0;
	for(sum=0.0,i=0;i<n;i++) sum+=p[i]*p[i];
	sum=sqrt(sum);
	if(sum>stpmax)
	{
	for(i=0;i<n;i++) p[i]*=stpmax/sum;
	}
	for(slope=0.0,i=0;i<n;i++) slope+=g[i]*p[i];
	test=0.0;
	for(i=0;i<n;i++)
	{
	temp=fabs(p[i])/FMAX(fabs(xold[i]),1.0);
	if(temp>test) test=temp;
	}
	alamin=TOLX/test;alam=1.0;
	for(;;){
		for(i=0;i<n;i++) x[i]=xold[i]+p[i]*alam;
		*f=(*func)(x);
		val=*f-fold;
		val=ALF*alam*slope;
		if(alam<alamin) {
				for(i=0;i<n;i++) x[i]=xold[i];
				*check=1;
				return;
				}
		else if(*f<=fold+ALF*alam*slope) return;
		else{
		    if(alam==1.0) tmplam=-slope/(2.0*(*f-fold-slope));
		    else{
		    rhs1=*f-fold-alam*slope;
		    rhs2=f2-fold2-alam2*slope;
		    a=(rhs1/(alam*alam)-rhs2/(alam2*alam2))/(alam-alam2);
		    b=(-alam2*rhs1/(alam*alam)+alam*rhs2/(alam2*alam2))/(alam-alam2);
		if(a==0.0) tmplam=-slope/(2.0*b);
		else{
		     disc=b*b-3.0*a*slope;
		if(disc<0.0) fprintf(stderr,"roundoff problem in lnsrch");
			     tmplam=(-b+sqrt(disc))/(3.0*a);
			     }
		if(tmplam>0.5*alam) tmplam=0.5*alam;
			}
			}
			alam2=alam; f2=*f;
			fold2=fold;alam=FMAX(tmplam,0.1*alam);
			}
			}

double Get_Hn(double *p,double vos)
{
	extern int NUM;
	extern int K;
	COMPLEX temp1,temp2,temp3,temp4,result,result1,com1,com2;
	int i;
	result=make_com(1.0,0.0);
	com1=make_com(cos(vos),-sin(vos));
	com2=make_com(cos(2*vos),-sin(2*vos));
	for(i=0;i<K;i++)
	{
	temp1=FCmul(p+4*i,&com1);
	temp2=FCmul(p+4*i+1,&com2);
	temp2=CCadd(&temp1,&temp2);
	temp1=FCadd(1.0,&temp2);
	temp3=FCmul(p+4*i+2,&com1);
	temp4=FCmul(p+4*i+3,&com2);
	temp4=CCadd(&temp3,&temp4);
	temp3=FCadd(1.0,&temp4);
	result1=CCdiv(&temp1,&temp3);
	result=CCmul(&result,&result1);
	}
	com=result;
	return mode(&result);
	}

double Funct(double *p)
{
	extern double *fp;
	extern double *amp;
	extern int N;
	extern int NUM;
	extern double Cq;
	double f=0.0,temp;
	int i;
	double sum1,sum2,temp1,temp2;
	temp1=temp2=sum1=sum2=0.0;
	for(i=0;i<N;i++)
	{
	temp2=Get_Hn(p,fp[i]);
	temp1=amp[i]*temp2;
	temp2=pow(temp2,2);
	sum1+=temp1;
	sum2+=temp2;
	}
	Cq=sum1/sum2;
	for(i=0;i<N;i++)
	{
	temp=Cq*Get_Hn(p,fp[i]);
	f+=(amp[i]-temp)*(amp[i]-temp);
	}
	return f;
	}

	double Hn_Dfunct(double *p,int row,int col,int index)
	{
	COMPLEX temp1,temp2,result;
	switch(col)
		{
		 case 0:
			temp1=make_com(cos(fp[index]),-sin(fp[index]));
			temp1=FCmul(p+4*row,&temp1);
			temp2=make_com(cos(2*fp[index]),-sin(2*fp[index]));
			temp2=FCmul(p+4*row+1,&temp2);
			temp2=CCadd(&temp1,&temp2);
			temp2=FCadd(1.0,&temp2);
			temp1=make_com(cos(fp[index]),-sin(fp[index]));
			temp1=CCdiv(&temp1,&temp2);
			return Get_Hn(p,fp[index])*temp1.real;
		  case 1:
			temp1=make_com(cos(fp[index]),-sin(fp[index]));
			temp1=FCmul(p+4*row,&temp1);
			temp2=make_com(cos(2*fp[index]),-sin(2*fp[index]));
			temp2=FCmul(p+4*row+1,&temp2);
			temp2=CCadd(&temp1,&temp2);
			temp2=FCadd(1.0,&temp2);
			temp1=make_com(cos(2*fp[index]),-sin(2*fp[index]));
			temp1=CCdiv(&temp1,&temp2);
			return Get_Hn(p,fp[index])*temp1.real;
		   case 2:
			temp1=make_com(cos(fp[index]),-sin(fp[index]));
			temp1=FCmul(p+4*row+2,&temp1);
			temp2=make_com(cos(2*fp[index]),-sin(2*fp[index]));
			temp2=FCmul(p+4*row+3,&temp2);
			temp2=CCadd(&temp1,&temp2);
			temp2=FCadd(1.0,&temp2);
			temp1=make_com(cos(fp[index]),-sin(fp[index]));
			temp1=CCdiv(&temp1,&temp2);
			return Get_Hn(p,fp[index])*(-temp1.real);
		   case 3:
			temp1=make_com(cos(fp[index]),-sin(fp[index]));
			temp1=FCmul(p+4*row+2,&temp1);
			temp2=make_com(cos(2*fp[index]),-sin(2*fp[index]));
			temp2=FCmul(p+4*row+3,&temp2);
			temp2=CCadd(&temp1,&temp2);
			temp2=FCadd(1.0,&temp2);
			temp1=make_com(cos(2*fp[index]),-sin(2*fp[index]));
			temp1=CCdiv(&temp1,&temp2);
			return Get_Hn(p,fp[index])*(-temp1.real);
		   default:return;
			}
			}
	void Dfunct(double *p,double *g)
	{
	int i,j,m;
	double Hn,temp,sum;
	for(i=0;i<K;i++)
		{
		 for(j=0;j<4;j++)
			 {
			  sum=0.0;
			  for(m=0;m<N;m++)
				  {
				   Hn=Get_Hn(p,fp[m]);
				   temp=Hn*Cq-amp[m];
				   temp=temp*Hn_Dfunct(p,i,j,m);
				   sum+=temp;
				   }
			   g[4*i+j]=2*Cq*sum;
			  }
			  }
			  }

void Root_check(double *Coeff,int *check)
{
double ck,dk;
int i;
double delta;
double root1,root2;
COMPLEX croot1,croot2;
double temp;
for(i=0;i<K;i++)
{
ck=*(Coeff+i*4+2);
dk=*(Coeff+i*4+3);
delta=ck*ck-4*dk;
if(delta>=0)
{
root1=(sqrt(delta)-ck)/(2*dk);
root2=(-sqrt(delta)-ck)/(2*dk);
if(root1<1) { root1=1/root1;*check=0;}
if(root2<1) {root2=1/root2;*check=0;}
*(Coeff+i*4+2)=-(root1+root2)/(root1*root2);
*(Coeff+i*4+3)=1/(root1*root2);
}
else{
     delta=-delta;
     croot1.real=croot2.real=-ck/(2*dk);
     croot1.image=sqrt(delta)/(2*dk);
     croot2.image=-sqrt(delta)/(2*dk);
     if(mode(&croot1)<1){
			*check=0;
			temp=mode(&croot1);
			*(Coeff+i*4+2)=-2*croot1.real;
			*(Coeff+i*4+3)=temp*temp;
			}
	}
}

}

void ErrorMsg()
{
	printf("\nWrong filename or illegal parameter!");
	printf("\nExecute formate:Fletcher -option datafile");
	printf("\noption  r:design the optimal filter");
	printf("\n        c:create the datafile");
	printf("\ndatafile :the file contain the data for the algorithm");
}
main(int arc,char *argv[])
{
int i,j,iter;
double gtol,fret;
double *famp;
double fs;
int check=1;
FILE *fpointer;
K=1;
NUM=4*K;
FNUM=256;
gtol=1.0e-4;

if(arc!=3)
	{
	 ErrorMsg();
	 exit(0);
	}


if((!strcmp(argv[1],"-c"))||(!strcmp(argv[1],"-C"))) MakeFile(argv[2]);
else if((!strcmp(argv[1],"-r"))||(!strcmp(argv[1],"-R")))
{
fpointer=fopen(argv[2],"rb");
if(fpointer==NULL) {
		     printf("can't open file");
		     exit(1);
		    }
fread(&N,sizeof(int),1,fpointer);
famp=(double *)calloc(FNUM,sizeof(double));
Coeff=(double *)calloc(NUM,sizeof(double));
fp=(double *)calloc(N,sizeof(double));
amp=(double *)calloc(N,sizeof(double));
if(amp==NULL)
	   {
	   printf("not enough memory\n");
	   exit(1);
	   }
for(i=0;i<K;i++)
{
printf("input a[%d] b[%d] c[%d] d[%d]:",i,i,i,i);
scanf("%lf%lf%lf%lf",Coeff+4*i,Coeff+4*i+1,Coeff+4*i+2,Coeff+4*i+3);
}

for(i=0;i<N;i++)
{
fread(fp+i,sizeof(double),1,fpointer);
fread(amp+i,sizeof(double),1,fpointer);
}
do{
check=1;
dfp(Coeff,NUM,gtol,&iter,&fret,Funct,Dfunct);
Root_check(Coeff,&check);
}while(check==0);
for(i=0;i<NUM;i++)
{
printf("%lf\n",Coeff[i]);
}
printf("Cq=%lf\n",Cq);
printf("E[n]=%lf\n",fret);
printf("ITER=%d\n",iter);

for(i=0;i<FNUM;i++)
{
fs=3.14*i/FNUM;
famp[i]=Cq*Get_Hn(Coeff,fs);
}
printf("press any key to continue");
getch();
draw_image(famp,FNUM,"Frequency Response","Amplitude","0","PAI",0);
for(i=0;i<FNUM;i++)
{
fs=3.14*i/FNUM;
Get_Hn(Coeff,fs);
famp[i]=angel(&com);
}
draw_image(famp,FNUM,"PHASE PROPERTY","PHASE","0","PAI",0);
fclose(fpointer);
free(famp);
free(amp);
free(Coeff);
free(fp);
}
else {
       ErrorMsg();
       }
}