DECLARE FUNCTION DotProduct# (n%, a#(), b#()) DECLARE SUB initpt (n%, x#(), nprob%, factor#) DECLARE SUB ssqfcn (m%, n%, x#(), fvec#(), nprob%) DECLARE SUB lmdif1 (m%, n%, x#(), fvec#(), tol#, info%, iwa%()) DECLARE FUNCTION XMAX# (a#, b#) DECLARE FUNCTION XMIN# (a#, b#) DECLARE SUB fcn (m%, n%, x#(), fvec#(), iflag%) DECLARE SUB lmdif (m%, n%, x#(), fvec#(), ftol#, xtol#, gtol#, maxfev%, epsfcn#, mode%, factor#, nprint%, info%, nfev%, fjac#(), ipvt%()) DECLARE SUB fdjac2 (m%, n%, x#(), fvec#(), fjac#(), iflag%, epsfcn#) DECLARE SUB qrfac (m%, n%, a#(), pivot AS ANY, ipvt%(), rdiag#(), acnorm#()) DECLARE SUB lmpar (n%, r#(), ipvt%(), diag#(), qtb#(), delta#, par#, x#(), sdiag#()) DECLARE SUB lmder (m%, n%, x#(), fvec#(), fjac#(), ftol#, xtol#, gtol#, maxfev%, mode%, factor#, nprint%, info%, nfev%, njev%, ipvt%()) DECLARE SUB qrsolv (n%, r#(), ipvt%(), diag#(), qtb#(), x#(), sdiag#()) DECLARE FUNCTION enorm# (n%, x#()) '********************************************************************* '* THIS PROGRAM TESTS CODES FOR THE LEAST-SQUARES SOLUTION OF * '* M NONLINEAR EQUATIONS IN N VARIABLES. IT CONSISTS OF A DRIVER * '* AND AN INTERFACE SUBROUTINE FCN. THE DRIVER READS IN DATA, * '* CALLS THE NONLINEAR LEAST-SQUARES SOLVER, AND FINALLY PRINTS * '* OUT INFORMATION ON THE PERFORMANCE OF THE SOLVER. THIS IS * '* ONLY A SAMPLE DRIVER, MANY OTHER DRIVERS ARE POSSIBLE. THE * '* INTERFACE SUBROUTINE FCN IS NECESSARY TO TAKE INTO ACCOUNT THE * '* FORMS OF CALLING SEQUENCES USED BY THE FUNCTION AND JACOBIAN * '* SUBROUTINES IN THE VARIOUS NONLINEAR LEAST-SQUARES SOLVERS. * '* * '* PROCEDURES OR FUNCTIONS CALLED (SEE UNIT LM.PAS): * '* * '* INITPT, SSQFCN, LMDIF1, ENORM, FCN * '* * '* From F77 program By: * '* ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980. * '* BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE * '* ----------------------------------------------------------------- * '* SAMPLE RUNS: * '* Example #1 (size 2) * '* Solve following system (with initial conditions x1=1, x2=1): * '* X1^2 + X1 + X2^2 - 2 = 0 * '* X1^2 + X2 - X2^2 - 1 + log(X1) = 0 * '* * '* Input #example(1 to 3): 1 * '* PROBLEM 1 DIMENSIONS 2 2 * '* INITIAL L2 NORM OF THE RESIDUALS: 1 * '* FINAL L2 NORM OF THE RESIDUALS..: 2.598106764581068D-17 * '* NUMBER OF FUNCTION EVALUATION...: 25 * '* NUMBER OF JACOBIAN EVALUATIONS..: 6 * '* EXIT PARAMETER..................: 2 * '* FINAL APPROXIMATE SOLUTION: * '* .915554 .496191 * '* * '* SUMMARY OF 1 CALL(S) TO LMDIF1: * '* NPROB N M NFEV NJEV INFO FINAL L2 NORM * '* 1 2 2 25 6 2 2.598106764581068D-17 * '* * '* Example #2 (size 4) * '* Solve following system (with initial conditions x1..x4 = 1): * '* 10.0*x + x2 + x3 + x4 - 20.0 + Sqr(sin(x1)) + Sqr(cos(x2)) = 0 * '* x1 + 20.0*x2 + x3 + x4 - 48.0 + one/pow^6 = 0 * '* Sqr(x1 + x2) + 30.0*x3 + x4 - 97.0 + log(x1) + log(x2+x3) = 0 * '* x1 + x2 + x3 + 40.0*x4 - 166.0 + Sqr(x1) = 0 * '* * '* Input #example(1 to 3): 2 * '* PROBLEM 2 DIMENSIONS 4 4 * '* INITIAL L2 NORM OF THE RESIDUALS: 138.760694011757 * '* FINAL L2 NORM OF THE RESIDUALS..: 2.745272956152202D-15 * '* NUMBER OF FUNCTION EVALUATION...: 31 * '* NUMBER OF JACOBIAN EVALUATIONS..: 5 * '* EXIT PARAMETER..................: 2 * '* FINAL APPROXIMATE SOLUTION......: * '* 1.040647 1.972398 2.745049 3.978973 * '* * '* SUMMARY OF 1 CALL(S) TO LMDIF1: * '* NPROB N M NFEV NJEV INFO FINAL L2 NORM * '* 2 4 4 31 5 2 2.745272956152202D-15 * '* * '* Example #3 (size 6) - Stiff system * '* Solve following system (with initial conditions x1..x6 = 1): * '* X1 + X2 + X4 - .001 = 0 * '* X5 + X6 -55 = 0 * '* X1 + X2 + X3 + 2X5 + X6 - 110.001 = 0 * '* X1 - 0.1X2 = 0 * '* X1 - 10000 X3 X4 = 0 * '* X5 - 5.5e15 X3 X6 = 0 * '* * '* Input #example(1 to 3): 3 * '* PROBLEM 3 DIMENSIONS 6 6 * '* INITIAL L2 NORM OF THE RESIDUALS: 5.499D+15 * '* FINAL L2 NORM OF THE RESIDUALS..: 4.405117050795294D-15 * '* NUMBER OF FUNCTION EVALUATION...: 161 * '* NUMBER OF JACOBIAN EVALUATIONS..: 20 * '* EXIT PARAMETER..................: 2 * '* FINAL APPROXIMATE SOLUTION......: * '* 0.000082 0.000826 0.00009 0.00009 54.999999 0 * '* * '* SUMMARY OF 1 CALL(S) TO LMDIF1: * '* NPROB N M NFEV NJEV INFO FINAL L2 NORM * '* 3 6 6 161 20 2 4.405117050795294D-15 * '* * '* Quick Basic Release 1.0 By J-P Moreau, Paris. * '* (www.jpmoreau.fr) * '********************************************************************* 'PROGRAM test_lmdif DEFDBL A-H, O-Z DEFINT I-N DIM SHARED ISIZE DIM SHARED nprob, nfev, njev DIM SHARED zero, zp25, zp5, one, two, five, eight DIM SHARED ten, c13, c14, c29, c45 DIM SHARED p5, p25, p05 DIM SHARED epsmch ISIZE = 25 DIM iwa(ISIZE), ma(ISIZE), na(ISIZE), nf(ISIZE), nj(ISIZE), np(ISIZE), nx(ISIZE) DIM xfnm(ISIZE), fvec(ISIZE), x(ISIZE) DIM SHARED True AS INTEGER DIM SHARED False AS INTEGER zero = 0#: zp25 = .25: zp5 = .5: one = 1#: two = 2#: five = 5#: eight = 8# ten = 10#: c13 = 13#: c14 = 14#: c29 = 29#: c45 = 45# p5 = .5: p25 = .25: p05 = .05 epsmch = 2.25E-16 True = 1: False = 0 tol = .00000001# ic = 0 CLS PRINT INPUT " Input #example (1 to 3): ", nprob ' Visual Basic ' nprob = InputBox("Input # example (1 to 3):", "Data", "1") IF nprob = 1 THEN n = 2 ELSEIF nprob = 2 THEN n = 4 ELSEIF nprob = 3 THEN n = 6 ELSE PRINT " Warning: Example not implemented!" END ' Visual Basic ' Res = MsgBox("Example not implemented", vbOKCancel, "Warning") ' Exit Sub END IF m = n ntries = 1 factor = one FOR k = 1 TO ntries ic = ic + 1 initpt n, x(), nprob, factor ssqfcn m, n, x(), fvec(), nprob xnorm1 = enorm(m, fvec()) PRINT PRINT " PROBLEM "; nprob; " DIMENSIONS "; n; " "; m nfev = 0 njev = 0 lmdif1 m, n, x(), fvec(), tol, info, iwa() ssqfcn m, n, x(), fvec(), nprob xnorm2 = enorm(m, fvec()) np(ic) = nprob na(ic) = n ma(ic) = m nf(ic) = nfev njev = njev / n nj(ic) = njev nx(ic) = info xfnm(ic) = xnorm2 PRINT " INITIAL L2 NORM OF THE RESIDUALS: "; xnorm1 PRINT " FINAL L2 NORM OF THE RESIDUALS..: "; xnorm2 PRINT " NUMBER OF FUNCTION EVALUATION...: "; nfev PRINT " NUMBER OF JACOBIAN EVALUATIONS..: "; njev PRINT " EXIT PARAMETER..................: "; info PRINT " FINAL APPROXIMATE SOLUTION: " FOR i = 1 TO n PRINT " "; INT(x(i) * 1000000) / 1000000; NEXT i PRINT factor = ten * factor NEXT k PRINT PRINT " SUMMARY OF "; ic; " CALL(S) TO LMDIF1:" PRINT " NPROB N M NFEV NJEV INFO FINAL L2 NORM" FOR i = 1 TO ic PRINT " "; np(i); " "; na(i); " "; ma(i); " "; nf(i); " "; nj(i); " "; nx(i); " "; xfnm(i) NEXT i END 'of main program FUNCTION DotProduct (n, a(), b()) Sum = 0# FOR i = 1 TO n Sum = Sum + a(i) * b(i) NEXT i DotProduct = Sum END FUNCTION FUNCTION enorm (n, x()) 'calculate norm of vector x of size n temp = 0# FOR i = 1 TO n temp = temp + x(i) ^ 2 NEXT i enorm = SQR(temp) END FUNCTION SUB fcn (m, n, x(), fvec(), iflag) ' ************************************************************ ' THE CALLING SEQUENCE OF FCN SHOULD BE IDENTICAL TO THE ' CALLING SEQUENCE OF THE FUNCTION SUBROUTINE IN THE NONLINEAR ' LEAST-SQUARES SOLVER. FCN SHOULD ONLY CALL THE TESTING ' FUNCTION SUBROUTINE SSQFCN WITH THE APPROPRIATE VALUE OF ' PROBLEM NUMBER (NPROB). ' SUBPROGRAMS CALLED ' USER SUPPLIED: SSQFCN ' ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980. ' BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE ' ************************************************************ } ssqfcn m, n, x(), fvec(), nprob IF iflag = 1 THEN nfev = nfev + 1 IF iflag = 2 THEN njev = njev + 1 END SUB SUB fdjac2 (m, n, x(), fvec(), fjac(), iflag, epsfcn) ' *********************************************************************** ' Subroutine fdjac2 ' this Subroutine computes a forward-difference approximation ' to the m by n jacobian matrix associated with a specified ' problem of m functions in n variables. ' the Subroutine statement is ' Subroutine fdjac2(m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) ' where: ' fcn is the name of the user-supplied Subroutine which calculates ' the functions (removed from list of arguments). ' fcn should be written as follows: ' Subroutine fcn (m, n:integer; x, fvec:pVec; var iflag:integer); ' begin ' ---------- ' calculate the functions at x and return this vector in fvec. ' ---------- ' end; ' the value of iflag should not be changed by fcn unless ' the user wants to terminate execution of lmdif1. ' 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 input array of length n. ' fvec is an input array of length m which must contain the ' functions evaluated at x. ' fjac is an output m by n array which contains the ' approximation to the jacobian matrix evaluated at x. ' ldfjac is a positive integer input variable not less than m ' which specifies the leading dimension of the array fjac. ' iflag is an integer variable which can be used to terminate ' the execution of fdjac2. see description of fcn. ' 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. ' wa is a work array of length m. ' subprograms called ' user-supplied ...... fcn, ssqfcn ' minpack-supplied ... dpmpa, max ' pascal-supplied ... ABS, SQRT ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' ************************************************************************** DIM wa(ISIZE) eps = SQR(XMAX(epsfcn, epsmch)) FOR j = 1 TO n temp = x(j) h = eps * ABS(temp) IF h = zero THEN h = eps x(j) = temp + h fcn m, n, x(), wa(), iflag IF iflag < 0 THEN EXIT SUB x(j) = temp FOR i = 1 TO m fjac(n * (i - 1) + j) = (wa(i) - fvec(i)) / h NEXT i NEXT j END SUB SUB initpt (n, x(), nprob, factor) ' ************************************************************** ' Procedure INITPT ' THIS Procedure SPECIFIES THE STANDARD STARTING POINTS FOR THE ' FUNCTIONS DEFINED BY Procedure SSQFCN. THE Procedure RETURNS ' IN X A MULTIPLE (FACTOR) OF THE STANDARD STARTING POINT. FOR ' THE 11TH FUNCTION THE STANDARD STARTING POINT IS ZERO, SO IN ' THIS CASE, IF FACTOR IS NOT UNITY, THEN THE Procedure RETURNS ' THE VECTOR X(J) = FACTOR, J=1,...,N. ' THE Procedure STATEMENT IS ' PROCEDURE INITPT(N,X,NPROB,FACTOR) ' WHERE ' N IS A POSITIVE INTEGER INPUT VARIABLE. ' X IS AN OUTPUT ARRAY OF LENGTH N WHICH CONTAINS THE STANDARD ' STARTING POINT FOR PROBLEM NPROB MULTIPLIED BY FACTOR. ' NPROB IS A POSITIVE INTEGER INPUT VARIABLE WHICH DEFINES THE ' NUMBER OF THE PROBLEM. NPROB MUST NOT EXCEED 18. ' FACTOR IS AN INPUT VARIABLE WHICH SPECIFIES THE MULTIPLE OF ' THE STANDARD STARTING POINT. IF FACTOR IS UNITY, NO ' MULTIPLICATION IS PERFORMED. ' ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980. ' BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE ' **************************************************************** ' SELECTION OF INITIAL POINT FOR j = 1 TO n x(j) = one NEXT j ' COMPUTE MULTIPLE OF INITIAL POINT IF factor = 1# THEN EXIT SUB FOR j = 1 TO n x(j) = factor * x(j) NEXT j END SUB SUB lmder (m, n, x(), fvec(), fjac(), ftol, xtol, gtol, maxfev, mode, factor, nprint, info, nfev, njev, ipvt()) ' **************************************************************************** ' Subroutine lmder ' the purpose of lmder 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 and the jacobian. ' the Subroutine statement is ' Subroutine lmder(m,n,x,fvec,fjac,ldfjac,ftol,xtol,gtol, ' maxfev,diag,mode,factor,nprint,info,nfev, ' njev,ipvt,qtf,wa1,wa2,wa3,wa4) ' where: ' fcn is the name of the user-supplied Subroutine which calculates ' the functions (removed from list of arguments). ' fcn should be written as follows: ' Subroutine fcn (m, n:integer; x, fvec:pVec; var iflag:integer); ' begin ' ---------- ' calculate the functions at x and return this vector in fvec. ' ---------- ' end; ' the value of iflag should not be changed by fcn unless ' the user wants to terminate execution of lmdif1. ' 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. ' 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. ' 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 with iflag = 1 has reached maxfev. ' 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, fvec, ' and fjac available for printing. fvec and fjac should not be ' altered. 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 with iflag = 1 has reached 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 with iflag = 1. ' njev is an integer output variable set to the number of ' calls to fcn with iflag = 2. ' 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, ssqfcn ' dpmpar, enorm, lmpar, qrfac, xmax, xmin ' Basic-supplied ... ABS, SQR, Mod ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' ****************************************************************************** 'Labels: 20,30,40,60,80,120,170,200,260,290,300 DIM diag(ISIZE), qtf(ISIZE), tmp(ISIZE), tmp1(ISIZE), wa1(ISIZE), wa2(ISIZE), wa3(ISIZE), wa4(ISIZE) CONST p1 = .1, p75 = .75, p0001 = .0001 info = 0 iflag = 0 nfev = 0 njev = 0 ' check the input parameters for errors IF (n <= 0) OR (m < n) OR (ftol < zero) OR (xtol < zero) OR (gtol < zero) OR (maxfev <= 0) OR (factor <= zero) THEN GOTO 300 IF mode <> 2 THEN GOTO 20 FOR j = 1 TO n IF diag(j) <= zero THEN GOTO 300 NEXT j ' evaluate the function at the starting point and calculate its norm 20 iflag = 1 fcn m, n, x(), fvec(), iflag nfev = 1 IF iflag < 0 THEN GOTO 300 fxnorm = enorm(m, fvec()) ' initialize levenberg-marquardt parameter and iteration counter par = zero iter = 1 ' beginning of the outer loop. ' calculate the jacobian matrix 30 iflag = 2 fcn m, n, x(), fvec(), iflag njev = njev + 1 IF iflag < 0 THEN GOTO 300 ' if requested, call fcn to enable printing of iterates IF nprint <= 0 THEN GOTO 40 iflag = 0 IF (iter - 1) MOD nprint = 0 THEN fcn m, n, x(), fvec(), iflag IF iflag < 0 THEN GOTO 300 ' compute the qr factorization of the jacobian 40 qrfac m, n, fjac(), 1, ipvt(), wa1(), wa2() ' on the first iteration and if mode is 1, scale according ' to the norms of the columns of the initial jacobian IF iter <> 1 THEN GOTO 80 IF mode = 2 THEN GOTO 60 FOR j = 1 TO n diag(j) = wa2(j) IF wa2(j) = zero THEN diag(j) = one NEXT j ' on the first iteration, calculate the norm of the scaled x ' and initialize the step bound delta 60 FOR j = 1 TO n wa3(j) = diag(j) * x(j) NEXT j xnorm = enorm(n, wa3()) delta = factor * xnorm IF delta = zero THEN delta = factor ' form (q transpose)*fvec and store the first n components in qtf 80 FOR j = 1 TO m wa4(j) = fvec(j) NEXT j FOR j = 1 TO n IF fjac(n * (j - 1) + j) = zero THEN GOTO 120 FOR i = j TO m tmp(i - j + 1) = fjac(n * (i - 1) + j) tmp1(i - j + 1) = wa4(i) NEXT i Sum = DotProduct(m - j + 1, tmp(), tmp1()) temp = -Sum / fjac(n * (j - 1) + j) FOR i = j TO m wa4(i) = wa4(i) + fjac(n * (i - 1) + j) * temp NEXT i 120 fjac(n * (j - 1) + j) = wa1(j) qtf(j) = wa4(j) NEXT j ' compute the norm of the scaled gradient gnorm = zero IF fxnorm = zero THEN GOTO 170 FOR j = 1 TO n l = ipvt(j) IF wa2(l) = zero THEN EXIT SUB Sum = zero FOR i = 1 TO j Sum = Sum + fjac(n * (i - 1) + j) * (qtf(i) / fxnorm) NEXT i gnorm = XMAX(gnorm, ABS(Sum / wa2(l))) NEXT j ' test for convergence of the gradient norm 170 IF gnorm <= gtol THEN info = 4 IF info <> 0 THEN GOTO 300 ' rescale if necessary IF mode = 2 THEN GOTO 200 FOR j = 1 TO n diag(j) = XMAX(diag(j), wa2(j)) NEXT j ' beginning of the inner loop. ' determine the levenberg-marquardt parameter 200 lmpar n, fjac(), ipvt(), diag(), qtf(), delta, par, wa1(), wa2() ' store the direction p and x + p. calculate the norm of p FOR j = 1 TO n wa1(j) = -wa1(j) wa2(j) = x(j) + wa1(j) wa3(j) = diag(j) * wa1(j) NEXT j pnorm = enorm(n, wa3()) ' on the first iteration, adjust the initial step bound IF iter = 1 THEN delta = XMIN(delta, pnorm) ' evaluate the function at x + p and calculate its norm iflag = 1 fcn m, n, wa2(), wa4(), iflag nfev = nfev + 1 IF iflag < 0 THEN GOTO 300 xnorm1 = enorm(m, wa4()) ' compute the scaled actual reduction actred = -one IF p1 * xnorm1 < fxnorm THEN actred = one - (xnorm1 / fxnorm) ^ 2 ' compute the scaled predicted reduction and ' the scaled directional derivative FOR j = 1 TO n wa3(j) = zero l = ipvt(j) temp = wa1(l) FOR i = 1 TO j wa3(i) = wa3(i) + fjac(n * (i - 1) + j) * temp NEXT i NEXT j temp1 = enorm(n, wa3()) / fxnorm temp2 = (SQR(par) * pnorm) / fxnorm 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 THEN ratio = actred / prered ' update the step bound IF ratio <= p25 THEN IF actred >= zero THEN temp = p5 IF actred < zero THEN temp = p5 * dirder / (dirder + p5 * actred) IF (p1 * xnorm1 >= fxnorm) OR (temp < p1) THEN temp = p1 delta = temp * XMIN(delta, pnorm / p1) par = par / temp ELSE IF (par <> zero) AND (ratio < p75) THEN GOTO 260 delta = pnorm / p5 par = p5 * par END IF ' test for successful iteration 260 IF ratio < p0001 THEN GOTO 290 ' successful iteration. update x, fvec, and their norms FOR j = 1 TO n x(j) = wa2(j) wa2(j) = diag(j) * x(j) NEXT j FOR i = 1 TO m fvec(i) = wa4(i) NEXT i xnorm = enorm(n, wa2()) fxnorm = xnorm1 iter = iter + 1 ' tests for convergence 290 IF (ABS(actred) <= ftol) AND (prered <= ftol) AND (p5 * ratio <= one) THEN info = 1 IF delta <= xtol * xnorm THEN info = 2 IF (ABS(actred) <= ftol) AND (prered <= ftol) AND (p5 * ratio <= one) AND (info = 2) THEN info = 3 IF info <> 0 THEN GOTO 300 ' tests for termination and stringent tolerances IF nfev >= maxfev THEN info = 5 IF (ABS(actred) <= epsmch) AND (prered <= epsmch) AND (p5 * ratio <= one) THEN info = 6 IF delta <= epsmch * xnorm THEN info = 7 IF gnorm <= epsmch THEN info = 8 IF info <> 0 THEN GOTO 300 ' end of the inner loop. repeat if iteration unsuccessful IF ratio < p0001 THEN GOTO 200 ' end of the outer loop GOTO 30 ' termination, either normal or user imposed 300 IF iflag < 0 THEN info = iflag iflag = 0 IF nprint > 0 THEN fcn m, n, x(), fvec(), iflag END SUB SUB lmder1 (m, n, x(), fvec(), fjac(), tol, info, ipvt()) ' *************************************************************** ' Subroutine lmder1 ' The purpose of lmder1 is to minimize the sum of the squares of ' m nonlinear functions in n variables by a modification of the ' levenberg-marquardt algorithm. This is done by using the more ' general least-squares solver lmder. The user must provide a ' subroutine which calculates the functions and the jacobian. ' the Subroutine statement is ' Subroutine lmder1(m, n, x, fvec, fjac, tol, info, ipvt) ' where ' fcn is the name of the user-supplied Subroutine which calculates ' the functions (removed from list of arguments). ' fcn should be written as follows: ' Subroutine fcn (m, n:integer; x, fvec:pVec; var iflag:integer); ' begin ' ---------- ' calculate the functions at x and return this vector in fvec. ' ---------- ' end; ' the value of iflag should not be changed by fcn unless ' the user wants to terminate execution of lmdif1. ' 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. ' 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. ' tol is a nonnegative input variable. termination occurs ' when the algorithm estimates either that the relative ' error in the sum of squares is at most tol or that ' the relative error between x and the solution is at most tol. ' 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 algorithm estimates that the relative error ' in the sum of squares is at most tol. ' info = 2 algorithm estimates that the relative error ' between x and the solution is at most tol. ' info = 3 conditions for info = 1 and info = 2 both hold. ' info = 4 fvec is orthogonal to the columns of the ' jacobian to machine precision. ' info = 5 number of calls to fcn with iflag = 1 has reached 100*(n+1). ' info = 6 tol is too small. No further reduction in ' the sum of squares is possible. ' info = 7 tol is too small. No further improvement in ' the approximate solution x is possible. ' 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. ' wa is a work array of length lwa. ' lwa is a positive integer input variable not less than 5*n+m. ' subprograms called ' user-supplied ...... fcn ' lmder ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' **************************************************************************** CONST factor = 100# info = 0 ' check the input parameters for errors IF (n <= 0) OR (m < n) OR (tol < zero) THEN EXIT SUB ' call lmder maxfev = 100 * (n + 1) ftol = tol xtol = tol gtol = zero mode = 1 nprint = 0 lmder m, n, x(), fvec(), fjac(), ftol, xtol, gtol, maxfev, mode, factor, nprint, info, nfev, njev, ipvt() IF info = 8 THEN info = 4 END SUB SUB lmdif (m, n, x(), fvec(), ftol, xtol, gtol, maxfev, epsfcn, mode, factor, nprint, info, nfev, fjac(), ipvt()) ' *************************************************************************** ' 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(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 (removed from list of arguments). ' fcn should be written as follows: ' Subroutine fcn (m, n:integer; x, fvec:pVec; var iflag:integer); ' begin ' ---------- ' calculate the functions at x and return this vector in fvec. ' ---------- ' end; ' the value of iflag should not be changed by fcn unless ' the user wants to terminate execution of lmdif1. ' 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 <= 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 ' dpmpar, enorm, fdjac2, lmpar, xmax, xmin, qrfac ' Basic-supplied ... abs, sqr, mod ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' ************************************************************************* } 'Labels: 21,31,41,61,81,121,171,201,261,291,301 DIM diag(ISIZE), qtf(ISIZE), tmp(ISIZE), tmp1(ISIZE), wa1(ISIZE), wa2(ISIZE), wa3(ISIZE), wa4(ISIZE) CONST p1 = .1, p75 = .75, p0001 = .0001 info = 0 iflag = 0 nfev = 0 ' check the input parameters for errors IF n <= 0 OR m < n OR ftol < zero OR xtol < zero OR gtol < zero OR maxfev <= 0 OR factor <= zero THEN GOTO 301 IF mode <> 2 THEN GOTO 21 FOR j = 1 TO n IF diag(j) <= zero THEN GOTO 301 NEXT j ' evaluate the function at the starting point and calculate its norm 21 iflag = 1 fcn m, n, x(), fvec(), iflag nfev = 1 IF iflag < 0 THEN GOTO 301 fxnorm = enorm(m, fvec()) ' initialize levenberg-marquardt parameter and iteration counter par = zero iter = 1 ' beginning of the outer loop. ' calculate the jacobian matrix 31 iflag = 2 fdjac2 m, n, x(), fvec(), fjac(), iflag, epsfcn nfev = nfev + n IF iflag < 0 THEN GOTO 301 ' If requested, call fcn to enable printing of iterates IF nprint <= 0 THEN GOTO 41 iflag = 0 IF iter - 1 MOD nprint = 0 THEN fcn m, n, x(), fvec(), iflag IF iflag < 0 THEN GOTO 301 ' Compute the qr factorization of the jacobian 41 qrfac m, n, fjac(), 1, ipvt(), wa1(), wa2() ' On the first iteration and if mode is 1, scale according ' to the norms of the columns of the initial jacobian IF iter <> 1 THEN GOTO 81 IF mode = 2 THEN GOTO 61 FOR j = 1 TO n diag(j) = wa2(j) IF wa2(j) = zero THEN diag(j) = one NEXT j ' On the first iteration, calculate the norm of the scaled x ' and initialize the step bound delta 61 FOR j = 1 TO n wa3(j) = diag(j) * x(j) NEXT j xnorm = enorm(n, wa3()) delta = factor * xnorm IF delta = zero THEN delta = factor ' Form (q transpose)*fvec and store the first n components in qtf 81 FOR j = 1 TO m wa4(j) = fvec(j) NEXT j FOR j = 1 TO n IF fjac(n * (j - 1) + j) = zero THEN GOTO 121 FOR i = j TO m tmp(i - j + 1) = fjac(n * (i - 1) + j) tmp1(i - j + 1) = wa4(i) NEXT i Sum = DotProduct(m - j + 1, tmp(), tmp1()) temp = -Sum / fjac(n * (j - 1) + j) FOR i = j TO m wa4(i) = wa4(i) + fjac(n * (i - 1) + j) * temp NEXT i 121 fjac(n * (j - 1) + j) = wa1(j) qtf(j) = wa4(j) NEXT j ' compute the norm of the scaled gradient gnorm = zero IF fxnorm = zero THEN GOTO 171 FOR j = 1 TO n l = ipvt(j) IF wa2(l) = zero THEN EXIT SUB Sum = zero FOR i = 1 TO j Sum = Sum + fjac(n * (i - 1) + j) * (qtf(i) / fxnorm) NEXT i gnorm = XMAX(gnorm, ABS(Sum / wa2(l))) NEXT j ' test for convergence of the gradient norm 171 IF gnorm <= gtol THEN info = 4 IF info <> 0 THEN GOTO 301 ' rescale if necessary IF mode = 2 THEN GOTO 201 FOR j = 1 TO n diag(j) = XMAX(diag(j), wa2(j)) NEXT j ' beginning of the inner loop. ' determine the Levenberg-Marquardt parameter 201 lmpar n, fjac(), ipvt(), diag(), qtf(), delta, par, wa1(), wa2() ' store the direction p and x + p. calculate the norm of p FOR j = 1 TO n wa1(j) = -wa1(j) wa2(j) = x(j) + wa1(j) wa3(j) = diag(j) * wa1(j) NEXT j pnorm = enorm(n, wa3()) ' on the first iteration, adjust the initial step bound IF iter = 1 THEN delta = XMIN(delta, pnorm) ' evaluate the function at x + p and calculate its norm iflag = 1 fcn m, n, wa2(), wa4(), iflag nfev = nfev + 1 IF iflag < 0 THEN GOTO 301 xnorm1 = enorm(m, wa4()) ' compute the scaled actual reduction actred = -one IF p1 * xnorm1 < fxnorm THEN actred = one - SQR(xnorm1 / fxnorm) ' Compute the scaled predicted reduction and ' the scaled directional derivative FOR j = 1 TO n wa3(j) = zero l = ipvt(j) temp = wa1(l) FOR i = 1 TO j wa3(i) = wa3(i) + fjac(n * (i - 1) + j) * temp NEXT i NEXT j temp1 = enorm(n, wa3()) / fxnorm temp2 = (SQR(par) * pnorm) / fxnorm 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 THEN ratio = actred / prered ' update the step bound IF ratio <= p25 THEN IF actred >= zero THEN temp = p5 IF actred < zero THEN temp = p5 * dirder / (dirder + p5 * actred) IF (p1 * xnorm1 >= fxnorm) OR (temp < p1) THEN temp = p1 delta = temp * XMIN(delta, pnorm / p1) par = par / temp ELSE IF (par <> zero) AND (ratio < p75) THEN GOTO 261 delta = pnorm / p5 par = p5 * par END IF ' test for successful iteration 261 IF ratio < p0001 THEN GOTO 291 ' successful iteration. update x, fvec, and their norms FOR j = 1 TO n x(j) = wa2(j) wa2(j) = diag(j) * x(j) NEXT j FOR j = 1 TO m fvec(j) = wa4(j) NEXT j xnorm = enorm(n, wa2()) fxnorm = xnorm1 iter = iter + 1 ' tests for convergence 291 IF (ABS(actred) <= ftol) AND (prered <= ftol) AND (p5 * ratio <= one) THEN info = 1 IF delta <= xtol * xnorm THEN info = 2 IF (ABS(actred) <= ftol) AND (prered <= ftol) AND (p5 * ratio <= one) AND (info = 2) THEN info = 3 IF info <> 0 THEN GOTO 301 ' tests for termination and stringent tolerances IF nfev >= maxfev THEN info = 5 IF (ABS(actred) <= epsmch) AND (prered <= epsmch) AND (p5 * ratio <= one) THEN info = 6 IF delta <= epsmch * xnorm THEN info = 7 IF gnorm <= epsmch THEN info = 8 IF info <> 0 THEN GOTO 301 ' end of the inner loop. repeat if iteration unsuccessful IF ratio < p0001 THEN GOTO 201 ' end of the outer loop GOTO 31 ' termination, either normal or user imposed 301 IF iflag < 0 THEN info = iflag iflag = 0 IF nprint > 0 THEN fcn m, n, x(), fvec(), iflag END SUB SUB lmdif1 (m, n, x(), fvec(), tol, info, iwa()) ' From F90 Code converted using TO_F90 by Alan Miller ' ************************************************************************** ' Subroutine lmdif1 ' The purpose of lmdif1 is to minimize the sum of the squares of m nonlinear ' functions in n variables by a modification of the Levenberg-Marquardt ' algorithm. This is done by using the more general least-squares ' solver lmdif. 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 lmdif1(m, n, x, fvec, tol, info, iwa) ' where ' fcn is the name of the user-supplied Subroutine which calculates ' the functions (removed from list of arguments). ' fcn should be written as follows: ' Subroutine fcn (m, n:integer; x, fvec:pVec; var iflag:integer); ' begin ' ---------- ' calculate the functions at x and return this vector in fvec. ' ---------- ' end; ' the value of iflag should not be changed by fcn unless ' the user wants to terminate execution of lmdif1. ' 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. ' tol is a nonnegative input variable. Termination occurs when the ' algorithm estimates either that the relative error in the sum of ' squares is at most tol or that the relative error between x and the ' solution is at most tol. ' 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 algorithm estimates that the relative error ' in the sum of squares is at most tol. ' info = 2 algorithm estimates that the relative error ' between x and the solution is at most tol. ' info = 3 conditions for info = 1 and info = 2 both hold. ' info = 4 fvec is orthogonal to the columns of the ' jacobian to machine precision. ' info = 5 number of calls to fcn has reached or exceeded 200*(n+1). ' info = 6 tol is too small. no further reduction in ' the sum of squares is possible. ' info = 7 tol is too small. No further improvement in ' the approximate solution x is possible. ' iwa is an integer work array of length n. ' wa is a work array of length lwa. ' lwa is a positive integer input variable not less than m*n+5*n+m. ' subprograms called ' user-supplied ...... fcn ' lmdif ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' ************************************************************************* } DIM fjac(ISIZE * ISIZE) 'NOTE: Jacobian matrix fjac is stored line by line CONST factor = 100# ' in a vector of minimum size n*n. Element i,j ' is the vector element n*(i-1)+j info = 0 ' check the input parameters for errors IF n <= 0 OR m < n OR tol < zero THEN EXIT SUB maxfev = 200 * (n + 1) ftol = tol xtol = tol gtol = zero epsfcn = zero mode = 1 nprint = 0 lmdif m, n, x(), fvec(), ftol, xtol, gtol, maxfev, epsfcn, mode, factor, nprint, info, nfev, fjac(), iwa() IF info = 8 THEN info = 4 END SUB SUB lmpar (n, r(), ipvt(), diag(), qtb(), delta, par, x(), sdiag()) ' ************************************************************************* ' Subroutine lmpar ' 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 zero and ' (dxnorm-delta) <= 0.1*delta , ' or par is positive and ' abs(dxnorm-delta) <= 0.1*delta . ' This Subroutine completes the solution of the problem if it is provided ' with the necessary information from the r factorization, with column ' qpivoting, 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. ' the Subroutine statement is ' Subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, wa1,wa2) ' where ' 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. ' subprograms called ' dpmpar, enorm, xmax, xmin, qrsolv ' Basic-supplied ... ABS, SQR ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' ************************************************************* } 'Labels: 122, 150, 220 DIM tmp(ISIZE), wa1(ISIZE), wa2(ISIZE) CONST p1 = .1, p001 = .001 ' dwarf is the smallest positive magnitude dwarf = 2.5E-16 ' compute and store in x the gauss-newton direction. if the ' jacobian is rank-deficient, obtain a least squares solution nsing = n FOR j = 1 TO n wa1(j) = qtb(j) IF (r(n * (j - 1) + j) = zero) AND (nsing = n) THEN nsing = j - 1 IF nsing < n THEN wa1(j) = zero NEXT j FOR k = 1 TO nsing j = nsing - k + 1 wa1(j) = wa1(j) / r(n * (j - 1) + j) temp = wa1(j) jm1 = j - 1 FOR i = 1 TO jm1 wa1(i) = wa1(i) - r(n * (i - 1) + j) * temp NEXT i NEXT k FOR j = 1 TO n l = ipvt(j) x(l) = wa1(j) NEXT j ' initialize the iteration counter. ' evaluate the function at the origin, and test ' for accepXtance of the gauss-newton direction iter = 0 FOR i = 1 TO n wa2(i) = diag(i) * x(i) NEXT i dxnorm = enorm(n, wa2()) fp = dxnorm - delta IF fp <= p1 * delta THEN GOTO 220 ' 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 = zero IF nsing < n THEN GOTO 122 FOR j = 1 TO n l = ipvt(j) wa1(j) = diag(l) * (wa2(l) / dxnorm) NEXT j FOR j = 1 TO n FOR i = 1 TO j - 1 tmp(i) = r(n * (i - 1) + j) NEXT i Sum = DotProduct(j - 1, tmp(), wa1()) wa1(j) = (wa1(j) - Sum) / r(n * (j - 1) + j) NEXT j temp = enorm(n, wa1()) parl = ((fp / delta) / temp) / temp ' calculate an upper bound, paru, for the zero of the function 122 FOR j = 1 TO n FOR i = 1 TO j tmp(i) = r(n * (i - 1) + j) NEXT i Sum = DotProduct(j, tmp(), qtb()) l = ipvt(j) wa1(j) = Sum / diag(l) NEXT j gnorm = enorm(n, wa1()) paru = gnorm / delta IF paru = zero THEN paru = dwarf / XMIN(delta, p1) ' if the input par lies outside of the interval (parl,paru), ' set par to the closer endpoint par = XMAX(par, parl) par = XMIN(par, paru) IF par = zero THEN par = gnorm / dxnorm ' beginning of an iteration 150 iter = iter + 1 ' evaluate the function at the current value of par IF par = zero THEN par = XMAX(dwarf, p001 * paru) temp = SQR(par) FOR i = 1 TO n wa1(i) = temp * diag(i) NEXT i qrsolv n, r(), ipvt(), wa1(), qtb(), x(), sdiag() FOR i = 1 TO n wa2(i) = diag(i) * x(i) NEXT i dxnorm = enorm(n, wa2()) temp = 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 ((ABS(fp) <= p1 * delta) OR (parl = zero)) AND ((fp <= temp) AND (temp < zero) OR (iter = 10)) THEN GOTO 220 ' compute the newton correction FOR j = 1 TO n l = ipvt(j) wa1(j) = diag(l) * (wa2(l) / dxnorm) NEXT j FOR j = 1 TO n wa1(j) = wa1(j) / sdiag(j) temp = wa1(j) jp1 = j + 1 FOR i = jp1 TO n wa1(i) = wa1(i) - r(n * (i - 1) + j) * temp NEXT i NEXT j temp = enorm(n, wa1()) parc = ((fp / delta) / temp) / temp ' depending on the sign of the function, update parl or paru IF fp > zero THEN parl = XMAX(parl, par) IF fp < zero THEN paru = XMIN(paru, par) ' compute an improved estimate for par par = XMAX(parl, par + parc) ' end of an iteration GOTO 150 220 IF iter = 0 THEN par = zero END SUB SUB qrfac (m, n, a(), pivot AS INTEGER, ipvt(), rdiag(), acnorm()) ' ************************************************************************* ' Subroutine qrfac ' 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 zeros in the first k-1 positions. The form of this ' transformation and the method of pivoting first appeared in the ' corresponding linpack Subroutine. ' the Subroutine statement is ' Subroutine qrfac(m, n, a, lda, pivot, ipvt, lipvt, rdiag, acnorm, wa) ' N.B. 3 of these arguments have been omitted in this version. ' where ' 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). ' lda is a positive integer input variable not less than m ' which specifies the leading dimension of the array a. ' 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. ' lipvt is a positive integer input variable. If pivot is false, ' then lipvt may be as small as 1. If pivot is true, then ' lipvt must be at least n. ' 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. ' Subroutines or functions called: ' dpmpar, enorm, MAX, MIN ' pascal-supplied ... SQRT ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more '***************************************************************************** 'Labels: 42, 45, 50 DIM tmp(ISIZE), tmp1(ISIZE), wa(ISIZE) ' compute the initial column norms and initialize several arrays FOR j = 1 TO n FOR i = 1 TO m tmp(i) = a(n * (i - 1) + j) NEXT i acnorm(j) = enorm(m, tmp()) rdiag(j) = acnorm(j) wa(j) = rdiag(j) IF pivot = True THEN ipvt(j) = j NEXT j ' Reduce a to r with Householder transformations IF m <= n THEN minmn = m ELSE minmn = n END IF FOR j = 1 TO minmn IF pivot = False THEN GOTO 42 ' Bring the column of largest norm into the pivot position kmax = j FOR k = j TO n IF rdiag(k) > rdiag(kmax) THEN kmax = k NEXT k IF kmax = j THEN GOTO 42 FOR i = 1 TO m temp = a(n * (i - 1) + j) a(n * (i - 1) + j) = a(n * (i - 1) + kmax) a(n * (i - 1) + kmax) = temp NEXT i rdiag(kmax) = rdiag(j) wa(kmax) = wa(j) k = ipvt(j) ipvt(j) = ipvt(kmax) ipvt(kmax) = k ' Compute the Householder transformation to reduce the ' j-th column of a to a multiple of the j-th unit vector 42 FOR i = j TO m tmp(i - j + 1) = a(n * (i - 1) + j) NEXT i ajnorm = enorm(m - j + 1, tmp()) IF ajnorm = zero THEN EXIT SUB IF a(n * (j - 1) + j) < zero THEN ajnorm = -ajnorm FOR i = j TO m a(n * (i - 1) + j) = a(n * (i - 1) + j) / ajnorm NEXT i a(n * (j - 1) + j) = a(n * (j - 1) + j) + one ' Apply the transformation to the remaining columns and update the norms jp1 = j + 1 FOR k = jp1 TO n FOR i = j TO m tmp(i - j + 1) = a(n * (i - 1) + j) tmp1(i - j + 1) = a(n * (i - 1) + k) NEXT i Sum = DotProduct(m - j + 1, tmp(), tmp1()) temp = Sum / a(n * (j - 1) + j) FOR i = j TO m a(n * (i - 1) + k) = a(n * (i - 1) + k) - temp * a(n * (i - 1) + j) NEXT i IF (pivot = False) OR (rdiag(k) = zero) THEN EXIT SUB temp = a(n * (j - 1) + k) / rdiag(k) rdiag(k) = rdiag(k) * SQR(XMAX(zero, one - temp * temp)) IF p05 * SQR(rdiag(k) / wa(k)) > epsmch THEN GOTO 45 FOR i = jp1 TO m tmp(i - j) = a(n * (i - 1) + k) NEXT i rdiag(k) = enorm(m - j, tmp()) wa(k) = rdiag(k) 45 NEXT k rdiag(j) = -ajnorm NEXT j END SUB SUB qrsolv (n, r(), ipvt(), diag(), qtb(), x(), sdiag()) '************************************************************************** ' Subroutine qrsolv ' Given an m by n matrix a, an n by n diagonal matrix d, and an m-vector b, ' the problem is to determine an x which solves the system ' a*x = b , d*x = 0 , ' in the least squares sense. ' 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 qrsolv expects the full upper ' triangle of r, the permutation matrix p, and the first n components of ' (q transpose)*b. The system a*x = b, d*x = 0, is then equivalent to ' t t ' r*z = q *b , p *d*p*z = 0 , ' where x = p*z. if this system does not have full rank, ' then a least squares solution is obtained. On output qrsolv ' also provides an upper triangular matrix s such that ' t t t ' p *(a *a + d*d)*p = s *s . ' s is computed within qrsolv and may be of separate interest. ' the Subroutine statement is ' Subroutine qrsolv(n, r, ldr, ipvt, diag, qtb, x, sdiag, wa) ' N.B. Arguments LDR and WA have been removed in this version. ' where ' 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. ' x is an output array of length n which contains the least ' squares solution of the system a*x = b, d*x = 0. ' sdiag is an output array of length n which contains the ' diagonal elements of the upper triangular matrix s. ' wa is a work array of length n. ' subprograms called ' Basic-supplied ... ABS, SQR ' argonne national laboratory. minpack project. march 1980. ' burton s. garbow, kenneth e. hillstrom, jorge j. more ' ************************************************************************* } DIM tmp(ISIZE), tmp1(ISIZE), wa(ISIZE) ' Copy r and (q transpose)*b to preserve input and initialize s. ' In particular, save the diagonal elements of r in x FOR j = 1 TO n FOR i = j TO n r(n * (i - 1) + j) = r(n * (j - 1) + i) NEXT i x(j) = r(n * (j - 1) + j) wa(j) = qtb(j) NEXT j ' Eliminate the diagonal matrix d using a givens rotation FOR j = 1 TO n ' Prepare the row of d to be eliminated, locating the ' diagonal element using p from the qr factorization l = ipvt(j) IF diag(l) = zero THEN EXIT SUB FOR i = j TO n sdiag(i) = zero NEXT i sdiag(j) = diag(l) ' The transformations to eliminate the row of d modify only a single ' element of (q transpose)*b beyond the first n, which is initially zero qtbpj = zero FOR k = j TO n ' Determine a givens rotation which eliminates the ' appropriate element in the current row of d IF sdiag(k) = zero THEN EXIT SUB IF ABS(r(n * (k - 1) + k)) < ABS(sdiag(k)) THEN cotan = r(n * (k - 1) + k) / sdiag(k) Xsin = p5 / SQR(p25 + p25 * cotan * cotan) Xcos = Xsin * cotan ELSE Xtan = sdiag(k) / r(n * (k - 1) + k) Xcos = p5 / SQR(p25 + p25 * Xtan * Xtan) Xsin = Xcos * Xtan END IF ' Compute the modified diagonal element of r and ' the modified element of ((q transpose)*b,0) r(n * (k - 1) + k) = Xcos * r(n * (k - 1) + k) + Xsin * sdiag(k) temp = Xcos * wa(k) + Xsin * qtbpj qtbpj = -Xsin * wa(k) + Xcos * qtbpj wa(k) = temp ' Accumulate the tranformation in the row of s kp1 = k + 1 FOR i = kp1 TO n temp = Xcos * r(n * (i - 1) + k) + Xsin * sdiag(i) sdiag(i) = -Xsin * r(n * (i - 1) + k) + Xcos * sdiag(i) r(n * (i - 1) + k) = temp NEXT i NEXT k ' Store the diagonal element of s and restore ' the corresponding diagonal element of r sdiag(j) = r(n * (j - 1) + j) r(n * (j - 1) + j) = x(j) NEXT j ' Solve the triangular system for z. If the system is singular, ' then obtain a least squares solution nsing = n FOR j = 1 TO n IF (sdiag(j) = zero) AND (nsing = n) THEN nsing = j - 1 IF nsing < n THEN wa(j) = zero NEXT j FOR k = 1 TO nsing j = nsing - k + 1 FOR i = j + 1 TO nsing tmp(i - j) = r(n * (i - 1) + j) tmp1(i - j) = wa(i) NEXT i Sum = DotProduct(nsing - j, tmp(), tmp1()) wa(j) = (wa(j) - Sum) / sdiag(j) NEXT k ' Permute the components of z back to components of x FOR j = 1 TO n l = ipvt(j) x(l) = wa(j) NEXT j END SUB SUB ssqfcn (m, n, x(), fvec(), nprob) ' **************************************************************** ' Subroutine SSQFCN ' THIS Subroutine DEFINES THE FUNCTIONS OF THREE NONLINEAR ' LEAST SQUARES PROBLEMS. ' THE Subroutine STATEMENT IS ' Sub SSQFCN(M, N, X, FVEC, NPROB) ' WHERE ' M AND N ARE POSITIVE INTEGER INPUT VARIABLES. N MUST NOT ' EXCEED M. ' X IS AN INPUT ARRAY OF LENGTH N (OF TYPE pVec). ' FVEC IS AN OUTPUT ARRAY OF LENGTH M WHICH CONTAINS THE NPROB ' FUNCTION EVALUATED AT X (OF TYPE pVec). ' NPROB IS A POSITIVE INTEGER INPUT VARIABLE WHICH DEFINES THE ' NUMBER OF THE PROBLEM. NPROB MUST NOT EXCEED 18. ' ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980. ' BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE ' **************************************************************** ' FUNCTION ROUTINE SELECTOR IF nprob = 1 THEN ' Example #1 (n=2) ' X1^2 + X1 + X2^2 - 2 = 0 ' X1^2 + X2 - X2^2 - 1 + log(X1) = 0 fvec(1) = x(1) * x(1) + x(1) + x(2) * x(2) - two fvec(2) = x(1) * x(1) + x(2) - x(2) * x(2) - one + LOG(x(1)) ELSEIF nprob = 2 THEN ' Example #2 (n=4) ' 10.0*x + x2 + x3 + x4 - 20.0 + Sqr(Xsin(x1)) + Sqr(Xcos(x2)) = 0 ' X1 20# * X2 + X3 + X4 - 48# + one / pow ^ 6 = 0 ' Sqr (X1 + X2) + 30# * X3 + X4 - 97# + Log(X1) + Log(X2 + X3) = 0 ' x1 + x2 + x3 + 40.0*x4 - 166.0 + Sqr(x1) = 0 fvec(1) = 10# * x(1) + x(2) + x(3) + x(4) - 20# + SIN(x(1)) ^ 2 + COS(x(2)) ^ 2 fvec(2) = x(1) + 20# * x(2) + x(3) + x(4) - 48# + one / (x(1) * x(1) * x(1) * x(1) * x(1) * x(1)) fvec(3) = (x(1) + x(2)) ^ 2 + 30# * x(3) + x(4) - 97# + LOG(x(1)) + LOG(x(2) + x(3)) fvec(4) = x(1) + x(2) + x(3) + 40# * x(4) - 166# + x(1) ^ 2 ELSEIF nprob = 3 THEN ' Example #3 (n=6) Stiff system ' Hiebert's 2nd Chemical Engineering Problem ' source: Hiebert; Sandia Technical Report #SAND80-0181 ' Sandia National Laboratories, Albuquerque, NM (1980) ' X1 X2 + X4 - 0.001 = 0 ' X5 X6 - 55 = 0 ' X1 + X2 + X3 + 2X5 + X6 - 110.001 = 0 ' X1 - 0.1X2 = 0 ' X1 - 10000 X3 X4 = 0 ' X5 - 5.5e15 X3 X6 = 0 ' solution: (8.264e-5, 8.264e-4, 9.091e-5, 9.091e-5, 55, 1.1e-10) fvec(1) = x(1) + x(2) + x(4) - .001 fvec(2) = x(5) + x(6) - 55# fvec(3) = x(1) + x(2) + x(3) + two * x(5) + x(6) - 110.001 fvec(4) = x(1) - .1 * x(2) fvec(5) = x(1) - 10000# * x(3) * x(4) fvec(6) = x(5) - 5.5E+15 * x(3) * x(6) END IF END SUB FUNCTION XMAX (a, b) IF a >= b THEN XMAX = a ELSE XMAX = b END IF END FUNCTION FUNCTION XMIN (a, b) IF a <= b THEN XMIN = a ELSE XMIN = b END IF END FUNCTION