clsq.c

王小平《遗传算法——理论、应用与软件实现》随书光盘

/* 
  ============================================================================
  Levenberg-Marquardt Least Squares Optimisation Routine
  ============================================================================
  
  Mark Hinchliffe    22/1/96
  ----------------------------------------------------------------------------
  
  update: 19/2/96 - no. of constants extended to 30.

  Functions:

	mexFunction, eqn.

 
  Functions adapted from 'Numerical Recipes in C': 
	
	mrqcof, mrqmin, covsrt, gaussj, vector, matrix, ivector, free_ivector,
	free_vector, ree_matrix, nerror.
	
	
*/	

#include <malloc.h>
#include <stdio.h>
#include <math.h>
#include "mex.h"

#define SWAP(a,b) {float temp=(a);(a)=(b);(b)=temp;}

/* gateway routine */
void mexFunction(
	int nlhs, Matrix *plhs[],
	int nrhs, Matrix *prhs[])
/*void mexFunction(nlhs,plhs,nrhs,prhs)
int nlhs;
Matrix *plhs[];
int nrhs;
Matrix *prhs[];*/
{	
	
	double *xp,*ap,*yp,*sigp,*ndatap,*mfitp,*map,*listap;
	float alamda,**covar,chisq,oldchisq,**alpha,**matrix(),*vector(),*x,*y,*sig;
	double *xout;
	float a[31];
	int mfit,ma,ndata,lista[31],ctr,maxit,npar,vecl,count;
	void ree_matrix(),free_vector();
	void mrqmin();
	void (*funcs)();
	void eqn();
	Matrix *strptr[1];
	
	
	funcs=&eqn;
		
	/* Dereference arguments */
	
	yp=mxGetPr(prhs[0]);
	ndata=*mxGetPr(prhs[1]);
	ap=mxGetPr(prhs[2]);
	mfit=(int)*mxGetPr(prhs[3]);
	ma=mfit;
	
	strptr[0]=prhs[4];
	
	if (mfit > 20) mexErrMsgTxt("Too many constants ....\n");
	
	/* Create matrix for return argument */
	
	plhs[0]=mxCreateFull(1,ma,REAL);
	xout=mxGetPr(plhs[0]);
	
	a[0]=0.0;lista[0]=0;
	/* Create arrays lista[] and a[] */

	for (ctr=1;ctr<=ma;ctr++)
	{
		lista[ctr]=ctr;
		a[ctr]=*(ap+(ctr-1));
	}
	
	/* Create y vector */

	y=vector(1,ndata);
	
	for (ctr=1;ctr<=ndata;ctr++) y[ctr]=*(yp+(ctr-1));
	
	/* Create alpha and covar matrices */

	alpha=matrix(1,ma,1,ma);
	covar=matrix(1,ma,1,ma);
	
	/* initialise */
	
	chisq=0.0;
	alamda= -1.0;
	maxit=20;/* maximum number of iterations */
	
	mrqmin(y,ndata,a,ma,lista,mfit,covar,alpha,&chisq,funcs,&alamda,strptr); 
	/*printf("%f \n",chisq);*/
	count=0;
	for (ctr=1;ctr<=maxit;ctr++)
	{
		oldchisq=chisq;
		mrqmin(y,ndata,a,ma,lista,mfit,covar,alpha,&chisq,funcs,&alamda,strptr);
		/*printf("%f \n",chisq);*/
	
		if (((oldchisq-chisq) > 0.0) && ((oldchisq-chisq) < 0.1)) break;
		if ((oldchisq == chisq) && (chisq < 10.0)) break;
		if (oldchisq == chisq) count+=1;
		if (count == 10) break;
		/*if (((fabs((oldchisq-chisq)/oldchisq)) < 0.001) && (chisq != oldchisq)) break;*/
		if (ctr==maxit) printf("max iterations reached!\n");
	}
	
	/* final call */
	
	alamda=0.0;
	
	mrqmin(y,ndata,a,ma,lista,mfit,covar,alpha,&chisq,funcs,&alamda,strptr);
	
	for (ctr=1;ctr<=ma;ctr++)
	{	
		xout[ctr-1]=a[ctr];
	}
	
	/*free_vector(sig,1,ndata);*/
	free_vector(y,1,ndata);
	ree_matrix(alpha,1,ma,1,ma);
	ree_matrix(covar,1,ma,1,ma);
}
void mrqmin(y,ndata,a,ma,lista,mfit,covar,alpha,chisq,funcs,alamda,strptr)
float *y,a[],**covar,**alpha,*chisq,*alamda;
Matrix *strptr[1];
int ndata,ma,lista[],mfit;
void (*funcs)();
{
	int k,kk,j,ihit;
	static float *da,*atry,**oneda,*beta,ochisq;
	float *vector(),**matrix();
	void mrqcof(),gaussj(),covsrt(),nrerror(),ree_matrix(),free_vector();
	
	if (*alamda < 0.0) {
		oneda=matrix(1,mfit,1,1);
		atry=vector(1,ma);
		da=vector(1,ma);
		beta=vector(1,ma);
		kk=mfit+1;
		for (j=1;j<=ma;j++) {
			ihit=0;
			for (k=1;k<=mfit;k++)
				if (lista[k] == j) ihit++;
			if (ihit == 0)
				lista[kk++]=j;
			else if (ihit > 1) nrerror("Bad LISTA permutation in MRQMIN-1");
		}
		
		if (kk != ma+1) nrerror("Bad LISTA permutation in MRQMIN-2");
		*alamda=0.001;
		mrqcof(y,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs,strptr);
		ochisq=(*chisq);
		
	}
	
	for (j=1;j<=mfit;j++) {
		for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
		covar[j][j]=alpha[j][j]*(1.0+(*alamda));
		oneda[j][1]=beta[j];
	}
	
	gaussj(covar,mfit,oneda,1);
	for (j=1;j<=mfit;j++)
		da[j]=oneda[j][1];
	if (*alamda == 0.0) {
		/*covsrt(covar,ma,lista,mfit);*/
		free_vector(beta,1,ma);
		free_vector(da,1,ma);
		free_vector(atry,1,ma);
		ree_matrix(oneda,1,mfit,1,1);
		return;
	}
	for (j=1;j<=ma;j++) atry[j]=a[j];
	for (j=1;j<=mfit;j++)
		atry[lista[j]] = a[lista[j]]+da[j];
	mrqcof(y,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs,strptr);
	if (*chisq < ochisq) {
		*alamda *= 0.1;
		ochisq=(*chisq);
		for (j=1;j<=mfit;j++) {
			for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
			beta[j]=da[j];
			a[lista[j]]=atry[lista[j]];
		}
	} else {
		*alamda *= 10.0;
		*chisq=ochisq;
	}
	return;
}
void eqn(a,ymod,dyda,ma,ndata,strptr)
float a[],*ymod,**dyda;
Matrix *strptr[1];
int ma,ndata;
{
	float olda,maxdyda[11],**y1,**y2,*vector(),**matrix(),sum,ta;
	double chg[31],oldchg;
	int j,k,m;
	Matrix *out[3],*mptra[31],*maptr[31];
	char *aa[31];
	double *aptr[31],*y1p,*y2p,*yyp,*ymodp;
	void ree_matrix(),free_vector();
	
	
	aa[0]="";aa[1]="a1";aa[2]="a2";aa[3]="a3";aa[4]="a4";aa[5]="a5";
	aa[6]="a6";aa[7]="a7";aa[8]="a8";aa[9]="a9";aa[10]="a10";
	aa[11]="a11";aa[12]="a12";aa[13]="a13";aa[14]="a14";aa[15]="a15";
	aa[16]="a16";aa[17]="a17";aa[18]="a18";aa[19]="a19";aa[20]="a20";
	aa[21]="a21";aa[22]="a22";aa[23]="a23";aa[24]="a24";aa[25]="a25";
	aa[26]="a26";aa[27]="a27";aa[28]="a28";aa[29]="a29";aa[30]="a30";
	
	for(j=1;j<=ma;j++)
	{
		
		mptra[j-1]=mxCreateFull(1,1,0);
		mxSetName(mptra[j-1],aa[j]);
		mexPutMatrix(mptra[j-1]);
		maptr[j-1]=mexGetMatrixPtr(aa[j]);
		aptr[j-1]=mxGetPr(maptr[j-1]);
		*aptr[j-1]=a[j];
		mxFreeMatrix(mptra[j-1]);
		
	}
		
	for (m=1;m<=ma;m++)
	{
		chg[m-1]=(1e-7)*a[m];
		if (chg[m-1] <= 1e-8) chg[m-1]=1e-8;
	}
	
	for (j=1;j<=ma;j++)
	{
		
			
		olda=a[j];
		a[j]=a[j]+chg[j-1];
		*aptr[j-1]=a[j];
		mexCallMATLAB(1,&out[0],1,strptr,"eval");
		a[j]=a[j]-(2*chg[j-1]);
		*aptr[j-1]=a[j];
		mexCallMATLAB(1,&out[1],1,strptr,"eval");
		a[j]=olda;
		*aptr[j-1]=a[j];
			
		
		y1p=mxGetPr(out[0]);
		y2p=mxGetPr(out[1]);
			
		
		for (k=1;k<=ndata;k++)
		{
			dyda[k][j]=((*(y1p+k-1))-(*(y2p+k-1)))/(2*chg[j-1]);
		}
		mxFreeMatrix(out[0]);
		mxFreeMatrix(out[1]);
			
	}
	mexCallMATLAB(1,&out[2],1,strptr,"eval");
	ymodp=mxGetPr(out[2]);
	
	for (k=1;k<=ndata;k++)	ymod[k]=(*(ymodp+k-1));
	
	mxFreeMatrix(out[2]);
	
	return;
}
void mrqcof(y,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs,strptr)
float *y,a[],**alpha,beta[],*chisq;
Matrix *strptr[1];
int ndata,ma,lista[],mfit;
void (*funcs)();
{
	int k,j,i;
	float *ymod,wt,sig2i,dy,**dyda,*vector(),**matrix();
	void free_vector(),ree_matrix();
	dyda=matrix(1,ndata,1,ma);
	ymod=vector(1,ndata);
	
	for (j=1;j<=mfit;j++) {
		
		for (k=1;k<=j;k++) {
		
		alpha[j][k]=0.0;
		
		}	
		
		beta[j]=0.0;
		
	}
	*chisq=0.0;
	(*funcs)(a,ymod,dyda,ma,ndata,strptr);		
	
	for (i=1;i<=ndata;i++) {
		
		
		/*sig2i=1.0*//*/(sig[i]*sig[i]);*/
		dy=y[i]-ymod[i];
		for (j=1;j<=mfit;j++) {
			
			wt=dyda[i][lista[j]]/**sig2i*/;
			for (k=1;k<=j;k++)
				alpha[j][k] += wt*dyda[i][lista[k]];
			beta[j] += dy*wt;
			
		}
		(*chisq) += dy*dy/**sig2i*/;
		
		
	}
	
	for (j=2;j<=mfit;j++)
		for (k=1;k<=j-1;k++) alpha[k][j]=alpha[j][k];
	
	free_vector(ymod,1,ndata);
	ree_matrix(dyda,1,ndata,1,ma);
}



void gaussj(a,n,b,m)
float **a,**b;
int n,m;
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll,*ivector();
	float big,dum,pivinv;
	void nrerror(),free_ivector();

	indxc=ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);
	for (j=1;j<=n;j++) ipiv[j]=0;
	for (i=1;i<=n;i++) {
		big=0.0;
		for (j=1;j<=n;j++)
			if (ipiv[j] != 1)
				for (k=1;k<=n;k++) {
					if (ipiv[k] == 0) {
						if (fabs(a[j][k]) >= big) {
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
						}
					} else if (ipiv[k] > 1) /*nrerror*/
						{
							printf("GAUSSJ: Singular Matrix-1\n");
							free_ivector(ipiv,1,n);
							free_ivector(indxr,1,n);
							free_ivector(indxc,1,n);
							return;
						}
				}
		++(ipiv[icol]);
		if (irow != icol) {
			for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
			for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
		}
		indxr[i]=irow;
		indxc[i]=icol;
		if (a[icol][icol] == 0.0) /*nrerror*/
		{
			/*printf("GAUSSJ: Singular Matrix-2\n");*/
			free_ivector(ipiv,1,n);
			free_ivector(indxr,1,n);
			free_ivector(indxc,1,n);
			return;
		}
		pivinv=1.0/a[icol][icol];
		a[icol][icol]=1.0;
		for (l=1;l<=n;l++) a[icol][l] *= pivinv;
		for (l=1;l<=m;l++) b[icol][l] *= pivinv;
		for (ll=1;ll<=n;ll++)
			if (ll != icol) {
				dum=a[ll][icol];
				a[ll][icol]=0.0;
				for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
				for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
			}
	}
	for (l=n;l>=1;l--) {
		if (indxr[l] != indxc[l])
			for (k=1;k<=n;k++)
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
	}
	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
}


void covsrt(covar,ma,lista,mfit)
float **covar;
int ma,lista[],mfit;
{
	int i,j;
	float swap;

	for (j=1;j<ma;j++)
		for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
	for (i=1;i<mfit;i++)
		for (j=i+1;j<=mfit;j++) {
			if (lista[j] > lista[i])
				covar[lista[j]][lista[i]]=covar[i][j];
			else
				covar[lista[i]][lista[j]]=covar[i][j];
		}
	swap=covar[1][1];
	for (j=1;j<=ma;j++) {
		covar[1][j]=covar[j][j];
		covar[j][j]=0.0;
	}
	covar[lista[1]][lista[1]]=swap;
	for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
	for (j=2;j<=ma;j++)
		for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
}
void free_vector(v,nl,nh)
float *v;
int nl,nh;
{
	mxFree((char*) (v+nl));
}
void ree_matrix(m,nrl,nrh,ncl,nch)
float **m;
int nrl,nrh,ncl,nch;
{
	int i;

	for(i=nrh;i>=nrl;i--) mxFree((char*) (m[i]+ncl));
	mxFree((char*) (m+nrl));
}
void nrerror(error_text)
char error_text[];
{
	void exit();

	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	/*exit(0);*/
	return;
	
}
float *vector(nl,nh)
int nl,nh;
{
	float *v;

	v=(float *) mxCalloc((unsigned) (nh-nl+1), sizeof(float));
	if (!v) nrerror("allocation failure in vector()");
	return v-nl;
}
int *ivector(nl,nh)
int nl,nh;
{
	int *v;

	v=(int *) mxCalloc((unsigned) (nh-nl+1), sizeof(int));
	if (!v) nrerror("allocation failure in ivector()");
	return v-nl;
}
float **matrix(nrl,nrh,ncl,nch)
int nrl,nrh,ncl,nch;
{
	int i;
	float **m;

	m=(float **)  mxCalloc((unsigned) (nrh-nrl+1), sizeof(float*));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m -= nrl;

	for(i=nrl;i<=nrh;i++) {
		m[i]=(float *) mxCalloc((unsigned) (nch-ncl+1), sizeof(float));
		if (!m[i]) nrerror("allocation failure 2 in matrix()");
		m[i] -= ncl;
	}
	return m;
}
void free_ivector(v,nl,nh)
int *v,nl,nh;
{
	mxFree((char*) (v+nl));
}

#undef SWAP