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);
}
}
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. ***/