void lmmin( int n_par, double *par, int m_dat, const void *data,
void (*evaluate) (const double *par, int m_dat, const void *data,
double *fvec, int *info),
const lm_control_struct *control, lm_status_struct *status,
void (*printout) (int n_par, const double *par, int m_dat,
const void *data, const double *fvec,
int printflags, int iflag, int iter, int nfev) )
{
/*** allocate work space. ***/
double *fvec, *diag, *fjac, *qtf, *wa1, *wa2, *wa3, *wa4;
int *ipvt, j;
int n = n_par;
int m = m_dat;
/* One malloc call to allocate several arrays (Frank Polchow, 2013) */
fvec = (double *) malloc( (2*m+5*n+n*m)*sizeof(double) + n*sizeof(int));
if (NULL==fvec) {//fail in allocation
status->info = 9;
return;
}
diag = (double *) &fvec[m];
qtf = (double *) &diag[n];
fjac = (double *) &qtf[n];
wa1 = (double *) &fjac[n*m];
wa2 = (double *) &wa1[n];
wa3 = (double *) &wa2[n];
wa4 = (double *) &wa3[n];
ipvt = (int *) &wa4[m];
/* default diagonal */
if( ! control->scale_diag )
for( j=0; j<n_par; ++j )
diag[j] = 1;
/*** perform fit. ***/
status->info = 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,
( control->scale_diag ? 1 : 2 ), control->stepbound, &(status->info),
&(status->nfev), fjac, ipvt, qtf, wa1, wa2, wa3, wa4,
evaluate, printout, control->printflags, data );
if ( printout )
(*printout)(
n, par, m, data, fvec, control->printflags, -1, 0, status->nfev );
status->fnorm = lm_enorm(m, fvec);
if ( status->info < 0 )
status->info = 11;
/*** clean up. ***/
free(fvec);
} /*** lmmin. ***/
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. ***/
void lm_printout_std( int n_par, const double *par, int m_dat,
const void *data, const double *fvec,
int printflags, int iflag, int iter, int nfev)
/*
* data : for soft control of printout behaviour, add control
* variables to the data struct
* iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
* iter : outer loop counter
* nfev : number of calls to *evaluate
*/
{
int i;
if( !printflags )
return;
if( printflags & 1 ){
/* location of printout call within lmdif */
if (iflag == 2) {
printf("trying step in gradient direction ");
} else if (iflag == 1) {
printf("determining gradient (iteration %2d)", iter);
} else if (iflag == 0) {
printf("starting minimization ");
} else if (iflag == -1) {
printf("terminated after %3d evaluations ", nfev);
}
}
if( printflags & 2 ){
printf(" par: ");
for (i = 0; i < n_par; ++i)
printf(" %18.11g", par[i]);
printf(" => norm: %18.11g", lm_enorm(m_dat, fvec));
}
if( printflags & 3 )
printf( "\n" );
if ( (printflags & 8) || ((printflags & 4) && iflag == -1) ) {
printf(" residuals:\n");
for (i = 0; i < m_dat; ++i)
printf(" fvec[%2d]=%12g\n", i, fvec[i] );
}
}
void lm_print_default( int n_par, double* par, int m_dat, double* fvec,
void *data, int iflag, int iter, int nfev )
/*
* data : for soft control of printout behaviour, add control
* variables to the data struct
* iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
* iter : outer loop counter
* nfev : number of calls to *evaluate
*/
{
double f, y, t;
int i;
lm_data_type *mydata;
mydata = (lm_data_type*)data;
if (iflag==2) {
printf ("trying step in gradient direction\n");
} else if (iflag==1) {
printf ("determining gradient (iteration %d)\n", iter);
} else if (iflag==0) {
printf ("starting minimization\n");
} else if (iflag==-1) {
printf ("terminated after %d evaluations\n", nfev);
}
printf( " par: " );
for( i=0; i<n_par; ++i )
printf( " %12g", par[i] );
printf ( " => norm: %12g\n", lm_enorm( m_dat, fvec ) );
if ( iflag == -1 ) {
printf( " fitting data as follows:\n" );
for( i=0; i<m_dat; ++i ) {
t = (mydata->user_t)[i];
y = (mydata->user_y)[i];
f = mydata->user_func( t, par );
printf( " t[%2d]=%12g y=%12g fit=%12g residue=%12g\n",
i, t, y, f, y-f );
}
}
}
void lm_qrfac(int m, int n, double* a, int pivot, int* ipvt,
double* rdiag, double* acnorm, double* wa)
{
/*
* this subroutine uses householder transformations with column
* pivoting (optional) to compute a qr factorization of the
* m by n matrix a. that is, qrfac determines an orthogonal
* matrix q, a permutation matrix p, and an upper trapezoidal
* matrix r with diagonal elements of nonincreasing magnitude,
* such that a*p = q*r. the householder transformation for
* column k, k = 1,2,...,min(m,n), is of the form
*
* t
* i - (1/u(k))*u*u
*
* where u has 0.s in the first k-1 positions. the form of
* this transformation and the method of pivoting first
* appeared in the corresponding linpack subroutine.
*
* parameters:
*
* m is a positive integer input variable set to the number
* of rows of a.
*
* n is a positive integer input variable set to the number
* of columns of a.
*
* a is an m by n array. on input a contains the matrix for
* which the qr factorization is to be computed. on output
* the strict upper trapezoidal part of a contains the strict
* upper trapezoidal part of r, and the lower trapezoidal
* part of a contains a factored form of q (the non-trivial
* elements of the u vectors described above).
*
* pivot is a logical input variable. if pivot is set true,
* then column pivoting is enforced. if pivot is set false,
* then no column pivoting is done.
*
* ipvt is an integer output array of length lipvt. ipvt
* defines the permutation matrix p such that a*p = q*r.
* column j of p is column ipvt(j) of the identity matrix.
* if pivot is false, ipvt is not referenced.
*
* rdiag is an output array of length n which contains the
* diagonal elements of r.
*
* acnorm is an output array of length n which contains the
* norms of the corresponding columns of the input matrix a.
* if this information is not needed, then acnorm can coincide
* with rdiag.
*
* wa is a work array of length n. if pivot is false, then wa
* can coincide with rdiag.
*
*/
int i, j, k, kmax, minmn;
double ajnorm, sum, temp;
static double p05 = 0.05;
// *** compute the initial column norms and initialize several arrays.
for ( j=0; j<n; j++ )
{
acnorm[j] = lm_enorm(m, &a[j*m]);
rdiag[j] = acnorm[j];
wa[j] = rdiag[j];
if ( pivot )
ipvt[j] = j;
}
#if BUG
printf( "qrfac\n" );
#endif
// *** reduce a to r with householder transformations.
minmn = MIN(m,n);
for ( j=0; j<minmn; j++ )
{
if ( !pivot ) goto pivot_ok;
// *** bring the column of largest norm into the pivot position.
kmax = j;
for ( k=j+1; k<n; k++ )
if (rdiag[k] > rdiag[kmax])
kmax = k;
if (kmax == j) goto pivot_ok; // bug fixed in rel 2.1
for ( i=0; i<m; i++ )
{
temp = a[j*m+i];
a[j*m+i] = a[kmax*m+i];
a[kmax*m+i] = temp;
}
rdiag[kmax] = rdiag[j];
wa[kmax] = wa[j];
k = ipvt[j];
ipvt[j] = ipvt[kmax];
ipvt[kmax] = k;
pivot_ok:
// *** compute the Householder transformation to reduce the
// j-th column of a to a multiple of the j-th unit vector.
ajnorm = lm_enorm( m-j, &a[j*m+j] );
if (ajnorm == 0.)
{
rdiag[j] = 0;
continue;
}
if (a[j*m+j] < 0.)
ajnorm = -ajnorm;
for ( i=j; i<m; i++ )
a[j*m+i] /= ajnorm;
a[j*m+j] += 1;
// *** apply the transformation to the remaining columns
// and update the norms.
for ( k=j+1; k<n; k++ )
{
sum = 0;
for ( i=j; i<m; i++ )
sum += a[j*m+i]*a[k*m+i];
temp = sum/a[j+m*j];
for ( i=j; i<m; i++ )
a[k*m+i] -= temp * a[j*m+i];
if ( pivot && rdiag[k] != 0. )
{
temp = a[m*k+j]/rdiag[k];
temp = MAX( 0., 1-temp*temp );
rdiag[k] *= sqrt(temp);
temp = rdiag[k]/wa[k];
if ( p05*SQR(temp) <= LM_MACHEP )
{
rdiag[k] = lm_enorm( m-j-1, &a[m*k+j+1]);
wa[k] = rdiag[k];
}
}
}
rdiag[j] = -ajnorm;
}
}
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)
{
/* given an m by n matrix a, an n by n nonsingular diagonal
* matrix d, an m-vector b, and a positive number delta,
* the problem is to determine a value for the parameter
* par such that if x solves the system
*
* a*x = b , sqrt(par)*d*x = 0 ,
*
* in the least squares sense, and dxnorm is the euclidean
* norm of d*x, then either par is 0. and
*
* (dxnorm-delta) .le. 0.1*delta ,
*
* or par is positive and
*
* abs(dxnorm-delta) .le. 0.1*delta .
*
* this subroutine completes the solution of the problem
* if it is provided with the necessary information from the
* qr factorization, with column pivoting, of a. that is, if
* a*p = q*r, where p is a permutation matrix, q has orthogonal
* columns, and r is an upper triangular matrix with diagonal
* elements of nonincreasing magnitude, then lmpar expects
* the full upper triangle of r, the permutation matrix p,
* and the first n components of (q transpose)*b. on output
* lmpar also provides an upper triangular matrix s such that
*
* t t t
* p *(a *a + par*d*d)*p = s *s .
*
* s is employed within lmpar and may be of separate interest.
*
* only a few iterations are generally needed for convergence
* of the algorithm. if, however, the limit of 10 iterations
* is reached, then the output par will contain the best
* value obtained so far.
*
* parameters:
*
* n is a positive integer input variable set to the order of r.
*
* r is an n by n array. on input the full upper triangle
* must contain the full upper triangle of the matrix r.
* on output the full upper triangle is unaltered, and the
* strict lower triangle contains the strict upper triangle
* (transposed) of the upper triangular matrix s.
*
* ldr is a positive integer input variable not less than n
* which specifies the leading dimension of the array r.
*
* ipvt is an integer input array of length n which defines the
* permutation matrix p such that a*p = q*r. column j of p
* is column ipvt(j) of the identity matrix.
*
* diag is an input array of length n which must contain the
* diagonal elements of the matrix d.
*
* qtb is an input array of length n which must contain the first
* n elements of the vector (q transpose)*b.
*
* delta is a positive input variable which specifies an upper
* bound on the euclidean norm of d*x.
*
* par is a nonnegative variable. on input par contains an
* initial estimate of the levenberg-marquardt parameter.
* on output par contains the final estimate.
*
* x is an output array of length n which contains the least
* squares solution of the system a*x = b, sqrt(par)*d*x = 0,
* for the output par.
*
* sdiag is an output array of length n which contains the
* diagonal elements of the upper triangular matrix s.
*
* wa1 and wa2 are work arrays of length n.
*
*/
int i, iter, j, nsing;
double dxnorm, fp, fp_old, gnorm, parc, parl, paru;
double sum, temp;
static double p1 = 0.1;
static double p001 = 0.001;
#if BUG
printf( "lmpar\n" );
#endif
// *** compute and store in x the gauss-newton direction. if the
// jacobian is rank-deficient, obtain a least squares solution.
nsing = n;
for ( j=0; j<n; j++ )
{
wa1[j] = qtb[j];
if ( r[j*ldr+j] == 0 && nsing == n )
nsing = j;
if (nsing < n)
wa1[j] = 0;
}
#if BUG
printf( "nsing %d ", nsing );
#endif
for ( j=nsing-1; j>=0; j-- )
{
wa1[j] = wa1[j]/r[j+ldr*j];
temp = wa1[j];
for ( i=0; i<j; i++ )
wa1[i] -= r[j*ldr+i]*temp;
}
for ( j=0; j<n; j++ )
x[ ipvt[j] ] = wa1[j];
// *** initialize the iteration counter.
// evaluate the function at the origin, and test
// for acceptance of the gauss-newton direction.
iter = 0;
for ( j=0; j<n; j++ )
wa2[j] = diag[j]*x[j];
dxnorm = lm_enorm(n,wa2);
fp = dxnorm - delta;
if (fp <= p1*delta)
{
#if BUG
printf( "lmpar/ terminate (fp<delta/10\n" );
#endif
*par = 0;
return;
}
// *** if the jacobian is not rank deficient, the newton
// step provides a lower bound, parl, for the 0. of
// the function. otherwise set this bound to 0..
parl = 0;
if (nsing >= n)
{
for ( j=0; j<n; j++ )
wa1[j] = diag[ ipvt[j] ] * wa2[ ipvt[j] ] / dxnorm;
for ( j=0; j<n; j++ )
{
sum = 0.;
for ( i=0; i<j; i++ )
sum += r[j*ldr+i]*wa1[i];
wa1[j] = (wa1[j] - sum)/r[j+ldr*j];
}
temp = lm_enorm(n,wa1);
parl = fp/delta/temp/temp;
}
// *** calculate an upper bound, paru, for the 0. of the function.
for ( j=0; j<n; j++ )
{
sum = 0;
for ( i=0; i<=j; i++ )
sum += r[j*ldr+i]*qtb[i];
wa1[j] = sum/diag[ ipvt[j] ];
}
gnorm = lm_enorm(n,wa1);
paru = gnorm/delta;
if (paru == 0.)
paru = LM_DWARF/MIN(delta,p1);
// *** if the input par lies outside of the interval (parl,paru),
// set par to the closer endpoint.
*par = MAX( *par,parl);
*par = MIN( *par,paru);
if ( *par == 0.)
*par = gnorm/dxnorm;
#if BUG
printf( "lmpar/ parl %.4e par %.4e paru %.4e\n", parl, *par, paru );
#endif
// *** iterate.
for ( ; ; iter++ ) {
// *** evaluate the function at the current value of par.
if ( *par == 0.)
*par = MAX(LM_DWARF,p001*paru);
temp = sqrt( *par );
for ( j=0; j<n; j++ )
wa1[j] = temp*diag[j];
lm_qrsolv( n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
for ( j=0; j<n; j++ )
wa2[j] = diag[j]*x[j];
dxnorm = lm_enorm(n,wa2);
fp_old = fp;
fp = dxnorm - delta;
// *** if the function is small enough, accept the current value
// of par. also test for the exceptional cases where parl
// is 0. or the number of iterations has reached 10.
if ( fabs(fp) <= p1*delta
|| (parl == 0. && fp <= fp_old && fp_old < 0.)
|| iter == 10 )
break; // the only exit from this loop
// *** compute the Newton correction.
for ( j=0; j<n; j++ )
wa1[j] = diag[ ipvt[j] ] * wa2[ ipvt[j] ] / dxnorm;
for ( j=0; j<n; j++ )
{
wa1[j] = wa1[j]/sdiag[j];
for ( i=j+1; i<n; i++ )
wa1[i] -= r[j*ldr+i]*wa1[j];
}
temp = lm_enorm( n, wa1);
parc = fp/delta/temp/temp;
// *** depending on the sign of the function, update parl or paru.
if (fp > 0)
parl = MAX(parl, *par);
else if (fp < 0)
paru = MIN(paru, *par);
// the case fp==0 is precluded by the break condition
// *** compute an improved estimate for par.
*par = MAX(parl, *par + parc);
}
}
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 lmfit 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 (.1,100.). 100. is a generally recommended value.
*
* 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. ipvt
* 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 functions.
* a default implementation lm_evaluate_default is provided in lm_eval.c;
* alternatively, evaluate can be provided by a user calling program.
* it 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;
* alternatively, printout can be provided by a user calling program.
* it 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; // just to prevent a warning (initialization within if-clause)
xnorm = 0; // dito
temp = MAX(epsfcn,LM_MACHEP);
eps = sqrt(temp); // used in calculating the Jacobian by forward differences
// *** check the 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
// *** evaluate the function at the starting point and calculate its 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);
// *** the outer loop.
do {
#if BUG
printf( "lmdif/ outer loop iter=%d nfev=%d fnorm=%.10e\n",
iter, *nfev, fnorm );
#endif
// O** 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
x[j] = temp;
for ( i=0; i<m; i++ )
fjac[j*m+i] = (wa4[i] - fvec[i]) / step;
}
#if BUG>1
// DEBUG: print the entire matrix
for ( i=0; i<m; i++ )
{
for ( j=0; j<n; j++ )
printf( "%.5e ", y[j*m+i] );
printf( "\n" );
}
#endif
// O** compute the qr factorization of the jacobian.
lm_qrfac( m, n, fjac, 1, ipvt, wa1, wa2, wa3);
// O** on the first iteration ...
if (iter == 1)
{
if (mode != 2)
// ... scale according to the norms of the columns of the initial jacobian.
{
for ( j=0; j<n; j++ )
{
diag[j] = wa2[j];
if ( wa2[j] == 0. )
diag[j] = 1.;
}
}
// ... calculate the norm of the scaled x and
// initialize the step bound delta.
for ( j=0; j<n; j++ )
wa3[j] = diag[j] * x[j];
xnorm = lm_enorm( n, wa3 );
delta = factor*xnorm;
if (delta == 0.)
delta = factor;
}
// O** form (q transpose)*fvec and store the first n components in qtf.
for ( i=0; i<m; i++ )
wa4[i] = fvec[i];
for ( j=0; j<n; j++ )
{
temp3 = fjac[j*m+j];
if (temp3 != 0.)
{
sum = 0;
for ( i=j; i<m; i++ )
sum += fjac[j*m+i] * wa4[i];
temp = -sum / temp3;
for ( i=j; i<m; i++ )
wa4[i] += fjac[j*m+i] * temp;
}
fjac[j*m+j] = wa1[j];
qtf[j] = wa4[j];
}
// O** compute the norm of the scaled gradient and test for convergence.
gnorm = 0;
if ( fnorm != 0 )
{
for ( j=0; j<n; j++ )
{
if ( wa2[ ipvt[j] ] == 0 ) continue;
sum = 0.;
for ( i=0; i<=j; i++ )
sum += fjac[j*m+i] * qtf[i] / fnorm;
gnorm = MAX( gnorm, fabs(sum/wa2[ ipvt[j] ]) );
}
}
if ( gnorm <= gtol )
{
*info = 4;
return;
}
// O** rescale if necessary.
if ( mode != 2 )
{
for ( j=0; j<n; j++ )
diag[j] = MAX(diag[j],wa2[j]);
}
// O** the inner loop.
do {
#if BUG
printf( "lmdif/ inner loop iter=%d nfev=%d\n", iter, *nfev );
#endif
// OI* determine the levenberg-marquardt parameter.
lm_lmpar( n,fjac,m,ipvt,diag,qtf,delta,&par,wa1,wa2,wa3,wa4 );
// OI* store the direction p and x + p. calculate the norm of p.
for ( j=0; j<n; j++ )
{
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j]*wa1[j];
}
pnorm = lm_enorm(n,wa3);
// OI* on the first iteration, adjust the initial step bound.
if ( *nfev<= 1+n ) // bug corrected by J. Wuttke in 2004
delta = MIN(delta,pnorm);
// OI* evaluate the function at x + p and calculate its norm.
*info = 0;
(*evaluate)( wa2, m, wa4, data, info );
(*printout)( n, x, m, wa4, data, 2, iter, ++(*nfev) );
if ( *info < 0 ) return; // user requested break
fnorm1 = lm_enorm(m,wa4);
#if BUG
printf( "lmdif/ pnorm %.10e fnorm1 %.10e fnorm %.10e"
" delta=%.10e par=%.10e\n",
pnorm, fnorm1, fnorm, delta, par );
#endif
// OI* compute the scaled actual reduction.
if ( p1*fnorm1 < fnorm )
actred = 1 - SQR( fnorm1/fnorm );
else
actred = -1;
// OI* compute the scaled predicted reduction and
// the scaled directional derivative.
for ( j=0; j<n; j++ )
{
wa3[j] = 0;
for ( i=0; i<=j; i++ )
wa3[i] += fjac[j*m+i]*wa1[ ipvt[j] ];
}
temp1 = lm_enorm(n,wa3) / fnorm;
temp2 = sqrt(par) * pnorm / fnorm;
prered = SQR(temp1) + 2 * SQR(temp2);
dirder = - ( SQR(temp1) + SQR(temp2) );
// OI* compute the ratio of the actual to the predicted reduction.
ratio = prered!=0 ? actred/prered : 0;
#if BUG
printf( "lmdif/ actred=%.10e prered=%.10e ratio=%.10e"
" sq(1)=%.10e sq(2)=%.10e dd=%.10e\n",
actred, prered, prered!=0 ? ratio : 0.,
SQR(temp1), SQR(temp2), dirder );
#endif
// OI* update the step bound.
if (ratio <= p25)
{
if (actred >= 0.)
temp = p5;
else
temp = p5*dirder/(dirder + p5*actred);
if ( p1*fnorm1 >= fnorm || temp < p1 )
temp = p1;
delta = temp * MIN(delta,pnorm/p1);
par /= temp;
}
else if ( par == 0. || ratio >= p75 )
{
delta = pnorm/p5;
par *= p5;
}
// OI* test for successful iteration...
if (ratio >= p0001)
{
// ... successful iteration. update x, fvec, and their norms.
for ( j=0; j<n; j++ )
{
x[j] = wa2[j];
wa2[j] = diag[j]*x[j];
}
for ( i=0; i<m; i++ )
fvec[i] = wa4[i];
xnorm = lm_enorm(n,wa2);
fnorm = fnorm1;
iter++;
}
#if BUG
else {
printf( "ATTN: iteration considered unsuccessful\n" );
}
#endif
// OI* tests for convergence ( otherwise *info = 1, 2, or 3 )
*info = 0; // do not terminate (unless overwritten by nonzero value)
if ( fabs(actred) <= ftol && prered <= ftol && p5*ratio <= 1 )
*info = 1;
if (delta <= xtol*xnorm)
*info += 2;
if ( *info != 0)
return;
// OI* tests for termination and stringent tolerances.
if ( *nfev >= maxfev)
*info = 5;
if ( fabs(actred) <= LM_MACHEP &&
prered <= LM_MACHEP && p5*ratio <= 1 )
*info = 6;
if (delta <= LM_MACHEP*xnorm)
*info = 7;
if (gnorm <= LM_MACHEP)
*info = 8;
if ( *info != 0)
return;
// OI* end of the inner loop. repeat if iteration unsuccessful.
} while (ratio < p0001);
// O** end of the outer loop.
} while (1);
}
static void lm_qrfac(const int m, const int n, double* A, int* Pivot, double* Rdiag,
double* Acnorm, double* W)
/*
* This subroutine uses Householder transformations with column pivoting
* to compute a QR factorization of the m by n matrix A. That is, qrfac
* determines an orthogonal matrix Q, a permutation matrix P, and an
* upper trapezoidal matrix R with diagonal elements of nonincreasing
* magnitude, such that A*P = Q*R. The Householder transformation for
* column k, k = 1,2,...,n, is of the form
*
* I - 2*w*wT/|w|^2
*
* where w has zeroes in the first k-1 positions.
*
* Parameters:
*
* m is an INPUT parameter set to the number of rows of A.
*
* n is an INPUT parameter set to the number of columns of A.
*
* A is an m by n array. On INPUT, A contains the matrix for which the
* QR factorization is to be computed. On OUTPUT the strict upper
* trapezoidal part of A contains the strict upper trapezoidal part
* of R, and the lower trapezoidal part of A contains a factored form
* of Q (the non-trivial elements of the vectors w described above).
*
* Pivot is an integer OUTPUT array of length n that describes the
* permutation matrix P. Column j of P is column Pivot(j) of the
* identity matrix.
*
* Rdiag is an OUTPUT array of length n which contains the diagonal
* elements of R.
*
* Acnorm is an OUTPUT array of length n which contains the norms of
* the corresponding columns of the input matrix A. If this information
* is not needed, then Acnorm can share storage with Rdiag.
*
* W is a work array of length n.
*
*/
{
int i, j, k, kmax;
double ajnorm, sum, temp;
#ifdef LMFIT_DEBUG_MESSAGES
printf("debug qrfac\n");
#endif
/** Compute initial column norms;
initialize Pivot with identity permutation. ***/
for (j = 0; j < n; j++) {
W[j] = Rdiag[j] = Acnorm[j] = lm_enorm(m, &A[j*m]);
Pivot[j] = j;
}
/** Loop over columns of A. **/
assert(n <= m);
for (j = 0; j < n; j++) {
/** Bring the column of largest norm into the pivot position. **/
kmax = j;
for (k = j+1; k < n; k++)
if (Rdiag[k] > Rdiag[kmax])
kmax = k;
if (kmax != j) {
/* Swap columns j and kmax. */
k = Pivot[j];
Pivot[j] = Pivot[kmax];
Pivot[kmax] = k;
for (i = 0; i < m; i++) {
temp = A[j*m+i];
A[j*m+i] = A[kmax*m+i];
A[kmax*m+i] = temp;
}
/* Half-swap: Rdiag[j], W[j] won't be needed any further. */
Rdiag[kmax] = Rdiag[j];
W[kmax] = W[j];
}
/** Compute the Householder reflection vector w_j to reduce the
j-th column of A to a multiple of the j-th unit vector. **/
ajnorm = lm_enorm(m-j, &A[j*m+j]);
if (ajnorm == 0) {
Rdiag[j] = 0;
continue;
}
/* Let the partial column vector A[j][j:] contain w_j := e_j+-a_j/|a_j|,
where the sign +- is chosen to avoid cancellation in w_jj. */
if (A[j*m+j] < 0)
ajnorm = -ajnorm;
for (i = j; i < m; i++)
A[j*m+i] /= ajnorm;
A[j*m+j] += 1;
/** Apply the Householder transformation U_w := 1 - 2*w_j.w_j/|w_j|^2
to the remaining columns, and update the norms. **/
for (k = j+1; k < n; k++) {
/* Compute scalar product w_j * a_j. */
sum = 0;
for (i = j; i < m; i++)
sum += A[j*m+i] * A[k*m+i];
/* Normalization is simplified by the coincidence |w_j|^2=2w_jj. */
temp = sum / A[j*m+j];
/* Carry out transform U_w_j * a_k. */
for (i = j; i < m; i++)
A[k*m+i] -= temp * A[j*m+i];
/* No idea what happens here. */
if (Rdiag[k] != 0) {
temp = A[m*k+j] / Rdiag[k];
if (fabs(temp) < 1) {
Rdiag[k] *= sqrt(1 - SQR(temp));
temp = Rdiag[k] / W[k];
} else
temp = 0;
if (temp == 0 || 0.05 * SQR(temp) <= LM_MACHEP) {
Rdiag[k] = lm_enorm(m-j-1, &A[m*k+j+1]);
W[k] = Rdiag[k];
}
}
}
Rdiag[j] = -ajnorm;
}
} /*** lm_qrfac. ***/
static void lm_lmpar(const int n, double* r, const int ldr, const int* Pivot,
const double* diag, const double* qtb, const double delta,
double* par, double* x, double* Sdiag, double* aux, double* xdi)
/* Given an m by n matrix A, an n by n nonsingular diagonal matrix D,
* an m-vector b, and a positive number delta, the problem is to
* determine a parameter value par such that if x solves the system
*
* A*x = b and sqrt(par)*D*x = 0
*
* in the least squares sense, and dxnorm is the Euclidean norm of D*x,
* then either par=0 and (dxnorm-delta) < 0.1*delta, or par>0 and
* abs(dxnorm-delta) < 0.1*delta.
*
* Using lm_qrsolv, this subroutine completes the solution of the
* problem if it is provided with the necessary information from the
* QR factorization, with column pivoting, of A. That is, if A*P = Q*R,
* where P is a permutation matrix, Q has orthogonal columns, and R is
* an upper triangular matrix with diagonal elements of nonincreasing
* magnitude, then lmpar expects the full upper triangle of R, the
* permutation matrix P, and the first n components of Q^T*b. On output
* lmpar also provides an upper triangular matrix S such that
*
* P^T*(A^T*A + par*D*D)*P = S^T*S.
*
* S is employed within lmpar and may be of separate interest.
*
* Only a few iterations are generally needed for convergence of the
* algorithm. If, however, the limit of 10 iterations is reached, then
* the output par will contain the best value obtained so far.
*
* Parameters:
*
* n is a positive integer INPUT variable set to the order of r.
*
* r is an n by n array. On INPUT the full upper triangle must contain
* the full upper triangle of the matrix R. On OUTPUT the full upper
* triangle is unaltered, and the strict lower triangle contains the
* strict upper triangle (transposed) of the upper triangular matrix S.
*
* ldr is a positive integer INPUT variable not less than n which
* specifies the leading dimension of the array R.
*
* Pivot is an integer INPUT array of length n which defines the
* permutation matrix P such that A*P = Q*R. Column j of P is column
* Pivot(j) of the identity matrix.
*
* diag is an INPUT array of length n which must contain the diagonal
* elements of the matrix D.
*
* qtb is an INPUT array of length n which must contain the first
* n elements of the vector Q^T*b.
*
* delta is a positive INPUT variable which specifies an upper bound
* on the Euclidean norm of D*x.
*
* par is a nonnegative variable. On INPUT par contains an initial
* estimate of the Levenberg-Marquardt parameter. On OUTPUT par
* contains the final estimate.
*
* x is an OUTPUT array of length n which contains the least-squares
* solution of the system A*x = b, sqrt(par)*D*x = 0, for the output par.
*
* Sdiag is an array of length n needed as workspace; on OUTPUT it
* contains the diagonal elements of the upper triangular matrix S.
*
* aux is a multi-purpose work array of length n.
*
* xdi is a work array of length n. On OUTPUT: diag[j] * x[j].
*
*/
{
int i, iter, j, nsing;
double dxnorm, fp, fp_old, gnorm, parc, parl, paru;
double sum, temp;
static double p1 = 0.1;
/*** Compute and store in x the Gauss-Newton direction. If the Jacobian
is rank-deficient, obtain a least-squares solution. ***/
nsing = n;
for (j = 0; j < n; j++) {
aux[j] = qtb[j];
if (r[j*ldr+j] == 0 && nsing == n)
nsing = j;
if (nsing < n)
aux[j] = 0;
}
for (j = nsing-1; j >= 0; j--) {
aux[j] = aux[j] / r[j+ldr*j];
temp = aux[j];
for (i = 0; i < j; i++)
aux[i] -= r[j*ldr+i] * temp;
}
for (j = 0; j < n; j++)
x[Pivot[j]] = aux[j];
/*** Initialize the iteration counter, evaluate the function at the origin,
and test for acceptance of the Gauss-Newton direction. ***/
for (j = 0; j < n; j++)
xdi[j] = diag[j] * x[j];
dxnorm = lm_enorm(n, xdi);
fp = dxnorm - delta;
if (fp <= p1 * delta) {
#ifdef LMFIT_DEBUG_MESSAGES
printf("debug lmpar nsing=%d, n=%d, terminate[fp<=p1*del]\n", nsing, n);
#endif
*par = 0;
return;
}
/*** If the Jacobian is not rank deficient, the Newton step provides a
lower bound, parl, for the zero of the function. Otherwise set this
bound to zero. ***/
parl = 0;
if (nsing >= n) {
for (j = 0; j < n; j++)
aux[j] = diag[Pivot[j]] * xdi[Pivot[j]] / dxnorm;
for (j = 0; j < n; j++) {
sum = 0;
for (i = 0; i < j; i++)
sum += r[j*ldr+i] * aux[i];
aux[j] = (aux[j] - sum) / r[j+ldr*j];
}
temp = lm_enorm(n, aux);
parl = fp / delta / temp / temp;
}
/*** Calculate an upper bound, paru, for the zero of the function. ***/
for (j = 0; j < n; j++) {
sum = 0;
for (i = 0; i <= j; i++)
sum += r[j*ldr+i] * qtb[i];
aux[j] = sum / diag[Pivot[j]];
}
gnorm = lm_enorm(n, aux);
paru = gnorm / delta;
if (paru == 0)
paru = LM_DWARF / MIN(delta, p1);
/*** If the input par lies outside of the interval (parl,paru),
set par to the closer endpoint. ***/
*par = MAX(*par, parl);
*par = MIN(*par, paru);
if (*par == 0)
*par = gnorm / dxnorm;
/*** Iterate. ***/
for (iter = 0;; iter++) {
/** Evaluate the function at the current value of par. **/
if (*par == 0)
*par = MAX(LM_DWARF, 0.001 * paru);
temp = sqrt(*par);
for (j = 0; j < n; j++)
aux[j] = temp * diag[j];
lm_qrsolv(n, r, ldr, Pivot, aux, qtb, x, Sdiag, xdi);
/* return values are r, x, Sdiag */
for (j = 0; j < n; j++)
xdi[j] = diag[j] * x[j]; /* used as output */
dxnorm = lm_enorm(n, xdi);
fp_old = fp;
fp = dxnorm - delta;
/** If the function is small enough, accept the current value
of par. Also test for the exceptional cases where parl
is zero or the number of iterations has reached 10. **/
if (fabs(fp) <= p1 * delta ||
(parl == 0 && fp <= fp_old && fp_old < 0) || iter == 10) {
#ifdef LMFIT_DEBUG_MESSAGES
printf("debug lmpar nsing=%d, iter=%d, "
"par=%.4e [%.4e %.4e], delta=%.4e, fp=%.4e\n",
nsing, iter, *par, parl, paru, delta, fp);
#endif
break; /* the only exit from the iteration. */
}
/** Compute the Newton correction. **/
for (j = 0; j < n; j++)
aux[j] = diag[Pivot[j]] * xdi[Pivot[j]] / dxnorm;
for (j = 0; j < n; j++) {
aux[j] = aux[j] / Sdiag[j];
for (i = j+1; i < n; i++)
aux[i] -= r[j*ldr+i] * aux[j];
}
temp = lm_enorm(n, aux);
parc = fp / delta / temp / temp;
/** Depending on the sign of the function, update parl or paru. **/
if (fp > 0)
parl = MAX(parl, *par);
else /* fp < 0 [the case fp==0 is precluded by the break condition] */
paru = MIN(paru, *par);
/** Compute an improved estimate for par. **/
*par = MAX(parl, *par + parc);
}
} /*** lm_lmpar. ***/
void lmmin(const int n, double* x, const int m, const void* data,
void (*evaluate)(const double* par, const int m_dat,
const void* data, double* fvec, int* userbreak),
const lm_control_struct* C, lm_status_struct* S)
{
int j, i;
double actred, dirder, fnorm, fnorm1, gnorm, pnorm, prered, ratio, step,
sum, temp, temp1, temp2, temp3;
/*** Initialize internal variables. ***/
int maxfev = C->patience * (n+1);
int inner_success; /* flag for loop control */
double lmpar = 0; /* Levenberg-Marquardt parameter */
double delta = 0;
double xnorm = 0;
double eps = sqrt(MAX(C->epsilon, LM_MACHEP)); /* for forward differences */
int nout = C->n_maxpri == -1 ? n : MIN(C->n_maxpri, n);
/* Reinterpret C->msgfile=NULL as stdout (which is unavailable for
compile-time initialization of lm_control_double and similar). */
FILE* msgfile = C->msgfile ? C->msgfile : stdout;
/*** Default status info; must be set before first return statement. ***/
S->outcome = 0; /* status code */
S->userbreak = 0;
S->nfev = 0; /* function evaluation counter */
/*** Check input parameters for errors. ***/
if (n <= 0) {
fprintf(stderr, "lmmin: invalid number of parameters %i\n", n);
S->outcome = 10;
return;
}
if (m < n) {
fprintf(stderr, "lmmin: number of data points (%i) "
"smaller than number of parameters (%i)\n",
m, n);
S->outcome = 10;
return;
}
if (C->ftol < 0 || C->xtol < 0 || C->gtol < 0) {
fprintf(stderr,
"lmmin: negative tolerance (at least one of %g %g %g)\n",
C->ftol, C->xtol, C->gtol);
S->outcome = 10;
return;
}
if (maxfev <= 0) {
fprintf(stderr, "lmmin: nonpositive function evaluations limit %i\n",
maxfev);
S->outcome = 10;
return;
}
if (C->stepbound <= 0) {
fprintf(stderr, "lmmin: nonpositive stepbound %g\n", C->stepbound);
S->outcome = 10;
return;
}
if (C->scale_diag != 0 && C->scale_diag != 1) {
fprintf(stderr, "lmmin: logical variable scale_diag=%i, "
"should be 0 or 1\n",
C->scale_diag);
S->outcome = 10;
return;
}
/*** Allocate work space. ***/
/* Allocate total workspace with just one system call */
char* ws;
if ((ws = (char *)malloc((2*m + 5*n + m*n) * sizeof(double) +
n * sizeof(int))) == NULL) {
S->outcome = 9;
return;
}
/* Assign workspace segments. */
char* pws = ws;
double* fvec = (double*)pws;
pws += m * sizeof(double) / sizeof(char);
double* diag = (double*)pws;
pws += n * sizeof(double) / sizeof(char);
double* qtf = (double*)pws;
pws += n * sizeof(double) / sizeof(char);
double* fjac = (double*)pws;
pws += n * m * sizeof(double) / sizeof(char);
double* wa1 = (double*)pws;
pws += n * sizeof(double) / sizeof(char);
double* wa2 = (double*)pws;
pws += n * sizeof(double) / sizeof(char);
double* wa3 = (double*)pws;
pws += n * sizeof(double) / sizeof(char);
double* wf = (double*)pws;
pws += m * sizeof(double) / sizeof(char);
int* Pivot = (int*)pws;
//pws += n * sizeof(int) / sizeof(char);
/* Initialize diag. */
if (!C->scale_diag)
for (j = 0; j < n; j++)
diag[j] = 1;
/*** Evaluate function at starting point and calculate norm. ***/
if (C->verbosity) {
fprintf(msgfile, "lmmin start ");
lm_print_pars(nout, x, msgfile);
}
(*evaluate)(x, m, data, fvec, &(S->userbreak));
if (C->verbosity > 4)
for (i = 0; i < m; ++i)
fprintf(msgfile, " fvec[%4i] = %18.8g\n", i, fvec[i]);
S->nfev = 1;
if (S->userbreak)
goto terminate;
fnorm = lm_enorm(m, fvec);
if (C->verbosity)
fprintf(msgfile, " fnorm = %18.8g\n", fnorm);
if (!isfinite(fnorm)) {
S->outcome = 12; /* nan */
goto terminate;
} else if (fnorm <= LM_DWARF) {
S->outcome = 0; /* sum of squares almost zero, nothing to do */
goto terminate;
}
/*** The outer loop: compute gradient, then descend. ***/
for (int outer = 0;; ++outer) {
/** Calculate the Jacobian. **/
for (j = 0; j < n; j++) {
temp = x[j];
step = MAX(eps * eps, eps * fabs(temp));
x[j] += step; /* replace temporarily */
(*evaluate)(x, m, data, wf, &(S->userbreak));
++(S->nfev);
if (S->userbreak)
goto terminate;
for (i = 0; i < m; i++)
fjac[j*m+i] = (wf[i] - fvec[i]) / step;
x[j] = temp; /* restore */
}
if (C->verbosity >= 10) {
/* print the entire matrix */
printf("\nlmmin Jacobian\n");
for (i = 0; i < m; i++) {
printf(" ");
for (j = 0; j < n; j++)
printf("%.5e ", fjac[j*m+i]);
printf("\n");
}
}
/** Compute the QR factorization of the Jacobian. **/
/* fjac is an m by n array. The upper n by n submatrix of fjac is made
* to contain an upper triangular matrix R with diagonal elements of
* nonincreasing magnitude such that
*
* P^T*(J^T*J)*P = R^T*R
*
* (NOTE: ^T stands for matrix transposition),
*
* where P is a permutation matrix and J is the final calculated
* Jacobian. Column j of P is column Pivot(j) of the identity matrix.
* The lower trapezoidal part of fjac contains information generated
* during the computation of R.
*
* Pivot is an integer 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 Pivot(j) of the identity matrix.
*/
lm_qrfac(m, n, fjac, Pivot, wa1, wa2, wa3);
/* return values are Pivot, wa1=rdiag, wa2=acnorm */
/** Form Q^T * fvec, and store first n components in qtf. **/
for (i = 0; i < m; i++)
wf[i] = fvec[i];
for (j = 0; j < n; j++) {
temp3 = fjac[j*m+j];
if (temp3 != 0) {
sum = 0;
for (i = j; i < m; i++)
sum += fjac[j*m+i] * wf[i];
temp = -sum / temp3;
for (i = j; i < m; i++)
wf[i] += fjac[j*m+i] * temp;
}
fjac[j*m+j] = wa1[j];
qtf[j] = wf[j];
}
/** Compute norm of scaled gradient and detect degeneracy. **/
gnorm = 0;
for (j = 0; j < n; j++) {
if (wa2[Pivot[j]] == 0)
continue;
sum = 0;
for (i = 0; i <= j; i++)
sum += fjac[j*m+i] * qtf[i];
gnorm = MAX(gnorm, fabs(sum / wa2[Pivot[j]] / fnorm));
}
if (gnorm <= C->gtol) {
S->outcome = 4;
goto terminate;
}
/** Initialize or update diag and delta. **/
if (!outer) { /* first iteration only */
if (C->scale_diag) {
/* diag := norms of the columns of the initial Jacobian */
for (j = 0; j < n; j++)
diag[j] = wa2[j] ? wa2[j] : 1;
/* xnorm := || D x || */
for (j = 0; j < n; j++)
wa3[j] = diag[j] * x[j];
xnorm = lm_enorm(n, wa3);
if (C->verbosity >= 2) {
fprintf(msgfile, "lmmin diag ");
lm_print_pars(nout, x, msgfile); // xnorm
fprintf(msgfile, " xnorm = %18.8g\n", xnorm);
}
/* Only now print the header for the loop table. */
if (C->verbosity >= 3) {
fprintf(msgfile, " o i lmpar prered"
" ratio dirder delta"
" pnorm fnorm");
for (i = 0; i < nout; ++i)
fprintf(msgfile, " p%i", i);
fprintf(msgfile, "\n");
}
} else {
xnorm = lm_enorm(n, x);
}
if (!isfinite(xnorm)) {
S->outcome = 12; /* nan */
goto terminate;
}
/* Initialize the step bound delta. */
if (xnorm)
delta = C->stepbound * xnorm;
else
delta = C->stepbound;
} else {
if (C->scale_diag) {
for (j = 0; j < n; j++)
diag[j] = MAX(diag[j], wa2[j]);
}
}
/** The inner loop. **/
int inner = 0;
do {
/** Determine the Levenberg-Marquardt parameter. **/
lm_lmpar(n, fjac, m, Pivot, diag, qtf, delta, &lmpar,
wa1, wa2, wf, wa3);
/* used return values are fjac (partly), lmpar, wa1=x, wa3=diag*x */
/* Predict scaled reduction. */
pnorm = lm_enorm(n, wa3);
if (!isfinite(pnorm)) {
S->outcome = 12; /* nan */
goto terminate;
}
temp2 = lmpar * SQR(pnorm / fnorm);
for (j = 0; j < n; j++) {
wa3[j] = 0;
for (i = 0; i <= j; i++)
wa3[i] -= fjac[j*m+i] * wa1[Pivot[j]];
}
temp1 = SQR(lm_enorm(n, wa3) / fnorm);
if (!isfinite(temp1)) {
S->outcome = 12; /* nan */
goto terminate;
}
prered = temp1 + 2*temp2;
dirder = -temp1 + temp2; /* scaled directional derivative */
/* At first call, adjust the initial step bound. */
if (!outer && pnorm < delta)
delta = pnorm;
/** Evaluate the function at x + p. **/
for (j = 0; j < n; j++)
wa2[j] = x[j] - wa1[j];
(*evaluate)(wa2, m, data, wf, &(S->userbreak));
++(S->nfev);
if (S->userbreak)
goto terminate;
fnorm1 = lm_enorm(m, wf);
if (!isfinite(fnorm1)) {
S->outcome = 12; /* nan */
goto terminate;
}
/** Evaluate the scaled reduction. **/
/* Actual scaled reduction. */
actred = 1 - SQR(fnorm1 / fnorm);
/* Ratio of actual to predicted reduction. */
ratio = prered ? actred / prered : 0;
if (C->verbosity == 2) {
fprintf(msgfile, "lmmin (%i:%i) ", outer, inner);
lm_print_pars(nout, wa2, msgfile); // fnorm1,
} else if (C->verbosity >= 3) {
printf("%3i %2i %9.2g %9.2g %14.6g"
" %9.2g %10.3e %10.3e %21.15e",
outer, inner, lmpar, prered, ratio,
dirder, delta, pnorm, fnorm1);
for (i = 0; i < nout; ++i)
fprintf(msgfile, " %16.9g", wa2[i]);
fprintf(msgfile, "\n");
}
/* Update the step bound. */
if (ratio <= 0.25) {
if (actred >= 0)
temp = 0.5;
else if (actred > -99) /* -99 = 1-1/0.1^2 */
temp = MAX(dirder / (2*dirder + actred), 0.1);
else
temp = 0.1;
delta = temp * MIN(delta, pnorm / 0.1);
lmpar /= temp;
} else if (ratio >= 0.75) {
delta = 2 * pnorm;
lmpar *= 0.5;
} else if (!lmpar) {
delta = 2 * pnorm;
}
/** On success, update solution, and test for convergence. **/
inner_success = ratio >= 1e-4;
if (inner_success) {
/* Update x, fvec, and their norms. */
if (C->scale_diag) {
for (j = 0; j < n; j++) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
} else {
for (j = 0; j < n; j++)
x[j] = wa2[j];
}
for (i = 0; i < m; i++)
fvec[i] = wf[i];
xnorm = lm_enorm(n, wa2);
if (!isfinite(xnorm)) {
S->outcome = 12; /* nan */
goto terminate;
}
fnorm = fnorm1;
}
/* Convergence tests. */
S->outcome = 0;
if (fnorm <= LM_DWARF)
goto terminate; /* success: sum of squares almost zero */
/* Test two criteria (both may be fulfilled). */
if (fabs(actred) <= C->ftol && prered <= C->ftol && ratio <= 2)
S->outcome = 1; /* success: x almost stable */
if (delta <= C->xtol * xnorm)
S->outcome += 2; /* success: sum of squares almost stable */
if (S->outcome != 0) {
goto terminate;
}
/** Tests for termination and stringent tolerances. **/
if (S->nfev >= maxfev) {
S->outcome = 5;
goto terminate;
}
if (fabs(actred) <= LM_MACHEP && prered <= LM_MACHEP &&
ratio <= 2) {
S->outcome = 6;
goto terminate;
}
if (delta <= LM_MACHEP * xnorm) {
S->outcome = 7;
goto terminate;
}
if (gnorm <= LM_MACHEP) {
S->outcome = 8;
goto terminate;
}
/** End of the inner loop. Repeat if iteration unsuccessful. **/
++inner;
} while (!inner_success);
}; /*** End of the outer loop. ***/
terminate:
S->fnorm = lm_enorm(m, fvec);
if (C->verbosity >= 2)
printf("lmmin outcome (%i) xnorm %g ftol %g xtol %g\n", S->outcome,
xnorm, C->ftol, C->xtol);
if (C->verbosity & 1) {
fprintf(msgfile, "lmmin final ");
lm_print_pars(nout, x, msgfile); // S->fnorm,
fprintf(msgfile, " fnorm = %18.8g\n", S->fnorm);
}
if (S->userbreak) /* user-requested break */
S->outcome = 11;
/*** Deallocate the workspace. ***/
free(ws);
} /*** lmmin. ***/
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,
void (*evaluate) (const double *par, int m_dat, const void *data,
double *fvec, int *info),
void (*printout) (int n_par, const double *par, int m_dat,
const void *data, const double *fvec,
int printflags, int iflag, int iter, int nfev),
int printflags, const 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 lmmin provided above.
*
* 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 choosing a step length for
* the forward-difference approximation. The relative errors in
* the functions are assumed to be of the order of epsfcn.
*
* 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.
*
* 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 fnorm almost vanishing;
*
* 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;
*
* info =10 improper input parameters;
*
* 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
*
* pT*(jacT*jac)*p = rT*r
*
* (NOTE: T stands for matrix transposition),
*
* 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, used among others to hold
* residuals from evaluate.
*
* evaluate points to the subroutine which calculates the
* m nonlinear functions. Implementations should be written as follows:
*
* void evaluate( double* par, int m_dat, void *data,
* double* fvec, 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 points to the subroutine which informs about fit progress.
* Call with printout=0 if no printout is desired.
* Call with printout=lm_printout_std to use the default implementation.
*
* printflags is passed to printout.
*
* 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 p0001 = 1.0e-4;
*nfev = 0; /* function evaluation counter */
iter = 0; /* 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 = 10; // 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 = 10; // invalid parameter
return;
}
}
}
#ifdef LMFIT_DEBUG_MESSAGES
printf("lmdif\n");
#endif
/*** lmdif: evaluate function at starting point and calculate norm. ***/
*info = 0;
(*evaluate) (x, m, data, fvec, info);
++(*nfev);
if( printout )
(*printout) (n, x, m, data, fvec, printflags, 0, 0, *nfev);
if (*info < 0)
return;
fnorm = lm_enorm(m, fvec);
if( fnorm <= LM_DWARF ){
*info = 0;
return;
}
/*** lmdif: the outer loop. ***/
do {
#ifdef LMFIT_DEBUG_MESSAGES
printf("lmdif/ outer loop iter=%d nfev=%d fnorm=%.10e\n",
iter, *nfev, fnorm);
#endif
/*** outer: calculate the Jacobian. ***/
for (j = 0; j < n; j++) {
temp = x[j];
step = MAX(eps*eps, eps * fabs(temp));
x[j] = temp + step; /* replace temporarily */
*info = 0;
(*evaluate) (x, m, data, wa4, info);
++(*nfev);
if( printout )
(*printout) (n, x, m, data, wa4, printflags, 1, iter, *nfev);
if (*info < 0)
return; /* user requested break */
for (i = 0; i < m; i++)
fjac[j*m+i] = (wa4[i] - fvec[i]) / step;
x[j] = temp; /* restore */
}
#ifdef LMFIT_DEBUG_MATRIX
/* print the entire matrix */
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++)
printf("%.5e ", fjac[j*m+i]);
printf("\n");
}
#endif
/*** outer: compute the qr factorization of the Jacobian. ***/
lm_qrfac(m, n, fjac, 1, ipvt, wa1, wa2, wa3);
/* return values are ipvt, wa1=rdiag, wa2=acnorm */
if (!iter) {
/* first iteration only */
if (mode != 2) {
/* diag := norms of the columns of the initial Jacobian */
for (j = 0; j < n; j++) {
diag[j] = wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
}
/* use diag to scale x, then calculate the norm */
for (j = 0; j < n; j++)
wa3[j] = diag[j] * x[j];
xnorm = lm_enorm(n, wa3);
/* initialize the step bound delta. */
delta = factor * xnorm;
if (delta == 0.)
delta = factor;
} else {
if (mode != 2) {
for (j = 0; j < n; j++)
diag[j] = MAX( diag[j], wa2[j] );
}
}
/*** outer: form (q transpose)*fvec and store first n components in qtf. ***/
for (i = 0; i < m; i++)
wa4[i] = fvec[i];
for (j = 0; j < n; j++) {
temp3 = fjac[j*m+j];
if (temp3 != 0.) {
sum = 0;
for (i = j; i < m; i++)
sum += fjac[j*m+i] * wa4[i];
temp = -sum / temp3;
for (i = j; i < m; i++)
wa4[i] += fjac[j*m+i] * temp;
}
fjac[j*m+j] = wa1[j];
qtf[j] = wa4[j];
}
/*** outer: compute norm of scaled gradient and test for convergence. ***/
gnorm = 0;
for (j = 0; j < n; j++) {
if (wa2[ipvt[j]] == 0)
continue;
sum = 0.;
for (i = 0; i <= j; i++)
sum += fjac[j*m+i] * qtf[i];
gnorm = MAX( gnorm, fabs( sum / wa2[ipvt[j]] / fnorm ) );
}
if (gnorm <= gtol) {
*info = 4;
return;
}
/*** the inner loop. ***/
do {
#ifdef LMFIT_DEBUG_MESSAGES
printf("lmdif/ inner loop iter=%d nfev=%d\n", iter, *nfev);
#endif
/*** inner: determine the levenberg-marquardt parameter. ***/
lm_lmpar( n, fjac, m, ipvt, diag, qtf, delta, &par,
wa1, wa2, wa4, wa3 );
/* used return values are fjac (partly), par, wa1=x, wa3=diag*x */
for (j = 0; j < n; j++)
wa2[j] = x[j] - wa1[j]; /* new parameter vector ? */
pnorm = lm_enorm(n, wa3);
/* at first call, adjust the initial step bound. */
if (*nfev <= 1+n)
delta = MIN(delta, pnorm);
/*** inner: evaluate the function at x + p and calculate its norm. ***/
*info = 0;
(*evaluate) (wa2, m, data, wa4, info);
++(*nfev);
if( printout )
(*printout) (n, wa2, m, data, wa4, printflags, 2, iter, *nfev);
if (*info < 0)
return; /* user requested break. */
fnorm1 = lm_enorm(m, wa4);
#ifdef LMFIT_DEBUG_MESSAGES
printf("lmdif/ pnorm %.10e fnorm1 %.10e fnorm %.10e"
" delta=%.10e par=%.10e\n",
pnorm, fnorm1, fnorm, delta, par);
#endif
/*** inner: compute the scaled actual reduction. ***/
if (p1 * fnorm1 < fnorm)
actred = 1 - SQR(fnorm1 / fnorm);
else
actred = -1;
/*** inner: compute the scaled predicted reduction and
the scaled directional derivative. ***/
for (j = 0; j < n; j++) {
wa3[j] = 0;
for (i = 0; i <= j; i++)
wa3[i] -= fjac[j*m+i] * wa1[ipvt[j]];
}
temp1 = lm_enorm(n, wa3) / fnorm;
temp2 = sqrt(par) * pnorm / fnorm;
prered = SQR(temp1) + 2 * SQR(temp2);
dirder = -(SQR(temp1) + SQR(temp2));
/*** inner: compute the ratio of the actual to the predicted reduction. ***/
ratio = prered != 0 ? actred / prered : 0;
#ifdef LMFIT_DEBUG_MESSAGES
printf("lmdif/ actred=%.10e prered=%.10e ratio=%.10e"
" sq(1)=%.10e sq(2)=%.10e dd=%.10e\n",
actred, prered, prered != 0 ? ratio : 0.,
SQR(temp1), SQR(temp2), dirder);
#endif
/*** inner: update the step bound. ***/
if (ratio <= 0.25) {
if (actred >= 0.)
temp = 0.5;
else
temp = 0.5 * dirder / (dirder + 0.5 * actred);
if (p1 * fnorm1 >= fnorm || temp < p1)
temp = p1;
delta = temp * MIN(delta, pnorm / p1);
par /= temp;
} else if (par == 0. || ratio >= 0.75) {
delta = pnorm / 0.5;
par *= 0.5;
}
/*** inner: test for successful iteration. ***/
if (ratio >= p0001) {
/* yes, success: update x, fvec, and their norms. */
for (j = 0; j < n; j++) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
for (i = 0; i < m; i++)
fvec[i] = wa4[i];
xnorm = lm_enorm(n, wa2);
fnorm = fnorm1;
iter++;
}
#ifdef LMFIT_DEBUG_MESSAGES
else {
printf("ATTN: iteration considered unsuccessful\n");
}
#endif
/*** inner: test for convergence. ***/
if( fnorm<=LM_DWARF ){
*info = 0;
return;
}
*info = 0;
if (fabs(actred) <= ftol && prered <= ftol && 0.5 * ratio <= 1)
*info = 1;
if (delta <= xtol * xnorm)
*info += 2;
if (*info != 0)
return;
/*** inner: tests for termination and stringent tolerances. ***/
if (*nfev >= maxfev){
*info = 5;
return;
}
if (fabs(actred) <= LM_MACHEP &&
prered <= LM_MACHEP && 0.5 * ratio <= 1){
*info = 6;
return;
}
if (delta <= LM_MACHEP * xnorm){
*info = 7;
return;
}
if (gnorm <= LM_MACHEP){
*info = 8;
return;
}
/*** inner: end of the loop. repeat if iteration unsuccessful. ***/
} while (ratio < p0001);
/*** outer: end of the loop. ***/
} while (1);
} /*** lm_lmdif. ***/
void lmmin( int n_par, double *par, int m_dat, const void *data,
void (*evaluate) (const double *par, int m_dat, const void *data,
double *fvec, int *info),
const lm_control_struct *control, lm_status_struct *status,
void (*printout) (int n_par, const double *par, int m_dat,
const void *data, const double *fvec,
int printflags, int iflag, int iter, int nfev) )
{
/*** 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))) == NULL ||
(diag = (double *) malloc(n * sizeof(double))) == NULL ||
(qtf = (double *) malloc(n * sizeof(double))) == NULL ||
(fjac = (double *) malloc(n*m*sizeof(double))) == NULL ||
(wa1 = (double *) malloc(n * sizeof(double))) == NULL ||
(wa2 = (double *) malloc(n * sizeof(double))) == NULL ||
(wa3 = (double *) malloc(n * sizeof(double))) == NULL ||
(wa4 = (double *) malloc(m * sizeof(double))) == NULL ||
(ipvt = (int *) malloc(n * sizeof(int) )) == NULL ) {
status->info = 9;
return;
}
int j;
if( ! control->scale_diag )
for( j=0; j<n_par; ++j )
diag[j] = 1;
/*** perform fit. ***/
status->info = 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,
( control->scale_diag ? 1 : 2 ),
control->stepbound, &(status->info),
&(status->nfev), fjac, ipvt, qtf, wa1, wa2, wa3, wa4,
evaluate, printout, control->printflags, data );
if ( printout )
(*printout)( n, par, m, data, fvec,
control->printflags, -1, 0, status->nfev );
status->fnorm = lm_enorm(m, fvec);
if ( status->info < 0 )
status->info = 11;
/*** clean up. ***/
free(fvec);
free(diag);
free(qtf);
free(fjac);
free(wa1);
free(wa2);
free(wa3);
free(wa4);
free(ipvt);
} /*** lm_minimize. ***/
void lmmin(
const int n, double *const x, const int m, const double* y,
const void *const data,
void (*const evaluate)(
const double *const par, const int m_dat, const void *const data,
double *const fvec, int *const userbreak),
const lm_control_struct *const C, lm_status_struct *const S)
{
int j, i;
double actred, dirder, fnorm, fnorm1, gnorm, pnorm,
prered, ratio, step, sum, temp, temp1, temp2, temp3;
static double p1 = 0.1, p0001 = 1.0e-4;
int maxfev = C->patience * (n+1);
int inner_success; /* flag for loop control */
double lmpar = 0; /* Levenberg-Marquardt parameter */
double delta = 0;
double xnorm = 0;
double eps = sqrt(MAX(C->epsilon, LM_MACHEP)); /* for forward differences */
int nout = C->n_maxpri==-1 ? n : MIN(C->n_maxpri, n);
/* The workaround msgfile=NULL is needed for default initialization */
FILE* msgfile = C->msgfile ? C->msgfile : stdout;
/* Default status info; must be set ahead of first return statements */
S->outcome = 0; /* status code */
S->userbreak = 0;
S->nfev = 0; /* function evaluation counter */
/*** Check input parameters for errors. ***/
if ( n < 0 ) {
fprintf(stderr, "lmmin: invalid number of parameters %i\n", n);
S->outcome = 10; /* invalid parameter */
return;
}
if (m < n) {
fprintf(stderr, "lmmin: number of data points (%i) "
"smaller than number of parameters (%i)\n", m, n);
S->outcome = 10;
return;
}
if (C->ftol < 0 || C->xtol < 0 || C->gtol < 0) {
fprintf(stderr,
"lmmin: negative tolerance (at least one of %g %g %g)\n",
C->ftol, C->xtol, C->gtol);
S->outcome = 10;
return;
}
if (maxfev <= 0) {
fprintf(stderr, "lmmin: nonpositive function evaluations limit %i\n",
maxfev);
S->outcome = 10;
return;
}
if (C->stepbound <= 0) {
fprintf(stderr, "lmmin: nonpositive stepbound %g\n", C->stepbound);
S->outcome = 10;
return;
}
if (C->scale_diag != 0 && C->scale_diag != 1) {
fprintf(stderr, "lmmin: logical variable scale_diag=%i, "
"should be 0 or 1\n", C->scale_diag);
S->outcome = 10;
return;
}
/*** Allocate work space. ***/
/* Allocate total workspace with just one system call */
char *ws;
if ( ( ws = static_cast<char *>(malloc(
(2*m+5*n+m*n)*sizeof(double) + n*sizeof(int)) ) ) == NULL ) {
S->outcome = 9;
return;
}
/* Assign workspace segments. */
char *pws = ws;
double *fvec = (double*) pws; pws += m * sizeof(double)/sizeof(char);
double *diag = (double*) pws; pws += n * sizeof(double)/sizeof(char);
double *qtf = (double*) pws; pws += n * sizeof(double)/sizeof(char);
double *fjac = (double*) pws; pws += n*m*sizeof(double)/sizeof(char);
double *wa1 = (double*) pws; pws += n * sizeof(double)/sizeof(char);
double *wa2 = (double*) pws; pws += n * sizeof(double)/sizeof(char);
double *wa3 = (double*) pws; pws += n * sizeof(double)/sizeof(char);
double *wf = (double*) pws; pws += m * sizeof(double)/sizeof(char);
int *ipvt = (int*) pws; /*pws += n * sizeof(int) /sizeof(char);*/
/* Initialize diag */ // TODO: check whether this is still needed
if (!C->scale_diag) {
for (j = 0; j < n; j++)
diag[j] = 1.;
}
/*** Evaluate function at starting point and calculate norm. ***/
if( C->verbosity&1 )
fprintf(msgfile, "lmmin start (ftol=%g gtol=%g xtol=%g)\n",
C->ftol, C->gtol, C->xtol);
if( C->verbosity&2 )
lm_print_pars(nout, x, msgfile);
(*evaluate)(x, m, data, fvec, &(S->userbreak));
if( C->verbosity&8 )
{
if (y) {
for( i=0; i<m; ++i )
fprintf(msgfile, " i, f, y-f: %4i %18.8g %18.8g\n",
i, fvec[i], y[i]-fvec[i]);
} else {
for( i=0; i<m; ++i )
fprintf(msgfile, " i, f: %4i %18.8g\n", i, fvec[i]);
}
}
S->nfev = 1;
if ( S->userbreak )
goto terminate;
if ( n == 0 ) {
S->outcome = 13; /* won't fit */
goto terminate;
}
fnorm = lm_fnorm(m, fvec, y);
if( C->verbosity&2 )
fprintf(msgfile, " fnorm = %24.16g\n", fnorm);
if( !isfinite(fnorm) ){
if( C->verbosity )
fprintf(msgfile, "nan case 1\n");
S->outcome = 12; /* nan */
goto terminate;
} else if( fnorm <= LM_DWARF ){
S->outcome = 0; /* sum of squares almost zero, nothing to do */
goto terminate;
}
/*** The outer loop: compute gradient, then descend. ***/
for( int outer=0; ; ++outer ) {
/*** [outer] Calculate the Jacobian. ***/
for (j = 0; j < n; j++) {
temp = x[j];
step = MAX(eps*eps, eps * fabs(temp));
x[j] += step; /* replace temporarily */
(*evaluate)(x, m, data, wf, &(S->userbreak));
++(S->nfev);
if ( S->userbreak )
goto terminate;
for (i = 0; i < m; i++)
fjac[j*m+i] = (wf[i] - fvec[i]) / step;
x[j] = temp; /* restore */
}
if ( C->verbosity&16 ) {
/* print the entire matrix */
printf("Jacobian\n");
for (i = 0; i < m; i++) {
printf(" ");
for (j = 0; j < n; j++)
printf("%.5e ", fjac[j*m+i]);
printf("\n");
}
}
/*** [outer] Compute the QR factorization of the Jacobian. ***/
/* fjac is an m by n array. The upper n by n submatrix of fjac
* is made to contain an upper triangular matrix R with diagonal
* elements of nonincreasing magnitude such that
*
* P^T*(J^T*J)*P = R^T*R
*
* (NOTE: ^T stands for matrix transposition),
*
* where P is a permutation matrix and J is the final calculated
* Jacobian. Column j of P is column ipvt(j) of the identity matrix.
* The lower trapezoidal part of fjac contains information generated
* during the computation of R.
*
* ipvt is an integer 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.
*/
lm_qrfac(m, n, fjac, ipvt, wa1, wa2, wa3);
/* return values are ipvt, wa1=rdiag, wa2=acnorm */
/*** [outer] Form Q^T * fvec, and store first n components in qtf. ***/
if (y)
for (i = 0; i < m; i++)
wf[i] = fvec[i] - y[i];
else
for (i = 0; i < m; i++)
wf[i] = fvec[i];
for (j = 0; j < n; j++) {
temp3 = fjac[j*m+j];
if (temp3 != 0) {
sum = 0;
for (i = j; i < m; i++)
sum += fjac[j*m+i] * wf[i];
temp = -sum / temp3;
for (i = j; i < m; i++)
wf[i] += fjac[j*m+i] * temp;
}
fjac[j*m+j] = wa1[j];
qtf[j] = wf[j];
}
/*** [outer] Compute norm of scaled gradient and detect degeneracy. ***/
gnorm = 0;
for (j = 0; j < n; j++) {
if (wa2[ipvt[j]] == 0)
continue;
sum = 0;
for (i = 0; i <= j; i++)
sum += fjac[j*m+i] * qtf[i];
gnorm = MAX(gnorm, fabs( sum / wa2[ipvt[j]] / fnorm ));
}
if (gnorm <= C->gtol) {
S->outcome = 4;
goto terminate;
}
/*** [outer] Initialize / update diag and delta. ***/
if ( !outer ) {
/* first iteration only */
if (C->scale_diag) {
/* diag := norms of the columns of the initial Jacobian */
for (j = 0; j < n; j++)
diag[j] = wa2[j] ? wa2[j] : 1;
/* xnorm := || D x || */
for (j = 0; j < n; j++)
wa3[j] = diag[j] * x[j];
xnorm = lm_enorm(n, wa3);
} else {
xnorm = lm_enorm(n, x);
}
if( !isfinite(xnorm) ){
if( C->verbosity )
fprintf(msgfile, "nan case 2\n");
S->outcome = 12; /* nan */
goto terminate;
}
/* initialize the step bound delta. */
if ( xnorm )
delta = C->stepbound * xnorm;
else
delta = C->stepbound;
/* only now print the header for the loop table */
if( C->verbosity&2 ) {
fprintf(msgfile, " #o #i lmpar prered actred"
" ratio dirder delta"
" pnorm fnorm");
for (i = 0; i < nout; ++i)
fprintf(msgfile, " p%i", i);
fprintf(msgfile, "\n");
}
} else {
if (C->scale_diag) {
for (j = 0; j < n; j++)
diag[j] = MAX( diag[j], wa2[j] );
}
}
/*** The inner loop. ***/
int inner = 0;
do {
/*** [inner] Determine the Levenberg-Marquardt parameter. ***/
lm_lmpar(n, fjac, m, ipvt, diag, qtf, delta, &lmpar,
wa1, wa2, wf, wa3);
/* used return values are fjac (partly), lmpar, wa1=x, wa3=diag*x */
/* predict scaled reduction */
pnorm = lm_enorm(n, wa3);
if( !isfinite(pnorm) ){
if( C->verbosity )
fprintf(msgfile, "nan case 3\n");
S->outcome = 12; /* nan */
goto terminate;
}
temp2 = lmpar * SQR( pnorm / fnorm );
for (j = 0; j < n; j++) {
wa3[j] = 0;
for (i = 0; i <= j; i++)
wa3[i] -= fjac[j*m+i] * wa1[ipvt[j]];
}
temp1 = SQR( lm_enorm(n, wa3) / fnorm );
if( !isfinite(temp1) ){
if( C->verbosity )
fprintf(msgfile, "nan case 4\n");
S->outcome = 12; /* nan */
goto terminate;
}
prered = temp1 + 2 * temp2;
dirder = -temp1 + temp2; /* scaled directional derivative */
/* at first call, adjust the initial step bound. */
if ( !outer && !inner && pnorm < delta )
delta = pnorm;
/*** [inner] Evaluate the function at x + p. ***/
for (j = 0; j < n; j++)
wa2[j] = x[j] - wa1[j];
(*evaluate)( wa2, m, data, wf, &(S->userbreak) );
++(S->nfev);
if ( S->userbreak )
goto terminate;
fnorm1 = lm_fnorm(m, wf, y);
// exceptionally, for this norm we do not test for infinity
// because we can deal with it without terminating.
/*** [inner] Evaluate the scaled reduction. ***/
/* actual scaled reduction (supports even the case fnorm1=infty) */
if (p1 * fnorm1 < fnorm)
actred = 1 - SQR(fnorm1 / fnorm);
else
actred = -1;
/* ratio of actual to predicted reduction */
ratio = prered ? actred/prered : 0;
if( C->verbosity&32 ) {
if (y) {
for( i=0; i<m; ++i )
fprintf(msgfile, " i, f, y-f: %4i %18.8g %18.8g\n",
i, fvec[i], y[i]-fvec[i]);
} else {
for( i=0; i<m; ++i )
fprintf(msgfile, " i, f, y-f: %4i %18.8g\n",
i, fvec[i]);
}
}
if( C->verbosity&2 ) {
printf("%3i %2i %9.2g %9.2g %9.2g %14.6g"
" %9.2g %10.3e %10.3e %21.15e",
outer, inner, lmpar, prered, actred, ratio,
dirder, delta, pnorm, fnorm1);
for (i = 0; i < nout; ++i)
fprintf(msgfile, " %16.9g", wa2[i]);
fprintf(msgfile, "\n");
}
/* update the step bound */
if (ratio <= 0.25) {
if (actred >= 0)
temp = 0.5;
else
temp = 0.5 * dirder / (dirder + 0.5 * actred);
if (p1 * fnorm1 >= fnorm || temp < p1)
temp = p1;
delta = temp * MIN(delta, pnorm / p1);
lmpar /= temp;
} else if (lmpar == 0 || ratio >= 0.75) {
delta = 2 * pnorm;
lmpar *= 0.5;
}
/*** [inner] On success, update solution, and test for convergence. ***/
inner_success = ratio >= p0001;
if ( inner_success ) {
/* update x, fvec, and their norms */
if (C->scale_diag) {
for (j = 0; j < n; j++) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
} else {
for (j = 0; j < n; j++)
x[j] = wa2[j];
}
for (i = 0; i < m; i++)
fvec[i] = wf[i];
xnorm = lm_enorm(n, wa2);
if( !isfinite(xnorm) ){
if( C->verbosity )
fprintf(msgfile, "nan case 6\n");
S->outcome = 12; /* nan */
goto terminate;
}
fnorm = fnorm1;
}
/* convergence tests */
S->outcome = 0;
if( fnorm<=LM_DWARF )
goto terminate; /* success: sum of squares almost zero */
/* test two criteria (both may be fulfilled) */
if (fabs(actred) <= C->ftol && prered <= C->ftol && ratio <= 2)
S->outcome = 1; /* success: x almost stable */
if (delta <= C->xtol * xnorm)
S->outcome += 2; /* success: sum of squares almost stable */
if (S->outcome != 0) {
goto terminate;
}
/*** [inner] Tests for termination and stringent tolerances. ***/
if ( S->nfev >= maxfev ){
S->outcome = 5;
goto terminate;
}
if ( fabs(actred) <= LM_MACHEP &&
prered <= LM_MACHEP && ratio <= 2 ){
S->outcome = 6;
goto terminate;
}
if ( delta <= LM_MACHEP*xnorm ){
S->outcome = 7;
goto terminate;
}
if ( gnorm <= LM_MACHEP ){
S->outcome = 8;
goto terminate;
}
/*** [inner] End of the loop. Repeat if iteration unsuccessful. ***/
++inner;
} while ( !inner_success );
/*** [outer] End of the loop. ***/
};
terminate:
S->fnorm = lm_fnorm(m, fvec, y);
if( C->verbosity&1 )
fprintf(msgfile, "lmmin terminates with outcome %i\n", S->outcome);
if( C->verbosity&2 )
lm_print_pars(nout, x, msgfile);
if( C->verbosity&8 ) {
if (y) {
for( i=0; i<m; ++i )
fprintf(msgfile, " i, f, y-f: %4i %18.8g %18.8g\n",
i, fvec[i], y[i]-fvec[i] );
} else {
for( i=0; i<m; ++i )
fprintf(msgfile, " i, f, y-f: %4i %18.8g\n", i, fvec[i]);
}
}
if( C->verbosity&2 )
fprintf(msgfile, " fnorm=%24.16g xnorm=%24.16g\n", S->fnorm, xnorm);
if ( S->userbreak ) /* user-requested break */
S->outcome = 11;
/*** Deallocate the workspace. ***/
free(ws);
} /*** lmmin. ***/
double lm_fnorm(const int n, const double *const x, const double *const y)
{
/* This function calculates the Euclidean norm of an n-vector x-y.
*
* The Euclidean norm is computed by accumulating the sum of
* squares in three different sums. The sums of squares for the
* small and large components are scaled so that no overflows
* occur. Non-destructive underflows are permitted. Underflows
* and overflows do not occur in the computation of the unscaled
* sum of squares for the intermediate components.
* The definitions of small, intermediate and large components
* depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. The main
* restrictions on these constants are that LM_SQRT_DWARF**2 not
* underflow and LM_SQRT_GIANT**2 not overflow.
*
* Parameters:
*
* n is a positive integer INPUT variable.
*
* x, y are INPUT arrays of length n.
*/
if (!y)
return lm_enorm(n, x);
int i;
double agiant, s1, s2, s3, xabs, x1max, x3max, temp;
s1 = 0;
s2 = 0;
s3 = 0;
x1max = 0;
x3max = 0;
agiant = LM_SQRT_GIANT / n;
/** sum squares. **/
for (i = 0; i < n; i++) {
xabs = fabs(x[i]-y[i]);
if (xabs > LM_SQRT_DWARF) {
if ( xabs < agiant ) {
s2 += xabs * xabs;
} else if ( xabs > x1max ) {
temp = x1max / xabs;
s1 = 1 + s1 * SQR(temp);
x1max = xabs;
} else {
temp = xabs / x1max;
s1 += SQR(temp);
}
} else if ( xabs > x3max ) {
temp = x3max / xabs;
s3 = 1 + s3 * SQR(temp);
x3max = xabs;
} else if (xabs != 0) {
temp = xabs / x3max;
s3 += SQR(temp);
}
}
/** calculation of norm. **/
if (s1 != 0)
return x1max * sqrt(s1 + (s2 / x1max) / x1max);
else if (s2 != 0)
if (s2 >= x3max)
return sqrt(s2 * (1 + (x3max / s2) * (x3max * s3)));
else
return sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
else
return x3max * sqrt(s3);
} /*** lm_fnorm. ***/