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 static double *dvector(int nl, int nh)
  79 {
  80   double *v;
  81
  82   v=(double *)malloc((unsigned) (nh-nl+1)*sizeof(double));
  83   if (!v) nrerror("allocation failure in dvector()", TRUE);
  84   return v-nl;
  85 }
  86
  87
  88
  89 static real **matrix1(int nrl, int nrh, int ncl, int nch)
  90 {
  91         int i;
  92         real **m;
  93
  94         m=(real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*));
  95         if (!m) nrerror("allocation failure 1 in matrix1()", TRUE);
  96         m -= nrl;
  97
  98         for(i=nrl;i<=nrh;i++) {
  99                 m[i]=(real *) malloc((unsigned) (nch-ncl+1)*sizeof(real));
 100                 if (!m[i]) nrerror("allocation failure 2 in matrix1()", TRUE);
 101                 m[i] -= ncl;
 102         }
 103         return m;
 104 }
 105
 106 static double **dmatrix(int nrl, int nrh, int ncl, int nch)
 107 {
 108         int i;
 109         double **m;
 110
 111         m=(double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
 112         if (!m) nrerror("allocation failure 1 in dmatrix()", TRUE);
 113         m -= nrl;
 114
 115         for(i=nrl;i<=nrh;i++) {
 116                 m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
 117                 if (!m[i]) nrerror("allocation failure 2 in dmatrix()", TRUE);
 118                 m[i] -= ncl;
 119         }
 120         return m;
 121 }
 122
 123 static int **imatrix1(int nrl, int nrh, int ncl, int nch)
 124 {
 125         int i,**m;
 126
 127         m=(int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
 128         if (!m) nrerror("allocation failure 1 in imatrix1()", TRUE);
 129         m -= nrl;
 130
 131         for(i=nrl;i<=nrh;i++) {
 132                 m[i]=(int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
 133                 if (!m[i]) nrerror("allocation failure 2 in imatrix1()", TRUE);
 134                 m[i] -= ncl;
 135         }
 136         return m;
 137 }
 138
 139
 140
 141 static real **submatrix(real **a, int oldrl, int oldrh, int oldcl,
 142                  int newrl, int newcl)
 143 {
 144         int i,j;
 145         real **m;
 146
 147         m=(real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*));
 148         if (!m) nrerror("allocation failure in submatrix()", TRUE);
 149         m -= newrl;
 150
 151         for(i=oldrl,j=newrl;i<=oldrh;i++,j++) m[j]=a[i]+oldcl-newcl;
 152
 153         return m;
 154 }
 155
 156
 157
 158 static void free_vector(real *v, int nl)
 159 {
 160         free((char*) (v+nl));
 161 }
 162
 163 static void free_ivector(int *v, int nl)
 164 {
 165         free((char*) (v+nl));
 166 }
 167
 168 static void free_dvector(int *v, int nl)
 169 {
 170         free((char*) (v+nl));
 171 }
 172
 173
 174
 175 static void free_matrix(real **m, int nrl, int nrh, int ncl)
 176 {
 177         int i;
 178
 179         for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
 180         free((char*) (m+nrl));
 181 }
 182
 183 static void free_dmatrix(double **m, int nrl, int nrh, int ncl)
 184 {
 185         int i;
 186
 187         for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
 188         free((char*) (m+nrl));
 189 }
 190
 191 static void free_imatrix(int **m, int nrl, int nrh, int ncl)
 192 {
 193         int i;
 194
 195         for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
 196         free((char*) (m+nrl));
 197 }
 198
 199
 200
 201 static void free_submatrix(real **b, int nrl)
 202 {
 203         free((char*) (b+nrl));
 204 }
 205
 206
 207
 208 static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch)
 209 {
 210         int i,j,nrow,ncol;
 211         real **m;
 212
 213         nrow=nrh-nrl+1;
 214         ncol=nch-ncl+1;
 215         m = (real **) malloc((unsigned) (nrow)*sizeof(real*));
 216         if (!m) nrerror("allocation failure in convert_matrix()", TRUE);
 217         m -= nrl;
 218         for(i=0,j=nrl;i<=nrow-1;i++,j++) m[j]=a+ncol*i-ncl;
 219         return m;
 220 }
 221
 222
 223
 224 static void free_convert_matrix(real **b, int nrl)
 225 {
 226         free((char*) (b+nrl));
 227 }
 228
 229 #define SWAP(a,b) {real temp=(a);(a)=(b);(b)=temp;}
 230
 231 static void dump_mat(int n,real **a)
 232 {
 233   int i,j;
 234
 235   for(i=1; (i<=n); i++) {
 236     for(j=1; (j<=n); j++)
 237       fprintf(stderr,"  %10.3f",a[i][j]);
 238     fprintf(stderr,"\n");
 239   }
 240 }
 241
 242 gmx_bool gaussj(real **a, int n, real **b, int m)
 243 {
 244   int *indxc,*indxr,*ipiv;
 245   int i,icol=0,irow=0,j,k,l,ll;
 246   real big,dum,pivinv;
 247
 248   indxc=ivector(1,n);
 249   indxr=ivector(1,n);
 250   ipiv=ivector(1,n);
 251   for (j=1;j<=n;j++) ipiv[j]=0;
 252   for (i=1;i<=n;i++) {
 253     big=0.0;
 254     for (j=1;j<=n;j++)
 255       if (ipiv[j] != 1)
 256         for (k=1;k<=n;k++) {
 257           if (ipiv[k] == 0) {
 258             if (fabs(a[j][k]) >= big) {
 259               big=fabs(a[j][k]);
 260               irow=j;
 261               icol=k;
 262             }
 263           } else if (ipiv[k] > 1) {
 264             nrerror("GAUSSJ: Singular Matrix-1", FALSE);
 265             return FALSE;
 266           }
 267         }
 268     ++(ipiv[icol]);
 269     if (irow != icol) {
 270       for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
 271         for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
 272     }
 273     indxr[i]=irow;
 274     indxc[i]=icol;
 275     if (a[icol][icol] == 0.0) {
 276       fprintf(stderr,"irow = %d, icol = %d\n",irow,icol);
 277       dump_mat(n,a);
 278       nrerror("GAUSSJ: Singular Matrix-2", FALSE);
 279       return FALSE;
 280     }
 281     pivinv=1.0/a[icol][icol];
 282     a[icol][icol]=1.0;
 283     for (l=1;l<=n;l++) a[icol][l] *= pivinv;
 284     for (l=1;l<=m;l++) b[icol][l] *= pivinv;
 285     for (ll=1;ll<=n;ll++)
 286       if (ll != icol) {
 287         dum=a[ll][icol];
 288         a[ll][icol]=0.0;
 289         for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
 290         for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
 291       }
 292   }
 293   for (l=n;l>=1;l--) {
 294     if (indxr[l] != indxc[l])
 295       for (k=1;k<=n;k++)
 296         SWAP(a[k][indxr[l]],a[k][indxc[l]]);
 297   }
 298   free_ivector(ipiv,1);
 299   free_ivector(indxr,1);
 300   free_ivector(indxc,1);
 301
 302   return TRUE;
 303 }
 304
 305 #undef SWAP
 306
 307
 308 static void covsrt(real **covar, int ma, int lista[], int mfit)
 309 {
 310   int i,j;
 311   real swap;
 312
 313   for (j=1;j<ma;j++)
 314     for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
 315   for (i=1;i<mfit;i++)
 316     for (j=i+1;j<=mfit;j++) {
 317       if (lista[j] > lista[i])
 318         covar[lista[j]][lista[i]]=covar[i][j];
 319       else
 320         covar[lista[i]][lista[j]]=covar[i][j];
 321     }
 322   swap=covar[1][1];
 323   for (j=1;j<=ma;j++) {
 324     covar[1][j]=covar[j][j];
 325     covar[j][j]=0.0;
 326   }
 327   covar[lista[1]][lista[1]]=swap;
 328   for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
 329   for (j=2;j<=ma;j++)
 330     for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
 331 }
 332
 333 #define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
 334
 335 static void covsrt_new(real **covar,int ma, int ia[], int mfit)
 336      /* Expand in storage the covariance matrix covar, so as to take
 337       * into account parameters that are being held fixed. (For the
 338       * latter, return zero covariances.)
 339       */
 340 {
 341   int i,j,k;
 342   real swap;
 343   for (i=mfit+1;i<=ma;i++)
 344     for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
 345   k=mfit;
 346   for (j=ma;j>=1;j--) {
 347     if (ia[j]) {
 348       for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
 349       for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
 350       k--;
 351     }
 352   }
 353 }
 354 #undef SWAP
 355
 356 static void mrqcof(real x[], real y[], real sig[], int ndata, real a[],
 357                    int ma, int lista[], int mfit,
 358                    real **alpha, real beta[], real *chisq,
 359                    void (*funcs)(real,real *,real *,real *))
 360 {
 361   int k,j,i;
 362   real ymod,wt,sig2i,dy,*dyda;
 363
 364   dyda=rvector(1,ma);
 365   for (j=1;j<=mfit;j++) {
 366     for (k=1;k<=j;k++) alpha[j][k]=0.0;
 367     beta[j]=0.0;
 368   }
 369   *chisq=0.0;
 370   for (i=1;i<=ndata;i++) {
 371     (*funcs)(x[i],a,&ymod,dyda);
 372     sig2i=1.0/(sig[i]*sig[i]);
 373     dy=y[i]-ymod;
 374     for (j=1;j<=mfit;j++) {
 375       wt=dyda[lista[j]]*sig2i;
 376       for (k=1;k<=j;k++)
 377         alpha[j][k] += wt*dyda[lista[k]];
 378       beta[j] += dy*wt;
 379     }
 380     (*chisq) += dy*dy*sig2i;
 381   }
 382   for (j=2;j<=mfit;j++)
 383     for (k=1;k<=j-1;k++) alpha[k][j]=alpha[j][k];
 384   free_vector(dyda,1);
 385 }
 386
 387
 388 gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
 389             int ma, int lista[], int mfit,
 390             real **covar, real **alpha, real *chisq,
 391             void (*funcs)(real,real *,real *,real *),
 392             real *alamda)
 393 {
 394   int k,kk,j,ihit;
 395   static real *da,*atry,**oneda,*beta,ochisq;
 396
 397   if (*alamda < 0.0) {
 398     oneda=matrix1(1,mfit,1,1);
 399     atry=rvector(1,ma);
 400     da=rvector(1,ma);
 401     beta=rvector(1,ma);
 402     kk=mfit+1;
 403     for (j=1;j<=ma;j++) {
 404       ihit=0;
 405       for (k=1;k<=mfit;k++)
 406         if (lista[k] == j) ihit++;
 407       if (ihit == 0)
 408         lista[kk++]=j;
 409       else if (ihit > 1) {
 410         nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
 411         return FALSE;
 412       }
 413     }
 414     if (kk != ma+1) {
 415       nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
 416       return FALSE;
 417     }
 418     *alamda=0.001;
 419     mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
 420     ochisq=(*chisq);
 421   }
 422   for (j=1;j<=mfit;j++) {
 423     for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
 424     covar[j][j]=alpha[j][j]*(1.0+(*alamda));
 425     oneda[j][1]=beta[j];
 426   }
 427   if (!gaussj(covar,mfit,oneda,1))
 428     return FALSE;
 429   for (j=1;j<=mfit;j++)
 430     da[j]=oneda[j][1];
 431   if (*alamda == 0.0) {
 432     covsrt(covar,ma,lista,mfit);
 433     free_vector(beta,1);
 434     free_vector(da,1);
 435     free_vector(atry,1);
 436     free_matrix(oneda,1,mfit,1);
 437     return TRUE;
 438   }
 439   for (j=1;j<=ma;j++) atry[j]=a[j];
 440   for (j=1;j<=mfit;j++)
 441     atry[lista[j]] = a[lista[j]]+da[j];
 442   mrqcof(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
 443   if (*chisq < ochisq) {
 444     *alamda *= 0.1;
 445     ochisq=(*chisq);
 446     for (j=1;j<=mfit;j++) {
 447       for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
 448       beta[j]=da[j];
 449       a[lista[j]]=atry[lista[j]];
 450     }
 451   } else {
 452     *alamda *= 10.0;
 453     *chisq=ochisq;
 454   }
 455   return TRUE;
 456 }
 457
 458
 459 gmx_bool mrqmin_new(real x[],real y[],real sig[],int ndata,real a[],
 460                 int ia[],int ma,real **covar,real **alpha,real *chisq,
 461                 void (*funcs)(real, real [], real *, real []),
 462                 real *alamda)
 463      /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
 464       * of a fit between a set of data points x[1..ndata], y[1..ndata]
 465       * with individual standard deviations sig[1..ndata], and a nonlinear
 466       * function dependent on ma coefficients a[1..ma]. The input array
 467       * ia[1..ma] indicates by nonzero entries those components of a that
 468       * should be fitted for, and by zero entries those components that
 469       * should be held fixed at their input values. The program returns
 470       * current best-fit values for the parameters a[1..ma], and
 471       * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
 472       * are used as working space during most iterations. Supply a routine
 473       * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
 474       * and its derivatives dyda[1..ma] with respect to the fitting
 475       * parameters a at x. On the first call provide an initial guess for
 476       * the parameters a, and set alamda < 0 for initialization (which then
 477       * sets alamda=.001). If a step succeeds chisq becomes smaller and
 478       * alamda de-creases by a factor of 10. If a step fails alamda grows by
 479       * a factor of 10. You must call this routine repeatedly until
 480       * convergence is achieved. Then, make one final call with alamda=0,
 481       * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
 482       * the curvature matrix.
 483       * (Parameters held fixed will return zero covariances.)
 484       */
 485 {
 486   void covsrt(real **covar, int ma, int ia[], int mfit);
 487   gmx_bool gaussj(real **a, int n, real **b,int m);
 488   void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
 489               int ia[], int ma, real **alpha, real beta[], real *chisq,
 490               void (*funcs)(real, real [], real *, real []));
 491   int j,k,l;
 492   static int mfit;
 493   static real ochisq,*atry,*beta,*da,**oneda;
 494
 495   if (*alamda < 0.0) {                    /* Initialization. */
 496     atry=rvector(1,ma);
 497     beta=rvector(1,ma);
 498     da=rvector(1,ma);
 499     for (mfit=0,j=1;j<=ma;j++)
 500       if (ia[j]) mfit++;
 501     oneda=matrix1(1,mfit,1,1);
 502     *alamda=0.001;
 503     mrqcof_new(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
 504     ochisq=(*chisq);
 505     for (j=1;j<=ma;j++)
 506       atry[j]=a[j];
 507   }
 508   for (j=1;j<=mfit;j++) { /* Alter linearized fitting matrix, by augmenting. */
 509     for (k=1;k<=mfit;k++)
 510       covar[j][k]=alpha[j][k]; /* diagonal elements. */
 511     covar[j][j]=alpha[j][j]*(1.0+(*alamda));
 512     oneda[j][1]=beta[j];
 513   }
 514   if (!gaussj(covar,mfit,oneda,1))      /* Matrix solution. */
 515     return FALSE;
 516   for (j=1;j<=mfit;j++)
 517     da[j]=oneda[j][1];
 518   if (*alamda == 0.0) { /* Once converged, evaluate covariance matrix. */
 519     covsrt_new(covar,ma,ia,mfit);
 520     free_matrix(oneda,1,mfit,1);
 521     free_vector(da,1);
 522     free_vector(beta,1);
 523     free_vector(atry,1);
 524     return TRUE;
 525   }
 526   for (j=0,l=1;l<=ma;l++) /* Did the trial succeed? */
 527     if (ia[l]) atry[l]=a[l]+da[++j];
 528   mrqcof_new(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
 529   if (*chisq < ochisq) {
 530     /* Success, accept the new solution. */
 531     *alamda *= 0.1;
 532     ochisq=(*chisq);
 533     for (j=1;j<=mfit;j++) {
 534         for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
 535         beta[j]=da[j];
 536     }
 537     for (l=1;l<=ma;l++) a[l]=atry[l];
 538   } else {               /* Failure, increase alamda and return. */
 539     *alamda *= 10.0;
 540     *chisq=ochisq;
 541   }
 542   return TRUE;
 543 }
 544
 545 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
 546             int ia[], int ma, real **alpha, real beta[], real *chisq,
 547             void (*funcs)(real, real [], real *, real[]))
 548      /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
 549       * vector beta as in (15.5.8), and calculate Chi^2.
 550       */
 551 {
 552   int i,j,k,l,m,mfit=0;
 553   real ymod,wt,sig2i,dy,*dyda;
 554
 555   dyda=rvector(1,ma);
 556   for (j=1;j<=ma;j++)
 557     if (ia[j]) mfit++;
 558   for (j=1;j<=mfit;j++) { /* Initialize (symmetric) alpha), beta. */
 559     for (k=1;k<=j;k++) alpha[j][k]=0.0;
 560     beta[j]=0.0;
 561   }
 562   *chisq=0.0;
 563   for (i=1;i<=ndata;i++) { /* Summation loop over all data. */
 564     (*funcs)(x[i],a,&ymod,dyda);
 565     sig2i=1.0/(sig[i]*sig[i]);
 566     dy=y[i]-ymod;
 567     for (j=0,l=1;l<=ma;l++) {
 568       if (ia[l]) {
 569         wt=dyda[l]*sig2i;
 570         for (j++,k=0,m=1;m<=l;m++)
 571           if (ia[m]) alpha[j][++k] += wt*dyda[m];
 572         beta[j] += dy*wt;
 573       }
 574     }
 575     *chisq += dy*dy*sig2i;  /* And find Chi^2. */
 576   }
 577   for (j=2;j<=mfit;j++)     /* Fill in the symmetric side. */
 578     for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
 579   free_vector(dyda,1);
 580 }