/* * Project:  LevenbergMarquardtLeastSquaresFitting * * File:     lmmin.c * * Contents: Levenberg-Marquardt core implementation, *           and simplified user interface. * * Authors:  Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More *           (lmdif and other routines from the public-domain library *           netlib::minpack, Argonne National Laboratories, March 1980); *           Steve Moshier (initial C translation); *           Joachim Wuttke (conversion into C++ compatible ANSI style, *           corrections, comments, wrappers, hosting). *  * Homepage: www.messen-und-deuten.de/lmfit * * Licence:  Public domain. * * Make:     For instance: gcc -c lmmin.c; ar rc liblmmin.a lmmin.o */ #include <stdlib.h>#include <math.h>#include <float.h>#include "lmmin.h"#define _LMDIF/* *********************** high-level interface **************************** *//* machine-dependent constants from float.h */#define LM_MACHEP     DBL_EPSILON   /* resolution of arithmetic */#define LM_DWARF      DBL_MIN       /* smallest nonzero number */#define LM_SQRT_DWARF sqrt(DBL_MIN) /* square should not underflow */#define LM_SQRT_GIANT sqrt(DBL_MAX) /* square should not overflow */#define LM_USERTOL    30*LM_MACHEP  /* users are recommened to require this *//* If the above values do not work, the following seem good for an x86: LM_MACHEP     .555e-16 LM_DWARF      9.9e-324	 LM_SQRT_DWARF 1.e-160    LM_SQRT_GIANT 1.e150  LM_USER_TOL   1.e-14   The following values should work on any machine: LM_MACHEP     1.2e-16 LM_DWARF      1.0e-38 LM_SQRT_DWARF 3.834e-20 LM_SQRT_GIANT 1.304e19 LM_USER_TOL   1.e-14*/void lm_initialize_control(lm_control_type * control){    control->maxcall = 100;    control->epsilon = LM_USERTOL;    control->stepbound = 100.;    control->ftol = LM_USERTOL;    control->xtol = LM_USERTOL;    control->gtol = LM_USERTOL;}void lm_minimize(int m_dat, int n_par, double *par,		 lm_evaluate_ftype * evaluate, lm_print_ftype * printout,		 void *data, lm_control_type * control){/*** allocate work space. ***/    double *fvec, *diag, *fjac, *qtf, *wa1, *wa2, *wa3, *wa4;    int *ipvt;    int n = n_par;    int m = m_dat;    if (!(fvec = (double *) malloc(m * sizeof(double))) ||	!(diag = (double *) malloc(n * sizeof(double))) ||	!(qtf = (double *) malloc(n * sizeof(double))) ||	!(fjac = (double *) malloc(n * m * sizeof(double))) ||	!(wa1 = (double *) malloc(n * sizeof(double))) ||	!(wa2 = (double *) malloc(n * sizeof(double))) ||	!(wa3 = (double *) malloc(n * sizeof(double))) ||	!(wa4 = (double *) malloc(m * sizeof(double))) ||	!(ipvt = (int *) malloc(n * sizeof(int)))) {	control->info = 9;	return;    }/*** perform fit. ***/    control->info = 0;    control->nfev = 0;    /* this goes through the modified legacy interface: */    lm_lmdif(m, n, par, fvec, control->ftol, control->xtol, control->gtol,	     control->maxcall * (n + 1), control->epsilon, diag, 1,	     control->stepbound, &(control->info),	     &(control->nfev), fjac, ipvt, qtf, wa1, wa2, wa3, wa4,	     evaluate, printout, data);    (*printout) (n, par, m, fvec, data, -1, 0, control->nfev);    control->fnorm = lm_enorm(m, fvec);    if (control->info < 0)	control->info = 10;/*** clean up. ***/    free(fvec);    free(diag);    free(qtf);    free(fjac);    free(wa1);    free(wa2);    free(wa3);    free(wa4);    free(ipvt);} /*** lm_minimize. ***//*** the following messages are referenced by the variable info. ***/const char *lm_infmsg[] = {    "improper input parameters",    "the relative error in the sum of squares is at most tol",    "the relative error between x and the solution is at most tol",    "both errors are at most tol",    "fvec is orthogonal to the columns of the jacobian to machine precision",    "number of calls to fcn has reached or exceeded maxcall*(n+1)",    "ftol is too small: no further reduction in the sum of squares is possible",    "xtol too small: no further improvement in approximate solution x possible",    "gtol too small: no further improvement in approximate solution x possible",    "not enough memory",    "break requested within function evaluation"};const char *lm_shortmsg[] = {    "invalid input",    "success (f)",    "success (p)",    "success (f,p)",    "degenerate",    "call limit",    "failed (f)",    "failed (p)",    "failed (o)",    "no memory",    "user break"};/* ************************** implementation ******************************* */#define BUG 0#if BUG#include <stdio.h>#endifvoid lm_qrfac(int m, int n, double *a, int pivot, int *ipvt,	      double *rdiag, double *acnorm, double *wa);void lm_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,	       double *qtb, double *x, double *sdiag, double *wa);void lm_lmpar(int n, double *r, int ldr, int *ipvt, double *diag,	      double *qtb, double delta, double *par, double *x,	      double *sdiag, double *wa1, double *wa2);#define MIN(a,b) (((a)<=(b)) ? (a) : (b))#define MAX(a,b) (((a)>=(b)) ? (a) : (b))#define SQR(x)   (x)*(x)/***** the low-level legacy interface for full control. *****/void lm_lmdif(int m, int n, double *x, double *fvec, double ftol,	      double xtol, double gtol, int maxfev, double epsfcn,	      double *diag, int mode, double factor, int *info, int *nfev,	      double *fjac, int *ipvt, double *qtf, double *wa1,	      double *wa2, double *wa3, double *wa4,	      lm_evaluate_ftype * evaluate, lm_print_ftype * printout,	      void *data){/* *   The purpose of lmdif is to minimize the sum of the squares of *   m nonlinear functions in n variables by a modification of *   the levenberg-marquardt algorithm. The user must provide a *   subroutine evaluate which calculates the functions. The jacobian *   is then calculated by a forward-difference approximation. * *   The multi-parameter interface lm_lmdif is for users who want *   full control and flexibility. Most users will be better off using *   the simpler interface lm_minimize provided above. * *   The parameters are the same as in the legacy FORTRAN implementation, *   with the following exceptions: *      the old parameter ldfjac which gave leading dimension of fjac has *        been deleted because this C translation makes no use of two- *        dimensional arrays; *      the old parameter nprint has been deleted; printout is now controlled *        by the user-supplied routine *printout; *      the parameter field *data and the function parameters *evaluate and *        *printout have been added; they help avoiding global variables. * *   Parameters: * *	m is a positive integer input variable set to the number *	  of functions. * *	n is a positive integer input variable set to the number *	  of variables; n must not exceed m. * *	x is an array of length n. On input x must contain *	  an initial estimate of the solution vector. on output x *	  contains the final estimate of the solution vector. * *	fvec is an output array of length m which contains *	  the functions evaluated at the output x. * *	ftol is a nonnegative input variable. termination *	  occurs when both the actual and predicted relative *	  reductions in the sum of squares are at most ftol. *	  Therefore, ftol measures the relative error desired *	  in the sum of squares. * *	xtol is a nonnegative input variable. Termination *	  occurs when the relative error between two consecutive *	  iterates is at most xtol. Therefore, xtol measures the *	  relative error desired in the approximate solution. * *	gtol is a nonnegative input variable. Termination *	  occurs when the cosine of the angle between fvec and *	  any column of the jacobian is at most gtol in absolute *	  value. Therefore, gtol measures the orthogonality *	  desired between the function vector and the columns *	  of the jacobian. * *	maxfev is a positive integer input variable. Termination *	  occurs when the number of calls to lm_fcn is at least *	  maxfev by the end of an iteration. * *	epsfcn is an input variable used in determining a suitable *	  step length for the forward-difference approximation. This *	  approximation assumes that the relative errors in the *	  functions are of the order of epsfcn. If epsfcn is less *	  than the machine precision, it is assumed that the relative *	  errors in the functions are of the order of the machine *	  precision. * *	diag is an array of length n. If mode = 1 (see below), diag is *        internally set. If mode = 2, diag must contain positive entries *        that serve as multiplicative scale factors for the variables. * *	mode is an integer input variable. If mode = 1, the *	  variables will be scaled internally. If mode = 2, *	  the scaling is specified by the input diag. other *	  values of mode are equivalent to mode = 1. * *	factor is a positive input variable used in determining the *	  initial step bound. This bound is set to the product of *	  factor and the euclidean norm of diag*x if nonzero, or else *	  to factor itself. In most cases factor should lie in the *	  interval (0.1,100.0). Generally, the value 100.0 is recommended. * *	info is an integer output variable that indicates the termination *        status of lm_lmdif as follows: * *        info < 0  termination requested by user-supplied routine *evaluate; * *	  info = 0  improper input parameters; * *	  info = 1  both actual and predicted relative reductions *		    in the sum of squares are at most ftol; * *	  info = 2  relative error between two consecutive iterates *		    is at most xtol; * *	  info = 3  conditions for info = 1 and info = 2 both hold; * *	  info = 4  the cosine of the angle between fvec and any *		    column of the jacobian is at most gtol in *		    absolute value; * *	  info = 5  number of calls to lm_fcn has reached or *		    exceeded maxfev; * *	  info = 6  ftol is too small: no further reduction in *		    the sum of squares is possible; * *	  info = 7  xtol is too small: no further improvement in *		    the approximate solution x is possible; * *	  info = 8  gtol is too small: fvec is orthogonal to the *		    columns of the jacobian to machine precision; * *	nfev is an output variable set to the number of calls to the *        user-supplied routine *evaluate. * *	fjac is an output m by n array. The upper n by n submatrix *	  of fjac contains an upper triangular matrix r with *	  diagonal elements of nonincreasing magnitude such that * *		 t     t	   t *		p *(jac *jac)*p = r *r, * *	  where p is a permutation matrix and jac is the final *	  calculated jacobian. Column j of p is column ipvt(j) *	  (see below) of the identity matrix. The lower trapezoidal *	  part of fjac contains information generated during *	  the computation of r. * *	ipvt is an integer output array of length n. It defines a *        permutation matrix p such that jac*p = q*r, where jac is *        the final calculated jacobian, q is orthogonal (not stored), *        and r is upper triangular with diagonal elements of *        nonincreasing magnitude. Column j of p is column ipvt(j) *        of the identity matrix. * *	qtf is an output array of length n which contains *	  the first n elements of the vector (q transpose)*fvec. * *	wa1, wa2, and wa3 are work arrays of length n. * *	wa4 is a work array of length m. * *   The following parameters are newly introduced in this C translation: * *      evaluate is the name of the subroutine which calculates the *        m nonlinear functions. A default implementation lm_evaluate_default *        is provided in lm_eval.c. Alternative implementations should *        be written as follows: * *        void evaluate ( double* par, int m_dat, double* fvec,  *                       void *data, int *info ) *        { *           // for ( i=0; i<m_dat; ++i ) *           //     calculate fvec[i] for given parameters par; *           // to stop the minimization,  *           //     set *info to a negative integer. *        } * *      printout is the name of the subroutine which nforms about fit progress. *        A default implementation lm_print_default is provided in lm_eval.c. *        Alternative implementations should be written as follows: * *        void printout ( int n_par, double* par, int m_dat, double* fvec,  *                       void *data, int iflag, int iter, int nfev ) *        { *           // iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated) *           // iter  : outer loop counter *           // nfev  : number of calls to *evaluate *        } * *      data is an input pointer to an arbitrary structure that is passed to *        evaluate. Typically, it contains experimental data to be fitted. * */    int i, iter, j;    double actred, delta, dirder, eps, fnorm, fnorm1, gnorm, par, pnorm,	prered, ratio, step, sum, temp, temp1, temp2, temp3, xnorm;    static double p1 = 0.1;    static double p5 = 0.5;    static double p25 = 0.25;    static double p75 = 0.75;    static double p0001 = 1.0e-4;    *nfev = 0;			/* function evaluation counter */    iter = 1;			/* outer loop counter */    par = 0;			/* levenberg-marquardt parameter */    delta = 0;	 /* to prevent a warning (initialization within if-clause) */    xnorm = 0;	 /* ditto */    temp = MAX(epsfcn, LM_MACHEP);    eps = sqrt(temp); /* for calculating the Jacobian by forward differences *//*** lmdif: check input parameters for errors. ***/    if ((n <= 0) || (m < n) || (ftol < 0.)	|| (xtol < 0.) || (gtol < 0.) || (maxfev <= 0) || (factor <= 0.)) {	*info = 0;		// invalid parameter	return;    }    if (mode == 2) {		/* scaling by diag[] */	for (j = 0; j < n; j++) {	/* check for nonpositive elements */	    if (diag[j] <= 0.0) {		*info = 0;	// invalid parameter		return;	    }	}    }#if BUG    printf("lmdif\n");#endif/*** lmdif: evaluate function at starting point and calculate norm. ***/    *info = 0;    (*evaluate) (x, m, fvec, data, info);    (*printout) (n, x, m, fvec, data, 0, 0, ++(*nfev));    if (*info < 0)	return;    fnorm = lm_enorm(m, fvec);/*** lmdif: the outer loop. ***/    do {#if BUG	printf("lmdif/ outer loop iter=%d nfev=%d fnorm=%.10e\n",	       iter, *nfev, fnorm);#endif/*** outer: calculate the jacobian matrix. ***/	for (j = 0; j < n; j++) {	    temp = x[j];	    step = eps * fabs(temp);	    if (step == 0.)		step = eps;	    x[j] = temp + step;	    *info = 0;	    (*evaluate) (x, m, wa4, data, info);	    (*printout) (n, x, m, wa4, data, 1, iter, ++(*nfev));	    if (*info < 0)		return;	/* user requested break */	    for (i = 0; i < m; i++) /* changed in 2.3, Mark Bydder */		fjac[j * m + i] = (wa4[i] - fvec[i]) / (x[j] - temp);	    x[j] = temp;