Debian Codegraph @

contrib/lmfit/lmmin.c

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