src/tools/levenmar.c

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