{*********************************************************************
* PROCEDURES 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 *
* *
* Pascal Release By J-P Moreau, Paris. *
* (www.jpmoreau.fr) *
*********************************************************************}
UNIT LM;
INTERFACE
Const SIZE = 25;
zero = 0.0; zp25 = 0.25; zp5 = 0.5; one = 1.0; two = 2.0; five = 5.0; eight = 8.0;
ten = 10.0; c13 = 13.0; c14 = 14.0; c29 = 29.0; c45 = 45.0;
p5 = 0.5; p25 = 0.25; p05 = 0.05;
epsmch = 2.25E-16;
Type
pMat = ^MAT;
MAT = Array[1..SIZE,1..SIZE] of double;
pVec = ^VEC;
VEC = Array[1..SIZE] of double;
pIVec = ^IVEC;
IVEC = Array[1..SIZE] of integer;
Var
nprob, nfev, njev: Integer;
Procedure ssqfcn (m, n:integer; x, fvec:pVec; nprob:integer);
Function enorm(n:integer; x:pVec): double;
Procedure lmdif1(m, n: integer; x, fvec: pVec; tol: double; Var info:integer; iwa: pIVec);
IMPLEMENTATION
Procedure fdjac2(m, n:integer; x, fvec:pVec; fjac:pMat; var iflag:integer;
epsfcn: double); Forward;
Procedure qrfac(m, n:integer; a:pMat; pivot:Boolean; ipvt:pIVec; rdiag, acnorm:pVec);
Forward;
Procedure lmpar(n:integer; r:pMat; ipvt:pIVec; diag, qtb:pVec; delta:double;
var par:double; x, sdiag: pVec); Forward;
Procedure lmder(m, n:integer; x, fvec:pVec; fjac:pMat; ftol, xtol, gtol:double;
var maxfev:integer; mode, factor, nprint:integer; var info, nfev,
njev:integer; ipvt:pIVec); Forward;
Procedure qrsolv(n:integer; r:pMat; ipvt:pIVec; diag, qtb, x, sdiag:pVec); Forward;
{Utility functions}
Function Dot_Product(n:integer; a,b:pVec):double;
Var i:integer; sum:double;
begin
sum:=zero;
For i:=1 to n do sum:=sum+a^[i]*b^[i];
Dot_Product:=sum
end;
Function MAX(a,b:double): double;
begin
if a>=b then MAX:=a else MAX:=b
end;
Function MIN(a,b:double): double;
begin
if a<=b then MIN:=a else MIN:=b
end;
Procedure ssqfcn (m, n:integer; x, fvec:pVec; nprob:integer);
{ ****************************************************************
! Procedure SSQFCN
! THIS PROCEDURE DEFINES THE FUNCTIONS OF THREE NONLINEAR
! LEAST SQUARES PROBLEMS.
! THE PROCEDURE STATEMENT IS
! PROCEDURE 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
! *************************************************************** }
Var
i, iev, j, nm1: integer;
Begin
{ FUNCTION ROUTINE SELECTOR }
Case nprob of
1:begin { 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 + ln(x^[1])
end;
2:begin { Example #2 (n=4) }
{ 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 }
fvec^[1] := 10.0*x^[1] + x^[2] + x^[3] + x^[4] - 20.0 + Sqr(sin(x^[1])) + Sqr(cos(x^[2]));
fvec^[2] := x^[1] + 20.0*x^[2] + x^[3] + x^[4] - 48.0 + one/(x^[1]*x^[1]*x^[1]*x^[1]*x^[1]*x^[1]);
fvec^[3] := Sqr(x^[1] + x^[2]) + 30.0*x^[3] + x^[4] - 97.0 + ln(x^[1]) + ln(x^[2]+x^[3]);
fvec^[4] := x^[1] + x^[2] + x^[3] + 40.0*x^[4] - 166.0 + Sqr(x^[1])
end;
3:begin { 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 - .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.0;
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] - 1.0E04 * x^[3] * x^[4];
fvec^[6] := x^[5] - 5.5E15 * x^[3] * x^[6]
end
end
End; {ssqfcn}
Procedure fcn (m, n:integer; x, fvec:pVec; var iflag:integer);
{ ************************************************************
! 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
! ************************************************************ }
Begin
ssqfcn (m, n, x, fvec, nprob);
IF iflag = 1 then nfev := nfev + 1;
IF iflag = 2 then njev := njev + 1
End; {fcn}
Procedure lmdif(m, n:integer; x, fvec:pVec; ftol, xtol:double; gtol:double;
var maxfev:integer; epsfcn:double; mode:integer; factor:double;
nprint:integer; var info, nfev:integer; fjac:pMat; ipvt:pIVec);
Forward;
Procedure lmdif1(m, n: integer; x, fvec: pVec; tol: double; Var info:integer; iwa: pIVec);
{ From F90 Code converted using TO_F90 by Alan Miller
! **************************************************************************
! procedure 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
! procedure lmdif1(m, n, x, fvec, tol, info, iwa)
! where
! fcn is the name of the user-supplied procedure which calculates
! the functions (removed from list of arguments).
! fcn should be written as follows:
! Procedure 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
! minpack-supplied ... lmdif
! argonne national laboratory. minpack project. march 1980.
! burton s. garbow, kenneth e. hillstrom, jorge j. more
! ************************************************************************* }
Label Return;
Var
maxfev, mode, nprint: Integer;
epsfcn, ftol, gtol, xtol: double;
fjac: pMat;
Const
factor = 100.0;
Begin
info := 0;
New(fjac);
{ check the input parameters for errors }
IF (n <= 0) OR (m < n) OR (tol < zero) Then goto Return;
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;
Return: Dispose(fjac);
End; {lmdif1}
Procedure lmdif(m, n:integer; x, fvec:pVec; ftol, xtol:double; gtol:double;
var maxfev:integer; epsfcn:double; mode:integer; factor:double;
nprint:integer; var info, nfev:integer; fjac:pMat; ipvt:pIVec);
{ ***************************************************************************
! procedure 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 procedure statement is
! procedure 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 procedure which calculates
! the functions (removed from list of arguments).
! fcn should be written as follows:
! Procedure 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, max, min, qrfac
! pascal-supplied ... abs, sqrt, mod
! argonne national laboratory. minpack project. march 1980.
! burton s. garbow, kenneth e. hillstrom, jorge j. more
! ************************************************************************* }
Label 20,30,40,60,80,120,170,200,260,290,300;
Var
i, iflag, iter, j, l: Integer;
actred, delta, dirder, fnorm, fnorm1, gnorm,
par, pnorm, prered, ratio, sum, temp, temp1, temp2, xnorm: double;
diag, qtf, tmp, tmp1, wa1, wa2, wa3, wa4: pVec;
Const
p1 = 0.1; p75 = 0.75; p0001 = 0.0001;
Begin
New(diag); New(qtf); New(tmp); New(tmp1); New(wa1); New(wa2);
New(wa3); New(wa4);
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 do
IF diag^[j] <= zero then GOTO 300;
{ 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 do
begin
diag^[j] := wa2^[j];
IF wa2^[j] = zero then diag^[j] := one
end;
{ On the first iteration, calculate the norm of the scaled x
and initialize the step bound delta }
60: For j := 1 to n do wa3^[j] := diag^[j]*x^[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 do wa4^[j] := fvec^[j];
For j := 1 to n do
begin
IF fjac^[j,j] = zero then GOTO 120;
For i:=j to m do
begin
tmp^[i-j+1]:=fjac^[i,j];
tmp1^[i-j+1]:=wa4^[i]
end;
sum := DOT_PRODUCT(m-j+1, tmp, tmp1);
temp := -sum/fjac^[j,j];
For i := j to m do
wa4^[i] := wa4^[i] + fjac^[i,j]*temp;
120:fjac^[j,j] := wa1^[j];
qtf^[j] := wa4^[j]
end;
{ compute the norm of the scaled gradient }
gnorm := zero;
IF fnorm = zero then GOTO 170;
For j := 1 to n do
begin
l := ipvt^[j];
IF wa2^[l] = zero then exit;
sum := zero;
For i := 1 to j do
sum := sum + fjac^[i,j]*(qtf^[i]/fnorm);
gnorm := MAX(gnorm, ABS(sum/wa2^[l]))
end;
{ 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 do
diag^[j] := MAX(diag^[j], wa2^[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 do
begin
wa1^[j] := -wa1^[j];
wa2^[j] := x^[j] + wa1^[j];
wa3^[j] := diag^[j]*wa1^[j]
end;
pnorm := enorm(n, wa3);
{ on the first iteration, adjust the initial step bound }
IF iter = 1 then delta := MIN(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 do
begin
wa3^[j] := zero;
l := ipvt^[j];
temp := wa1^[l];
For i := 1 to j do
wa3^[i] := wa3^[i] + fjac^[i,j]*temp
end;
temp1 := enorm(n,wa3)/fnorm;
temp2 := (SQRT(par)*pnorm)/fnorm;
prered := temp1*temp1 + temp2*temp2/p5;
dirder := -(temp1*temp1 + temp2*temp2);
{ compute the ratio of the actual to the predicted reduction }
ratio := zero;
IF prered <> zero then ratio := actred/prered;
{ update the step bound }
IF ratio <= p25 THEN
begin
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*MIN(delta,pnorm/p1);
par := par/temp
end
ELSE
begin
IF (par <> zero) AND (ratio < p75) then GOTO 260;
delta := pnorm/p5;
par := p5*par
end;
{ test for successful iteration }
260: IF ratio < p0001 then GOTO 290;
{ successful iteration. update x, fvec, and their norms }
For j := 1 to n do
begin
x^[j] := wa2^[j];
wa2^[j] := diag^[j]*x^[j]
end;
For j:=1 to m do fvec^[j] := wa4^[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);
Dispose(diag); Dispose(qtf); Dispose(tmp); Dispose(tmp1); Dispose(wa1);
Dispose(wa2); Dispose(wa3); Dispose(wa4)
End; {lmdif}
Procedure lmder1(m, n:integer; x, fvec:pVec; fjac:pMat; tol:double;
var info:integer; ipvt:pIVec);
{ ***************************************************************
! procedure 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 procedure statement is
! procedure lmder1(m, n, x, fvec, fjac, tol, info, ipvt)
! where
! fcn is the name of the user-supplied procedure which calculates
! the functions (removed from list of arguments).
! fcn should be written as follows:
! Procedure 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
! minpack-supplied ... lmder
! argonne national laboratory. minpack project. march 1980.
! burton s. garbow, kenneth e. hillstrom, jorge j. more
! ************************************************************************** }
Label 10;
Var
maxfev, mode, njev, nprint: Integer;
ftol, gtol, xtol: double;
Const
factor = 100;
Begin
info := 0;
{ check the input parameters for errors }
IF (n <= 0) OR (m < n) OR (tol < zero) then GOTO 10;
{ 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;
10: End; {lmder1}
Procedure lmder(m, n:integer; x, fvec:pVec; fjac:pMat; ftol, xtol, gtol:double;
var maxfev:integer; mode, factor, nprint:integer; var info, nfev,
njev:integer; ipvt:pIVec);
{ ****************************************************************************
! procedure 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
! procedure which calculates the functions and the jacobian.
! the procedure statement is
! procedure 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 procedure which calculates
! the functions (removed from list of arguments).
! fcn should be written as follows:
! Procedure 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,max,min
! pascal-supplied ... ABS, SQRT, mod
! argonne national laboratory. minpack project. march 1980.
! burton s. garbow, kenneth e. hillstrom, jorge j. more
! ***************************************************************************** }
Label 20,30,40,60,80,120,170,200,260,290,300;
Var
i, iflag, iter, j, l: Integer;
actred, delta, dirder, fnorm, fnorm1, gnorm, par, pnorm, prered,
ratio, sum, temp, temp1, temp2, xnorm: double;
diag, qtf, tmp, tmp1, wa1, wa2, wa3, wa4: pVec;
Const
p1 = 0.1; p75 = 0.75; p0001 = 0.0001;
Begin
New(diag); New(qtf); New(tmp); New(tmp1); New(wa1); New(wa2); New(wa3); New(wa4);
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 do
IF diag^[j] <= zero then GOTO 300;
{ 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 do
begin
diag^[j] := wa2^[j];
IF wa2^[j] = zero then diag^[j] := one
end;
{ on the first iteration, calculate the norm of the scaled x
and initialize the step bound delta }
60:For j:=1 to n do wa3^[j] := diag^[j]*x^[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 do wa4^[j] := fvec^[j];
For j := 1 to n do
begin
IF fjac^[j,j] = zero then goto 120;
For i:=j to m do
begin
tmp^[i-j+1]:=fjac^[i,j];
tmp1^[i-j+1]:=wa4^[i]
end;
sum := DOT_PRODUCT(m-j+1,tmp,tmp1);
temp := -sum/fjac^[j,j];
For i := j to m do wa4^[i] := wa4^[i] + fjac^[i,j]*temp;
120: fjac^[j,j] := wa1^[j];
qtf^[j] := wa4^[j]
end;
{ compute the norm of the scaled gradient }
gnorm := zero;
IF fnorm = zero then GOTO 170;
For j := 1 to n do
begin
l := ipvt^[j];
IF wa2^[l] = zero then exit;
sum := zero;
For i := 1 to j do sum := sum + fjac^[i,j]*(qtf^[i]/fnorm);
gnorm := MAX(gnorm,ABS(sum/wa2^[l]))
end;
{ 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 do diag^[j] := MAX(diag^[j], wa2^[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 do
begin
wa1^[j] := -wa1^[j];
wa2^[j] := x^[j] + wa1^[j];
wa3^[j] := diag^[j]*wa1^[j]
end;
pnorm := enorm(n, wa3);
{ on the first iteration, adjust the initial step bound }
IF iter = 1 then delta := MIN(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 do
begin
wa3^[j] := zero;
l := ipvt^[j];
temp := wa1^[l];
For i:=1 to j do wa3^[i] := wa3^[i] + fjac^[i,j]*temp
end;
temp1 := enorm(n,wa3)/fnorm;
temp2 := (SQRT(par)*pnorm)/fnorm;
prered := temp1*temp1 + temp2*temp2/p5;
dirder := -(temp1*temp1 + temp2*temp2);
{ compute the ratio of the actual to the predicted reduction }
ratio := zero;
IF prered <> zero then ratio := actred/prered;
{ update the step bound }
IF ratio <= p25 THEN
begin
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*MIN(delta, pnorm/p1);
par := par/temp
end
ELSE
begin
IF (par <> zero) AND (ratio < p75) then GOTO 260;
delta := pnorm/p5;
par := p5*par
end;
{ test for successful iteration }
260:IF ratio < p0001 then GOTO 290;
{ successful iteration. update x, fvec, and their norms }
For j := 1 to n do
begin
x^[j] := wa2^[j];
wa2^[j] := diag^[j]*x^[j]
end;
For i:=1 to m do fvec^[i] := wa4^[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);
Dispose(diag); Dispose(qtf); Dispose(tmp); Dispose(tmp1); Dispose(wa1);
Dispose(wa2); Dispose(wa3); Dispose(wa4)
End; {lmder}
Procedure lmpar(n:integer; r:pMat; ipvt:pIVec; diag, qtb:pVec; delta:double;
var par:double; x, sdiag: pVec);
{ *************************************************************************
! procedure 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 procedure 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 procedure statement is
! procedure 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, max, min, qrsolv
! pascal-supplied ... ABS, SQRT
! argonne national laboratory. minpack project. march 1980.
! burton s. garbow, kenneth e. hillstrom, jorge j. more
! ************************************************************* }
Label 120,150,220;
Var
i, iter, j, jm1, jp1, k, l, nsing: Integer;
dxnorm, dwarf, fp, gnorm, parc, parl, paru, sum, temp: double;
tmp, wa1, wa2: pVec;
Const
p1 = 0.1; p001 = 0.001;
Begin
New(tmp); New(wa1); New(wa2);
{ 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 do
begin
wa1^[j] := qtb^[j];
IF (r^[j,j] = zero) AND (nsing = n) then nsing := j - 1;
IF nsing < n then wa1^[j] := zero
end;
For k := 1 to nsing do
begin
j := nsing - k + 1;
wa1^[j] := wa1^[j]/r^[j,j];
temp := wa1^[j];
jm1 := j - 1;
For i:=1 to jm1 do
wa1^[i] := wa1^[i] - r^[i,j]*temp
end;
For j := 1 to n do
begin
l := ipvt^[j];
x^[l] := wa1^[j]
end;
{ initialize the iteration counter.
evaluate the function at the origin, and test
for acceptance of the gauss-newton direction }
iter := 0;
For i:=1 to n do wa2^[i] := diag^[i]*x^[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 do
begin
l := ipvt^[j];
wa1^[j] := diag^[l]*(wa2^[l]/dxnorm)
end;
For j := 1 to n do
begin
For i:=1 to j-1 do tmp^[i]:=r^[i,j];
sum := DOT_PRODUCT(j-1, tmp, wa1);
wa1^[j] := (wa1^[j] - sum)/r^[j,j]
end;
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 do
begin
For i:=1 to j do tmp^[i]:=r^[i,j];
sum := DOT_PRODUCT(j, tmp, qtb);
l := ipvt^[j];
wa1^[j] := sum/diag^[l]
end;
gnorm := enorm(n,wa1);
paru := gnorm/delta;
IF paru = zero then paru := dwarf/MIN(delta,p1);
{ if the input par lies outside of the interval (parl,paru),
set par to the closer endpoint }
par := MAX(par,parl);
par := MIN(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 := MAX(dwarf, p001*paru);
temp := SQRT(par);
For i:=1 to n do wa1^[i] := temp*diag^[i];
qrsolv(n, r, ipvt, wa1, qtb, x, sdiag);
For i:=1 to n do wa2^[i] := diag^[i]*x^[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 do
begin
l := ipvt^[j];
wa1^[j] := diag^[l]*(wa2^[l]/dxnorm)
end;
For j := 1 to n do
begin
wa1^[j] := wa1^[j]/sdiag^[j];
temp := wa1^[j];
jp1 := j + 1;
For i:=jp1 to n do wa1^[i] := wa1^[i] - r^[i,j]*temp
end;
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 := MAX(parl,par);
IF fp < zero then paru := MIN(paru,par);
{ compute an improved estimate for par }
par := MAX(parl, par+parc);
{ end of an iteration }
GOTO 150;
220:IF iter = 0 then par := zero;
Dispose(tmp); Dispose(wa1); Dispose(wa2)
End; {lmpar}
Procedure qrfac(m, n:integer; a:pMat; pivot:Boolean; ipvt:pIVec; rdiag, acnorm:pVec);
{ *************************************************************************
! procedure qrfac
! This procedure 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 procedure.
! the procedure statement is
! procedure 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.
! procedures 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
! *************************************************************************** }
Label 40,45,50;
Var
i, j, jp1, k, kmax, minmn: Integer;
ajnorm, sum, temp: double;
tmp, tmp1, wa: pVec;
Begin
New(tmp); New(tmp1); New(wa);
{ compute the initial column norms and initialize several arrays }
For j := 1 to n do
begin
For i:=1 to m do tmp^[i]:=a^[i,j];
acnorm^[j] := enorm(m,tmp);
rdiag^[j] := acnorm^[j];
wa^[j] := rdiag^[j];
IF (pivot) then ipvt^[j] := j
end;
{ Reduce a to r with Householder transformations }
if m<=n then minmn := m else minmn := n;
For j := 1 to minmn do
begin
IF NOT pivot then GOTO 40;
{ Bring the column of largest norm into the pivot position }
kmax := j;
For k := j to n do
IF rdiag^[k] > rdiag^[kmax] then kmax := k;
IF kmax = j then GOTO 40;
For i := 1 to m do
begin
temp := a^[i,j];
a^[i,j] := a^[i,kmax];
a^[i,kmax] := temp
end;
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 do tmp^[i-j+1]:=a^[i,j];
ajnorm := enorm(m-j+1, tmp);
IF ajnorm = zero then goto 50;
IF a^[j,j] < zero then ajnorm := -ajnorm;
For i:=j to m do a^[i,j] := a^[i,j]/ajnorm;
a^[j,j] := a^[j,j] + one;
{ Apply the transformation to the remaining columns and update the norms }
jp1 := j + 1;
For k := jp1 to n do
begin
For i:=j to m do
begin
tmp^[i-j+1]:=a^[i,j];
tmp1^[i-j+1]:=a^[i,k]
end;
sum := DOT_PRODUCT(m-j+1,tmp,tmp1);
temp := sum/a^[j,j];
For i:=j to m do a^[i,k] := a^[i,k] - temp*a^[i,j];
IF (NOT pivot) OR (rdiag^[k] = zero) then goto 50;
temp := a^[j,k]/rdiag^[k];
rdiag^[k] := rdiag^[k]*SQRT(MAX(zero, one-temp*temp));
IF p05*Sqr(rdiag^[k]/wa^[k]) > epsmch then goto 45;
For i:=jp1 to m do tmp^[i-j]:=a^[i,k];
rdiag^[k] := enorm(m-j, tmp);
wa^[k] := rdiag^[k];
45: end;
rdiag^[j] := -ajnorm
end; {j loop}
50: Dispose(tmp); Dispose(tmp1); Dispose(wa)
End; {qrfac}
Procedure qrsolv(n:integer; r:pMat; ipvt:pIVec; diag, qtb, x, sdiag:pVec);
{ *************************************************************************
! procedure 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 procedure 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 procedure statement is
! procedure 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
! pascal-supplied ... ABS, SQRT
! argonne national laboratory. minpack project. march 1980.
! burton s. garbow, kenneth e. hillstrom, jorge j. more
! ************************************************************************* }
Var
i, j, k, kp1, l, nsing: Integer;
COS, cotan, qtbpj, SIN, sum, TAN, temp: double;
tmp, tmp1, wa: pVec;
Begin
New(tmp); New(tmp1); New(wa);
{ 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 do
begin
For i:=j to n do r^[i,j] := r^[j,i];
x^[j] := r^[j,j];
wa^[j] := qtb^[j]
end;
{ Eliminate the diagonal matrix d using a givens rotation }
For j := 1 to n do
begin
{ 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;
For i:=j to n do sdiag^[i] := zero;
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 do
begin
{ Determine a givens rotation which eliminates the
appropriate element in the current row of d }
IF sdiag^[k] = zero then exit;
IF ABS(r^[k,k]) < ABS(sdiag^[k]) THEN
begin
cotan := r^[k,k]/sdiag^[k];
SIN := p5/SQRT(p25 + p25*cotan*cotan);
COS := SIN*cotan
end
ELSE
begin
TAN := sdiag^[k]/r^[k,k];
COS := p5/SQRT(p25 + p25*TAN*TAN);
SIN := COS*TAN
end;
{ Compute the modified diagonal element of r and
the modified element of ((q transpose)*b,0) }
r^[k,k] := COS*r^[k,k] + SIN*sdiag^[k];
temp := COS*wa^[k] + SIN*qtbpj;
qtbpj := -SIN*wa^[k] + COS*qtbpj;
wa^[k] := temp;
{ Accumulate the tranformation in the row of s }
kp1 := k + 1;
For i := kp1 to n do
begin
temp := COS*r^[i,k] + SIN*sdiag^[i];
sdiag^[i] := -SIN*r^[i,k] + COS*sdiag^[i];
r^[i,k] := temp
end
end; {k loop}
{ Store the diagonal element of s and restore
the corresponding diagonal element of r }
sdiag^[j] := r^[j,j];
r^[j,j] := x^[j]
end; {j loop}
{ Solve the triangular system for z. If the system is singular,
then obtain a least squares solution }
nsing := n;
For j := 1 to n do
begin
IF (sdiag^[j] = zero) AND (nsing = n) then nsing := j - 1;
IF nsing < n then wa^[j] := zero
end;
For k := 1 to nsing do
begin
j := nsing - k + 1;
For i:=j+1 to nsing do
begin
tmp^[i-j]:=r^[i,j];
tmp1^[i-j]:=wa^[i]
end;
sum := DOT_PRODUCT(nsing-j, tmp, tmp1);
wa^[j] := (wa^[j] - sum)/sdiag^[j]
end;
{ Permute the components of z back to components of x }
For j := 1 to n do
begin
l := ipvt^[j];
x^[l] := wa^[j]
end;
Dispose(tmp); Dispose(tmp1); Dispose(wa)
End; {qrsolv}
FUNCTION enorm(n:integer; x:pVec): double;
Var i:word; temp:double;
Begin
temp:=zero;
For i:=1 to n do temp:=temp+Sqr(x^[i]);
enorm:=SQRT(temp)
End;
Procedure fdjac2(m, n:integer; x, fvec:pVec; fjac:pMat; var iflag:integer;
epsfcn: double);
{ ***********************************************************************
! procedure fdjac2
! this procedure computes a forward-difference approximation
! to the m by n jacobian matrix associated with a specified
! problem of m functions in n variables.
! the procedure statement is
! procedure fdjac2(m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
! where:
! fcn is the name of the user-supplied procedure which calculates
! the functions (removed from list of arguments).
! fcn should be written as follows:
! Procedure 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
! ********************************************************** }
Var
i,j: Integer;
eps, h, temp: double;
wa: pVec;
Begin
New(wa);
eps := SQRT(MAX(epsfcn, epsmch));
For j := 1 to n do
begin
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;
x^[j] := temp;
For i:=1 to m do fjac^[i,j] := (wa^[i] - fvec^[i])/h
end;
Dispose(wa)
End; {fdjac2}
END. {UNIT Levenberg_Marquardt}