| 1 |
/* |
| 2 |
* Library: lmfit (Levenberg-Marquardt least squares fitting) |
| 3 |
* |
| 4 |
* File: lmmin.c |
| 5 |
* |
| 6 |
* Contents: Levenberg-Marquardt minimization. |
| 7 |
* |
| 8 |
* Copyright: MINPACK authors, The University of Chikago (1980-1999) |
| 9 |
* Joachim Wuttke, Forschungszentrum Juelich GmbH (2004-2013) |
| 10 |
* |
| 11 |
* License: see ../COPYING (FreeBSD) |
| 12 |
* |
| 13 |
* Homepage: apps.jcns.fz-juelich.de/lmfit |
| 14 |
*/ |
| 15 |
|
| 16 |
#include <assert.h> |
| 17 |
#include <stdlib.h> |
| 18 |
#include <stdio.h> |
| 19 |
#include <math.h> |
| 20 |
#include <float.h> |
| 21 |
#include "lmmin.h" |
| 22 |
|
| 23 |
#define MIN(a,b) (((a)<=(b)) ? (a) : (b)) |
| 24 |
#define MAX(a,b) (((a)>=(b)) ? (a) : (b)) |
| 25 |
#define SQR(x) (x)*(x) |
| 26 |
|
| 27 |
/* Declare functions that do the heavy numerics. |
| 28 |
Implementions are in this source file, below lmmin. |
| 29 |
Dependences: lmmin calls qrfac and lmpar; lmpar calls qrsolv. */ |
| 30 |
void lm_lmpar( |
| 31 |
const int n, double *const r, const int ldr, int *const ipvt, |
| 32 |
double *const diag, double *const qtb, double delta, double *const par, |
| 33 |
double *const x, |
| 34 |
double *const sdiag, double *const aux, double *const xdi); |
| 35 |
void lm_qrfac( |
| 36 |
const int m, const int n, double *const a, int *const ipvt, |
| 37 |
double *const rdiag, double *const acnorm, double *const wa ); |
| 38 |
void lm_qrsolv( |
| 39 |
const int n, double *const r, const int ldr, int *const ipvt, |
| 40 |
double *const diag, double *const qtb, double *const x, |
| 41 |
double *const sdiag, double *const wa); |
| 42 |
|
| 43 |
|
| 44 |
/*****************************************************************************/ |
| 45 |
/* Numeric constants */ |
| 46 |
/*****************************************************************************/ |
| 47 |
|
| 48 |
/* machine-dependent constants from float.h */ |
| 49 |
#define LM_MACHEP DBL_EPSILON /* resolution of arithmetic */ |
| 50 |
#define LM_DWARF DBL_MIN /* smallest nonzero number */ |
| 51 |
#define LM_SQRT_DWARF sqrt(DBL_MIN) /* square should not underflow */ |
| 52 |
#define LM_SQRT_GIANT sqrt(DBL_MAX) /* square should not overflow */ |
| 53 |
#define LM_USERTOL 30*LM_MACHEP /* users are recommended to require this */ |
| 54 |
|
| 55 |
/* If the above values do not work, the following seem good for an x86: |
| 56 |
LM_MACHEP .555e-16 |
| 57 |
LM_DWARF 9.9e-324 |
| 58 |
LM_SQRT_DWARF 1.e-160 |
| 59 |
LM_SQRT_GIANT 1.e150 |
| 60 |
LM_USER_TOL 1.e-14 |
| 61 |
The following values should work on any machine: |
| 62 |
LM_MACHEP 1.2e-16 |
| 63 |
LM_DWARF 1.0e-38 |
| 64 |
LM_SQRT_DWARF 3.834e-20 |
| 65 |
LM_SQRT_GIANT 1.304e19 |
| 66 |
LM_USER_TOL 1.e-14 |
| 67 |
*/ |
| 68 |
|
| 69 |
const lm_control_struct lm_control_double = { |
| 70 |
LM_USERTOL, LM_USERTOL, LM_USERTOL, LM_USERTOL, 100., 100, 1, |
| 71 |
NULL, 0, -1, -1 }; |
| 72 |
const lm_control_struct lm_control_float = { |
| 73 |
1.e-7, 1.e-7, 1.e-7, 1.e-7, 100., 100, 1, |
| 74 |
NULL, 0, -1, -1 }; |
| 75 |
|
| 76 |
|
| 77 |
/*****************************************************************************/ |
| 78 |
/* Message texts (indexed by status.info) */ |
| 79 |
/*****************************************************************************/ |
| 80 |
|
| 81 |
const char *lm_infmsg[] = { |
| 82 |
"found zero (sum of squares below underflow limit)", |
| 83 |
"converged (the relative error in the sum of squares is at most tol)", |
| 84 |
"converged (the relative error of the parameter vector is at most tol)", |
| 85 |
"converged (both errors are at most tol)", |
| 86 |
"trapped (by degeneracy; increasing epsilon might help)", |
| 87 |
"exhausted (number of function calls exceeding preset patience)", |
| 88 |
"failed (ftol<tol: cannot reduce sum of squares any further)", |
| 89 |
"failed (xtol<tol: cannot improve approximate solution any further)", |
| 90 |
"failed (gtol<tol: cannot improve approximate solution any further)", |
| 91 |
"crashed (not enough memory)", |
| 92 |
"exploded (fatal coding error: improper input parameters)", |
| 93 |
"stopped (break requested within function evaluation)", |
| 94 |
"found nan (function value is not-a-number or infinite)", |
| 95 |
"won't fit (no free parameter)" |
| 96 |
}; |
| 97 |
|
| 98 |
const char *lm_shortmsg[] = { |
| 99 |
"found zero", // 0 |
| 100 |
"converged (f)", // 1 |
| 101 |
"converged (p)", // 2 |
| 102 |
"converged (2)", // 3 |
| 103 |
"degenerate", // 4 |
| 104 |
"call limit", // 5 |
| 105 |
"failed (f)", // 6 |
| 106 |
"failed (p)", // 7 |
| 107 |
"failed (o)", // 8 |
| 108 |
"no memory", // 9 |
| 109 |
"invalid input", // 10 |
| 110 |
"user break", // 11 |
| 111 |
"found nan", // 12 |
| 112 |
"no free par" // 13 |
| 113 |
}; |
| 114 |
|
| 115 |
|
| 116 |
/*****************************************************************************/ |
| 117 |
/* Monitoring auxiliaries. */ |
| 118 |
/*****************************************************************************/ |
| 119 |
|
| 120 |
void lm_print_pars(const int nout, const double *par, FILE* fout) |
| 121 |
{ |
| 122 |
fprintf(fout, " pars:"); |
| 123 |
for (int i = 0; i < nout; ++i) |
| 124 |
fprintf(fout, " %23.16g", par[i]); |
| 125 |
fprintf(fout, "\n"); |
| 126 |
} |
| 127 |
|
| 128 |
|
| 129 |
/*****************************************************************************/ |
| 130 |
/* lmmin (main minimization routine) */ |
| 131 |
/*****************************************************************************/ |
| 132 |
|
| 133 |
void lmmin( |
| 134 |
const int n, double *const x, const int m, const double* y, |
| 135 |
const void *const data, |
| 136 |
void (*const evaluate)( |
| 137 |
const double *const par, const int m_dat, const void *const data, |
| 138 |
double *const fvec, int *const userbreak), |
| 139 |
const lm_control_struct *const C, lm_status_struct *const S) |
| 140 |
{ |
| 141 |
int j, i; |
| 142 |
double actred, dirder, fnorm, fnorm1, gnorm, pnorm, |
| 143 |
prered, ratio, step, sum, temp, temp1, temp2, temp3; |
| 144 |
static double p1 = 0.1, p0001 = 1.0e-4; |
| 145 |
|
| 146 |
int maxfev = C->patience * (n+1); |
| 147 |
|
| 148 |
int inner_success; /* flag for loop control */ |
| 149 |
double lmpar = 0; /* Levenberg-Marquardt parameter */ |
| 150 |
double delta = 0; |
| 151 |
double xnorm = 0; |
| 152 |
double eps = sqrt(MAX(C->epsilon, LM_MACHEP)); /* for forward differences */ |
| 153 |
|
| 154 |
int nout = C->n_maxpri==-1 ? n : MIN(C->n_maxpri, n); |
| 155 |
|
| 156 |
/* The workaround msgfile=NULL is needed for default initialization */ |
| 157 |
FILE* msgfile = C->msgfile ? C->msgfile : stdout; |
| 158 |
|
| 159 |
/* Default status info; must be set ahead of first return statements */ |
| 160 |
S->outcome = 0; /* status code */ |
| 161 |
S->userbreak = 0; |
| 162 |
S->nfev = 0; /* function evaluation counter */ |
| 163 |
|
| 164 |
/*** Check input parameters for errors. ***/ |
| 165 |
|
| 166 |
if ( n < 0 ) { |
| 167 |
fprintf(stderr, "lmmin: invalid number of parameters %i\n", n); |
| 168 |
S->outcome = 10; /* invalid parameter */ |
| 169 |
return; |
| 170 |
} |
| 171 |
if (m < n) { |
| 172 |
fprintf(stderr, "lmmin: number of data points (%i) " |
| 173 |
"smaller than number of parameters (%i)\n", m, n); |
| 174 |
S->outcome = 10; |
| 175 |
return; |
| 176 |
} |
| 177 |
if (C->ftol < 0 || C->xtol < 0 || C->gtol < 0) { |
| 178 |
fprintf(stderr, |
| 179 |
"lmmin: negative tolerance (at least one of %g %g %g)\n", |
| 180 |
C->ftol, C->xtol, C->gtol); |
| 181 |
S->outcome = 10; |
| 182 |
return; |
| 183 |
} |
| 184 |
if (maxfev <= 0) { |
| 185 |
fprintf(stderr, "lmmin: nonpositive function evaluations limit %i\n", |
| 186 |
maxfev); |
| 187 |
S->outcome = 10; |
| 188 |
return; |
| 189 |
} |
| 190 |
if (C->stepbound <= 0) { |
| 191 |
fprintf(stderr, "lmmin: nonpositive stepbound %g\n", C->stepbound); |
| 192 |
S->outcome = 10; |
| 193 |
return; |
| 194 |
} |
| 195 |
if (C->scale_diag != 0 && C->scale_diag != 1) { |
| 196 |
fprintf(stderr, "lmmin: logical variable scale_diag=%i, " |
| 197 |
"should be 0 or 1\n", C->scale_diag); |
| 198 |
S->outcome = 10; |
| 199 |
return; |
| 200 |
} |
| 201 |
|
| 202 |
/*** Allocate work space. ***/ |
| 203 |
|
| 204 |
/* Allocate total workspace with just one system call */ |
| 205 |
char *ws; |
| 206 |
if ( ( ws = malloc( |
| 207 |
(2*m+5*n+m*n)*sizeof(double) + n*sizeof(int) ) ) == NULL ) { |
| 208 |
S->outcome = 9; |
| 209 |
return; |
| 210 |
} |
| 211 |
|
| 212 |
/* Assign workspace segments. */ |
| 213 |
char *pws = ws; |
| 214 |
double *fvec = (double*) pws; pws += m * sizeof(double)/sizeof(char); |
| 215 |
double *diag = (double*) pws; pws += n * sizeof(double)/sizeof(char); |
| 216 |
double *qtf = (double*) pws; pws += n * sizeof(double)/sizeof(char); |
| 217 |
double *fjac = (double*) pws; pws += n*m*sizeof(double)/sizeof(char); |
| 218 |
double *wa1 = (double*) pws; pws += n * sizeof(double)/sizeof(char); |
| 219 |
double *wa2 = (double*) pws; pws += n * sizeof(double)/sizeof(char); |
| 220 |
double *wa3 = (double*) pws; pws += n * sizeof(double)/sizeof(char); |
| 221 |
double *wf = (double*) pws; pws += m * sizeof(double)/sizeof(char); |
| 222 |
int *ipvt = (int*) pws; pws += n * sizeof(int) /sizeof(char); |
| 223 |
|
| 224 |
/* Initialize diag */ // TODO: check whether this is still needed |
| 225 |
if (!C->scale_diag) { |
| 226 |
for (j = 0; j < n; j++) |
| 227 |
diag[j] = 1.; |
| 228 |
} |
| 229 |
|
| 230 |
/*** Evaluate function at starting point and calculate norm. ***/ |
| 231 |
|
| 232 |
if( C->verbosity&1 ) |
| 233 |
fprintf(msgfile, "lmmin start (ftol=%g gtol=%g xtol=%g)\n", |
| 234 |
C->ftol, C->gtol, C->xtol); |
| 235 |
if( C->verbosity&2 ) |
| 236 |
lm_print_pars(nout, x, msgfile); |
| 237 |
(*evaluate)(x, m, data, fvec, &(S->userbreak)); |
| 238 |
if( C->verbosity&8 ){ |
| 239 |
if (y) |
| 240 |
for( i=0; i<m; ++i ) |
| 241 |
fprintf(msgfile, " i, f, y-f: %4i %18.8g %18.8g\n", |
| 242 |
i, fvec[i], y[i]-fvec[i]); |
| 243 |
else |
| 244 |
for( i=0; i<m; ++i ) |
| 245 |
fprintf(msgfile, " i, f: %4i %18.8g\n", i, fvec[i]); |
| 246 |
} |
| 247 |
S->nfev = 1; |
| 248 |
if ( S->userbreak ) |
| 249 |
goto terminate; |
| 250 |
if ( n == 0 ) { |
| 251 |
S->outcome = 13; /* won't fit */ |
| 252 |
goto terminate; |
| 253 |
} |
| 254 |
fnorm = lm_fnorm(m, fvec, y); |
| 255 |
if( C->verbosity&2 ) |
| 256 |
fprintf(msgfile, " fnorm = %24.16g\n", fnorm); |
| 257 |
if( !isfinite(fnorm) ){ |
| 258 |
if( C->verbosity ) |
| 259 |
fprintf(msgfile, "nan case 1\n"); |
| 260 |
S->outcome = 12; /* nan */ |
| 261 |
goto terminate; |
| 262 |
} else if( fnorm <= LM_DWARF ){ |
| 263 |
S->outcome = 0; /* sum of squares almost zero, nothing to do */ |
| 264 |
goto terminate; |
| 265 |
} |
| 266 |
|
| 267 |
/*** The outer loop: compute gradient, then descend. ***/ |
| 268 |
|
| 269 |
for( int outer=0; ; ++outer ) { |
| 270 |
|
| 271 |
/*** [outer] Calculate the Jacobian. ***/ |
| 272 |
|
| 273 |
for (j = 0; j < n; j++) { |
| 274 |
temp = x[j]; |
| 275 |
step = MAX(eps*eps, eps * fabs(temp)); |
| 276 |
x[j] += step; /* replace temporarily */ |
| 277 |
(*evaluate)(x, m, data, wf, &(S->userbreak)); |
| 278 |
++(S->nfev); |
| 279 |
if ( S->userbreak ) |
| 280 |
goto terminate; |
| 281 |
for (i = 0; i < m; i++) |
| 282 |
fjac[j*m+i] = (wf[i] - fvec[i]) / step; |
| 283 |
x[j] = temp; /* restore */ |
| 284 |
} |
| 285 |
if ( C->verbosity&16 ) { |
| 286 |
/* print the entire matrix */ |
| 287 |
printf("Jacobian\n"); |
| 288 |
for (i = 0; i < m; i++) { |
| 289 |
printf(" "); |
| 290 |
for (j = 0; j < n; j++) |
| 291 |
printf("%.5e ", fjac[j*m+i]); |
| 292 |
printf("\n"); |
| 293 |
} |
| 294 |
} |
| 295 |
|
| 296 |
/*** [outer] Compute the QR factorization of the Jacobian. ***/ |
| 297 |
|
| 298 |
/* fjac is an m by n array. The upper n by n submatrix of fjac |
| 299 |
* is made to contain an upper triangular matrix R with diagonal |
| 300 |
* elements of nonincreasing magnitude such that |
| 301 |
* |
| 302 |
* P^T*(J^T*J)*P = R^T*R |
| 303 |
* |
| 304 |
* (NOTE: ^T stands for matrix transposition), |
| 305 |
* |
| 306 |
* where P is a permutation matrix and J is the final calculated |
| 307 |
* Jacobian. Column j of P is column ipvt(j) of the identity matrix. |
| 308 |
* The lower trapezoidal part of fjac contains information generated |
| 309 |
* during the computation of R. |
| 310 |
* |
| 311 |
* ipvt is an integer array of length n. It defines a permutation |
| 312 |
* matrix P such that jac*P = Q*R, where jac is the final calculated |
| 313 |
* Jacobian, Q is orthogonal (not stored), and R is upper triangular |
| 314 |
* with diagonal elements of nonincreasing magnitude. Column j of P |
| 315 |
* is column ipvt(j) of the identity matrix. |
| 316 |
*/ |
| 317 |
|
| 318 |
lm_qrfac(m, n, fjac, ipvt, wa1, wa2, wa3); |
| 319 |
/* return values are ipvt, wa1=rdiag, wa2=acnorm */ |
| 320 |
|
| 321 |
/*** [outer] Form Q^T * fvec, and store first n components in qtf. ***/ |
| 322 |
|
| 323 |
if (y) |
| 324 |
for (i = 0; i < m; i++) |
| 325 |
wf[i] = fvec[i] - y[i]; |
| 326 |
else |
| 327 |
for (i = 0; i < m; i++) |
| 328 |
wf[i] = fvec[i]; |
| 329 |
|
| 330 |
for (j = 0; j < n; j++) { |
| 331 |
temp3 = fjac[j*m+j]; |
| 332 |
if (temp3 != 0) { |
| 333 |
sum = 0; |
| 334 |
for (i = j; i < m; i++) |
| 335 |
sum += fjac[j*m+i] * wf[i]; |
| 336 |
temp = -sum / temp3; |
| 337 |
for (i = j; i < m; i++) |
| 338 |
wf[i] += fjac[j*m+i] * temp; |
| 339 |
} |
| 340 |
fjac[j*m+j] = wa1[j]; |
| 341 |
qtf[j] = wf[j]; |
| 342 |
} |
| 343 |
|
| 344 |
/*** [outer] Compute norm of scaled gradient and detect degeneracy. ***/ |
| 345 |
|
| 346 |
gnorm = 0; |
| 347 |
for (j = 0; j < n; j++) { |
| 348 |
if (wa2[ipvt[j]] == 0) |
| 349 |
continue; |
| 350 |
sum = 0; |
| 351 |
for (i = 0; i <= j; i++) |
| 352 |
sum += fjac[j*m+i] * qtf[i]; |
| 353 |
gnorm = MAX(gnorm, fabs( sum / wa2[ipvt[j]] / fnorm )); |
| 354 |
} |
| 355 |
|
| 356 |
if (gnorm <= C->gtol) { |
| 357 |
S->outcome = 4; |
| 358 |
goto terminate; |
| 359 |
} |
| 360 |
|
| 361 |
/*** [outer] Initialize / update diag and delta. ***/ |
| 362 |
|
| 363 |
if ( !outer ) { |
| 364 |
/* first iteration only */ |
| 365 |
if (C->scale_diag) { |
| 366 |
/* diag := norms of the columns of the initial Jacobian */ |
| 367 |
for (j = 0; j < n; j++) |
| 368 |
diag[j] = wa2[j] ? wa2[j] : 1; |
| 369 |
/* xnorm := || D x || */ |
| 370 |
for (j = 0; j < n; j++) |
| 371 |
wa3[j] = diag[j] * x[j]; |
| 372 |
xnorm = lm_enorm(n, wa3); |
| 373 |
} else { |
| 374 |
xnorm = lm_enorm(n, x); |
| 375 |
} |
| 376 |
if( !isfinite(xnorm) ){ |
| 377 |
if( C->verbosity ) |
| 378 |
fprintf(msgfile, "nan case 2\n"); |
| 379 |
S->outcome = 12; /* nan */ |
| 380 |
goto terminate; |
| 381 |
} |
| 382 |
/* initialize the step bound delta. */ |
| 383 |
if ( xnorm ) |
| 384 |
delta = C->stepbound * xnorm; |
| 385 |
else |
| 386 |
delta = C->stepbound; |
| 387 |
/* only now print the header for the loop table */ |
| 388 |
if( C->verbosity&2 ) { |
| 389 |
fprintf(msgfile, " #o #i lmpar prered actred" |
| 390 |
" ratio dirder delta" |
| 391 |
" pnorm fnorm"); |
| 392 |
for (i = 0; i < nout; ++i) |
| 393 |
fprintf(msgfile, " p%i", i); |
| 394 |
fprintf(msgfile, "\n"); |
| 395 |
} |
| 396 |
} else { |
| 397 |
if (C->scale_diag) { |
| 398 |
for (j = 0; j < n; j++) |
| 399 |
diag[j] = MAX( diag[j], wa2[j] ); |
| 400 |
} |
| 401 |
} |
| 402 |
|
| 403 |
/*** The inner loop. ***/ |
| 404 |
int inner = 0; |
| 405 |
do { |
| 406 |
|
| 407 |
/*** [inner] Determine the Levenberg-Marquardt parameter. ***/ |
| 408 |
|
| 409 |
lm_lmpar(n, fjac, m, ipvt, diag, qtf, delta, &lmpar, |
| 410 |
wa1, wa2, wf, wa3); |
| 411 |
/* used return values are fjac (partly), lmpar, wa1=x, wa3=diag*x */ |
| 412 |
|
| 413 |
/* predict scaled reduction */ |
| 414 |
pnorm = lm_enorm(n, wa3); |
| 415 |
if( !isfinite(pnorm) ){ |
| 416 |
if( C->verbosity ) |
| 417 |
fprintf(msgfile, "nan case 3\n"); |
| 418 |
S->outcome = 12; /* nan */ |
| 419 |
goto terminate; |
| 420 |
} |
| 421 |
temp2 = lmpar * SQR( pnorm / fnorm ); |
| 422 |
for (j = 0; j < n; j++) { |
| 423 |
wa3[j] = 0; |
| 424 |
for (i = 0; i <= j; i++) |
| 425 |
wa3[i] -= fjac[j*m+i] * wa1[ipvt[j]]; |
| 426 |
} |
| 427 |
temp1 = SQR( lm_enorm(n, wa3) / fnorm ); |
| 428 |
if( !isfinite(temp1) ){ |
| 429 |
if( C->verbosity ) |
| 430 |
fprintf(msgfile, "nan case 4\n"); |
| 431 |
S->outcome = 12; /* nan */ |
| 432 |
goto terminate; |
| 433 |
} |
| 434 |
prered = temp1 + 2 * temp2; |
| 435 |
dirder = -temp1 + temp2; /* scaled directional derivative */ |
| 436 |
|
| 437 |
/* at first call, adjust the initial step bound. */ |
| 438 |
if ( !outer && !inner && pnorm < delta ) |
| 439 |
delta = pnorm; |
| 440 |
|
| 441 |
/*** [inner] Evaluate the function at x + p. ***/ |
| 442 |
|
| 443 |
for (j = 0; j < n; j++) |
| 444 |
wa2[j] = x[j] - wa1[j]; |
| 445 |
|
| 446 |
(*evaluate)( wa2, m, data, wf, &(S->userbreak) ); |
| 447 |
++(S->nfev); |
| 448 |
if ( S->userbreak ) |
| 449 |
goto terminate; |
| 450 |
fnorm1 = lm_fnorm(m, wf, y); |
| 451 |
// exceptionally, for this norm we do not test for infinity |
| 452 |
// because we can deal with it without terminating. |
| 453 |
|
| 454 |
/*** [inner] Evaluate the scaled reduction. ***/ |
| 455 |
|
| 456 |
/* actual scaled reduction (supports even the case fnorm1=infty) */ |
| 457 |
if (p1 * fnorm1 < fnorm) |
| 458 |
actred = 1 - SQR(fnorm1 / fnorm); |
| 459 |
else |
| 460 |
actred = -1; |
| 461 |
|
| 462 |
/* ratio of actual to predicted reduction */ |
| 463 |
ratio = prered ? actred/prered : 0; |
| 464 |
|
| 465 |
if( C->verbosity&32 ){ |
| 466 |
if (y) |
| 467 |
for( i=0; i<m; ++i ) |
| 468 |
fprintf(msgfile, " i, f, y-f: %4i %18.8g %18.8g\n", |
| 469 |
i, fvec[i], y[i]-fvec[i]); |
| 470 |
else |
| 471 |
for( i=0; i<m; ++i ) |
| 472 |
fprintf(msgfile, " i, f, y-f: %4i %18.8g\n", |
| 473 |
i, fvec[i]); |
| 474 |
} |
| 475 |
if( C->verbosity&2 ) { |
| 476 |
printf("%3i %2i %9.2g %9.2g %9.2g %14.6g" |
| 477 |
" %9.2g %10.3e %10.3e %21.15e", |
| 478 |
outer, inner, lmpar, prered, actred, ratio, |
| 479 |
dirder, delta, pnorm, fnorm1); |
| 480 |
for (i = 0; i < nout; ++i) |
| 481 |
fprintf(msgfile, " %16.9g", wa2[i]); |
| 482 |
fprintf(msgfile, "\n"); |
| 483 |
} |
| 484 |
|
| 485 |
/* update the step bound */ |
| 486 |
if (ratio <= 0.25) { |
| 487 |
if (actred >= 0) |
| 488 |
temp = 0.5; |
| 489 |
else |
| 490 |
temp = 0.5 * dirder / (dirder + 0.5 * actred); |
| 491 |
if (p1 * fnorm1 >= fnorm || temp < p1) |
| 492 |
temp = p1; |
| 493 |
delta = temp * MIN(delta, pnorm / p1); |
| 494 |
lmpar /= temp; |
| 495 |
} else if (lmpar == 0 || ratio >= 0.75) { |
| 496 |
delta = 2 * pnorm; |
| 497 |
lmpar *= 0.5; |
| 498 |
} |
| 499 |
|
| 500 |
/*** [inner] On success, update solution, and test for convergence. ***/ |
| 501 |
|
| 502 |
inner_success = ratio >= p0001; |
| 503 |
if ( inner_success ) { |
| 504 |
|
| 505 |
/* update x, fvec, and their norms */ |
| 506 |
if (C->scale_diag) { |
| 507 |
for (j = 0; j < n; j++) { |
| 508 |
x[j] = wa2[j]; |
| 509 |
wa2[j] = diag[j] * x[j]; |
| 510 |
} |
| 511 |
} else { |
| 512 |
for (j = 0; j < n; j++) |
| 513 |
x[j] = wa2[j]; |
| 514 |
} |
| 515 |
for (i = 0; i < m; i++) |
| 516 |
fvec[i] = wf[i]; |
| 517 |
xnorm = lm_enorm(n, wa2); |
| 518 |
if( !isfinite(xnorm) ){ |
| 519 |
if( C->verbosity ) |
| 520 |
fprintf(msgfile, "nan case 6\n"); |
| 521 |
S->outcome = 12; /* nan */ |
| 522 |
goto terminate; |
| 523 |
} |
| 524 |
fnorm = fnorm1; |
| 525 |
} |
| 526 |
|
| 527 |
/* convergence tests */ |
| 528 |
S->outcome = 0; |
| 529 |
if( fnorm<=LM_DWARF ) |
| 530 |
goto terminate; /* success: sum of squares almost zero */ |
| 531 |
/* test two criteria (both may be fulfilled) */ |
| 532 |
if (fabs(actred) <= C->ftol && prered <= C->ftol && ratio <= 2) |
| 533 |
S->outcome = 1; /* success: x almost stable */ |
| 534 |
if (delta <= C->xtol * xnorm) |
| 535 |
S->outcome += 2; /* success: sum of squares almost stable */ |
| 536 |
if (S->outcome != 0) { |
| 537 |
goto terminate; |
| 538 |
} |
| 539 |
|
| 540 |
/*** [inner] Tests for termination and stringent tolerances. ***/ |
| 541 |
|
| 542 |
if ( S->nfev >= maxfev ){ |
| 543 |
S->outcome = 5; |
| 544 |
goto terminate; |
| 545 |
} |
| 546 |
if ( fabs(actred) <= LM_MACHEP && |
| 547 |
prered <= LM_MACHEP && ratio <= 2 ){ |
| 548 |
S->outcome = 6; |
| 549 |
goto terminate; |
| 550 |
} |
| 551 |
if ( delta <= LM_MACHEP*xnorm ){ |
| 552 |
S->outcome = 7; |
| 553 |
goto terminate; |
| 554 |
} |
| 555 |
if ( gnorm <= LM_MACHEP ){ |
| 556 |
S->outcome = 8; |
| 557 |
goto terminate; |
| 558 |
} |
| 559 |
|
| 560 |
/*** [inner] End of the loop. Repeat if iteration unsuccessful. ***/ |
| 561 |
|
| 562 |
++inner; |
| 563 |
} while ( !inner_success ); |
| 564 |
|
| 565 |
/*** [outer] End of the loop. ***/ |
| 566 |
|
| 567 |
}; |
| 568 |
|
| 569 |
terminate: |
| 570 |
S->fnorm = lm_fnorm(m, fvec, y); |
| 571 |
if( C->verbosity&1 ) |
| 572 |
fprintf(msgfile, "lmmin terminates with outcome %i\n", S->outcome); |
| 573 |
if( C->verbosity&2 ) |
| 574 |
lm_print_pars(nout, x, msgfile); |
| 575 |
if( C->verbosity&8 ){ |
| 576 |
if (y) |
| 577 |
for( i=0; i<m; ++i ) |
| 578 |
fprintf(msgfile, " i, f, y-f: %4i %18.8g %18.8g\n", |
| 579 |
i, fvec[i], y[i]-fvec[i] ); |
| 580 |
else |
| 581 |
for( i=0; i<m; ++i ) |
| 582 |
fprintf(msgfile, " i, f, y-f: %4i %18.8g\n", i, fvec[i]); |
| 583 |
} |
| 584 |
if( C->verbosity&2 ) |
| 585 |
fprintf(msgfile, " fnorm=%24.16g xnorm=%24.16g\n", S->fnorm, xnorm); |
| 586 |
if ( S->userbreak ) /* user-requested break */ |
| 587 |
S->outcome = 11; |
| 588 |
|
| 589 |
/*** Deallocate the workspace. ***/ |
| 590 |
free(ws); |
| 591 |
|
| 592 |
} /*** lmmin. ***/ |
| 593 |
|
| 594 |
|
| 595 |
/*****************************************************************************/ |
| 596 |
/* lm_lmpar (determine Levenberg-Marquardt parameter) */ |
| 597 |
/*****************************************************************************/ |
| 598 |
|
| 599 |
void lm_lmpar( |
| 600 |
const int n, double *const r, const int ldr, int *const ipvt, |
| 601 |
double *const diag, double *const qtb, double delta, double *const par, |
| 602 |
double *const x, double *const sdiag, double *const aux, double *const xdi) |
| 603 |
{ |
| 604 |
/* Given an m by n matrix A, an n by n nonsingular diagonal matrix D, |
| 605 |
* an m-vector b, and a positive number delta, the problem is to |
| 606 |
* determine a parameter value par such that if x solves the system |
| 607 |
* |
| 608 |
* A*x = b and sqrt(par)*D*x = 0 |
| 609 |
* |
| 610 |
* in the least squares sense, and dxnorm is the euclidean |
| 611 |
* norm of D*x, then either par=0 and (dxnorm-delta) < 0.1*delta, |
| 612 |
* or par>0 and abs(dxnorm-delta) < 0.1*delta. |
| 613 |
* |
| 614 |
* Using lm_qrsolv, this subroutine completes the solution of the |
| 615 |
* problem if it is provided with the necessary information from |
| 616 |
* the QR factorization, with column pivoting, of A. That is, if |
| 617 |
* A*P = Q*R, where P is a permutation matrix, Q has orthogonal |
| 618 |
* columns, and R is an upper triangular matrix with diagonal |
| 619 |
* elements of nonincreasing magnitude, then lmpar expects the |
| 620 |
* full upper triangle of R, the permutation matrix P, and the |
| 621 |
* first n components of Q^T*b. On output lmpar also provides an |
| 622 |
* upper triangular matrix S such that |
| 623 |
* |
| 624 |
* P^T*(A^T*A + par*D*D)*P = S^T*S. |
| 625 |
* |
| 626 |
* S is employed within lmpar and may be of separate interest. |
| 627 |
* |
| 628 |
* Only a few iterations are generally needed for convergence |
| 629 |
* of the algorithm. If, however, the limit of 10 iterations |
| 630 |
* is reached, then the output par will contain the best value |
| 631 |
* obtained so far. |
| 632 |
* |
| 633 |
* Parameters: |
| 634 |
* |
| 635 |
* n is a positive integer INPUT variable set to the order of r. |
| 636 |
* |
| 637 |
* r is an n by n array. On INPUT the full upper triangle |
| 638 |
* must contain the full upper triangle of the matrix R. |
| 639 |
* On OUTPUT the full upper triangle is unaltered, and the |
| 640 |
* strict lower triangle contains the strict upper triangle |
| 641 |
* (transposed) of the upper triangular matrix S. |
| 642 |
* |
| 643 |
* ldr is a positive integer INPUT variable not less than n |
| 644 |
* which specifies the leading dimension of the array R. |
| 645 |
* |
| 646 |
* ipvt is an integer INPUT array of length n which defines the |
| 647 |
* permutation matrix P such that A*P = Q*R. Column j of P |
| 648 |
* is column ipvt(j) of the identity matrix. |
| 649 |
* |
| 650 |
* diag is an INPUT array of length n which must contain the |
| 651 |
* diagonal elements of the matrix D. |
| 652 |
* |
| 653 |
* qtb is an INPUT array of length n which must contain the first |
| 654 |
* n elements of the vector Q^T*b. |
| 655 |
* |
| 656 |
* delta is a positive INPUT variable which specifies an upper |
| 657 |
* bound on the euclidean norm of D*x. |
| 658 |
* |
| 659 |
* par is a nonnegative variable. On INPUT par contains an |
| 660 |
* initial estimate of the Levenberg-Marquardt parameter. |
| 661 |
* On OUTPUT par contains the final estimate. |
| 662 |
* |
| 663 |
* x is an OUTPUT array of length n which contains the least |
| 664 |
* squares solution of the system A*x = b, sqrt(par)*D*x = 0, |
| 665 |
* for the output par. |
| 666 |
* |
| 667 |
* sdiag is an array of length n needed as workspace; on OUTPUT |
| 668 |
* it contains the diagonal elements of the upper triangular |
| 669 |
* matrix S. |
| 670 |
* |
| 671 |
* aux is a multi-purpose work array of length n. |
| 672 |
* |
| 673 |
* xdi is a work array of length n. On OUTPUT: diag[j] * x[j]. |
| 674 |
* |
| 675 |
*/ |
| 676 |
int i, iter, j, nsing; |
| 677 |
double dxnorm, fp, fp_old, gnorm, parc, parl, paru; |
| 678 |
double sum, temp; |
| 679 |
static double p1 = 0.1; |
| 680 |
|
| 681 |
/*** lmpar: compute and store in x the gauss-newton direction. if the |
| 682 |
jacobian is rank-deficient, obtain a least squares solution. ***/ |
| 683 |
|
| 684 |
nsing = n; |
| 685 |
for (j = 0; j < n; j++) { |
| 686 |
aux[j] = qtb[j]; |
| 687 |
if (r[j * ldr + j] == 0 && nsing == n) |
| 688 |
nsing = j; |
| 689 |
if (nsing < n) |
| 690 |
aux[j] = 0; |
| 691 |
} |
| 692 |
for (j = nsing - 1; j >= 0; j--) { |
| 693 |
aux[j] = aux[j] / r[j + ldr * j]; |
| 694 |
temp = aux[j]; |
| 695 |
for (i = 0; i < j; i++) |
| 696 |
aux[i] -= r[j * ldr + i] * temp; |
| 697 |
} |
| 698 |
|
| 699 |
for (j = 0; j < n; j++) |
| 700 |
x[ipvt[j]] = aux[j]; |
| 701 |
|
| 702 |
/*** lmpar: initialize the iteration counter, evaluate the function at the |
| 703 |
origin, and test for acceptance of the gauss-newton direction. ***/ |
| 704 |
|
| 705 |
for (j = 0; j < n; j++) |
| 706 |
xdi[j] = diag[j] * x[j]; |
| 707 |
dxnorm = lm_enorm(n, xdi); |
| 708 |
fp = dxnorm - delta; |
| 709 |
if (fp <= p1 * delta) { |
| 710 |
#ifdef LMFIT_DEBUG_MESSAGES |
| 711 |
printf("debug lmpar nsing %d n %d, terminate (fp<p1*delta)\n", |
| 712 |
nsing, n); |
| 713 |
#endif |
| 714 |
*par = 0; |
| 715 |
return; |
| 716 |
} |
| 717 |
|
| 718 |
/*** lmpar: if the jacobian is not rank deficient, the newton |
| 719 |
step provides a lower bound, parl, for the zero of |
| 720 |
the function. otherwise set this bound to zero. ***/ |
| 721 |
|
| 722 |
parl = 0; |
| 723 |
if (nsing >= n) { |
| 724 |
for (j = 0; j < n; j++) |
| 725 |
aux[j] = diag[ipvt[j]] * xdi[ipvt[j]] / dxnorm; |
| 726 |
|
| 727 |
for (j = 0; j < n; j++) { |
| 728 |
sum = 0; |
| 729 |
for (i = 0; i < j; i++) |
| 730 |
sum += r[j * ldr + i] * aux[i]; |
| 731 |
aux[j] = (aux[j] - sum) / r[j + ldr * j]; |
| 732 |
} |
| 733 |
temp = lm_enorm(n, aux); |
| 734 |
parl = fp / delta / temp / temp; |
| 735 |
} |
| 736 |
|
| 737 |
/*** lmpar: calculate an upper bound, paru, for the zero of the function. ***/ |
| 738 |
|
| 739 |
for (j = 0; j < n; j++) { |
| 740 |
sum = 0; |
| 741 |
for (i = 0; i <= j; i++) |
| 742 |
sum += r[j * ldr + i] * qtb[i]; |
| 743 |
aux[j] = sum / diag[ipvt[j]]; |
| 744 |
} |
| 745 |
gnorm = lm_enorm(n, aux); |
| 746 |
paru = gnorm / delta; |
| 747 |
if (paru == 0) |
| 748 |
paru = LM_DWARF / MIN(delta, p1); |
| 749 |
|
| 750 |
/*** lmpar: if the input par lies outside of the interval (parl,paru), |
| 751 |
set par to the closer endpoint. ***/ |
| 752 |
|
| 753 |
*par = MAX(*par, parl); |
| 754 |
*par = MIN(*par, paru); |
| 755 |
if (*par == 0) |
| 756 |
*par = gnorm / dxnorm; |
| 757 |
|
| 758 |
/*** lmpar: iterate. ***/ |
| 759 |
|
| 760 |
for (iter=0; ; iter++) { |
| 761 |
|
| 762 |
/** evaluate the function at the current value of par. **/ |
| 763 |
|
| 764 |
if (*par == 0) |
| 765 |
*par = MAX(LM_DWARF, 0.001 * paru); |
| 766 |
temp = sqrt(*par); |
| 767 |
for (j = 0; j < n; j++) |
| 768 |
aux[j] = temp * diag[j]; |
| 769 |
|
| 770 |
lm_qrsolv(n, r, ldr, ipvt, aux, qtb, x, sdiag, xdi); |
| 771 |
/* return values are r, x, sdiag */ |
| 772 |
|
| 773 |
for (j = 0; j < n; j++) |
| 774 |
xdi[j] = diag[j] * x[j]; /* used as output */ |
| 775 |
dxnorm = lm_enorm(n, xdi); |
| 776 |
fp_old = fp; |
| 777 |
fp = dxnorm - delta; |
| 778 |
|
| 779 |
/** if the function is small enough, accept the current value |
| 780 |
of par. Also test for the exceptional cases where parl |
| 781 |
is zero or the number of iterations has reached 10. **/ |
| 782 |
|
| 783 |
if (fabs(fp) <= p1 * delta |
| 784 |
|| (parl == 0 && fp <= fp_old && fp_old < 0) |
| 785 |
|| iter == 10) { |
| 786 |
#ifdef LMFIT_DEBUG_MESSAGES |
| 787 |
printf("debug lmpar nsing %d iter %d " |
| 788 |
"par %.4e [%.4e %.4e] delta %.4e fp %.4e\n", |
| 789 |
nsing, iter, *par, parl, paru, delta, fp); |
| 790 |
#endif |
| 791 |
break; /* the only exit from the iteration. */ |
| 792 |
} |
| 793 |
|
| 794 |
/** compute the Newton correction. **/ |
| 795 |
|
| 796 |
for (j = 0; j < n; j++) |
| 797 |
aux[j] = diag[ipvt[j]] * xdi[ipvt[j]] / dxnorm; |
| 798 |
|
| 799 |
for (j = 0; j < n; j++) { |
| 800 |
aux[j] = aux[j] / sdiag[j]; |
| 801 |
for (i = j + 1; i < n; i++) |
| 802 |
aux[i] -= r[j * ldr + i] * aux[j]; |
| 803 |
} |
| 804 |
temp = lm_enorm(n, aux); |
| 805 |
parc = fp / delta / temp / temp; |
| 806 |
|
| 807 |
/** depending on the sign of the function, update parl or paru. **/ |
| 808 |
|
| 809 |
if (fp > 0) |
| 810 |
parl = MAX(parl, *par); |
| 811 |
else if (fp < 0) |
| 812 |
paru = MIN(paru, *par); |
| 813 |
/* the case fp==0 is precluded by the break condition */ |
| 814 |
|
| 815 |
/** compute an improved estimate for par. **/ |
| 816 |
|
| 817 |
*par = MAX(parl, *par + parc); |
| 818 |
|
| 819 |
} |
| 820 |
|
| 821 |
} /*** lm_lmpar. ***/ |
| 822 |
|
| 823 |
/*****************************************************************************/ |
| 824 |
/* lm_qrfac (QR factorization, from lapack) */ |
| 825 |
/*****************************************************************************/ |
| 826 |
|
| 827 |
void lm_qrfac( |
| 828 |
const int m, const int n, double *const A, int *const Pivot, |
| 829 |
double *const Rdiag, double *const Acnorm, double *const W) |
| 830 |
{ |
| 831 |
/* |
| 832 |
* This subroutine uses Householder transformations with column pivoting |
| 833 |
* to compute a QR factorization of the m by n matrix A. That is, qrfac |
| 834 |
* determines an orthogonal matrix Q, a permutation matrix P, and an |
| 835 |
* upper trapezoidal matrix R with diagonal elements of nonincreasing |
| 836 |
* magnitude, such that A*P = Q*R. The Householder transformation for |
| 837 |
* column k, k = 1,2,...,n, is of the form |
| 838 |
* |
| 839 |
* I - 2*w*wT/|w|^2 |
| 840 |
* |
| 841 |
* where w has zeroes in the first k-1 positions. |
| 842 |
* |
| 843 |
* Parameters: |
| 844 |
* |
| 845 |
* m is an INPUT parameter set to the number of rows of A. |
| 846 |
* |
| 847 |
* n is an INPUT parameter set to the number of columns of A. |
| 848 |
* |
| 849 |
* A is an m by n array. On INPUT, A contains the matrix for |
| 850 |
* which the QR factorization is to be computed. On OUTPUT |
| 851 |
* the strict upper trapezoidal part of A contains the strict |
| 852 |
* upper trapezoidal part of R, and the lower trapezoidal |
| 853 |
* part of A contains a factored form of Q (the non-trivial |
| 854 |
* elements of the vectors w described above). |
| 855 |
* |
| 856 |
* Pivot is an integer OUTPUT array of length n that describes the |
| 857 |
* permutation matrix P: |
| 858 |
* Column j of P is column ipvt(j) of the identity matrix. |
| 859 |
* |
| 860 |
* Rdiag is an OUTPUT array of length n which contains the |
| 861 |
* diagonal elements of R. |
| 862 |
* |
| 863 |
* Acnorm is an OUTPUT array of length n which contains the norms |
| 864 |
* of the corresponding columns of the input matrix A. If this |
| 865 |
* information is not needed, then Acnorm can share storage with Rdiag. |
| 866 |
* |
| 867 |
* W is a work array of length n. |
| 868 |
* |
| 869 |
*/ |
| 870 |
int i, j, k, kmax; |
| 871 |
double ajnorm, sum, temp; |
| 872 |
|
| 873 |
#ifdef LMFIT_DEBUG_MESSAGES |
| 874 |
printf("debug qrfac\n"); |
| 875 |
#endif |
| 876 |
|
| 877 |
/** Compute initial column norms; |
| 878 |
initialize Pivot with identity permutation. ***/ |
| 879 |
|
| 880 |
for (j = 0; j < n; j++) { |
| 881 |
W[j] = Rdiag[j] = Acnorm[j] = lm_enorm(m, &A[j*m]); |
| 882 |
Pivot[j] = j; |
| 883 |
} |
| 884 |
|
| 885 |
/** Loop over columns of A. **/ |
| 886 |
|
| 887 |
assert( n <= m ); |
| 888 |
for (j = 0; j < n; j++) { |
| 889 |
|
| 890 |
/** Bring the column of largest norm into the pivot position. **/ |
| 891 |
|
| 892 |
kmax = j; |
| 893 |
for (k = j+1; k < n; k++) |
| 894 |
if (Rdiag[k] > Rdiag[kmax]) |
| 895 |
kmax = k; |
| 896 |
|
| 897 |
if (kmax != j) { |
| 898 |
/* Swap columns j and kmax. */ |
| 899 |
k = Pivot[j]; |
| 900 |
Pivot[j] = Pivot[kmax]; |
| 901 |
Pivot[kmax] = k; |
| 902 |
for (i = 0; i < m; i++) { |
| 903 |
temp = A[j*m+i]; |
| 904 |
A[j*m+i] = A[kmax*m+i]; |
| 905 |
A[kmax*m+i] = temp; |
| 906 |
} |
| 907 |
/* Half-swap: Rdiag[j], W[j] won't be needed any further. */ |
| 908 |
Rdiag[kmax] = Rdiag[j]; |
| 909 |
W[kmax] = W[j]; |
| 910 |
} |
| 911 |
|
| 912 |
/** Compute the Householder reflection vector w_j to reduce the |
| 913 |
j-th column of A to a multiple of the j-th unit vector. **/ |
| 914 |
|
| 915 |
ajnorm = lm_enorm(m-j, &A[j*m+j]); |
| 916 |
if (ajnorm == 0) { |
| 917 |
Rdiag[j] = 0; |
| 918 |
continue; |
| 919 |
} |
| 920 |
|
| 921 |
/* Let the partial column vector A[j][j:] contain w_j := e_j+-a_j/|a_j|, |
| 922 |
where the sign +- is chosen to avoid cancellation in w_jj. */ |
| 923 |
if (A[j*m+j] < 0) |
| 924 |
ajnorm = -ajnorm; |
| 925 |
for (i = j; i < m; i++) |
| 926 |
A[j*m+i] /= ajnorm; |
| 927 |
A[j*m+j] += 1; |
| 928 |
|
| 929 |
/** Apply the Householder transformation U_w := 1 - 2*w_j.w_j/|w_j|^2 |
| 930 |
to the remaining columns, and update the norms. **/ |
| 931 |
|
| 932 |
for (k = j+1; k < n; k++) { |
| 933 |
/* Compute scalar product w_j * a_j. */ |
| 934 |
sum = 0; |
| 935 |
for (i = j; i < m; i++) |
| 936 |
sum += A[j*m+i] * A[k*m+i]; |
| 937 |
|
| 938 |
/* Normalization is simplified by the coincidence |w_j|^2=2w_jj. */ |
| 939 |
temp = sum / A[j*m+j]; |
| 940 |
|
| 941 |
/* Carry out transform U_w_j * a_k. */ |
| 942 |
for (i = j; i < m; i++) |
| 943 |
A[k*m+i] -= temp * A[j*m+i]; |
| 944 |
|
| 945 |
/* No idea what happens here. */ |
| 946 |
if (Rdiag[k] != 0) { |
| 947 |
temp = A[m*k+j] / Rdiag[k]; |
| 948 |
if ( fabs(temp)<1 ) { |
| 949 |
Rdiag[k] *= sqrt(1-SQR(temp)); |
| 950 |
temp = Rdiag[k] / W[k]; |
| 951 |
} else |
| 952 |
temp = 0; |
| 953 |
if ( temp == 0 || 0.05 * SQR(temp) <= LM_MACHEP ) { |
| 954 |
Rdiag[k] = lm_enorm(m-j-1, &A[m*k+j+1]); |
| 955 |
W[k] = Rdiag[k]; |
| 956 |
} |
| 957 |
} |
| 958 |
} |
| 959 |
|
| 960 |
Rdiag[j] = -ajnorm; |
| 961 |
} |
| 962 |
} /*** lm_qrfac. ***/ |
| 963 |
|
| 964 |
|
| 965 |
/*****************************************************************************/ |
| 966 |
/* lm_qrsolv (linear least-squares) */ |
| 967 |
/*****************************************************************************/ |
| 968 |
|
| 969 |
void lm_qrsolv( |
| 970 |
const int n, double *const r, const int ldr, int *const ipvt, |
| 971 |
double *const diag, double *const qtb, double *const x, |
| 972 |
double *const sdiag, double *const wa) |
| 973 |
{ |
| 974 |
/* |
| 975 |
* Given an m by n matrix A, an n by n diagonal matrix D, and an |
| 976 |
* m-vector b, the problem is to determine an x which solves the |
| 977 |
* system |
| 978 |
* |
| 979 |
* A*x = b and D*x = 0 |
| 980 |
* |
| 981 |
* in the least squares sense. |
| 982 |
* |
| 983 |
* This subroutine completes the solution of the problem if it is |
| 984 |
* provided with the necessary information from the QR factorization, |
| 985 |
* with column pivoting, of A. That is, if A*P = Q*R, where P is a |
| 986 |
* permutation matrix, Q has orthogonal columns, and R is an upper |
| 987 |
* triangular matrix with diagonal elements of nonincreasing magnitude, |
| 988 |
* then qrsolv expects the full upper triangle of R, the permutation |
| 989 |
* matrix P, and the first n components of Q^T*b. The system |
| 990 |
* A*x = b, D*x = 0, is then equivalent to |
| 991 |
* |
| 992 |
* R*z = Q^T*b, P^T*D*P*z = 0, |
| 993 |
* |
| 994 |
* where x = P*z. If this system does not have full rank, then a least |
| 995 |
* squares solution is obtained. On output qrsolv also provides an upper |
| 996 |
* triangular matrix S such that |
| 997 |
* |
| 998 |
* P^T*(A^T*A + D*D)*P = S^T*S. |
| 999 |
* |
| 1000 |
* S is computed within qrsolv and may be of separate interest. |
| 1001 |
* |
| 1002 |
* Parameters: |
| 1003 |
* |
| 1004 |
* n is a positive integer INPUT variable set to the order of R. |
| 1005 |
* |
| 1006 |
* r is an n by n array. On INPUT the full upper triangle must |
| 1007 |
* contain the full upper triangle of the matrix R. On OUTPUT |
| 1008 |
* the full upper triangle is unaltered, and the strict lower |
| 1009 |
* triangle contains the strict upper triangle (transposed) of |
| 1010 |
* the upper triangular matrix S. |
| 1011 |
* |
| 1012 |
* ldr is a positive integer INPUT variable not less than n |
| 1013 |
* which specifies the leading dimension of the array R. |
| 1014 |
* |
| 1015 |
* ipvt is an integer INPUT array of length n which defines the |
| 1016 |
* permutation matrix P such that A*P = Q*R. Column j of P |
| 1017 |
* is column ipvt(j) of the identity matrix. |
| 1018 |
* |
| 1019 |
* diag is an INPUT array of length n which must contain the |
| 1020 |
* diagonal elements of the matrix D. |
| 1021 |
* |
| 1022 |
* qtb is an INPUT array of length n which must contain the first |
| 1023 |
* n elements of the vector Q^T*b. |
| 1024 |
* |
| 1025 |
* x is an OUTPUT array of length n which contains the least |
| 1026 |
* squares solution of the system A*x = b, D*x = 0. |
| 1027 |
* |
| 1028 |
* sdiag is an OUTPUT array of length n which contains the |
| 1029 |
* diagonal elements of the upper triangular matrix S. |
| 1030 |
* |
| 1031 |
* wa is a work array of length n. |
| 1032 |
* |
| 1033 |
*/ |
| 1034 |
int i, kk, j, k, nsing; |
| 1035 |
double qtbpj, sum, temp; |
| 1036 |
double _sin, _cos, _tan, _cot; /* local variables, not functions */ |
| 1037 |
|
| 1038 |
/*** qrsolv: copy R and Q^T*b to preserve input and initialize S. |
| 1039 |
In particular, save the diagonal elements of R in x. ***/ |
| 1040 |
|
| 1041 |
for (j = 0; j < n; j++) { |
| 1042 |
for (i = j; i < n; i++) |
| 1043 |
r[j * ldr + i] = r[i * ldr + j]; |
| 1044 |
x[j] = r[j * ldr + j]; |
| 1045 |
wa[j] = qtb[j]; |
| 1046 |
} |
| 1047 |
|
| 1048 |
/*** qrsolv: eliminate the diagonal matrix D using a Givens rotation. ***/ |
| 1049 |
|
| 1050 |
for (j = 0; j < n; j++) { |
| 1051 |
|
| 1052 |
/*** qrsolv: prepare the row of D to be eliminated, locating the |
| 1053 |
diagonal element using P from the QR factorization. ***/ |
| 1054 |
|
| 1055 |
if (diag[ipvt[j]] == 0) |
| 1056 |
goto L90; |
| 1057 |
for (k = j; k < n; k++) |
| 1058 |
sdiag[k] = 0; |
| 1059 |
sdiag[j] = diag[ipvt[j]]; |
| 1060 |
|
| 1061 |
/*** qrsolv: the transformations to eliminate the row of D modify only |
| 1062 |
a single element of Q^T*b beyond the first n, which is initially 0. ***/ |
| 1063 |
|
| 1064 |
qtbpj = 0; |
| 1065 |
for (k = j; k < n; k++) { |
| 1066 |
|
| 1067 |
/** determine a Givens rotation which eliminates the |
| 1068 |
appropriate element in the current row of D. **/ |
| 1069 |
|
| 1070 |
if (sdiag[k] == 0) |
| 1071 |
continue; |
| 1072 |
kk = k + ldr * k; |
| 1073 |
if (fabs(r[kk]) < fabs(sdiag[k])) { |
| 1074 |
_cot = r[kk] / sdiag[k]; |
| 1075 |
_sin = 1 / sqrt(1 + SQR(_cot)); |
| 1076 |
_cos = _sin * _cot; |
| 1077 |
} else { |
| 1078 |
_tan = sdiag[k] / r[kk]; |
| 1079 |
_cos = 1 / sqrt(1 + SQR(_tan)); |
| 1080 |
_sin = _cos * _tan; |
| 1081 |
} |
| 1082 |
|
| 1083 |
/** compute the modified diagonal element of R and |
| 1084 |
the modified element of (Q^T*b,0). **/ |
| 1085 |
|
| 1086 |
r[kk] = _cos * r[kk] + _sin * sdiag[k]; |
| 1087 |
temp = _cos * wa[k] + _sin * qtbpj; |
| 1088 |
qtbpj = -_sin * wa[k] + _cos * qtbpj; |
| 1089 |
wa[k] = temp; |
| 1090 |
|
| 1091 |
/** accumulate the tranformation in the row of S. **/ |
| 1092 |
|
| 1093 |
for (i = k + 1; i < n; i++) { |
| 1094 |
temp = _cos * r[k * ldr + i] + _sin * sdiag[i]; |
| 1095 |
sdiag[i] = -_sin * r[k * ldr + i] + _cos * sdiag[i]; |
| 1096 |
r[k * ldr + i] = temp; |
| 1097 |
} |
| 1098 |
} |
| 1099 |
|
| 1100 |
L90: |
| 1101 |
/** store the diagonal element of S and restore |
| 1102 |
the corresponding diagonal element of R. **/ |
| 1103 |
|
| 1104 |
sdiag[j] = r[j * ldr + j]; |
| 1105 |
r[j * ldr + j] = x[j]; |
| 1106 |
} |
| 1107 |
|
| 1108 |
/*** qrsolv: solve the triangular system for z. If the system is |
| 1109 |
singular, then obtain a least squares solution. ***/ |
| 1110 |
|
| 1111 |
nsing = n; |
| 1112 |
for (j = 0; j < n; j++) { |
| 1113 |
if (sdiag[j] == 0 && nsing == n) |
| 1114 |
nsing = j; |
| 1115 |
if (nsing < n) |
| 1116 |
wa[j] = 0; |
| 1117 |
} |
| 1118 |
|
| 1119 |
for (j = nsing - 1; j >= 0; j--) { |
| 1120 |
sum = 0; |
| 1121 |
for (i = j + 1; i < nsing; i++) |
| 1122 |
sum += r[j * ldr + i] * wa[i]; |
| 1123 |
wa[j] = (wa[j] - sum) / sdiag[j]; |
| 1124 |
} |
| 1125 |
|
| 1126 |
/*** qrsolv: permute the components of z back to components of x. ***/ |
| 1127 |
|
| 1128 |
for (j = 0; j < n; j++) |
| 1129 |
x[ipvt[j]] = wa[j]; |
| 1130 |
|
| 1131 |
} /*** lm_qrsolv. ***/ |
| 1132 |
|
| 1133 |
|
| 1134 |
/*****************************************************************************/ |
| 1135 |
/* lm_enorm (Euclidean norm) */ |
| 1136 |
/*****************************************************************************/ |
| 1137 |
|
| 1138 |
double lm_enorm(const int n, const double *const x) |
| 1139 |
{ |
| 1140 |
/* This function calculates the Euclidean norm of an n-vector x. |
| 1141 |
* |
| 1142 |
* The Euclidean norm is computed by accumulating the sum of |
| 1143 |
* squares in three different sums. The sums of squares for the |
| 1144 |
* small and large components are scaled so that no overflows |
| 1145 |
* occur. Non-destructive underflows are permitted. Underflows |
| 1146 |
* and overflows do not occur in the computation of the unscaled |
| 1147 |
* sum of squares for the intermediate components. |
| 1148 |
* The definitions of small, intermediate and large components |
| 1149 |
* depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. The main |
| 1150 |
* restrictions on these constants are that LM_SQRT_DWARF**2 not |
| 1151 |
* underflow and LM_SQRT_GIANT**2 not overflow. |
| 1152 |
* |
| 1153 |
* Parameters: |
| 1154 |
* |
| 1155 |
* n is a positive integer INPUT variable. |
| 1156 |
* |
| 1157 |
* x is an INPUT array of length n. |
| 1158 |
*/ |
| 1159 |
int i; |
| 1160 |
double agiant, s1, s2, s3, xabs, x1max, x3max, temp; |
| 1161 |
|
| 1162 |
s1 = 0; |
| 1163 |
s2 = 0; |
| 1164 |
s3 = 0; |
| 1165 |
x1max = 0; |
| 1166 |
x3max = 0; |
| 1167 |
agiant = LM_SQRT_GIANT / n; |
| 1168 |
|
| 1169 |
/** sum squares. **/ |
| 1170 |
|
| 1171 |
for (i = 0; i < n; i++) { |
| 1172 |
xabs = fabs(x[i]); |
| 1173 |
if (xabs > LM_SQRT_DWARF) { |
| 1174 |
if ( xabs < agiant ) { |
| 1175 |
s2 += xabs * xabs; |
| 1176 |
} else if ( xabs > x1max ) { |
| 1177 |
temp = x1max / xabs; |
| 1178 |
s1 = 1 + s1 * SQR(temp); |
| 1179 |
x1max = xabs; |
| 1180 |
} else { |
| 1181 |
temp = xabs / x1max; |
| 1182 |
s1 += SQR(temp); |
| 1183 |
} |
| 1184 |
} else if ( xabs > x3max ) { |
| 1185 |
temp = x3max / xabs; |
| 1186 |
s3 = 1 + s3 * SQR(temp); |
| 1187 |
x3max = xabs; |
| 1188 |
} else if (xabs != 0) { |
| 1189 |
temp = xabs / x3max; |
| 1190 |
s3 += SQR(temp); |
| 1191 |
} |
| 1192 |
} |
| 1193 |
|
| 1194 |
/** calculation of norm. **/ |
| 1195 |
|
| 1196 |
if (s1 != 0) |
| 1197 |
return x1max * sqrt(s1 + (s2 / x1max) / x1max); |
| 1198 |
else if (s2 != 0) { |
| 1199 |
if (s2 >= x3max) |
| 1200 |
return sqrt(s2 * (1 + (x3max / s2) * (x3max * s3))); |
| 1201 |
else |
| 1202 |
return sqrt(x3max * ((s2 / x3max) + (x3max * s3))); |
| 1203 |
} else |
| 1204 |
return x3max * sqrt(s3); |
| 1205 |
|
| 1206 |
} /*** lm_enorm. ***/ |
| 1207 |
|
| 1208 |
|
| 1209 |
/*****************************************************************************/ |
| 1210 |
/* lm_fnorm (Euclidean norm of difference) */ |
| 1211 |
/*****************************************************************************/ |
| 1212 |
|
| 1213 |
double lm_fnorm(const int n, const double *const x, const double *const y) |
| 1214 |
{ |
| 1215 |
/* This function calculates the Euclidean norm of an n-vector x-y. |
| 1216 |
* |
| 1217 |
* The Euclidean norm is computed by accumulating the sum of |
| 1218 |
* squares in three different sums. The sums of squares for the |
| 1219 |
* small and large components are scaled so that no overflows |
| 1220 |
* occur. Non-destructive underflows are permitted. Underflows |
| 1221 |
* and overflows do not occur in the computation of the unscaled |
| 1222 |
* sum of squares for the intermediate components. |
| 1223 |
* The definitions of small, intermediate and large components |
| 1224 |
* depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. The main |
| 1225 |
* restrictions on these constants are that LM_SQRT_DWARF**2 not |
| 1226 |
* underflow and LM_SQRT_GIANT**2 not overflow. |
| 1227 |
* |
| 1228 |
* Parameters: |
| 1229 |
* |
| 1230 |
* n is a positive integer INPUT variable. |
| 1231 |
* |
| 1232 |
* x, y are INPUT arrays of length n. |
| 1233 |
*/ |
| 1234 |
if (!y) |
| 1235 |
return lm_enorm(n, x); |
| 1236 |
int i; |
| 1237 |
double agiant, s1, s2, s3, xabs, x1max, x3max, temp; |
| 1238 |
|
| 1239 |
s1 = 0; |
| 1240 |
s2 = 0; |
| 1241 |
s3 = 0; |
| 1242 |
x1max = 0; |
| 1243 |
x3max = 0; |
| 1244 |
agiant = LM_SQRT_GIANT / n; |
| 1245 |
|
| 1246 |
/** sum squares. **/ |
| 1247 |
|
| 1248 |
for (i = 0; i < n; i++) { |
| 1249 |
xabs = fabs(x[i]-y[i]); |
| 1250 |
if (xabs > LM_SQRT_DWARF) { |
| 1251 |
if ( xabs < agiant ) { |
| 1252 |
s2 += xabs * xabs; |
| 1253 |
} else if ( xabs > x1max ) { |
| 1254 |
temp = x1max / xabs; |
| 1255 |
s1 = 1 + s1 * SQR(temp); |
| 1256 |
x1max = xabs; |
| 1257 |
} else { |
| 1258 |
temp = xabs / x1max; |
| 1259 |
s1 += SQR(temp); |
| 1260 |
} |
| 1261 |
} else if ( xabs > x3max ) { |
| 1262 |
temp = x3max / xabs; |
| 1263 |
s3 = 1 + s3 * SQR(temp); |
| 1264 |
x3max = xabs; |
| 1265 |
} else if (xabs != 0) { |
| 1266 |
temp = xabs / x3max; |
| 1267 |
s3 += SQR(temp); |
| 1268 |
} |
| 1269 |
} |
| 1270 |
|
| 1271 |
/** calculation of norm. **/ |
| 1272 |
|
| 1273 |
if (s1 != 0) |
| 1274 |
return x1max * sqrt(s1 + (s2 / x1max) / x1max); |
| 1275 |
else if (s2 != 0) { |
| 1276 |
if (s2 >= x3max) |
| 1277 |
return sqrt(s2 * (1 + (x3max / s2) * (x3max * s3))); |
| 1278 |
else |
| 1279 |
return sqrt(x3max * ((s2 / x3max) + (x3max * s3))); |
| 1280 |
} else |
| 1281 |
return x3max * sqrt(s3); |
| 1282 |
|
| 1283 |
} /*** lm_fnorm. ***/ |