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