lmdif.cpp

L-M法的函数库

/* lmdif.f -- translated by f2c (version of 17 January 1992  0:17:58).
   You must link the resulting object file with the libraries:
	-lf77 -li77 -lm -lc   (in that order)
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "f2c.h"

/* Table of constant values */

static integer c__1 = 1;
/* Subroutine */ /*子程序*/
int fdjac2_( int (*fcn) (integer * ,integer * ,doublereal *,doublereal *,integer * ),
	     integer * m,
	     integer * n,
	     doublereal * x,
	     doublereal *fvec,
	     doublereal *fjac,
	     integer *ldfjac,
	     integer * iflag,
	     doublereal *epsfcn,
	     doublereal * wa)
/* Subroutine */
// int (*fcn) ();
//integer *m, *n;
//doublereal *x, *fvec, *fjac;
//integer *ldfjac, *iflag;
//doublereal *epsfcn, *wa;
{
    /* Initialized data */

    static doublereal zero = 0.;

    /* System generated locals */
    integer fjac_dim1,
	    fjac_offset,
	    i__1,
	    i__2;

    /* Builtin functions */
    double sqrt();

    /* Local variables */
    static doublereal temp, h;
    static integer    i, j;
    static doublereal epsmch;
    extern doublereal dpmpar_();
    static doublereal eps;

/*     ********** */

/*     subroutine fdjac2 */
/*      m个方程n个未知数问题的m×n Jacobin矩阵的一种前向差分逼近*/
/*     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(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) */

/*     where */

/*     1. 参数fcn is the name of the user-supplied subroutine which */
/*         calculates the functions. fcn must be declared */
/*         in an external statement in the user calling */
/*         program, and should be written as follows. */

/*         subroutine fcn(m,n,x,fvec,iflag) *//*参数fcn的声名，用于构造目标优化函数*/
/*        [ integer m,n,iflag */
/*         double precision x(n),fvec(m) ]*/
/*         ---------- */
/*         calculate the functions at x and *//*利用x计算各个方程的值*/
/*         return this vector in fvec. *//*各个方程的值返回再fvec中*/
/*         ---------- */
/*         return */
/*         end */

/*      2。the value of "iflag" should not be changed by fcn unless */
/*         the user wants to terminate execution of fdjac2. */
/*         in this case set iflag to a negative integer. */   /*"iflag"为负时终止计算*/

/*      3。m is a positive integer input variable set to the number */
/*         of functions. 方程的个数，正*/

/*      4。n is a positive integer input variable set to the number */
/*         of variables. n must not exceed m. 优化变量的个数，为正*/

/*      5。x is an input array of length n. 长度为n的优化向量 */

/*      6。fvec is an input array of length m which must contain the */
/*         functions evaluated at x. 长度为m的输入向量，包含用x计算的各个的方程的值*/

/*      7。fjac is an output m by n array which contains the */
/*         approximation to the jacobian matrix evaluated at x. */ /*利用x计算的m×n Jacobi近似矩阵*/

/*      8。ldfjac is a positive integer input variable not less than m */
/*         which specifies the leading dimension of the array fjac. *//* 一个正数，不小于m*/

/*      9。 iflag is an integer variable which can be used to terminate */
/*         the execution of fdjac2. see description of fcn. *//*结束fdjac2的标志数*/

/*      10。 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. */

 /*     11。wa is a work array of length m. 长度为m的向量*/

/*     subprograms called */

/*       user-supplied ...... fcn */

/*       minpack-supplied ... dpmpar */

/*       fortran-supplied ... dabs,dmax1,dsqrt */

/*     argonne national laboratory. minpack project. march 1980. */
/*     burton s. garbow, kenneth e. hillstrom, jorge j. more */

/*     ********** */
    /* Parameter adjustments *//*参数调整*/
    --wa;
    fjac_dim1 = *ldfjac; //Jacobin阵的列数
    fjac_offset = fjac_dim1 + 1;
    fjac -= fjac_offset;
    --fvec;
    --x;

    /* Function Body */

/*     epsmch is the machine precision. */

    epsmch = ::dpmpar_(&c__1);//设定双精度的机器精度

    eps = ::sqrt((xmax(*epsfcn,epsmch)));
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	temp = x[j];
	h = eps * abs(temp);//计算差分的步长
	if (h == zero) {
	    h = eps;
	}
	x[j] = temp + h;// 对x[j]实施前向差分的前一个点x'[j]
	(*fcn)(m, n, &x[1], &wa[1], iflag);// 计算在前一个点x'[j]的各个方程的值,存储在wa中
	if (*iflag < 0) {
	    goto L30;
	}
	x[j] = temp;
	i__2 = *m;
	for (i = 1; i <= i__2; ++i) {
	    fjac[i + j * fjac_dim1] = (wa[i] - fvec[i]) / h; //计算Jacobin矩阵,其中fvec存储的是在x[j]点的各个方程的值
/* L10: */
	}
/* L20: */
    }
L30:
    return 0;

/*     last card of subroutine fdjac2. */

} /* fdjac2_ */
//*********************************************************************************************
//*********************************************************************************************

/* Subroutine */ int lmdif_(int (*fcn)(integer * ,integer * ,doublereal *,doublereal *,integer * ),
integer *m,
integer *n,
doublereal *x, 
doublereal *fvec, 
doublereal *ftol,
doublereal *xtol, 
doublereal *gtol, 
integer *maxfev, 
doublereal *epsfcn, 
doublereal *diag, 
integer *mode, 
doublereal *factor, 
integer *nprint,
integer *info, 
integer *nfev, 
doublereal *fjac, 
integer *ldfjac, 
integer *ipvt, 
doublereal *qtf, 
doublereal *wa1, 
doublereal *wa2, 
doublereal *wa3, 
doublereal *wa4)
/*  int (*fcn) ();
integer *m, *n;
doublereal *x, *fvec, *ftol, *xtol, *gtol;
integer *maxfev;
doublereal *epsfcn, *diag;
integer *mode;
doublereal *factor;
integer *nprint, *info, *nfev;
doublereal *fjac;
integer *ldfjac, *ipvt;
doublereal *qtf, *wa1, *wa2, *wa3, *wa4;*/
{
    /* Initialized data */

    static doublereal one = 1.;
    static doublereal p1 = .1;
    static doublereal p5 = .5;
    static doublereal p25 = .25;
    static doublereal p75 = .75;
    static doublereal p0001 = 1e-4;
    static doublereal zero = 0.;

    /* System generated locals */
    integer fjac_dim1, fjac_offset, i__1, i__2;
    doublereal d__1, d__2, d__3;

    /* Builtin functions */
    double sqrt();

    /* Local variables */
    static integer iter;
    static doublereal temp, temp1, temp2;
    static integer i, j, l, iflag;
    static doublereal delta;
    extern /* Subroutine */ int qrfac_(), lmpar_();
    static doublereal ratio;
    extern doublereal enorm_();
    static doublereal fnorm, gnorm;
    extern /* Subroutine */ int fdjac2_();
    static doublereal pnorm, xnorm, fnorm1, actred, dirder, epsmch, prered;
    extern doublereal dpmpar_();
    static doublereal par, sum;

/*     ********** */

/*     subroutine lmdif */

/*     用龙伯格法优化非线性方程组，m个方程n个优化参数*/
/*     the purpose of lmdif is to minimize the sum of the squares of */
/*     m nonlinear functions in n variables by a modification of */
/*     the levenberg-marquardt algorithm. the user must provide a */
/*     subroutine which calculates the functions. the jacobian is */
/*     then calculated by a forward-difference approximation. */

/*     the subroutine statement is */

/*       subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, */
/*                        diag,mode,factor,nprint,info,nfev,fjac, */
/*                        ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) */

/*     where */
/*      1. fcn是计算方程组各个方程的值,跟雅克比阵的计算中是一样的*/
/*       fcn is the name of the user-supplied subroutine which */
/*         calculates the functions. fcn must be declared */
/*         in an external statement in the user calling */
/*         program, and should be written as follows. */

/*         subroutine fcn(m,n,x,fvec,iflag) */
/*         integer m,n,iflag */
/*         double precision x(n),fvec(m) */
/*         ---------- */
/*         calculate the functions at x and */
/*         return this vector in fvec. */
/*         ---------- */
/*         return */
/*         end */

/*       2.  the value of iflag should not be changed by fcn unless */
/*         the user wants to terminate execution of lmdif. */
/*         in this case set iflag to a negative integer. */

/*       3.m is a positive integer input variable set to the number */
/*         of functions. */   /*等于方程的个数*/

/*       4.n is a positive integer input variable set to the number */
/*         of variables. n must not exceed m. */  /*等于优化变量的个数<=m*/

/*       5.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. *//*输入优化变量,可能是初始值或者最终优化值*/

/*       6.fvec is an output array of length m which contains */
/*         the functions evaluated at the output x. *//*各个方程的计算值在fvec中*/

/*       7.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. */ /*平方和的上限*/

/*       8.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. *//*优化变量的期望误差*/

/*      9. 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. *//*非负数,检测fvec和jacobin阵中的任意一列的正交性*/

/*      10. 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. *//*正整数,调用fcn的最大次数*/

/*      11. 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. */

/*      12. diag is an array of length n. if mode = 1 (see */ /*n长度的数组*/
/*         below), diag is internally set. if mode = 2, diag */
/*         must contain positive entries that serve as */
/*         multiplicative scale factors for the variables. */

/*     13.  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. */

/*     14.  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. */

/*     15.  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. */

/*     16.  info is an integer output variable. if the user has */
/*         terminated execution, info is set to the (negative) */
/*         value of iflag. see description of fcn. otherwise, */
/*         info is set as follows. */

/*         info = 0  improper input parameters. */

/*         info = 1  both actual and predicted relative reductions */
/*                   in the sum of squares are at most ftol. */

/*         info = 2  relative error between two consecutive iterates */
/*                   is at most xtol. */

/*         info = 3  conditions for info = 1 and info = 2 both hold. */

/*         info = 4  the cosine of the angle between fvec and any */
/*                   column of the jacobian is at most gtol in */
/*                   absolute value. */

/*         info = 5  number of calls to fcn has reached or */
/*                   exceeded maxfev. */

/*         info = 6  ftol is too small. no further reduction in */
/*                   the sum of squares is possible. */

/*         info = 7  xtol is too small. no further improvement in */
/*                   the approximate solution x is possible. */

/*         info = 8  gtol is too small. fvec is orthogonal to the */
/*                   columns of the jacobian to machine precision. */

/*      18. nfev is an integer output variable set to the number of */
/*         calls to fcn. *//*统计调用fcn的次数*/

/*      19. 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. */

/*       20. ldfjac is a positive integer input variable not less than m */
/*         which specifies the leading dimension of the array fjac. *//**/

/*       21.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. */

/*       22.qtf is an output array of length n which contains */
/*         the first n elements of the vector (q transpose)*fvec. */

/*       23.wa1, wa2, and wa3 are work arrays of length n. *//*长度为n的工作数组*/

/*       24.wa4 is a work array of length m. *//*长度为m的中间数组*/

/*     subprograms called */

/*       user-supplied ...... fcn */