/*
|
* Library: lmfit (Levenberg-Marquardt least squares fitting)
|
*
|
* File: lmmin.c
|
*
|
* Contents: Levenberg-Marquardt minimization.
|
*
|
* Copyright: MINPACK authors, The University of Chikago (1980-1999)
|
* Joachim Wuttke, Forschungszentrum Juelich GmbH (2004-2013)
|
*
|
* License: see ../COPYING (FreeBSD)
|
*
|
* Homepage: apps.jcns.fz-juelich.de/lmfit
|
*/
|
|
#include <assert.h>
|
#include <stdlib.h>
|
#include <stdio.h>
|
#include <math.h>
|
#include <float.h>
|
#include "lmmin.h"
|
|
#define MIN(a, b) (((a) <= (b)) ? (a) : (b))
|
#define MAX(a, b) (((a) >= (b)) ? (a) : (b))
|
#define SQR(x) (x) * (x)
|
|
/* Declare functions that do the heavy numerics.
|
Implementions are in this source file, below lmmin.
|
Dependences: lmmin calls lmpar, which calls qrfac and qrsolv. */
|
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);
|
void lm_qrfac(const int m, const int n, double* A, int* Pivot, double* Rdiag,
|
double* Acnorm, double* W);
|
void lm_qrsolv(const int n, double* r, const int ldr, const int* Pivot,
|
const double* diag, const double* qtb, double* x,
|
double* Sdiag, double* W);
|
|
/******************************************************************************/
|
/* Numeric constants */
|
/******************************************************************************/
|
|
/* Set machine-dependent constants to values from float.h. */
|
#define LM_MACHEP DBL_EPSILON /* resolution of arithmetic */
|
#define LM_DWARF DBL_MIN /* smallest nonzero number */
|
#define LM_SQRT_DWARF sqrt(DBL_MIN) /* square should not underflow */
|
#define LM_SQRT_GIANT sqrt(DBL_MAX) /* square should not overflow */
|
#define LM_USERTOL 30 * LM_MACHEP /* users are recommended to require this */
|
|
/* If the above values do not work, the following seem good for an x86:
|
LM_MACHEP .555e-16
|
LM_DWARF 9.9e-324
|
LM_SQRT_DWARF 1.e-160
|
LM_SQRT_GIANT 1.e150
|
LM_USER_TOL 1.e-14
|
The following values should work on any machine:
|
LM_MACHEP 1.2e-16
|
LM_DWARF 1.0e-38
|
LM_SQRT_DWARF 3.834e-20
|
LM_SQRT_GIANT 1.304e19
|
LM_USER_TOL 1.e-14
|
*/
|
|
/* Predefined control parameter sets (msgfile=NULL means stdout). */
|
const lm_control_struct lm_control_double = {
|
LM_USERTOL, LM_USERTOL, LM_USERTOL, LM_USERTOL,
|
100., 100, 1, NULL, 0, -1, -1};
|
const lm_control_struct lm_control_float = {
|
1.e-7, 1.e-7, 1.e-7, 1.e-7,
|
100., 100, 1, NULL, 0, -1, -1};
|
|
/******************************************************************************/
|
/* Message texts (indexed by status.info) */
|
/******************************************************************************/
|
|
const char* lm_infmsg[] = {
|
"found zero (sum of squares below underflow limit)",
|
"converged (the relative error in the sum of squares is at most tol)",
|
"converged (the relative error of the parameter vector is at most tol)",
|
"converged (both errors are at most tol)",
|
"trapped (by degeneracy; increasing epsilon might help)",
|
"exhausted (number of function calls exceeding preset patience)",
|
"failed (ftol<tol: cannot reduce sum of squares any further)",
|
"failed (xtol<tol: cannot improve approximate solution any further)",
|
"failed (gtol<tol: cannot improve approximate solution any further)",
|
"crashed (not enough memory)",
|
"exploded (fatal coding error: improper input parameters)",
|
"stopped (break requested within function evaluation)",
|
"found nan (function value is not-a-number or infinite)"};
|
|
const char* lm_shortmsg[] = {
|
"found zero",
|
"converged (f)",
|
"converged (p)",
|
"converged (2)",
|
"degenerate",
|
"call limit",
|
"failed (f)",
|
"failed (p)",
|
"failed (o)",
|
"no memory",
|
"invalid input",
|
"user break",
|
"found nan"};
|
|
/******************************************************************************/
|
/* Monitoring auxiliaries. */
|
/******************************************************************************/
|
|
void lm_print_pars(int nout, const double* par, FILE* fout)
|
{
|
int i;
|
for (i = 0; i < nout; ++i)
|
fprintf(fout, " %16.9g", par[i]);
|
fprintf(fout, "\n");
|
}
|
|
/******************************************************************************/
|
/* lmmin (main minimization routine) */
|
/******************************************************************************/
|
|
void lmmin(const int n, double* x, const int m, const void* data,
|
void (*evaluate)(const double* par, const int m_dat,
|
const void* data, double* fvec, int* userbreak),
|
const lm_control_struct* C, lm_status_struct* S)
|
{
|
int j, i;
|
double actred, dirder, fnorm, fnorm1, gnorm, pnorm, prered, ratio, step,
|
sum, temp, temp1, temp2, temp3;
|
|
/*** Initialize internal variables. ***/
|
|
int maxfev = C->patience * (n+1);
|
|
int inner_success; /* flag for loop control */
|
double lmpar = 0; /* Levenberg-Marquardt parameter */
|
double delta = 0;
|
double xnorm = 0;
|
double eps = sqrt(MAX(C->epsilon, LM_MACHEP)); /* for forward differences */
|
|
int nout = C->n_maxpri == -1 ? n : MIN(C->n_maxpri, n);
|
|
/* Reinterpret C->msgfile=NULL as stdout (which is unavailable for
|
compile-time initialization of lm_control_double and similar). */
|
FILE* msgfile = C->msgfile ? C->msgfile : stdout;
|
|
/*** Default status info; must be set before first return statement. ***/
|
|
S->outcome = 0; /* status code */
|
S->userbreak = 0;
|
S->nfev = 0; /* function evaluation counter */
|
|
/*** Check input parameters for errors. ***/
|
|
if (n <= 0) {
|
fprintf(stderr, "lmmin: invalid number of parameters %i\n", n);
|
S->outcome = 10;
|
return;
|
}
|
if (m < n) {
|
fprintf(stderr, "lmmin: number of data points (%i) "
|
"smaller than number of parameters (%i)\n",
|
m, n);
|
S->outcome = 10;
|
return;
|
}
|
if (C->ftol < 0 || C->xtol < 0 || C->gtol < 0) {
|
fprintf(stderr,
|
"lmmin: negative tolerance (at least one of %g %g %g)\n",
|
C->ftol, C->xtol, C->gtol);
|
S->outcome = 10;
|
return;
|
}
|
if (maxfev <= 0) {
|
fprintf(stderr, "lmmin: nonpositive function evaluations limit %i\n",
|
maxfev);
|
S->outcome = 10;
|
return;
|
}
|
if (C->stepbound <= 0) {
|
fprintf(stderr, "lmmin: nonpositive stepbound %g\n", C->stepbound);
|
S->outcome = 10;
|
return;
|
}
|
if (C->scale_diag != 0 && C->scale_diag != 1) {
|
fprintf(stderr, "lmmin: logical variable scale_diag=%i, "
|
"should be 0 or 1\n",
|
C->scale_diag);
|
S->outcome = 10;
|
return;
|
}
|
|
/*** Allocate work space. ***/
|
|
/* Allocate total workspace with just one system call */
|
char* ws;
|
if ((ws = malloc((2*m + 5*n + m*n) * sizeof(double) +
|
n * sizeof(int))) == NULL) {
|
S->outcome = 9;
|
return;
|
}
|
|
/* Assign workspace segments. */
|
char* pws = ws;
|
double* fvec = (double*)pws;
|
pws += m * sizeof(double) / sizeof(char);
|
double* diag = (double*)pws;
|
pws += n * sizeof(double) / sizeof(char);
|
double* qtf = (double*)pws;
|
pws += n * sizeof(double) / sizeof(char);
|
double* fjac = (double*)pws;
|
pws += n * m * sizeof(double) / sizeof(char);
|
double* wa1 = (double*)pws;
|
pws += n * sizeof(double) / sizeof(char);
|
double* wa2 = (double*)pws;
|
pws += n * sizeof(double) / sizeof(char);
|
double* wa3 = (double*)pws;
|
pws += n * sizeof(double) / sizeof(char);
|
double* wf = (double*)pws;
|
pws += m * sizeof(double) / sizeof(char);
|
int* Pivot = (int*)pws;
|
pws += n * sizeof(int) / sizeof(char);
|
|
/* Initialize diag. */
|
if (!C->scale_diag)
|
for (j = 0; j < n; j++)
|
diag[j] = 1;
|
|
/*** Evaluate function at starting point and calculate norm. ***/
|
|
if (C->verbosity) {
|
fprintf(msgfile, "lmmin start ");
|
lm_print_pars(nout, x, msgfile);
|
}
|
(*evaluate)(x, m, data, fvec, &(S->userbreak));
|
if (C->verbosity > 4)
|
for (i = 0; i < m; ++i)
|
fprintf(msgfile, " fvec[%4i] = %18.8g\n", i, fvec[i]);
|
S->nfev = 1;
|
if (S->userbreak)
|
goto terminate;
|
fnorm = lm_enorm(m, fvec);
|
if (C->verbosity)
|
fprintf(msgfile, " fnorm = %18.8g\n", fnorm);
|
|
if (!isfinite(fnorm)) {
|
S->outcome = 12; /* nan */
|
goto terminate;
|
} else if (fnorm <= LM_DWARF) {
|
S->outcome = 0; /* sum of squares almost zero, nothing to do */
|
goto terminate;
|
}
|
|
/*** The outer loop: compute gradient, then descend. ***/
|
|
for (int outer = 0;; ++outer) {
|
|
/** Calculate the Jacobian. **/
|
for (j = 0; j < n; j++) {
|
temp = x[j];
|
step = MAX(eps * eps, eps * fabs(temp));
|
x[j] += step; /* replace temporarily */
|
(*evaluate)(x, m, data, wf, &(S->userbreak));
|
++(S->nfev);
|
if (S->userbreak)
|
goto terminate;
|
for (i = 0; i < m; i++)
|
fjac[j*m+i] = (wf[i] - fvec[i]) / step;
|
x[j] = temp; /* restore */
|
}
|
if (C->verbosity >= 10) {
|
/* print the entire matrix */
|
printf("\nlmmin Jacobian\n");
|
for (i = 0; i < m; i++) {
|
printf(" ");
|
for (j = 0; j < n; j++)
|
printf("%.5e ", fjac[j*m+i]);
|
printf("\n");
|
}
|
}
|
|
/** Compute the QR factorization of the Jacobian. **/
|
|
/* fjac is an m by n array. The upper n by n submatrix of fjac is made
|
* to contain an upper triangular matrix R with diagonal elements of
|
* nonincreasing magnitude such that
|
*
|
* P^T*(J^T*J)*P = R^T*R
|
*
|
* (NOTE: ^T stands for matrix transposition),
|
*
|
* where P is a permutation matrix and J is the final calculated
|
* Jacobian. Column j of P is column Pivot(j) of the identity matrix.
|
* The lower trapezoidal part of fjac contains information generated
|
* during the computation of R.
|
*
|
* Pivot is an integer array of length n. It 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 Pivot(j) of the identity matrix.
|
*/
|
|
lm_qrfac(m, n, fjac, Pivot, wa1, wa2, wa3);
|
/* return values are Pivot, wa1=rdiag, wa2=acnorm */
|
|
/** Form Q^T * fvec, and store first n components in qtf. **/
|
for (i = 0; i < m; i++)
|
wf[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] * wf[i];
|
temp = -sum / temp3;
|
for (i = j; i < m; i++)
|
wf[i] += fjac[j*m+i] * temp;
|
}
|
fjac[j*m+j] = wa1[j];
|
qtf[j] = wf[j];
|
}
|
|
/** Compute norm of scaled gradient and detect degeneracy. **/
|
gnorm = 0;
|
for (j = 0; j < n; j++) {
|
if (wa2[Pivot[j]] == 0)
|
continue;
|
sum = 0;
|
for (i = 0; i <= j; i++)
|
sum += fjac[j*m+i] * qtf[i];
|
gnorm = MAX(gnorm, fabs(sum / wa2[Pivot[j]] / fnorm));
|
}
|
|
if (gnorm <= C->gtol) {
|
S->outcome = 4;
|
goto terminate;
|
}
|
|
/** Initialize or update diag and delta. **/
|
if (!outer) { /* first iteration only */
|
if (C->scale_diag) {
|
/* diag := norms of the columns of the initial Jacobian */
|
for (j = 0; j < n; j++)
|
diag[j] = wa2[j] ? wa2[j] : 1;
|
/* xnorm := || D x || */
|
for (j = 0; j < n; j++)
|
wa3[j] = diag[j] * x[j];
|
xnorm = lm_enorm(n, wa3);
|
if (C->verbosity >= 2) {
|
fprintf(msgfile, "lmmin diag ");
|
lm_print_pars(nout, x, msgfile); // xnorm
|
fprintf(msgfile, " xnorm = %18.8g\n", xnorm);
|
}
|
/* Only now print the header for the loop table. */
|
if (C->verbosity >= 3) {
|
fprintf(msgfile, " o i lmpar prered"
|
" ratio dirder delta"
|
" pnorm fnorm");
|
for (i = 0; i < nout; ++i)
|
fprintf(msgfile, " p%i", i);
|
fprintf(msgfile, "\n");
|
}
|
} else {
|
xnorm = lm_enorm(n, x);
|
}
|
if (!isfinite(xnorm)) {
|
S->outcome = 12; /* nan */
|
goto terminate;
|
}
|
/* Initialize the step bound delta. */
|
if (xnorm)
|
delta = C->stepbound * xnorm;
|
else
|
delta = C->stepbound;
|
} else {
|
if (C->scale_diag) {
|
for (j = 0; j < n; j++)
|
diag[j] = MAX(diag[j], wa2[j]);
|
}
|
}
|
|
/** The inner loop. **/
|
int inner = 0;
|
do {
|
|
/** Determine the Levenberg-Marquardt parameter. **/
|
lm_lmpar(n, fjac, m, Pivot, diag, qtf, delta, &lmpar,
|
wa1, wa2, wf, wa3);
|
/* used return values are fjac (partly), lmpar, wa1=x, wa3=diag*x */
|
|
/* Predict scaled reduction. */
|
pnorm = lm_enorm(n, wa3);
|
if (!isfinite(pnorm)) {
|
S->outcome = 12; /* nan */
|
goto terminate;
|
}
|
temp2 = lmpar * SQR(pnorm / fnorm);
|
for (j = 0; j < n; j++) {
|
wa3[j] = 0;
|
for (i = 0; i <= j; i++)
|
wa3[i] -= fjac[j*m+i] * wa1[Pivot[j]];
|
}
|
temp1 = SQR(lm_enorm(n, wa3) / fnorm);
|
if (!isfinite(temp1)) {
|
S->outcome = 12; /* nan */
|
goto terminate;
|
}
|
prered = temp1 + 2*temp2;
|
dirder = -temp1 + temp2; /* scaled directional derivative */
|
|
/* At first call, adjust the initial step bound. */
|
if (!outer && pnorm < delta)
|
delta = pnorm;
|
|
/** Evaluate the function at x + p. **/
|
for (j = 0; j < n; j++)
|
wa2[j] = x[j] - wa1[j];
|
(*evaluate)(wa2, m, data, wf, &(S->userbreak));
|
++(S->nfev);
|
if (S->userbreak)
|
goto terminate;
|
fnorm1 = lm_enorm(m, wf);
|
if (!isfinite(fnorm1)) {
|
S->outcome = 12; /* nan */
|
goto terminate;
|
}
|
|
/** Evaluate the scaled reduction. **/
|
|
/* Actual scaled reduction. */
|
actred = 1 - SQR(fnorm1 / fnorm);
|
|
/* Ratio of actual to predicted reduction. */
|
ratio = prered ? actred / prered : 0;
|
|
if (C->verbosity == 2) {
|
fprintf(msgfile, "lmmin (%i:%i) ", outer, inner);
|
lm_print_pars(nout, wa2, msgfile); // fnorm1,
|
} else if (C->verbosity >= 3) {
|
printf("%3i %2i %9.2g %9.2g %14.6g"
|
" %9.2g %10.3e %10.3e %21.15e",
|
outer, inner, lmpar, prered, ratio,
|
dirder, delta, pnorm, fnorm1);
|
for (i = 0; i < nout; ++i)
|
fprintf(msgfile, " %16.9g", wa2[i]);
|
fprintf(msgfile, "\n");
|
}
|
|
/* Update the step bound. */
|
if (ratio <= 0.25) {
|
if (actred >= 0)
|
temp = 0.5;
|
else if (actred > -99) /* -99 = 1-1/0.1^2 */
|
temp = MAX(dirder / (2*dirder + actred), 0.1);
|
else
|
temp = 0.1;
|
delta = temp * MIN(delta, pnorm / 0.1);
|
lmpar /= temp;
|
} else if (ratio >= 0.75) {
|
delta = 2 * pnorm;
|
lmpar *= 0.5;
|
} else if (!lmpar) {
|
delta = 2 * pnorm;
|
}
|
|
/** On success, update solution, and test for convergence. **/
|
|
inner_success = ratio >= 1e-4;
|
if (inner_success) {
|
|
/* Update x, fvec, and their norms. */
|
if (C->scale_diag) {
|
for (j = 0; j < n; j++) {
|
x[j] = wa2[j];
|
wa2[j] = diag[j] * x[j];
|
}
|
} else {
|
for (j = 0; j < n; j++)
|
x[j] = wa2[j];
|
}
|
for (i = 0; i < m; i++)
|
fvec[i] = wf[i];
|
xnorm = lm_enorm(n, wa2);
|
if (!isfinite(xnorm)) {
|
S->outcome = 12; /* nan */
|
goto terminate;
|
}
|
fnorm = fnorm1;
|
}
|
|
/* Convergence tests. */
|
S->outcome = 0;
|
if (fnorm <= LM_DWARF)
|
goto terminate; /* success: sum of squares almost zero */
|
/* Test two criteria (both may be fulfilled). */
|
if (fabs(actred) <= C->ftol && prered <= C->ftol && ratio <= 2)
|
S->outcome = 1; /* success: x almost stable */
|
if (delta <= C->xtol * xnorm)
|
S->outcome += 2; /* success: sum of squares almost stable */
|
if (S->outcome != 0) {
|
goto terminate;
|
}
|
|
/** Tests for termination and stringent tolerances. **/
|
if (S->nfev >= maxfev) {
|
S->outcome = 5;
|
goto terminate;
|
}
|
if (fabs(actred) <= LM_MACHEP && prered <= LM_MACHEP &&
|
ratio <= 2) {
|
S->outcome = 6;
|
goto terminate;
|
}
|
if (delta <= LM_MACHEP * xnorm) {
|
S->outcome = 7;
|
goto terminate;
|
}
|
if (gnorm <= LM_MACHEP) {
|
S->outcome = 8;
|
goto terminate;
|
}
|
|
/** End of the inner loop. Repeat if iteration unsuccessful. **/
|
++inner;
|
} while (!inner_success);
|
|
}; /*** End of the outer loop. ***/
|
|
terminate:
|
S->fnorm = lm_enorm(m, fvec);
|
if (C->verbosity >= 2)
|
printf("lmmin outcome (%i) xnorm %g ftol %g xtol %g\n", S->outcome,
|
xnorm, C->ftol, C->xtol);
|
if (C->verbosity & 1) {
|
fprintf(msgfile, "lmmin final ");
|
lm_print_pars(nout, x, msgfile); // S->fnorm,
|
fprintf(msgfile, " fnorm = %18.8g\n", S->fnorm);
|
}
|
if (S->userbreak) /* user-requested break */
|
S->outcome = 11;
|
|
/*** Deallocate the workspace. ***/
|
free(ws);
|
|
} /*** lmmin. ***/
|
|
/******************************************************************************/
|
/* lm_lmpar (determine Levenberg-Marquardt parameter) */
|
/******************************************************************************/
|
|
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. ***/
|
|
/******************************************************************************/
|
/* lm_qrfac (QR factorization, from lapack) */
|
/******************************************************************************/
|
|
void lm_qrfac(const int m, const int n, double* A, int* Pivot, double* Rdiag,
|
double* Acnorm, double* W)
|
/*
|
* This subroutine uses Householder transformations with column pivoting
|
* 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,...,n, is of the form
|
*
|
* I - 2*w*wT/|w|^2
|
*
|
* where w has zeroes in the first k-1 positions.
|
*
|
* Parameters:
|
*
|
* m is an INPUT parameter set to the number of rows of A.
|
*
|
* n is an INPUT parameter 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 vectors w described above).
|
*
|
* Pivot is an integer OUTPUT array of length n that describes the
|
* permutation matrix P. Column j of P is column Pivot(j) of the
|
* identity matrix.
|
*
|
* 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 share storage with Rdiag.
|
*
|
* W is a work array of length n.
|
*
|
*/
|
{
|
int i, j, k, kmax;
|
double ajnorm, sum, temp;
|
|
#ifdef LMFIT_DEBUG_MESSAGES
|
printf("debug qrfac\n");
|
#endif
|
|
/** Compute initial column norms;
|
initialize Pivot with identity permutation. ***/
|
for (j = 0; j < n; j++) {
|
W[j] = Rdiag[j] = Acnorm[j] = lm_enorm(m, &A[j*m]);
|
Pivot[j] = j;
|
}
|
|
/** Loop over columns of A. **/
|
assert(n <= m);
|
for (j = 0; j < n; j++) {
|
|
/** Bring the column of largest norm into the pivot position. **/
|
kmax = j;
|
for (k = j+1; k < n; k++)
|
if (Rdiag[k] > Rdiag[kmax])
|
kmax = k;
|
|
if (kmax != j) {
|
/* Swap columns j and kmax. */
|
k = Pivot[j];
|
Pivot[j] = Pivot[kmax];
|
Pivot[kmax] = k;
|
for (i = 0; i < m; i++) {
|
temp = A[j*m+i];
|
A[j*m+i] = A[kmax*m+i];
|
A[kmax*m+i] = temp;
|
}
|
/* Half-swap: Rdiag[j], W[j] won't be needed any further. */
|
Rdiag[kmax] = Rdiag[j];
|
W[kmax] = W[j];
|
}
|
|
/** Compute the Householder reflection vector w_j to reduce the
|
j-th column of A to a multiple of the j-th unit vector. **/
|
ajnorm = lm_enorm(m-j, &A[j*m+j]);
|
if (ajnorm == 0) {
|
Rdiag[j] = 0;
|
continue;
|
}
|
|
/* Let the partial column vector A[j][j:] contain w_j := e_j+-a_j/|a_j|,
|
where the sign +- is chosen to avoid cancellation in w_jj. */
|
if (A[j*m+j] < 0)
|
ajnorm = -ajnorm;
|
for (i = j; i < m; i++)
|
A[j*m+i] /= ajnorm;
|
A[j*m+j] += 1;
|
|
/** Apply the Householder transformation U_w := 1 - 2*w_j.w_j/|w_j|^2
|
to the remaining columns, and update the norms. **/
|
for (k = j+1; k < n; k++) {
|
/* Compute scalar product w_j * a_j. */
|
sum = 0;
|
for (i = j; i < m; i++)
|
sum += A[j*m+i] * A[k*m+i];
|
|
/* Normalization is simplified by the coincidence |w_j|^2=2w_jj. */
|
temp = sum / A[j*m+j];
|
|
/* Carry out transform U_w_j * a_k. */
|
for (i = j; i < m; i++)
|
A[k*m+i] -= temp * A[j*m+i];
|
|
/* No idea what happens here. */
|
if (Rdiag[k] != 0) {
|
temp = A[m*k+j] / Rdiag[k];
|
if (fabs(temp) < 1) {
|
Rdiag[k] *= sqrt(1 - SQR(temp));
|
temp = Rdiag[k] / W[k];
|
} else
|
temp = 0;
|
if (temp == 0 || 0.05 * SQR(temp) <= LM_MACHEP) {
|
Rdiag[k] = lm_enorm(m-j-1, &A[m*k+j+1]);
|
W[k] = Rdiag[k];
|
}
|
}
|
}
|
|
Rdiag[j] = -ajnorm;
|
}
|
} /*** lm_qrfac. ***/
|
|
/******************************************************************************/
|
/* lm_qrsolv (linear least-squares) */
|
/******************************************************************************/
|
|
void lm_qrsolv(const int n, double* r, const int ldr, const int* Pivot,
|
const double* diag, const double* qtb, double* x,
|
double* Sdiag, double* W)
|
/*
|
* 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 and D*x = 0
|
*
|
* in the least squares sense.
|
*
|
* 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 qrsolv expects the full upper triangle of R, the permutation
|
* matrix P, and the first n components of Q^T*b. The system
|
* A*x = b, D*x = 0, is then equivalent to
|
*
|
* R*z = Q^T*b, P^T*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
|
*
|
* P^T*(A^T*A + D*D)*P = S^T*S.
|
*
|
* S is computed within qrsolv and may be of separate interest.
|
*
|
* 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.
|
*
|
* 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.
|
*
|
* W is a work array of length n.
|
*
|
*/
|
{
|
int i, kk, j, k, nsing;
|
double qtbpj, sum, temp;
|
double _sin, _cos, _tan, _cot; /* local variables, not functions */
|
|
/*** Copy R and Q^T*b to preserve input and initialize S.
|
In particular, save the diagonal elements of R in x. ***/
|
|
for (j = 0; j < n; j++) {
|
for (i = j; i < n; i++)
|
r[j*ldr+i] = r[i*ldr+j];
|
x[j] = r[j*ldr+j];
|
W[j] = qtb[j];
|
}
|
|
/*** Eliminate the diagonal matrix D using a Givens rotation. ***/
|
|
for (j = 0; j < n; j++) {
|
|
/*** Prepare the row of D to be eliminated, locating the diagonal
|
element using P from the QR factorization. ***/
|
|
if (diag[Pivot[j]] != 0) {
|
for (k = j; k < n; k++)
|
Sdiag[k] = 0;
|
Sdiag[j] = diag[Pivot[j]];
|
|
/*** The transformations to eliminate the row of D modify only
|
a single element of Q^T*b beyond the first n, which is
|
initially 0. ***/
|
|
qtbpj = 0;
|
for (k = j; k < n; k++) {
|
|
/** Determine a Givens rotation which eliminates the
|
appropriate element in the current row of D. **/
|
if (Sdiag[k] == 0)
|
continue;
|
kk = k + ldr * k;
|
if (fabs(r[kk]) < fabs(Sdiag[k])) {
|
_cot = r[kk] / Sdiag[k];
|
_sin = 1 / hypot(1, _cot);
|
_cos = _sin * _cot;
|
} else {
|
_tan = Sdiag[k] / r[kk];
|
_cos = 1 / hypot(1, _tan);
|
_sin = _cos * _tan;
|
}
|
|
/** Compute the modified diagonal element of R and
|
the modified element of (Q^T*b,0). **/
|
r[kk] = _cos * r[kk] + _sin * Sdiag[k];
|
temp = _cos * W[k] + _sin * qtbpj;
|
qtbpj = -_sin * W[k] + _cos * qtbpj;
|
W[k] = temp;
|
|
/** Accumulate the tranformation in the row of S. **/
|
for (i = k+1; i < n; i++) {
|
temp = _cos * r[k*ldr+i] + _sin * Sdiag[i];
|
Sdiag[i] = -_sin * r[k*ldr+i] + _cos * Sdiag[i];
|
r[k*ldr+i] = temp;
|
}
|
}
|
}
|
|
/** Store the diagonal element of S and restore
|
the corresponding diagonal element of R. **/
|
Sdiag[j] = r[j*ldr+j];
|
r[j*ldr+j] = x[j];
|
}
|
|
/*** Solve the triangular system for z. If the system is singular, then
|
obtain a least-squares solution. ***/
|
|
nsing = n;
|
for (j = 0; j < n; j++) {
|
if (Sdiag[j] == 0 && nsing == n)
|
nsing = j;
|
if (nsing < n)
|
W[j] = 0;
|
}
|
|
for (j = nsing-1; j >= 0; j--) {
|
sum = 0;
|
for (i = j+1; i < nsing; i++)
|
sum += r[j*ldr+i] * W[i];
|
W[j] = (W[j] - sum) / Sdiag[j];
|
}
|
|
/*** Permute the components of z back to components of x. ***/
|
|
for (j = 0; j < n; j++)
|
x[Pivot[j]] = W[j];
|
|
} /*** lm_qrsolv. ***/
|
|
/******************************************************************************/
|
/* lm_enorm (Euclidean norm) */
|
/******************************************************************************/
|
|
double lm_enorm(int n, const double* x)
|
/* This function calculates the Euclidean norm of an n-vector x.
|
*
|
* The Euclidean norm is computed by accumulating the sum of squares
|
* in three different sums. The sums of squares for the small and large
|
* components are scaled so that no overflows occur. Non-destructive
|
* underflows are permitted. Underflows and overflows do not occur in
|
* the computation of the unscaled sum of squares for the intermediate
|
* components. The definitions of small, intermediate and large components
|
* depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. The main
|
* restrictions on these constants are that LM_SQRT_DWARF**2 not underflow
|
* and LM_SQRT_GIANT**2 not overflow.
|
*
|
* Parameters:
|
*
|
* n is a positive integer INPUT variable.
|
*
|
* x is an INPUT array of length n.
|
*/
|
{
|
int i;
|
double agiant, s1, s2, s3, xabs, x1max, x3max;
|
|
s1 = 0;
|
s2 = 0;
|
s3 = 0;
|
x1max = 0;
|
x3max = 0;
|
agiant = LM_SQRT_GIANT / n;
|
|
/** Sum squares. **/
|
for (i = 0; i < n; i++) {
|
xabs = fabs(x[i]);
|
if (xabs > LM_SQRT_DWARF) {
|
if (xabs < agiant) {
|
s2 += SQR(xabs);
|
} else if (xabs > x1max) {
|
s1 = 1 + s1 * SQR(x1max / xabs);
|
x1max = xabs;
|
} else {
|
s1 += SQR(xabs / x1max);
|
}
|
} else if (xabs > x3max) {
|
s3 = 1 + s3 * SQR(x3max / xabs);
|
x3max = xabs;
|
} else if (xabs != 0) {
|
s3 += SQR(xabs / x3max);
|
}
|
}
|
|
/** Calculate the norm. **/
|
if (s1 != 0)
|
return x1max * sqrt(s1 + (s2 / x1max) / x1max);
|
else if (s2 != 0)
|
if (s2 >= x3max)
|
return sqrt(s2 * (1 + (x3max / s2) * (x3max * s3)));
|
else
|
return sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
|
else
|
return x3max * sqrt(s3);
|
|
} /*** lm_enorm. ***/
|
