Esempio n. 1
0
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 ,	  sqrt(par)*d*x = 0 ,
 *
 *     in the least squares sense, and dxnorm is the euclidean
 *     norm of d*x, then either par is 0. and
 *
 *	    (dxnorm-delta) .le. 0.1*delta ,
 *
 *     or par is positive and
 *
 *	    abs(dxnorm-delta) .le. 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

// *** 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];

// *** 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<delta/10\n");
#endif
	*par = 0;
	return;
    }
// *** 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;
    }
// *** 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);

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

// *** 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 0. 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 this loop

// *** 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++)
		wa1[i] -= r[j * ldr + i] * wa1[j];
	}
	temp = lm_enorm(n, wa1);
	parc = fp / delta / temp / temp;

// *** depending on the sign of the function, update parl or paru.

	if (fp > 0)
	    parl = MAX(parl, *par);
	else if (fp < 0)
	    paru = MIN(paru, *par);
	// the case fp==0 is precluded by the break condition 

// *** compute an improved estimate for par.

	*par = MAX(parl, *par + parc);

    }

}
Esempio n. 2
0
static void lm_lmpar(const int n, double* r, const int ldr, const int* Pivot,
                     const double* diag, const double* qtb, const double delta,
                     double* par, double* x, double* Sdiag, double* aux, double* xdi)
/*     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 parameter value 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.
 *
 *     Using lm_qrsolv, 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^T*b. On output
 *     lmpar also provides an upper triangular matrix S such that
 *
 *          P^T*(A^T*A + par*D*D)*P = S^T*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.
 *
 *      Pivot 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
 *        Pivot(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^T*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 array of length n needed as workspace; on OUTPUT it
 *        contains the diagonal elements of the upper triangular matrix S.
 *
 *      aux is a multi-purpose work array of length n.
 *
 *      xdi is a work array of length n. On OUTPUT: diag[j] * x[j].
 *
 */
{
    int i, iter, j, nsing;
    double dxnorm, fp, fp_old, gnorm, parc, parl, paru;
    double sum, temp;
    static double p1 = 0.1;

    /*** 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++) {
        aux[j] = qtb[j];
        if (r[j*ldr+j] == 0 && nsing == n)
            nsing = j;
        if (nsing < n)
            aux[j] = 0;
    }
    for (j = nsing-1; j >= 0; j--) {
        aux[j] = aux[j] / r[j+ldr*j];
        temp = aux[j];
        for (i = 0; i < j; i++)
            aux[i] -= r[j*ldr+i] * temp;
    }

    for (j = 0; j < n; j++)
        x[Pivot[j]] = aux[j];

    /*** Initialize the iteration counter, evaluate the function at the origin,
         and test for acceptance of the Gauss-Newton direction. ***/

    for (j = 0; j < n; j++)
        xdi[j] = diag[j] * x[j];
    dxnorm = lm_enorm(n, xdi);
    fp = dxnorm - delta;
    if (fp <= p1 * delta) {
#ifdef LMFIT_DEBUG_MESSAGES
        printf("debug lmpar nsing=%d, n=%d, terminate[fp<=p1*del]\n", nsing, n);
#endif
        *par = 0;
        return;
    }

    /*** 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 = 0;
    if (nsing >= n) {
        for (j = 0; j < n; j++)
            aux[j] = diag[Pivot[j]] * xdi[Pivot[j]] / dxnorm;

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

    /*** Calculate an upper bound, paru, for the zero of the function. ***/

    for (j = 0; j < n; j++) {
        sum = 0;
        for (i = 0; i <= j; i++)
            sum += r[j*ldr+i] * qtb[i];
        aux[j] = sum / diag[Pivot[j]];
    }
    gnorm = lm_enorm(n, aux);
    paru = gnorm / delta;
    if (paru == 0)
        paru = LM_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 == 0)
        *par = gnorm / dxnorm;

    /*** Iterate. ***/

    for (iter = 0;; iter++) {

        /** Evaluate the function at the current value of par. **/
        if (*par == 0)
            *par = MAX(LM_DWARF, 0.001 * paru);
        temp = sqrt(*par);
        for (j = 0; j < n; j++)
            aux[j] = temp * diag[j];

        lm_qrsolv(n, r, ldr, Pivot, aux, qtb, x, Sdiag, xdi);
        /* return values are r, x, Sdiag */

        for (j = 0; j < n; j++)
            xdi[j] = diag[j] * x[j]; /* used as output */
        dxnorm = lm_enorm(n, xdi);
        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) {
#ifdef LMFIT_DEBUG_MESSAGES
            printf("debug lmpar nsing=%d, iter=%d, "
                   "par=%.4e [%.4e %.4e], delta=%.4e, fp=%.4e\n",
                   nsing, iter, *par, parl, paru, delta, fp);
#endif
            break; /* the only exit from the iteration. */
        }

        /** Compute the Newton correction. **/
        for (j = 0; j < n; j++)
            aux[j] = diag[Pivot[j]] * xdi[Pivot[j]] / dxnorm;

        for (j = 0; j < n; j++) {
            aux[j] = aux[j] / Sdiag[j];
            for (i = j+1; i < n; i++)
                aux[i] -= r[j*ldr+i] * aux[j];
        }
        temp = lm_enorm(n, aux);
        parc = fp / delta / temp / temp;

        /** Depending on the sign of the function, update parl or paru. **/
        if (fp > 0)
            parl = MAX(parl, *par);
        else /* fp < 0 [the case fp==0 is precluded by the break condition] */
            paru = MIN(paru, *par);

        /** Compute an improved estimate for par. **/
        *par = MAX(parl, *par + parc);
    }

} /*** lm_lmpar. ***/