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_ */