Attribute VB_Name = "Module1" '*********************************************************************** '* SUBROUTINES FOR THE LEAST-SQUARES SOLUTION OF M NONLINEAR EQUATIONS * '* IN N VARIABLES USING THE Levenberg-Marquardt ALGORITHM. * '* ------------------------------------------------------------------- * '* REFERENCE * '* From F77 program By: * '* ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980. * '* BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE * '* * '* Visual Basic 4.0 Release By J-P Moreau, Paris. * '* (www.jpmoreau.fr) * '*********************************************************************** DefDbl A-H, O-Z DefInt I-N Public Const ISIZE = 25 Public Const zero = 0#, zp25 = 0.25, zp5 = 0.5, one = 1#, two = 2#, five = 5#, eight = 8# Public Const ten = 10#, c13 = 13#, c14 = 14#, c29 = 29#, c45 = 45# Public Const p5 = 0.5, p25 = 0.25, p05 = 0.05 Public Const epsmch = 2.25E-16 Public nprob, nfev, njev Function Dot_Product(n, a(), b()) Sum = zero For i = 1 To n Sum = Sum + a(i) * b(i) Next i Dot_Product = Sum End Function 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 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) - 0.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) - 0.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 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 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 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: 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 = 0.1, p75 = 0.75, p0001 = 0.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 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 fnorm = 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 fdjac2 m, n, x(), fvec(), fjac(), iflag, epsfcn nfev = nfev + n 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(), True, 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 = Dot_Product(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 fnorm = 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) / fnorm) 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 fnorm1 = enorm(m, wa4) ' compute the scaled actual reduction actred = -one If p1 * fnorm1 < fnorm Then actred = one - Sqr(fnorm1 / fnorm) ' 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) / fnorm temp2 = (Sqr(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 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 * fnorm1 >= fnorm) 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 j = 1 To m fvec(j) = wa4(j) Next j xnorm = enorm(n, wa2) fnorm = fnorm1 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 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 = 0.1, p75 = 0.75, p0001 = 0.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 fnorm = 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(), True, 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 = Dot_Product(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 fnorm = 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) / fnorm) 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 fnorm1 = enorm(m, wa4) ' compute the scaled actual reduction actred = -one If p1 * fnorm1 < fnorm Then actred = one - (fnorm1 / fnorm) ^ 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) / fnorm temp2 = (Sqr(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 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 * fnorm1 >= fnorm) 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) fnorm = fnorm1 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 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: 120, 150, 220 Dim tmp(ISIZE), wa1(ISIZE), wa2(ISIZE) Const p1 = 0.1, p001 = 0.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 120 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 = Dot_Product(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 120 For j = 1 To n For i = 1 To j tmp(i) = r(n * (i - 1) + j) Next i Sum = Dot_Product(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 Boolean, 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: 40, 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 40 ' 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 40 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 40 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 = Dot_Product(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 = Dot_Product(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 Function enorm(n, x()) 'calculate norm of vector x of size n temp = zero For i = 1 To n temp = temp + x(i) ^ 2 Next i enorm = Sqr(temp) End Function 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