int lmdif(custom_funcmult *funcmult, double *x, int M, int N, double *fvec, double *fjac, int ldfjac,
int maxfev,double *diag,int mode,double factor,int nprint,double eps,double epsfcn,double ftol,double gtol,
double xtol,int *nfev,int *njev,int *ipvt, double *qtf) {
int info;
int i,j,l,iter;
double actred,delta,dirder,epsmch,fnorm,fnorm1,gnorm,one,par,pnorm,prered,p1,p5,p25,p75,p0001,ratio,
sum,temp,temp1,temp2,xnorm,zero;
double *wa1,*wa2,*wa3,*wa4;
/*
* * This routine is a C translation of Fortran Code by
* argonne national laboratory. minpack project. march 1980.
burton s. garbow, kenneth e. hillstrom, jorge j. more
* M is a positive integer input variable set to the number
c of functions.
c
c N is a positive integer input variable set to the number
c of variables. N must not exceed M.
c
c x is an array of length N. on input x must contain
c an initial estimate of the solution vector. on output x
c contains the final estimate of the solution vector.
c
c fvec is an output array of length M which contains
c the functions evaluated at the output x.
c
c fjac is an output M by N array. the upper N by N submatrix
c of fjac contains an upper triangular matrix r with
c diagonal elements of nonincreasing magnitude such that
c
c t t t
c p *(jac *jac)*p = r *r,
c
c where p is a permutation matrix and jac is the final
c calculated jacobian. column j of p is column ipvt(j)
c (see below) of the identity matrix. the lower trapezoidal
c part of fjac contains information generated during
c the computation of r.
c
c ldfjac is a positive integer input variable not less than M
c which specifies the leading dimension of the array fjac.
c
c ftol is a nonnegative input variable. termination
c occurs when both the actual and predicted relative
c reductions in the sum of squares are at most ftol.
c therefore, ftol measures the relative error desired
c in the sum of squares.
c
c xtol is a nonnegative input variable. termination
c occurs when the relative error between two consecutive
c iterates is at most xtol. therefore, xtol measures the
c relative error desired in the approximate solution.
c
c gtol is a nonnegative input variable. termination
c occurs when the cosine of the angle between fvec and
c any column of the jacobian is at most gtol in absolute
c value. therefore, gtol measures the orthogonality
c desired between the function vector and the columns
c of the jacobian.
c
c maxfev is a positive integer input variable. termination
c occurs when the number of calls to fcn with iflag = 1
c has reached maxfev.
c
c epsfcn is an input variable used in determining a suitable
c step length for the forward-difference approximation. this
c approximation assumes that the relative errors in the
c functions are of the order of epsfcn. if epsfcn is less
c than the machine precision, it is assumed that the relative
c errors in the functions are of the order of the machine
c precision.
c
c diag is an array of length N. if mode = 1 (see
c below), diag is internally set. if mode = 2, diag
c must contain positive entries that serve as
c multiplicative scale factors for the variables.
c
c mode is an integer input variable. if mode = 1, the
c variables will be scaled internally. if mode = 2,
c the scaling is specified by the input diag. other
c values of mode are equivalent to mode = 1.
c
c factor is a positive input variable used in determining the
c initial step bound. this bound is set to the product of
c factor and the euclidean norm of diag*x if nonzero, or else
c to factor itself. in most cases factor should lie in the
c interval (.1,100.).100. is a generally recommended value.
c
c nprint is an integer input variable that enables controlled
c printing of iterates if it is positive. in this case,
c fcn is called with iflag = 0 at the beginning of the first
c iteration and every nprint iterations thereafter and
c immediately prior to return, with x, fvec, and fjac
c available for printing. fvec and fjac should not be
c altered. if nprint is not positive, no special calls
c of fcn with iflag = 0 are made.
c
c info is an integer output variable. if the user has
c terminated execution, info is set to the (negative)
c value of iflag. see description of fcn. otherwise,
c info is set as follows.
c
c info = 0 improper input parameters.
c
c info = 1 both actual and predicted relative reductions
c in the sum of squares are at most ftol.
c
c info = 2 relative error between two consecutive iterates
c is at most xtol.
c
c info = 3 conditions for info = 1 and info = 2 both hold.
c
c info = 4 the cosine of the angle between fvec and any
c column of the jacobian is at most gtol in
c absolute value.
c
c info = 5 number of calls to fcn with iflag = 1 has
c reached maxfev.
c
c info = 6 ftol is too small. no further reduction in
c the sum of squares is possible.
c
c info = 7 xtol is too small. no further improvement in
c the approximate solution x is possible.
c
c info = 8 gtol is too small. fvec is orthogonal to the
c columns of the jacobian to machine precision.
c
c nfev is an integer output variable set to the number of
c calls to fcn with iflag = 1.
c
c njev is an integer output variable set to the number of
c calls to fcn with iflag = 2.
c
c ipvt is an integer output array of length N. ipvt
c defines a permutation matrix p such that jac*p = q*r,
c where jac is the final calculated jacobian, q is
c orthogonal (not stored), and r is upper triangular
c with diagonal elements of nonincreasing magnitude.
c column j of p is column ipvt(j) of the identity matrix.
c
c qtf is an output array of length N which contains
c the first n elements of the vector (q transpose)*fvec.
*/
wa1 = (double*) malloc(sizeof(double) *N);
wa2 = (double*) malloc(sizeof(double) *N);
wa3 = (double*) malloc(sizeof(double) *N);
wa4 = (double*) malloc(sizeof(double) *M);
one = 1.0;
zero = 0.0;
p1 = 1.0e-1;
p5 = 5.0e-1;
p25 = 2.5e-1;
p75 = 7.5e-1;
p0001 = 1.0e-4;
epsmch = eps;
info = 0;
*nfev = 0;
*njev = 0;
if (N <= 0 || M < N || ldfjac < M || ftol < zero || xtol < zero || gtol < zero || maxfev <= 0 || factor <= zero) {
return info;
}
if (mode == 2) {
for(j = 0; j < N; ++j) {
if (diag[j] <= 0.0) {
return info;
}
}
}
// evaluate the function at the starting point
// and calculate its norm.
FUNCMULT_EVAL(funcmult,x,M,N,fvec);
*nfev= 1;
fnorm = enorm(fvec,M);
// initialize levenberg-marquardt parameter and iteration counter.
par = zero;
iter = 1;
ratio = zero;
// beginning of the outer loop.
while(1) {
// calculate the jacobian matrix.
ratio = zero;
fdjac2(funcmult,x,M,N,fvec,fjac,ldfjac,epsfcn,epsmch);
*njev = *njev + N;
// compute the qr factorization of the jacobian.
qrfac(fjac,M,N,ldfjac,1,ipvt,N,wa1,wa2,eps);
// on the first iteration and if mode is 1, scale according
// to the norms of the columns of the initial jacobian.
if (iter == 1) {//80
if (mode != 2) {//60
for(j = 0; j < N; ++j) {
diag[j] = wa2[j];
if (wa2[j] == zero) {
diag[j] = one;
}
}
}//60
// on the first iteration, 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 = enorm(wa3,N);
delta = factor*xnorm;
if (delta == zero) {
delta = factor;
}
}//80
// 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) { //130
if (fjac[j*N+j] != zero) {//120
sum = zero;
for(i = j; i < M; ++i) { //100
sum = sum + fjac[i*N+j]*wa4[i];
}//100
temp = -sum/fjac[j*N+j];
for(i = j; i < M; ++i) { //110
wa4[i] = wa4[i] + fjac[i*N+j]*temp;
}//110
}//120
fjac[j*N+j] = wa1[j];
qtf[j] = wa4[j];
}//130
// compute the norm of the scaled gradient.
gnorm = zero;
if (fnorm != zero) {//170
for(j = 0; j < N; ++j) { //160
l = ipvt[j];
if (wa2[l] != zero) {//150
sum = zero;
for(i = 0; i <= j; ++i) { //140
sum = sum + fjac[i*N+j]*(qtf[i]/fnorm);
}//140
gnorm = pmax(gnorm,fabs(sum/wa2[l]));
}//150
}//160
}//170
// test for convergence of the gradient norm.
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
break;
}
// rescale if necessary.
if (mode != 2) { //190
for(j = 0; j < N; ++j) {
diag[j] = pmax(diag[j],wa2[j]);
}
}//190
// beginning of the inner loop.
while(ratio < p0001) {
// determine the levenberg-marquardt parameter.
lmpar(fjac,ldfjac,N,ipvt,diag,qtf,delta,&par,wa1,wa2);
// 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 = enorm(wa3,N);
// on the first iteration, adjust the initial step bound.
if (iter == 1) {
delta = pmin(delta,pnorm);
}
// evaluate the function at x + p and calculate its norm.
FUNCMULT_EVAL(funcmult,wa2,M,N,wa4);
*nfev = *nfev + 1;
fnorm1 = enorm(wa4,M);
// compute the scaled actual reduction.
actred = -one;
if (p1*fnorm1 < fnorm) {
actred = one - (fnorm1/fnorm)*(fnorm1/fnorm);
}
// compute the scaled predicted reduction and
// the scaled directional derivative.
for(j = 0; j < N; ++j) {
wa3[j] = zero;
l = ipvt[j];
temp = wa1[l];
for(i = 0; i <= j; ++i) {
wa3[i] = wa3[i] + fjac[i*N+j]*temp;
}
}
temp1 = enorm(wa3,N);
temp1 = temp1/fnorm;
temp2 = (sqrt(par)*pnorm)/fnorm;
prered = temp1*temp1 + temp2*temp2/p5;
dirder = -(temp1*temp1 + temp2*temp2);
// compute the ratio of the actual to the predicted
// reduction.
ratio = zero;
if (prered != zero) {
ratio = actred/prered;
}
// update the step bound.
if (ratio <= p25) {//240
if (actred >= zero) {
temp = p5;
}
if (actred < zero) {
temp = p5*dirder/(dirder + p5*actred);
}
if (p1*fnorm1 >= fnorm || temp < p1) {
temp = p1;
}
delta = temp*pmin(delta,pnorm/p1);
par = par/temp;
} else if (par == zero || ratio >= p75) { //240 - 260
delta = pnorm/p5;
par = p5*par;
}//260
// test for successful iteration.
if (ratio >= p0001) {//290
// 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 = enorm(wa2,N);
fnorm = fnorm1;
iter = iter + 1;
}//290
// tests for convergence.
if ((fabs(actred) <= ftol) && (prered <= ftol) && (p5*ratio <= one)) {
info = 1;
}
if (delta <= xtol*xnorm) {
info = 2;
}
if ((fabs(actred) <= ftol) && (prered <= ftol) && (p5*ratio <= one) && (info == 2)) {
info = 3;
}
if (info != 0) {
break;
}
// tests for termination and stringent tolerances.
if (*nfev >= maxfev) {
info = 5;
}
if ((fabs(actred) <= epsmch) && (prered <= epsmch) && (p5*ratio <= one)) {
info = 6;
}
if (delta <= epsmch*xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
break;
}
}
if (info != 0) {
break;
}
}
free(wa1);
free(wa2);
free(wa3);
free(wa4);
return info;
}
/*
* **********
*
* subroutine lmdif
*
* 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 which calculates the functions. the jacobian is
* then calculated by a forward-difference approximation.
*
* the subroutine statement is
*
* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
* diag,mode,factor,nprint,info,nfev,fjac,
* ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
*
* where
*
* fcn is the name of the user-supplied subroutine which
* calculates the functions. fcn must be declared
* in an external statement in the user calling
* program, and should be written as follows.
*
* subroutine fcn(m,n,x,fvec,iflag)
* integer m,n,iflag
* double precision x(n),fvec(m)
* ----------
* calculate the functions at x and
* return this vector in fvec.
* ----------
* return
* end
*
* the value of iflag should not be changed by fcn unless
* the user wants to terminate execution of lmdif.
* in this case set iflag to a negative integer.
*
* 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 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.
*
* nprint is an integer input variable that enables controlled
* printing of iterates if it is positive. in this case,
* fcn is called with iflag = 0 at the beginning of the first
* iteration and every nprint iterations thereafter and
* immediately prior to return, with x and fvec available
* for printing. if nprint is not positive, no special calls
* of fcn with iflag = 0 are made.
*
* info is an integer output variable. if the user has
* terminated execution, info is set to the (negative)
* value of iflag. see description of fcn. otherwise,
* info is set as follows.
*
* 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 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 integer output variable set to the number of
* calls to fcn.
*
* 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.
*
* ldfjac is a positive integer input variable not less than m
* which specifies the leading dimension of the array fjac.
*
* 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.
*
* subprograms called
*
* user-supplied ...... fcn
*
* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
*
* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
*
* argonne national laboratory. minpack project. march 1980.
* burton s. garbow, kenneth e. hillstrom, jorge j. more
*
* **********
*/
void lmdif_C(
void (*fcn)(int, int, double[], double[], int *, void *),
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 nprint,
int *info,
int *nfev,
double fjac[],
int ldfjac,
int ipvt[],
double qtf[],
double wa1[],
double wa2[],
double wa3[],
double wa4[],
void *data)
{
int i;
int iflag;
int ij;
int jj;
int iter;
int j;
int l;
double actred;
double delta;
double dirder;
double fnorm;
double fnorm1;
double gnorm;
double par;
double pnorm;
double prered;
double ratio;
double sum;
double temp;
double temp1;
double temp2;
double temp3;
double xnorm;
static double one = 1.0;
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;
static double zero = 0.0;
//static double p05 = 0.05;
*info = 0;
iflag = 0;
*nfev = 0;
/*
* check the input parameters for errors.
*/
if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < zero)
|| (xtol < zero) || (gtol < zero) || (maxfev <= 0)
|| (factor <= zero))
goto L300;
if (mode == 2)
{
/* scaling by diag[] */
for (j=0; j<n; j++)
{
if (diag[j] <= 0.0)
goto L300;
}
}
#ifdef BUG
printf( "lmdif\n" );
#endif
/* evaluate the function at the starting point
* and calculate its norm.
*/
iflag = 1;
fcn(m,n,x,fvec,&iflag, data);
*nfev = 1;
if (iflag < 0)
goto L300;
fnorm = enorm(m,fvec);
/* initialize levenberg-marquardt parameter and iteration counter. */
par = zero;
iter = 1;
/* beginning of the outer loop. */
L30:
/* calculate the jacobian matrix. */
iflag = 2;
fdjac2(fcn, m,n,x,fvec,fjac,ldfjac,&iflag,epsfcn,wa4, data);
// commented out DKB
// *nfev += n;
if (iflag < 0)
goto L300;
/* if requested, call fcn to enable printing of iterates. */
if (nprint > 0)
{
iflag = 0;
if (mod(iter-1,nprint) == 0)
{
fcn(m,n,x,fvec,&iflag, data);
if (iflag < 0)
goto L300;
// printf( "fnorm %.15e\n", enorm(m,fvec));
}
}
/* compute the qr factorization of the jacobian. */
qrfac(m,n,fjac,ldfjac,1,ipvt,n,wa1,wa2,wa3);
// for (j = 0; j < n; j++)
// {
// printf("wa1[%d] = %e\n", j, wa1[j]);
// printf("wa2[%d] = %e\n", j, wa2[j]);
// printf("wa3[%d] = %e\n", j, wa3[j]);
// }
/* on the first iteration and if mode is 1, scale according
* to the norms of the columns of the initial jacobian.
*/
if (iter == 1)
{
// printf("iter = 1, mode = %d\n", mode);
if (mode != 2)
{
for (j=0; j<n; j++)
{
diag[j] = wa2[j];
if (wa2[j] == zero)
diag[j] = one;
}
}
/* on the first iteration, 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 = enorm(n,wa3);
delta = factor*xnorm;
// printf("iter1: xnorm = %e, delta = %e\n", xnorm, delta);
//dkb
if (fabs(delta) <= 1e-4)
// if (delta == zero)
delta = factor;
}
/* form (q transpose)*fvec and store the first n components in qtf. */
for (i=0; i<m; i++)
wa4[i] = fvec[i];
jj = 0;
for (j=0; j<n; j++)
{
temp3 = fjac[jj];
if (temp3 != zero)
{
sum = zero;
ij = jj;
for (i=j; i<m; i++)
{
sum += fjac[ij] * wa4[i];
ij += 1; /* fjac[i+m*j] */
}
temp = -sum / temp3;
ij = jj;
for (i=j; i<m; i++)
{
wa4[i] += fjac[ij] * temp;
ij += 1; /* fjac[i+m*j] */
}
}
fjac[jj] = wa1[j];
jj += m+1; /* fjac[j+m*j] */
qtf[j] = wa4[j];
}
/* compute the norm of the scaled gradient. */
gnorm = zero;
if (fnorm != zero)
{
jj = 0;
for (j=0; j<n; j++)
{
l = ipvt[j];
if (wa2[l] != zero)
{
sum = zero;
ij = jj;
for (i=0; i<=j; i++)
{
sum += fjac[ij]*(qtf[i]/fnorm);
ij += 1; /* fjac[i+m*j] */
}
gnorm = dmax1(gnorm,fabs(sum/wa2[l]));
}
jj += m;
}
}
/* test for convergence of the gradient norm. */
if (gnorm <= gtol)
*info = 4;
if (*info != 0)
goto L300;
//for (j = 0; j < n; j++)
// printf("diag[%d] = %e, wa2[%d] = %e\n", j, diag[j], j, wa2[j]);
/* rescale if necessary. */
if (mode != 2)
{
for (j=0; j<n; j++)
diag[j] = dmax1(diag[j],wa2[j]);
}
/* beginning of the inner loop. */
L200:
/* determine the levenberg-marquardt parameter. */
lmpar(n,fjac,ldfjac,ipvt,diag,qtf,delta,&par,wa1,wa2,wa3,wa4);
/* 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];
//printf("wa2[%d] = %e + %e = %e\n", j, x[j], wa1[j], wa2[j]);
}
pnorm = enorm(n,wa3);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
delta = dmin1(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */
iflag = 1;
//printf("evaluate at:\n");
//for (j=0; j<n; j++)
// printf("wa2[%d] = %e\n", j, wa2[j]);
fcn(m,n,wa2,wa4,&iflag, data);
*nfev += 1;
if (iflag < 0)
goto L300;
fnorm1 = enorm(m,wa4);
#ifdef BUG
printf( "pnorm %.10e fnorm1 %.10e\n", pnorm, fnorm1 );
#endif
/* compute the scaled actual reduction. */
actred = -one;
if ((p1*fnorm1) < fnorm)
{
temp = fnorm1/fnorm;
actred = one - temp * temp;
}
/* compute the scaled predicted reduction and
* the scaled directional derivative.
*/
jj = 0;
for (j=0; j<n; j++)
{
wa3[j] = zero;
l = ipvt[j];
temp = wa1[l];
ij = jj;
for (i=0; i<=j; i++)
{
wa3[i] += fjac[ij]*temp;
ij += 1; /* fjac[i+m*j] */
}
jj += m;
}
temp1 = enorm(n,wa3)/fnorm;
temp2 = (sqrt(par)*pnorm)/fnorm;
prered = temp1*temp1 + (temp2*temp2)/p5;
dirder = -(temp1*temp1 + temp2*temp2);
/* compute the ratio of the actual to the predicted reduction. */
ratio = zero;
if (prered != zero)
ratio = actred/prered;
/* update the step bound. */
if (ratio <= p25)
{
if (actred >= zero)
temp = p5;
else
temp = p5*dirder/(dirder + p5*actred);
if (((p1*fnorm1) >= fnorm) || (temp < p1))
temp = p1;
delta = temp*dmin1(delta,pnorm/p1);
par = par/temp;
}
else
{
if ((par == zero) || (ratio >= p75))
{
delta = pnorm/p5;
par = p5*par;
}
}
/* 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 = enorm(n,wa2);
fnorm = fnorm1;
iter += 1;
}
/* tests for convergence. */
if ((fabs(actred) <= ftol) && (prered <= ftol) && (p5*ratio <= one))
{
*info = 1;
}
if (delta <= xtol*xnorm)
*info = 2;
if ((fabs(actred) <= ftol) && (prered <= ftol)
&& (p5*ratio <= one) && (*info == 2))
{
*info = 3;
}
if (*info != 0)
goto L300;
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev)
*info = 5;
if ((fabs(actred) <= MACHEP) && (prered <= MACHEP) && (p5*ratio <= one))
{
*info = 6;
}
if (delta <= MACHEP*xnorm)
*info = 7;
if (gnorm <= MACHEP)
*info = 8;
if (*info != 0)
goto L300;
/* end of the inner loop. repeat if iteration unsuccessful. */
if (ratio < p0001)
goto L200;
/* end of the outer loop. */
goto L30;
L300:
/* termination, either normal or user imposed. */
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
fcn(m,n,x,fvec,&iflag, data);
}
/* Subroutine */ int lmdif(minpack_funcx_mn fcn, void *p, int m, int n, int mskip, double *x,
double *fvec, double ftol, double xtol, double
gtol, int maxfev, double epsfcn, double *diag, int
mode, double factor, int nprint, int *
nfev, double *fnorm, double *fjac, int ldfjac, int *ipvt, double *qtf,
double *wa1, double *wa2, double *wa3, double *wa4)
{
/* Initialized data */
#define p1 .1
#define p5 .5
#define p25 .25
#define p75 .75
#define p0001 1e-4
/* System generated locals */
double d1, d2;
/* Local variables */
int i, j, l;
double par, sum;
int iter;
double temp, temp1, temp2;
int iflag;
double delta = 0.;
double ratio;
double fnorm, gnorm;
double pnorm, xnorm = 0., fnorm1, actred, dirder, epsmch, prered;
int info;
/* ********** */
/* subroutine lmdif */
/* 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 which calculates the functions. the jacobian is */
/* then calculated by a forward-difference approximation. */
/* the subroutine statement is */
/* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, */
/* diag,mode,factor,nprint,info,nfev,fjac, */
/* ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) */
/* where */
/* fcn is the name of the user-supplied subroutine which */
/* calculates the functions. fcn must be declared */
/* in an external statement in the user calling */
/* program, and should be written as follows. */
/* subroutine fcn(m,n,x,fvec,iflag) */
/* integer m,n,iflag */
/* double precision x(n),fvec(m) */
/* ---------- */
/* calculate the functions at x and */
/* return this vector in fvec. */
/* ---------- */
/* return */
/* end */
/* the value of iflag should not be changed by fcn unless */
/* the user wants to terminate execution of lmdif. */
/* in this case set iflag to a negative integer. */
/* 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 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. */
/* nprint is an integer input variable that enables controlled */
/* printing of iterates if it is positive. in this case, */
/* fcn is called with iflag = 0 at the beginning of the first */
/* iteration and every nprint iterations thereafter and */
/* immediately prior to return, with x and fvec available */
/* for printing. if nprint is not positive, no special calls */
/* of fcn with iflag = 0 are made. */
/* info is an integer output variable. if the user has */
/* terminated execution, info is set to the (negative) */
/* value of iflag. see description of fcn. otherwise, */
/* info is set as follows. */
/* 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 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 integer output variable set to the number of */
/* calls to fcn. */
/* 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. */
/* ldfjac is a positive integer input variable not less than m */
/* which specifies the leading dimension of the array fjac. */
/* 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. */
/* subprograms called */
/* user-supplied ...... fcn */
/* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac */
/* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < m || ftol < 0. || xtol < 0. ||
gtol < 0. || maxfev <= 0 || factor <= 0.) {
goto TERMINATE;
}
if (mode == 2) {
for (j = 0; j < n; ++j) {
if (diag[j] <= 0.) {
goto TERMINATE;
}
}
}
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, m, n, mskip, x, fvec, 1);
*nfev = 1;
if (iflag < 0) {
goto TERMINATE;
}
fnorm = enorm(m, fvec);
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
/* beginning of the outer loop. */
for (;;) {
/* calculate the jacobian matrix. */
iflag = fdjac2(fcn, p, m, n, x, fvec, fjac, ldfjac,
epsfcn, wa4);
*nfev += n;
if (iflag < 0) {
goto TERMINATE;
}
/* if requested, call fcn to enable printing of iterates. */
if (nprint > 0) {
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, m, n, x, fvec, 0);
}
if (iflag < 0) {
goto TERMINATE;
}
}
/* compute the qr factorization of the jacobian. */
qrfac(m, n, fjac, ldfjac, TRUE_, ipvt, n,
wa1, wa2, wa3);
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter == 1) {
if (mode != 2) {
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
}
}
/* on the first iteration, 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 = enorm(n, wa3);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
}
/* 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) {
if (fjac[j + j * ldfjac] != 0.) {
sum = 0.;
for (i = j; i < m; ++i) {
sum += fjac[i + j * ldfjac] * wa4[i];
}
temp = -sum / fjac[j + j * ldfjac];
for (i = j; i < m; ++i) {
wa4[i] += fjac[i + j * ldfjac] * temp;
}
}
fjac[j + j * ldfjac] = wa1[j];
qtf[j] = wa4[j];
}
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm != 0.) {
for (j = 0; j < n; ++j) {
l = ipvt[j]-1;
if (wa2[l] != 0.) {
sum = 0.;
for (i = 0; i <= j; ++i) {
sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm);
}
/* Computing MAX */
d1 = fabs(sum / wa2[l]);
gnorm = max(gnorm,d1);
}
}
}
/* test for convergence of the gradient norm. */
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
goto TERMINATE;
}
/* rescale if necessary. */
if (mode != 2) {
for (j = 0; j < n; ++j) {
/* Computing MAX */
d1 = diag[j], d2 = wa2[j];
diag[j] = max(d1,d2);
}
}
/* beginning of the inner loop. */
do {
/* determine the levenberg-marquardt parameter. */
lmpar(n, fjac, ldfjac, ipvt, diag, qtf, delta,
&par, wa1, wa2, wa3, wa4);
/* 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 = enorm(n, wa3);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, wa2, wa4, 1);
++(*nfev);
if (iflag < 0) {
goto TERMINATE;
}
fnorm1 = enorm(m, wa4);
/* compute the scaled actual reduction. */
actred = -1.;
if (p1 * fnorm1 < fnorm) {
/* Computing 2nd power */
d1 = fnorm1 / fnorm;
actred = 1. - d1 * d1;
}
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
for (j = 0; j < n; ++j) {
wa3[j] = 0.;
l = ipvt[j]-1;
temp = wa1[l];
for (i = 0; i <= j; ++i) {
wa3[i] += fjac[i + j * ldfjac] * temp;
}
}
temp1 = enorm(n, wa3) / fnorm;
temp2 = (sqrt(par) * pnorm) / fnorm;
prered = temp1 * temp1 + temp2 * temp2 / p5;
dirder = -(temp1 * temp1 + temp2 * temp2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.) {
ratio = actred / prered;
}
/* 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;
}
/* Computing MIN */
d1 = pnorm / p1;
delta = temp * min(delta,d1);
par /= temp;
} else {
if (par == 0. || ratio >= p75) {
delta = pnorm / p5;
par = p5 * par;
}
}
/* 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 = enorm(n, wa2);
fnorm = fnorm1;
++iter;
}
/* tests for convergence. */
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1.) {
info = 1;
}
if (delta <= xtol * xnorm) {
info = 2;
}
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1. && info == 2) {
info = 3;
}
if (info != 0) {
goto TERMINATE;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 5;
}
if (fabs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) {
info = 6;
}
if (delta <= epsmch * xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
goto TERMINATE;
}
/* end of the inner loop. repeat if iteration unsuccessful. */
} while (ratio < p0001);
/* end of the outer loop. */
}
TERMINATE:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
if (nprint > 0) {
(*fcn)(p, m, n, x, fvec, 0);
}
return info;
/* last card of subroutine lmdif. */
} /* lmdif_ */
