gaussfit.c

Go to the documentation of this file.

#include "ds_extern.h"
#include "ds_proto.h"

#define GNPARM 100

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


int nrerror(error_text)
char *error_text;
{
  log_msg(error_text);

  return(0);
}





float *vector(nl,nh)
int nl,nh;
{
  float *v;

  v = (float *)malloc((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 *)malloc((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 **)malloc((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 *) malloc((unsigned) (nch-ncl+1)*sizeof(float));
    if(!m[i])
      nrerror("allocation failure 2 in matrix()");
    m[i] -= ncl;
  }

  return(m);
}


int free_vector(v,nl)
float *v;
int nl;
{
  free((char*) (v+nl));

  return(0);
}


int free_ivector(v,nl)
int *v,nl;
{
  free((char*) (v+nl));

  return(0);
}



int free_matrix(m,nrl,nrh,ncl)
float **m;
int nrl,nrh,ncl;
{
  int i;

  for(i=nrh;i>=nrl;i--)
    free((char*) (m[i]+ncl));
  free((char*) (m+nrl));

  return(0);
}



int gaussj(a,n,b,m)
float a[GNPARM+1][GNPARM+1],**b;
int n,m;
{
  int *indxc,*indxr,*ipiv;
  int i,icol,irow,j,k,l,ll;
  float big,dum,pivinv;

  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("GAUSSJ: Singular Matrix-1");
    }
    ++(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("GAUSSJ: Singular Matrix-2");
    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);
  free_ivector(indxr,1);
  free_ivector(indxc,1);

  return(0);
}

#undef SWAP





int mrqmin(x,y,sig,ndata,a,ma,lista,mfit,covar,alpha,chisq,funcs,alamda)
float x[],y[],sig[],a[],covar[GNPARM+1][GNPARM+1],alpha[GNPARM+1][GNPARM+1],*chisq,*alamda;
int ndata,ma,lista[],mfit;
void (*funcs)();
{
  int k,kk,j,ihit;
  static float *da,*atry,**oneda,*beta,ochisq;

  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(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
    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);
    free_vector(da,1);
    free_vector(atry,1);
    free_matrix(oneda,1,mfit,1);

    return(0);
  }
  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(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
  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(0);
}




int mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs)
float x[],y[],sig[],a[],alpha[GNPARM+1][GNPARM+1],beta[],*chisq;
int ndata,ma,lista[],mfit;
void (*funcs)();
{
  int k,j,i;
  float ymod,wt,sig2i,dy,*dyda;

  dyda=vector(1,ma);
  for(j=1;j<=mfit;j++)
  {
    for(k=1;k<=j;k++)
      alpha[j][k]=0.0;
    beta[j]=0.0;
  }
  *chisq=0.0;
  for(i=1;i<=ndata;i++)
  {
/*
    funcs(x[i],a,&ymod,dyda,ma);
 */
    funcs(x[i],a,&ymod,dyda);
    sig2i=1.0/(sig[i]*sig[i]);
    dy=y[i]-ymod;
    for(j=1;j<=mfit;j++)
    {
      wt=dyda[lista[j]]*sig2i;
      for(k=1;k<=j;k++)
        alpha[j][k] += wt*dyda[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(dyda,1);

  return(0);
}



int covsrt(covar,ma,lista,mfit)
float covar[GNPARM+1][GNPARM+1];
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];
    }
  }

  return(0);
}


#ifdef USE_FIT_ROUTINE

static float sqrarg;

#define SQR(a) (sqrarg=(a),sqrarg*sqrarg)

/*
int fit(x,y,ndata,sig,mwt,a,b,siga,sigb,chi2,q)
float x[],y[],sig[],*a,*b,*siga,*sigb,*chi2,*q;
int ndata,mwt;
{
  int i;
  float wt,t,sxoss,sx=0.0,sy=0.0,
        st2=0.0,ss,sigdat;

  *b=0.0;
  if(mwt)
  {
    ss=0.0;
    for(i=1;i<=ndata;i++)
    {
      wt=1.0/SQR(sig[i]);
      ss += wt;
      sx += x[i]*wt;
      sy += y[i]*wt;
    }
  }
  else
  {
    for(i=1;i<=ndata;i++)
    {
      sx += x[i];
      sy += y[i];
    }
    ss=ndata;
  }
  sxoss=sx/ss;
  if(mwt)
  {
    for(i=1;i<=ndata;i++)
    {
      t=(x[i]-sxoss)/sig[i];
      st2 += t*t;
      *b += t*y[i]/sig[i];
    }
  }
  else
  {
    for(i=1;i<=ndata;i++)
    {
      t=x[i]-sxoss;
      st2 += t*t;
      *b += t*y[i];
    }
  }
  *b /= st2;
  *a=(sy-sx*(*b))/ss;
  *siga=sqrt((1.0+sx*sx/(ss*st2))/ss);
  *sigb=sqrt(1.0/st2);
  *chi2=0.0;
  if(mwt == 0)
  {
    for(i=1;i<=ndata;i++)
    *chi2 += SQR(y[i]-(*a)-(*b)*x[i]);
    *q=1.0;
    sigdat=sqrt((*chi2)/(ndata-2));
    *siga *= sigdat;
    *sigb *= sigdat;
  }
  else
  {
    for(i=1;i<=ndata;i++)
      *chi2 += SQR((y[i]-(*a)-(*b)*x[i])/sig[i]);
    *q=gammq(0.5*(ndata-2),0.5*(*chi2));
  }

  return(0);
}
*/

#undef SQR

#endif


float gammq(a,x)
float a,x;
{
  float gamser,gammcf,gln;

  if(x < 0.0 || a <= 0.0)
    nrerror("Invalid arguments in routine GAMMQ");
  if(x < (a+1.0))
  {
    gser(&gamser,a,x,&gln);
    return(1.0-gamser);
  }
  else
  {
    gcf(&gammcf,a,x,&gln);
    return(gammcf);
  }

  return(0.0);
}



#define ITMAX 100
#define EPS 3.0e-7

int gser(gamser,a, x,gln)
float *gamser, a, x, *gln;
{
  int n;
  float sum,del,ap;

  *gln=gammln(a);
  if(x <= 0.0)
  {
    if(x < 0.0)
      nrerror("x less than 0 in routine GSER");
    *gamser=0.0;
    return(0);
  }
  else
  {
    ap=a;
    del=sum=1.0/a;
    for(n=1;n<=ITMAX;n++)
    {
      ap += 1.0;
      del *= x/ap;
      sum += del;
      if(fabs(del) < fabs(sum)*EPS)
      {
        *gamser=sum*exp(-x+a*log(x)-(*gln));
        return(0);
      }
    }
    nrerror("a too large, ITMAX too small in routine GSER");
    return(1);
  }

}

int gcf(gammcf,a,x,gln)
float *gammcf,a,x,*gln;
{
  int n;
  float gold=0.0,g,fac=1.0,b1=1.0;
  float b0=0.0,anf,ana,an,a1,a0=1.0;

  *gln=gammln(a);
  a1=x;
  for(n=1;n<=ITMAX;n++)
  {
    an=(float) n;
    ana=an-a;
    a0=(a1+a0*ana)*fac;
    b0=(b1+b0*ana)*fac;
    anf=an*fac;
    a1=x*a0+anf*a1;
    b1=x*b0+anf*b1;
    if(a1)
    {
      fac=1.0/a1;
      g=b1*fac;
      if(fabs((g-gold)/g) < EPS)
      {
        *gammcf=exp(-x+a*log(x)-(*gln))*g;
        return(0);
      }
      gold=g;
    }
  }
  nrerror("a too large, ITMAX too small in routine GCF");

  return(1);
}

#undef ITMAX
#undef EPS



float gammln(xx)
float xx;
{
  double x,tmp,ser;
  static double cof[6]={76.18009173,-86.50532033,24.01409822,
                -1.231739516,0.120858003e-2,-0.536382e-5};
  int j;

  x=xx-1.0;
  tmp=x+5.5;
  tmp -= (x+0.5)*log(tmp);
  ser=1.0;
  for(j=0;j<=5;j++)
  {
    x += 1.0;
    ser += cof[j]/x;
  }

  return( -tmp+log(2.50662827465*ser) );
}



/*
Tom:
   What follows is a pitiful attempt at C programming.  It has
little chance of running, although I hope it demonstrates the
logic of the fitting algorithm.  You might try to compile it and
clean up the gross syntax errors, then I'll help you get it
running next Monday.  I was particularly unsure about how to pass
multiple return parameters out of functions.  I know you have to
do it with pointers, but that's all Greek to me.

   To run the function "gauss_fit", you'll need to pass it the x
and y data arrays, and a first guess at the peak a[0], the center
position a[1], and the FWHM a[GNPARM+1].  The function will (should)
return new values of 'a'.

   It would be good to have a graphical display, at least during
the debugging stage, that shows the input data and the fitted
Gaussian.  You can use the function 'fgauss' to generate the
computed gaussian.

   Good luck.  You'll need it.

                                        Phil


*/


/*
 * Evaluate the Gaussian function y and its first partial
 * derivatives dyda[] at a given x value, with supplied
 * parameters a[].
 */
void fgauss(x, a, y, dyda)
float x, *a, *y, *dyda;
{
   float arg1, arg2, ex, fac;

   arg1 = -4. * log(2.0);
   arg2 = (x - a[2]) / a[3];
   ex  = exp(arg1 * arg2 * arg2);
   fac = a[1] * ex * arg1 * 2.0 * arg2;

   *y   = a[1] * ex;

   dyda[1] = ex;
   dyda[2] = -fac / a[3];
   dyda[3] = -fac * arg2 / a[3];

   return;
}


/* Control the iterations of mrqmin.  x and y are the input data array,
 * sig is the array of standard deviations (if any), npts are the number
 * of data points, and 'a' is the array of fitted parameters.  When
 * gauss_fit is first called, 'a' must contain the initial guesses for the
 * parameters.
 */
int gauss_fit(x, y,sig,npts, a)
float *x, *y, * sig, *a;
int npts;
{
  float alamda= -1.0, chisq=0.0, chisq1, g, cvtest, conv_crit=0.001,
        dyda[GNPARM+1], resid[GNPARM+1], alpha[GNPARM+1][GNPARM+1],
        covar[GNPARM+1][GNPARM+1];
  int ma=3, mfit=3, converged =0, i, j=1, niter=50, lista[GNPARM+1];

  lista[1] = 1;
  lista[2] = 2;
  lista[3] = 3;

        /* Calculate the CHI-SQUARED based on the initial-guess coefficients: */
  for(i = 1; i <=npts; ++i) {
    fgauss(x[i], a, &g, dyda);
    resid[i] = y[i] - g;
    chisq = chisq + resid[i] * resid[i];
  }
                                            /* Start the main iteration loop: */
  while((j <= niter) && (converged == 0))
  {

/*

    log_msg("trying Interation %d",j);
    log_msg("a[1] = %f, a[2] = %f, a[3] = %f", a[1], a[2] ,a[3]);
    for(t=1;t<=npts;t++)
      log_msg("x[%d] = %f, y[%d] = %f, sig[%d] = %f",
                                 t,x[t], t, y[t], t, sig[t]);
 */

    mrqmin(x,y,sig,npts,a,ma,lista,mfit,covar,alpha, &chisq1, (void (*)(void))fgauss, &alamda);
    if((chisq1 < chisq)  || (chisq1 < 0.001)) {
      cvtest = fabs( (chisq1 - chisq) / chisq );
      if(cvtest <= conv_crit)
        converged = 1;
      chisq = chisq1;
    }
    j++;
  }                              /* If converged, call mrqmin with alamda = 0 
                                    to get covariance and curvature matrices. */
  if(converged) {
    alamda = 0.0;
    mrqmin(x,y,sig,npts,a,ma,lista,mfit,covar,alpha, &chisq1, (void (*)(void))fgauss, &alamda);
    return(1);
  }

  return(0);
}