lmmin.c

Levenberg-Marquardt最小二乘拟合

	}
#if BUG>1
	/* DEBUG: print the entire matrix */
	for (i = 0; i < m; i++) {
	    for (j = 0; j < n; j++)
		printf("%.5e ", fjac[j * m + i]);
	    printf("\n");
	}
#endif

/*** outer: compute the qr factorization of the jacobian. ***/

	lm_qrfac(m, n, fjac, 1, ipvt, wa1, wa2, wa3);

	if (iter == 1) { /* first iteration */
	    if (mode != 2) {
                /* diag := norms of the columns of the initial jacobian */
		for (j = 0; j < n; j++) {
		    diag[j] = wa2[j];
		    if (wa2[j] == 0.)
			diag[j] = 1.;
		}
	    }
            /* use diag to scale x, then calculate the norm */
	    for (j = 0; j < n; j++)
		wa3[j] = diag[j] * x[j];
	    xnorm = lm_enorm(n, wa3);
            /* initialize the step bound delta. */
	    delta = factor * xnorm;
	    if (delta == 0.)
		delta = factor;
	}

/*** outer: form (q transpose)*fvec and store first n components in qtf. ***/

	for (i = 0; i < m; i++)
	    wa4[i] = fvec[i];

	for (j = 0; j < n; j++) {
	    temp3 = fjac[j * m + j];
	    if (temp3 != 0.) {
		sum = 0;
		for (i = j; i < m; i++)
		    sum += fjac[j * m + i] * wa4[i];
		temp = -sum / temp3;
		for (i = j; i < m; i++)
		    wa4[i] += fjac[j * m + i] * temp;
	    }
	    fjac[j * m + j] = wa1[j];
	    qtf[j] = wa4[j];
	}

/** outer: compute norm of scaled gradient and test for convergence. ***/

	gnorm = 0;
	if (fnorm != 0) {
	    for (j = 0; j < n; j++) {
		if (wa2[ipvt[j]] == 0)
		    continue;

		sum = 0.;
		for (i = 0; i <= j; i++)
		    sum += fjac[j * m + i] * qtf[i] / fnorm;
		gnorm = MAX(gnorm, fabs(sum / wa2[ipvt[j]]));
	    }
	}

	if (gnorm <= gtol) {
	    *info = 4;
	    return;
	}

/*** outer: rescale if necessary. ***/

	if (mode != 2) {
	    for (j = 0; j < n; j++)
		diag[j] = MAX(diag[j], wa2[j]);
	}

/*** the inner loop. ***/
	do {
#if BUG
	    printf("lmdif/ inner loop iter=%d nfev=%d\n", iter, *nfev);
#endif

/*** inner: determine the levenberg-marquardt parameter. ***/

	    lm_lmpar(n, fjac, m, ipvt, diag, qtf, delta, &par,
		     wa1, wa2, wa3, wa4);

/*** inner: store the direction p and x + p; calculate the norm of p. ***/

	    for (j = 0; j < n; j++) {
		wa1[j] = -wa1[j];
		wa2[j] = x[j] + wa1[j];
		wa3[j] = diag[j] * wa1[j];
	    }
	    pnorm = lm_enorm(n, wa3);

/*** inner: on the first iteration, adjust the initial step bound. ***/

	    if (*nfev <= 1 + n)
		delta = MIN(delta, pnorm);

            /* evaluate the function at x + p and calculate its norm. */

	    *info = 0;
	    (*evaluate) (wa2, m, wa4, data, info);
	    (*printout) (n, x, m, wa4, data, 2, iter, ++(*nfev));
	    if (*info < 0)
		return; /* user requested break. */

	    fnorm1 = lm_enorm(m, wa4);
#if BUG
	    printf("lmdif/ pnorm %.10e  fnorm1 %.10e  fnorm %.10e"
		   " delta=%.10e par=%.10e\n",
		   pnorm, fnorm1, fnorm, delta, par);
#endif

/*** inner: compute the scaled actual reduction. ***/

	    if (p1 * fnorm1 < fnorm)
		actred = 1 - SQR(fnorm1 / fnorm);
	    else
		actred = -1;

/*** inner: compute the scaled predicted reduction and 
     the scaled directional derivative. ***/

	    for (j = 0; j < n; j++) {
		wa3[j] = 0;
		for (i = 0; i <= j; i++)
		    wa3[i] += fjac[j * m + i] * wa1[ipvt[j]];
	    }
	    temp1 = lm_enorm(n, wa3) / fnorm;
	    temp2 = sqrt(par) * pnorm / fnorm;
	    prered = SQR(temp1) + 2 * SQR(temp2);
	    dirder = -(SQR(temp1) + SQR(temp2));

/*** inner: compute the ratio of the actual to the predicted reduction. ***/

	    ratio = prered != 0 ? actred / prered : 0;
#if BUG
	    printf("lmdif/ actred=%.10e prered=%.10e ratio=%.10e"
		   " sq(1)=%.10e sq(2)=%.10e dd=%.10e\n",
		   actred, prered, prered != 0 ? ratio : 0.,
		   SQR(temp1), SQR(temp2), dirder);
#endif

/*** inner: update the step bound. ***/

	    if (ratio <= p25) {
		if (actred >= 0.)
		    temp = p5;
		else
		    temp = p5 * dirder / (dirder + p5 * actred);
		if (p1 * fnorm1 >= fnorm || temp < p1)
		    temp = p1;
		delta = temp * MIN(delta, pnorm / p1);
		par /= temp;
	    } else if (par == 0. || ratio >= p75) {
		delta = pnorm / p5;
		par *= p5;
	    }

/*** inner: test for successful iteration. ***/

	    if (ratio >= p0001) {
                /* yes, success: update x, fvec, and their norms. */
		for (j = 0; j < n; j++) {
		    x[j] = wa2[j];
		    wa2[j] = diag[j] * x[j];
		}
		for (i = 0; i < m; i++)
		    fvec[i] = wa4[i];
		xnorm = lm_enorm(n, wa2);
		fnorm = fnorm1;
		iter++;
	    }
#if BUG
	    else {
		printf("ATTN: iteration considered unsuccessful\n");
	    }
#endif

/*** inner: tests for convergence ( otherwise *info = 1, 2, or 3 ). ***/

	    *info = 0; /* do not terminate (unless overwritten by nonzero) */
	    if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1)
		*info = 1;
	    if (delta <= xtol * xnorm)
		*info += 2;
	    if (*info != 0)
		return;

/*** inner: tests for termination and stringent tolerances. ***/

	    if (*nfev >= maxfev)
		*info = 5;
	    if (fabs(actred) <= LM_MACHEP &&
		prered <= LM_MACHEP && p5 * ratio <= 1)
		*info = 6;
	    if (delta <= LM_MACHEP * xnorm)
		*info = 7;
	    if (gnorm <= LM_MACHEP)
		*info = 8;
	    if (*info != 0)
		return;

/*** inner: end of the loop. repeat if iteration unsuccessful. ***/

	} while (ratio < p0001);

/*** outer: end of the loop. ***/

    } while (1);

} /*** lm_lmdif. ***/



void lm_lmpar(int n, double *r, int ldr, int *ipvt, double *diag,
	      double *qtb, double delta, double *par, double *x,
	      double *sdiag, double *wa1, double *wa2)
{
/*     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  and  sqrt(par)*d*x = 0
 *
 *     in the least squares sense, and dxnorm is the euclidean
 *     norm of d*x, then either par=0 and (dxnorm-delta) < 0.1*delta,
 *     or par>0 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
 *     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 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.
 *
 *     parameters:
 *
 *	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.
 *
 */
    int i, iter, j, nsing;
    double dxnorm, fp, fp_old, gnorm, parc, parl, paru;
    double sum, temp;
    static double p1 = 0.1;
    static double p001 = 0.001;

#if BUG
    printf("lmpar\n");
#endif

/*** lmpar: compute and store in x the gauss-newton direction. if the
     jacobian is rank-deficient, obtain a least squares solution. ***/

    nsing = n;
    for (j = 0; j < n; j++) {
	wa1[j] = qtb[j];
	if (r[j * ldr + j] == 0 && nsing == n)
	    nsing = j;
	if (nsing < n)
	    wa1[j] = 0;
    }
#if BUG
    printf("nsing %d ", nsing);
#endif
    for (j = nsing - 1; j >= 0; j--) {
	wa1[j] = wa1[j] / r[j + ldr * j];
	temp = wa1[j];
	for (i = 0; i < j; i++)
	    wa1[i] -= r[j * ldr + i] * temp;
    }

    for (j = 0; j < n; j++)
	x[ipvt[j]] = wa1[j];

/*** lmpar: initialize the iteration counter, evaluate the function at the
     origin, and test for acceptance of the gauss-newton direction. ***/

    iter = 0;
    for (j = 0; j < n; j++)
	wa2[j] = diag[j] * x[j];
    dxnorm = lm_enorm(n, wa2);
    fp = dxnorm - delta;
    if (fp <= p1 * delta) {
#if BUG
	printf("lmpar/ terminate (fp<p1*delta)\n");
#endif
	*par = 0;
	return;
    }

/*** lmpar: if the jacobian is not rank deficient, the newton
     step provides a lower bound, parl, for the 0. of
     the function. otherwise set this bound to 0.. ***/

    parl = 0;
    if (nsing >= n) {
	for (j = 0; j < n; j++)
	    wa1[j] = diag[ipvt[j]] * wa2[ipvt[j]] / dxnorm;

	for (j = 0; j < n; j++) {
	    sum = 0.;
	    for (i = 0; i < j; i++)
		sum += r[j * ldr + i] * wa1[i];
	    wa1[j] = (wa1[j] - sum) / r[j + ldr * j];
	}
	temp = lm_enorm(n, wa1);
	parl = fp / delta / temp / temp;
    }

/*** lmpar: calculate an upper bound, paru, for the 0. of the function. ***/

    for (j = 0; j < n; j++) {
	sum = 0;
	for (i = 0; i <= j; i++)
	    sum += r[j * ldr + i] * qtb[i];
	wa1[j] = sum / diag[ipvt[j]];
    }
    gnorm = lm_enorm(n, wa1);
    paru = gnorm / delta;
    if (paru == 0.)
	paru = LM_DWARF / MIN(delta, p1);

/*** lmpar: 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 == 0.)
	*par = gnorm / dxnorm;
#if BUG
    printf("lmpar/ parl %.4e  par %.4e  paru %.4e\n", parl, *par, paru);
#endif

/*** lmpar: iterate. ***/

    for (;; iter++) {

        /** evaluate the function at the current value of par. **/

	if (*par == 0.)
	    *par = MAX(LM_DWARF, p001 * paru);
	temp = sqrt(*par);
	for (j = 0; j < n; j++)
	    wa1[j] = temp * diag[j];
	lm_qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
	for (j = 0; j < n; j++)
	    wa2[j] = diag[j] * x[j];
	dxnorm = lm_enorm(n, wa2);
	fp_old = 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 (fabs(fp) <= p1 * delta
	    || (parl == 0. && fp <= fp_old && fp_old < 0.)
	    || iter == 10)
	    break; /* the only exit from the iteration. */
        
        /** compute the Newton correction. **/

	for (j = 0; j < n; j++)
	    wa1[j] = diag[ipvt[j]] * wa2[ipvt[j]] / dxnorm;

	for (j = 0; j < n; j++) {
	    wa1[j] = wa1[j] / sdiag[j];
	    for (i = j + 1; i < n; i++)