2 * $Id: levenmar.c,v 1.20 2004/01/23 18:11:02 lindahl Exp $
4 * This source code is part of
8 * GROningen MAchine for Chemical Simulations
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.
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.
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.
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.
31 * For more info, check our website at http://www.gromacs.org
34 * Green Red Orange Magenta Azure Cyan Skyblue
44 #include "types/simple.h"
46 static void nrerror(const char error_text[], gmx_bool bExit)
48 fprintf(stderr, "Numerical Recipes run-time error...\n");
49 fprintf(stderr, "%s\n", error_text);
52 fprintf(stderr, "...now exiting to system...\n");
57 /* dont use the keyword vector - it will clash with the
58 * altivec extensions used for powerpc processors.
61 static real *rvector(int nl, int nh)
65 v = (real *)malloc((unsigned) (nh-nl+1)*sizeof(real));
68 nrerror("allocation failure in rvector()", TRUE);
73 static int *ivector(int nl, int nh)
77 v = (int *)malloc((unsigned) (nh-nl+1)*sizeof(int));
80 nrerror("allocation failure in ivector()", TRUE);
86 static real **matrix1(int nrl, int nrh, int ncl, int nch)
91 m = (real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*));
94 nrerror("allocation failure 1 in matrix1()", TRUE);
98 for (i = nrl; i <= nrh; i++)
100 m[i] = (real *) malloc((unsigned) (nch-ncl+1)*sizeof(real));
103 nrerror("allocation failure 2 in matrix1()", TRUE);
110 static double **dmatrix(int nrl, int nrh, int ncl, int nch)
115 m = (double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
118 nrerror("allocation failure 1 in dmatrix()", TRUE);
122 for (i = nrl; i <= nrh; i++)
124 m[i] = (double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
127 nrerror("allocation failure 2 in dmatrix()", TRUE);
134 static int **imatrix1(int nrl, int nrh, int ncl, int nch)
138 m = (int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
141 nrerror("allocation failure 1 in imatrix1()", TRUE);
145 for (i = nrl; i <= nrh; i++)
147 m[i] = (int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
150 nrerror("allocation failure 2 in imatrix1()", TRUE);
159 static real **submatrix(real **a, int oldrl, int oldrh, int oldcl,
160 int newrl, int newcl)
165 m = (real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*));
168 nrerror("allocation failure in submatrix()", TRUE);
172 for (i = oldrl, j = newrl; i <= oldrh; i++, j++)
174 m[j] = a[i]+oldcl-newcl;
182 static void free_vector(real *v, int nl)
184 free((char*) (v+nl));
187 static void free_ivector(int *v, int nl)
189 free((char*) (v+nl));
192 static void free_dvector(int *v, int nl)
194 free((char*) (v+nl));
199 static void free_matrix(real **m, int nrl, int nrh, int ncl)
203 for (i = nrh; i >= nrl; i--)
205 free((char*) (m[i]+ncl));
207 free((char*) (m+nrl));
210 static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch)
212 int i, j, nrow, ncol;
217 m = (real **) malloc((unsigned) (nrow)*sizeof(real*));
220 nrerror("allocation failure in convert_matrix()", TRUE);
223 for (i = 0, j = nrl; i <= nrow-1; i++, j++)
232 static void free_convert_matrix(real **b, int nrl)
234 free((char*) (b+nrl));
237 #define SWAP(a, b) {real temp = (a); (a) = (b); (b) = temp; }
239 static void dump_mat(int n, real **a)
243 for (i = 1; (i <= n); i++)
245 for (j = 1; (j <= n); j++)
247 fprintf(stderr, " %10.3f", a[i][j]);
249 fprintf(stderr, "\n");
253 gmx_bool gaussj(real **a, int n, real **b, int m)
255 int *indxc, *indxr, *ipiv;
256 int i, icol = 0, irow = 0, j, k, l, ll;
257 real big, dum, pivinv;
259 indxc = ivector(1, n);
260 indxr = ivector(1, n);
261 ipiv = ivector(1, n);
262 for (j = 1; j <= n; j++)
266 for (i = 1; i <= n; i++)
269 for (j = 1; j <= n; j++)
273 for (k = 1; k <= n; k++)
277 if (fabs(a[j][k]) >= big)
284 else if (ipiv[k] > 1)
286 nrerror("GAUSSJ: Singular Matrix-1", FALSE);
295 for (l = 1; l <= n; l++)
297 SWAP(a[irow][l], a[icol][l]);
299 for (l = 1; l <= m; l++)
301 SWAP(b[irow][l], b[icol][l]);
306 if (a[icol][icol] == 0.0)
308 fprintf(stderr, "irow = %d, icol = %d\n", irow, icol);
310 nrerror("GAUSSJ: Singular Matrix-2", FALSE);
313 pivinv = 1.0/a[icol][icol];
315 for (l = 1; l <= n; l++)
317 a[icol][l] *= pivinv;
319 for (l = 1; l <= m; l++)
321 b[icol][l] *= pivinv;
323 for (ll = 1; ll <= n; ll++)
329 for (l = 1; l <= n; l++)
331 a[ll][l] -= a[icol][l]*dum;
333 for (l = 1; l <= m; l++)
335 b[ll][l] -= b[icol][l]*dum;
340 for (l = n; l >= 1; l--)
342 if (indxr[l] != indxc[l])
344 for (k = 1; k <= n; k++)
346 SWAP(a[k][indxr[l]], a[k][indxc[l]]);
350 free_ivector(ipiv, 1);
351 free_ivector(indxr, 1);
352 free_ivector(indxc, 1);
360 static void covsrt(real **covar, int ma, int lista[], int mfit)
365 for (j = 1; j < ma; j++)
367 for (i = j+1; i <= ma; i++)
372 for (i = 1; i < mfit; i++)
374 for (j = i+1; j <= mfit; j++)
376 if (lista[j] > lista[i])
378 covar[lista[j]][lista[i]] = covar[i][j];
382 covar[lista[i]][lista[j]] = covar[i][j];
387 for (j = 1; j <= ma; j++)
389 covar[1][j] = covar[j][j];
392 covar[lista[1]][lista[1]] = swap;
393 for (j = 2; j <= mfit; j++)
395 covar[lista[j]][lista[j]] = covar[1][j];
397 for (j = 2; j <= ma; j++)
399 for (i = 1; i <= j-1; i++)
401 covar[i][j] = covar[j][i];
406 #define SWAP(a, b) {swap = (a); (a) = (b); (b) = swap; }
408 static void covsrt_new(real **covar, int ma, int ia[], int mfit)
409 /* Expand in storage the covariance matrix covar, so as to take
410 * into account parameters that are being held fixed. (For the
411 * latter, return zero covariances.)
416 for (i = mfit+1; i <= ma; i++)
418 for (j = 1; j <= i; j++)
420 covar[i][j] = covar[j][i] = 0.0;
424 for (j = ma; j >= 1; j--)
428 for (i = 1; i <= ma; i++)
430 SWAP(covar[i][k], covar[i][j]);
432 for (i = 1; i <= ma; i++)
434 SWAP(covar[k][i], covar[j][i]);
442 static void mrqcof(real x[], real y[], real sig[], int ndata, real a[],
443 int ma, int lista[], int mfit,
444 real **alpha, real beta[], real *chisq,
445 void (*funcs)(real, real *, real *, real *))
448 real ymod, wt, sig2i, dy, *dyda;
450 dyda = rvector(1, ma);
451 for (j = 1; j <= mfit; j++)
453 for (k = 1; k <= j; k++)
460 for (i = 1; i <= ndata; i++)
462 (*funcs)(x[i], a, &ymod, dyda);
463 sig2i = 1.0/(sig[i]*sig[i]);
465 for (j = 1; j <= mfit; j++)
467 wt = dyda[lista[j]]*sig2i;
468 for (k = 1; k <= j; k++)
470 alpha[j][k] += wt*dyda[lista[k]];
474 (*chisq) += dy*dy*sig2i;
476 for (j = 2; j <= mfit; j++)
478 for (k = 1; k <= j-1; k++)
480 alpha[k][j] = alpha[j][k];
483 free_vector(dyda, 1);
487 gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
488 int ma, int lista[], int mfit,
489 real **covar, real **alpha, real *chisq,
490 void (*funcs)(real, real *, real *, real *),
494 static real *da, *atry, **oneda, *beta, ochisq;
498 oneda = matrix1(1, mfit, 1, 1);
499 atry = rvector(1, ma);
501 beta = rvector(1, ma);
503 for (j = 1; j <= ma; j++)
506 for (k = 1; k <= mfit; k++)
519 nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
525 nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
529 mrqcof(x, y, sig, ndata, a, ma, lista, mfit, alpha, beta, chisq, funcs);
532 for (j = 1; j <= mfit; j++)
534 for (k = 1; k <= mfit; k++)
536 covar[j][k] = alpha[j][k];
538 covar[j][j] = alpha[j][j]*(1.0+(*alamda));
539 oneda[j][1] = beta[j];
541 if (!gaussj(covar, mfit, oneda, 1))
545 for (j = 1; j <= mfit; j++)
551 covsrt(covar, ma, lista, mfit);
552 free_vector(beta, 1);
554 free_vector(atry, 1);
555 free_matrix(oneda, 1, mfit, 1);
558 for (j = 1; j <= ma; j++)
562 for (j = 1; j <= mfit; j++)
564 atry[lista[j]] = a[lista[j]]+da[j];
566 mrqcof(x, y, sig, ndata, atry, ma, lista, mfit, covar, da, chisq, funcs);
571 for (j = 1; j <= mfit; j++)
573 for (k = 1; k <= mfit; k++)
575 alpha[j][k] = covar[j][k];
578 a[lista[j]] = atry[lista[j]];
590 gmx_bool mrqmin_new(real x[], real y[], real sig[], int ndata, real a[],
591 int ia[], int ma, real **covar, real **alpha, real *chisq,
592 void (*funcs)(real, real [], real *, real []),
594 /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
595 * of a fit between a set of data points x[1..ndata], y[1..ndata]
596 * with individual standard deviations sig[1..ndata], and a nonlinear
597 * function dependent on ma coefficients a[1..ma]. The input array
598 * ia[1..ma] indicates by nonzero entries those components of a that
599 * should be fitted for, and by zero entries those components that
600 * should be held fixed at their input values. The program returns
601 * current best-fit values for the parameters a[1..ma], and
602 * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
603 * are used as working space during most iterations. Supply a routine
604 * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
605 * and its derivatives dyda[1..ma] with respect to the fitting
606 * parameters a at x. On the first call provide an initial guess for
607 * the parameters a, and set alamda < 0 for initialization (which then
608 * sets alamda=.001). If a step succeeds chisq becomes smaller and
609 * alamda de-creases by a factor of 10. If a step fails alamda grows by
610 * a factor of 10. You must call this routine repeatedly until
611 * convergence is achieved. Then, make one final call with alamda=0,
612 * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
613 * the curvature matrix.
614 * (Parameters held fixed will return zero covariances.)
617 void covsrt(real **covar, int ma, int ia[], int mfit);
618 gmx_bool gaussj(real **a, int n, real **b, int m);
619 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
620 int ia[], int ma, real **alpha, real beta[], real *chisq,
621 void (*funcs)(real, real [], real *, real []));
624 static real ochisq, *atry, *beta, *da, **oneda;
626 if (*alamda < 0.0) /* Initialization. */
628 atry = rvector(1, ma);
629 beta = rvector(1, ma);
631 for (mfit = 0, j = 1; j <= ma; j++)
638 oneda = matrix1(1, mfit, 1, 1);
640 mrqcof_new(x, y, sig, ndata, a, ia, ma, alpha, beta, chisq, funcs);
642 for (j = 1; j <= ma; j++)
647 for (j = 1; j <= mfit; j++) /* Alter linearized fitting matrix, by augmenting. */
649 for (k = 1; k <= mfit; k++)
651 covar[j][k] = alpha[j][k]; /* diagonal elements. */
653 covar[j][j] = alpha[j][j]*(1.0+(*alamda));
654 oneda[j][1] = beta[j];
656 if (!gaussj(covar, mfit, oneda, 1)) /* Matrix solution. */
660 for (j = 1; j <= mfit; j++)
664 if (*alamda == 0.0) /* Once converged, evaluate covariance matrix. */
666 covsrt_new(covar, ma, ia, mfit);
667 free_matrix(oneda, 1, mfit, 1);
669 free_vector(beta, 1);
670 free_vector(atry, 1);
673 for (j = 0, l = 1; l <= ma; l++) /* Did the trial succeed? */
677 atry[l] = a[l]+da[++j];
680 mrqcof_new(x, y, sig, ndata, atry, ia, ma, covar, da, chisq, funcs);
683 /* Success, accept the new solution. */
686 for (j = 1; j <= mfit; j++)
688 for (k = 1; k <= mfit; k++)
690 alpha[j][k] = covar[j][k];
694 for (l = 1; l <= ma; l++)
699 else /* Failure, increase alamda and return. */
707 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
708 int ia[], int ma, real **alpha, real beta[], real *chisq,
709 void (*funcs)(real, real [], real *, real[]))
710 /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
711 * vector beta as in (15.5.8), and calculate Chi^2.
714 int i, j, k, l, m, mfit = 0;
715 real ymod, wt, sig2i, dy, *dyda;
717 dyda = rvector(1, ma);
718 for (j = 1; j <= ma; j++)
725 for (j = 1; j <= mfit; j++) /* Initialize (symmetric) alpha), beta. */
727 for (k = 1; k <= j; k++)
734 for (i = 1; i <= ndata; i++) /* Summation loop over all data. */
736 (*funcs)(x[i], a, &ymod, dyda);
737 sig2i = 1.0/(sig[i]*sig[i]);
739 for (j = 0, l = 1; l <= ma; l++)
744 for (j++, k = 0, m = 1; m <= l; m++)
748 alpha[j][++k] += wt*dyda[m];
754 *chisq += dy*dy*sig2i; /* And find Chi^2. */
756 for (j = 2; j <= mfit; j++) /* Fill in the symmetric side. */
758 for (k = 1; k < j; k++)
760 alpha[k][j] = alpha[j][k];
763 free_vector(dyda, 1);