src/tools/levenmar.c

   1 /*
   2  * $Id: levenmar.c,v 1.20 2004/01/23 18:11:02 lindahl Exp $
   3  *
   4  *                This source code is part of
   5  *
   6  *                 G   R   O   M   A   C   S
   7  *
   8  *          GROningen MAchine for Chemical Simulations
   9  *
  10  *                        VERSION 3.2.0
  11  * Written by David van der Spoel, Erik Lindahl, Berk Hess, and others.
  12  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
  13  * Copyright (c) 2001-2004, The GROMACS development team,
  14  * check out http://www.gromacs.org for more information.
  15
  16  * This program is free software; you can redistribute it and/or
  17  * modify it under the terms of the GNU General Public License
  18  * as published by the Free Software Foundation; either version 2
  19  * of the License, or (at your option) any later version.
  20  *
  21  * If you want to redistribute modifications, please consider that
  22  * scientific software is very special. Version control is crucial -
  23  * bugs must be traceable. We will be happy to consider code for
  24  * inclusion in the official distribution, but derived work must not
  25  * be called official GROMACS. Details are found in the README & COPYING
  26  * files - if they are missing, get the official version at www.gromacs.org.
  27  *
  28  * To help us fund GROMACS development, we humbly ask that you cite
  29  * the papers on the package - you can find them in the top README file.
  30  *
  31  * For more info, check our website at http://www.gromacs.org
  32  *
  33  * And Hey:
  34  * Green Red Orange Magenta Azure Cyan Skyblue
  35  */
  36 #ifdef HAVE_CONFIG_H
  37 #include <config.h>
  38 #endif
  39
  40 #include <math.h>
  41 #include <stdio.h>
  42 #include <stdlib.h>
  43
  44 #include "types/simple.h"
  45
  46 static void nrerror(const char error_text[], gmx_bool bExit)
  47 {
  48   fprintf(stderr,"Numerical Recipes run-time error...\n");
  49   fprintf(stderr,"%s\n",error_text);
  50   if (bExit) {
  51     fprintf(stderr,"...now exiting to system...\n");
  52     exit(1);
  53   }
  54 }
  55
  56 /* dont use the keyword vector - it will clash with the
  57  * altivec extensions used for powerpc processors.
  58  */
  59
  60 static real *rvector(int nl,int nh)
  61 {
  62   real *v;
  63
  64   v=(real *)malloc((unsigned) (nh-nl+1)*sizeof(real));
  65   if (!v) nrerror("allocation failure in rvector()", TRUE);
  66   return v-nl;
  67 }
  68
  69 static int *ivector(int nl, int nh)
  70 {
  71   int *v;
  72
  73   v=(int *)malloc((unsigned) (nh-nl+1)*sizeof(int));
  74   if (!v) nrerror("allocation failure in ivector()", TRUE);
  75   return v-nl;
  76 }
  77
  78
  79 static real **matrix1(int nrl, int nrh, int ncl, int nch)
  80 {
  81         int i;
  82         real **m;
  83
  84         m=(real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*));
  85         if (!m) nrerror("allocation failure 1 in matrix1()", TRUE);
  86         m -= nrl;
  87
  88         for(i=nrl;i<=nrh;i++) {
  89                 m[i]=(real *) malloc((unsigned) (nch-ncl+1)*sizeof(real));
  90                 if (!m[i]) nrerror("allocation failure 2 in matrix1()", TRUE);
  91                 m[i] -= ncl;
  92         }
  93         return m;
  94 }
  95
  96 static double **dmatrix(int nrl, int nrh, int ncl, int nch)
  97 {
  98         int i;
  99         double **m;
 100
 101         m=(double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
 102         if (!m) nrerror("allocation failure 1 in dmatrix()", TRUE);
 103         m -= nrl;
 104
 105         for(i=nrl;i<=nrh;i++) {
 106                 m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
 107                 if (!m[i]) nrerror("allocation failure 2 in dmatrix()", TRUE);
 108                 m[i] -= ncl;
 109         }
 110         return m;
 111 }
 112
 113 static int **imatrix1(int nrl, int nrh, int ncl, int nch)
 114 {
 115         int i,**m;
 116
 117         m=(int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
 118         if (!m) nrerror("allocation failure 1 in imatrix1()", TRUE);
 119         m -= nrl;
 120
 121         for(i=nrl;i<=nrh;i++) {
 122                 m[i]=(int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
 123                 if (!m[i]) nrerror("allocation failure 2 in imatrix1()", TRUE);
 124                 m[i] -= ncl;
 125         }
 126         return m;
 127 }
 128
 129
 130
 131 static real **submatrix(real **a, int oldrl, int oldrh, int oldcl,
 132                  int newrl, int newcl)
 133 {
 134         int i,j;
 135         real **m;
 136
 137         m=(real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*));
 138         if (!m) nrerror("allocation failure in submatrix()", TRUE);
 139         m -= newrl;
 140
 141         for(i=oldrl,j=newrl;i<=oldrh;i++,j++) m[j]=a[i]+oldcl-newcl;
 142
 143         return m;
 144 }
 145
 146
 147
 148 static void free_vector(real *v, int nl)
 149 {
 150         free((char*) (v+nl));
 151 }
 152
 153 static void free_ivector(int *v, int nl)
 154 {
 155         free((char*) (v+nl));
 156 }
 157
 158 static void free_dvector(int *v, int nl)
 159 {
 160         free((char*) (v+nl));
 161 }
 162
 163
 164
 165 static void free_matrix(real **m, int nrl, int nrh, int ncl)
 166 {
 167         int i;
 168
 169         for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
 170         free((char*) (m+nrl));
 171 }
 172
 173 static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch)
 174 {
 175         int i,j,nrow,ncol;
 176         real **m;
 177
 178         nrow=nrh-nrl+1;
 179         ncol=nch-ncl+1;
 180         m = (real **) malloc((unsigned) (nrow)*sizeof(real*));
 181         if (!m) nrerror("allocation failure in convert_matrix()", TRUE);
 182         m -= nrl;
 183         for(i=0,j=nrl;i<=nrow-1;i++,j++) m[j]=a+ncol*i-ncl;
 184         return m;
 185 }
 186
 187
 188
 189 static void free_convert_matrix(real **b, int nrl)
 190 {
 191         free((char*) (b+nrl));
 192 }
 193
 194 #define SWAP(a,b) {real temp=(a);(a)=(b);(b)=temp;}
 195
 196 static void dump_mat(int n,real **a)
 197 {
 198   int i,j;
 199
 200   for(i=1; (i<=n); i++) {
 201     for(j=1; (j<=n); j++)
 202       fprintf(stderr,"  %10.3f",a[i][j]);
 203     fprintf(stderr,"\n");
 204   }
 205 }
 206
 207 gmx_bool gaussj(real **a, int n, real **b, int m)
 208 {
 209   int *indxc,*indxr,*ipiv;
 210   int i,icol=0,irow=0,j,k,l,ll;
 211   real big,dum,pivinv;
 212
 213   indxc=ivector(1,n);
 214   indxr=ivector(1,n);
 215   ipiv=ivector(1,n);
 216   for (j=1;j<=n;j++) ipiv[j]=0;
 217   for (i=1;i<=n;i++) {
 218     big=0.0;
 219     for (j=1;j<=n;j++)
 220       if (ipiv[j] != 1)
 221         for (k=1;k<=n;k++) {
 222           if (ipiv[k] == 0) {
 223             if (fabs(a[j][k]) >= big) {
 224               big=fabs(a[j][k]);
 225               irow=j;
 226               icol=k;
 227             }
 228           } else if (ipiv[k] > 1) {
 229             nrerror("GAUSSJ: Singular Matrix-1", FALSE);
 230             return FALSE;
 231           }
 232         }
 233     ++(ipiv[icol]);
 234     if (irow != icol) {
 235       for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
 236         for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
 237     }
 238     indxr[i]=irow;
 239     indxc[i]=icol;
 240     if (a[icol][icol] == 0.0) {
 241       fprintf(stderr,"irow = %d, icol = %d\n",irow,icol);
 242       dump_mat(n,a);
 243       nrerror("GAUSSJ: Singular Matrix-2", FALSE);
 244       return FALSE;
 245     }
 246     pivinv=1.0/a[icol][icol];
 247     a[icol][icol]=1.0;
 248     for (l=1;l<=n;l++) a[icol][l] *= pivinv;
 249     for (l=1;l<=m;l++) b[icol][l] *= pivinv;
 250     for (ll=1;ll<=n;ll++)
 251       if (ll != icol) {
 252         dum=a[ll][icol];
 253         a[ll][icol]=0.0;
 254         for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
 255         for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
 256       }
 257   }
 258   for (l=n;l>=1;l--) {
 259     if (indxr[l] != indxc[l])
 260       for (k=1;k<=n;k++)
 261         SWAP(a[k][indxr[l]],a[k][indxc[l]]);
 262   }
 263   free_ivector(ipiv,1);
 264   free_ivector(indxr,1);
 265   free_ivector(indxc,1);
 266
 267   return TRUE;
 268 }
 269
 270 #undef SWAP
 271
 272
 273 static void covsrt(real **covar, int ma, int lista[], int mfit)
 274 {
 275   int i,j;
 276   real swap;
 277
 278   for (j=1;j<ma;j++)
 279     for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
 280   for (i=1;i<mfit;i++)
 281     for (j=i+1;j<=mfit;j++) {
 282       if (lista[j] > lista[i])
 283         covar[lista[j]][lista[i]]=covar[i][j];
 284       else
 285         covar[lista[i]][lista[j]]=covar[i][j];
 286     }
 287   swap=covar[1][1];
 288   for (j=1;j<=ma;j++) {
 289     covar[1][j]=covar[j][j];
 290     covar[j][j]=0.0;
 291   }
 292   covar[lista[1]][lista[1]]=swap;
 293   for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
 294   for (j=2;j<=ma;j++)
 295     for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
 296 }
 297
 298 #define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
 299
 300 static void covsrt_new(real **covar,int ma, int ia[], int mfit)
 301      /* Expand in storage the covariance matrix covar, so as to take
 302       * into account parameters that are being held fixed. (For the
 303       * latter, return zero covariances.)
 304       */
 305 {
 306   int i,j,k;
 307   real swap;
 308   for (i=mfit+1;i<=ma;i++)
 309     for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
 310   k=mfit;
 311   for (j=ma;j>=1;j--) {
 312     if (ia[j]) {
 313       for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
 314       for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
 315       k--;
 316     }
 317   }
 318 }
 319 #undef SWAP
 320
 321 static void mrqcof(real x[], real y[], real sig[], int ndata, real a[],
 322                    int ma, int lista[], int mfit,
 323                    real **alpha, real beta[], real *chisq,
 324                    void (*funcs)(real,real *,real *,real *))
 325 {
 326   int k,j,i;
 327   real ymod,wt,sig2i,dy,*dyda;
 328
 329   dyda=rvector(1,ma);
 330   for (j=1;j<=mfit;j++) {
 331     for (k=1;k<=j;k++) alpha[j][k]=0.0;
 332     beta[j]=0.0;
 333   }
 334   *chisq=0.0;
 335   for (i=1;i<=ndata;i++) {
 336     (*funcs)(x[i],a,&ymod,dyda);
 337     sig2i=1.0/(sig[i]*sig[i]);
 338     dy=y[i]-ymod;
 339     for (j=1;j<=mfit;j++) {
 340       wt=dyda[lista[j]]*sig2i;
 341       for (k=1;k<=j;k++)
 342         alpha[j][k] += wt*dyda[lista[k]];
 343       beta[j] += dy*wt;
 344     }
 345     (*chisq) += dy*dy*sig2i;
 346   }
 347   for (j=2;j<=mfit;j++)
 348     for (k=1;k<=j-1;k++) alpha[k][j]=alpha[j][k];
 349   free_vector(dyda,1);
 350 }
 351
 352
 353 gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
 354             int ma, int lista[], int mfit,
 355             real **covar, real **alpha, real *chisq,
 356             void (*funcs)(real,real *,real *,real *),
 357             real *alamda)
 358 {
 359   int k,kk,j,ihit;
 360   static real *da,*atry,**oneda,*beta,ochisq;
 361
 362   if (*alamda < 0.0) {
 363     oneda=matrix1(1,mfit,1,1);
 364     atry=rvector(1,ma);
 365     da=rvector(1,ma);
 366     beta=rvector(1,ma);
 367     kk=mfit+1;
 368     for (j=1;j<=ma;j++) {
 369       ihit=0;
 370       for (k=1;k<=mfit;k++)
 371         if (lista[k] == j) ihit++;
 372       if (ihit == 0)
 373         lista[kk++]=j;
 374       else if (ihit > 1) {
 375         nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
 376         return FALSE;
 377       }
 378     }
 379     if (kk != ma+1) {
 380       nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
 381       return FALSE;
 382     }
 383     *alamda=0.001;
 384     mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
 385     ochisq=(*chisq);
 386   }
 387   for (j=1;j<=mfit;j++) {
 388     for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
 389     covar[j][j]=alpha[j][j]*(1.0+(*alamda));
 390     oneda[j][1]=beta[j];
 391   }
 392   if (!gaussj(covar,mfit,oneda,1))
 393     return FALSE;
 394   for (j=1;j<=mfit;j++)
 395     da[j]=oneda[j][1];
 396   if (*alamda == 0.0) {
 397     covsrt(covar,ma,lista,mfit);
 398     free_vector(beta,1);
 399     free_vector(da,1);
 400     free_vector(atry,1);
 401     free_matrix(oneda,1,mfit,1);
 402     return TRUE;
 403   }
 404   for (j=1;j<=ma;j++) atry[j]=a[j];
 405   for (j=1;j<=mfit;j++)
 406     atry[lista[j]] = a[lista[j]]+da[j];
 407   mrqcof(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
 408   if (*chisq < ochisq) {
 409     *alamda *= 0.1;
 410     ochisq=(*chisq);
 411     for (j=1;j<=mfit;j++) {
 412       for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
 413       beta[j]=da[j];
 414       a[lista[j]]=atry[lista[j]];
 415     }
 416   } else {
 417     *alamda *= 10.0;
 418     *chisq=ochisq;
 419   }
 420   return TRUE;
 421 }
 422
 423
 424 gmx_bool mrqmin_new(real x[],real y[],real sig[],int ndata,real a[],
 425                 int ia[],int ma,real **covar,real **alpha,real *chisq,
 426                 void (*funcs)(real, real [], real *, real []),
 427                 real *alamda)
 428      /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
 429       * of a fit between a set of data points x[1..ndata], y[1..ndata]
 430       * with individual standard deviations sig[1..ndata], and a nonlinear
 431       * function dependent on ma coefficients a[1..ma]. The input array
 432       * ia[1..ma] indicates by nonzero entries those components of a that
 433       * should be fitted for, and by zero entries those components that
 434       * should be held fixed at their input values. The program returns
 435       * current best-fit values for the parameters a[1..ma], and
 436       * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
 437       * are used as working space during most iterations. Supply a routine
 438       * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
 439       * and its derivatives dyda[1..ma] with respect to the fitting
 440       * parameters a at x. On the first call provide an initial guess for
 441       * the parameters a, and set alamda < 0 for initialization (which then
 442       * sets alamda=.001). If a step succeeds chisq becomes smaller and
 443       * alamda de-creases by a factor of 10. If a step fails alamda grows by
 444       * a factor of 10. You must call this routine repeatedly until
 445       * convergence is achieved. Then, make one final call with alamda=0,
 446       * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
 447       * the curvature matrix.
 448       * (Parameters held fixed will return zero covariances.)
 449       */
 450 {
 451   void covsrt(real **covar, int ma, int ia[], int mfit);
 452   gmx_bool gaussj(real **a, int n, real **b,int m);
 453   void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
 454               int ia[], int ma, real **alpha, real beta[], real *chisq,
 455               void (*funcs)(real, real [], real *, real []));
 456   int j,k,l;
 457   static int mfit;
 458   static real ochisq,*atry,*beta,*da,**oneda;
 459
 460   if (*alamda < 0.0) {                    /* Initialization. */
 461     atry=rvector(1,ma);
 462     beta=rvector(1,ma);
 463     da=rvector(1,ma);
 464     for (mfit=0,j=1;j<=ma;j++)
 465       if (ia[j]) mfit++;
 466     oneda=matrix1(1,mfit,1,1);
 467     *alamda=0.001;
 468     mrqcof_new(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
 469     ochisq=(*chisq);
 470     for (j=1;j<=ma;j++)
 471       atry[j]=a[j];
 472   }
 473   for (j=1;j<=mfit;j++) { /* Alter linearized fitting matrix, by augmenting. */
 474     for (k=1;k<=mfit;k++)
 475       covar[j][k]=alpha[j][k]; /* diagonal elements. */
 476     covar[j][j]=alpha[j][j]*(1.0+(*alamda));
 477     oneda[j][1]=beta[j];
 478   }
 479   if (!gaussj(covar,mfit,oneda,1))      /* Matrix solution. */
 480     return FALSE;
 481   for (j=1;j<=mfit;j++)
 482     da[j]=oneda[j][1];
 483   if (*alamda == 0.0) { /* Once converged, evaluate covariance matrix. */
 484     covsrt_new(covar,ma,ia,mfit);
 485     free_matrix(oneda,1,mfit,1);
 486     free_vector(da,1);
 487     free_vector(beta,1);
 488     free_vector(atry,1);
 489     return TRUE;
 490   }
 491   for (j=0,l=1;l<=ma;l++) /* Did the trial succeed? */
 492     if (ia[l]) atry[l]=a[l]+da[++j];
 493   mrqcof_new(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
 494   if (*chisq < ochisq) {
 495     /* Success, accept the new solution. */
 496     *alamda *= 0.1;
 497     ochisq=(*chisq);
 498     for (j=1;j<=mfit;j++) {
 499         for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
 500         beta[j]=da[j];
 501     }
 502     for (l=1;l<=ma;l++) a[l]=atry[l];
 503   } else {               /* Failure, increase alamda and return. */
 504     *alamda *= 10.0;
 505     *chisq=ochisq;
 506   }
 507   return TRUE;
 508 }
 509
 510 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
 511             int ia[], int ma, real **alpha, real beta[], real *chisq,
 512             void (*funcs)(real, real [], real *, real[]))
 513      /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
 514       * vector beta as in (15.5.8), and calculate Chi^2.
 515       */
 516 {
 517   int i,j,k,l,m,mfit=0;
 518   real ymod,wt,sig2i,dy,*dyda;
 519
 520   dyda=rvector(1,ma);
 521   for (j=1;j<=ma;j++)
 522     if (ia[j]) mfit++;
 523   for (j=1;j<=mfit;j++) { /* Initialize (symmetric) alpha), beta. */
 524     for (k=1;k<=j;k++) alpha[j][k]=0.0;
 525     beta[j]=0.0;
 526   }
 527   *chisq=0.0;
 528   for (i=1;i<=ndata;i++) { /* Summation loop over all data. */
 529     (*funcs)(x[i],a,&ymod,dyda);
 530     sig2i=1.0/(sig[i]*sig[i]);
 531     dy=y[i]-ymod;
 532     for (j=0,l=1;l<=ma;l++) {
 533       if (ia[l]) {
 534         wt=dyda[l]*sig2i;
 535         for (j++,k=0,m=1;m<=l;m++)
 536           if (ia[m]) alpha[j][++k] += wt*dyda[m];
 537         beta[j] += dy*wt;
 538       }
 539     }
 540     *chisq += dy*dy*sig2i;  /* And find Chi^2. */
 541   }
 542   for (j=2;j<=mfit;j++)     /* Fill in the symmetric side. */
 543     for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
 544   free_vector(dyda,1);
 545 }