Debian Codegraph @

libsrc/math/mpfit.c

raw
1 /*
2 * MINPACK-1 Least Squares Fitting Library
3 *
4 * Original public domain version by B. Garbow, K. Hillstrom, J. More'
5 * (Argonne National Laboratory, MINPACK project, March 1980)
6 * See the file DISCLAIMER for copyright information.
7 *
8 * Tranlation to C Language by S. Moshier (moshier.net)
9 *
10 * Enhancements and packaging by C. Markwardt
11 * (comparable to IDL fitting routine MPFIT
12 * see http://cow.physics.wisc.edu/~craigm/idl/idl.html)
13 */
14
15 /* Main mpfit library routines (double precision)
16 $Id: mpfit.c,v 1.20 2010/11/13 08:15:35 craigm Exp $
17 */
18
19 #include <stdio.h>
20 #include <stdlib.h>
21 #include <math.h>
22 #include <string.h>
23 #include "mpfit.h"
24
25 /* Forward declarations of functions in this module */
26 static int mp_fdjac2(mp_func funct,
27 int m, int n, int *ifree, int npar, double *x, double *fvec,
28 double *fjac, int ldfjac, double epsfcn,
29 double *wa, void *priv, int *nfev,
30 double *step, double *dstep, int *dside,
31 int *qulimited, double *ulimit,
32 int *ddebug, double *ddrtol, double *ddatol);
33 static void mp_qrfac(int m, int n, double *a, int lda,
34 int pivot, int *ipvt, int lipvt,
35 double *rdiag, double *acnorm, double *wa);
36 static void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,
37 double *qtb, double *x, double *sdiag, double *wa);
38 static void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag,
39 double *qtb, double delta, double *par, double *x,
40 double *sdiag, double *wa1, double *wa2);
41 static double mp_enorm(int n, double *x);
42 static double mp_dmax1(double a, double b);
43 static double mp_dmin1(double a, double b);
44 static int mp_min0(int a, int b);
45 static int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa);
46
47 /* Macro to call user function */
48 #define mp_call(funct, m, n, x, fvec, dvec, priv) (*(funct))(m,n,x,fvec,dvec,priv)
49
50 /* Macro to safely allocate memory */
51 #define mp_malloc(dest,type,size) \
52 dest = (type *) malloc( sizeof(type)*size ); \
53 if (dest == 0) { \
54 info = MP_ERR_MEMORY; \
55 goto CLEANUP; \
56 } else { \
57 int _k; \
58 for (_k=0; _k<(size); _k++) dest[_k] = 0; \
59 }
60
61 /*
62 * **********
63 *
64 * subroutine mpfit
65 *
66 * the purpose of mpfit is to minimize the sum of the squares of
67 * m nonlinear functions in n variables by a modification of
68 * the levenberg-marquardt algorithm. the user must provide a
69 * subroutine which calculates the functions. the jacobian is
70 * then calculated by a finite-difference approximation.
71 *
72 * mp_funct funct - function to be minimized
73 * int m - number of data points
74 * int npar - number of fit parameters
75 * double *xall - array of n initial parameter values
76 * upon return, contains adjusted parameter values
77 * mp_par *pars - array of npar structures specifying constraints;
78 * or 0 (null pointer) for unconstrained fitting
79 * [ see README and mpfit.h for definition & use of mp_par]
80 * mp_config *config - pointer to structure which specifies the
81 * configuration of mpfit(); or 0 (null pointer)
82 * if the default configuration is to be used.
83 * See README and mpfit.h for definition and use
84 * of config.
85 * void *private - any private user data which is to be passed directly
86 * to funct without modification by mpfit().
87 * mp_result *result - pointer to structure, which upon return, contains
88 * the results of the fit. The user should zero this
89 * structure. If any of the array values are to be
90 * returned, the user should allocate storage for them
91 * and assign the corresponding pointer in *result.
92 * Upon return, *result will be updated, and
93 * any of the non-null arrays will be filled.
94 *
95 *
96 * FORTRAN DOCUMENTATION BELOW
97 *
98 *
99 * the subroutine statement is
100 *
101 * subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
102 * diag,mode,factor,nprint,info,nfev,fjac,
103 * ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
104 *
105 * where
106 *
107 * fcn is the name of the user-supplied subroutine which
108 * calculates the functions. fcn must be declared
109 * in an external statement in the user calling
110 * program, and should be written as follows.
111 *
112 * subroutine fcn(m,n,x,fvec,iflag)
113 * integer m,n,iflag
114 * double precision x(n),fvec(m)
115 * ----------
116 * calculate the functions at x and
117 * return this vector in fvec.
118 * ----------
119 * return
120 * end
121 *
122 * the value of iflag should not be changed by fcn unless
123 * the user wants to terminate execution of lmdif.
124 * in this case set iflag to a negative integer.
125 *
126 * m is a positive integer input variable set to the number
127 * of functions.
128 *
129 * n is a positive integer input variable set to the number
130 * of variables. n must not exceed m.
131 *
132 * x is an array of length n. on input x must contain
133 * an initial estimate of the solution vector. on output x
134 * contains the final estimate of the solution vector.
135 *
136 * fvec is an output array of length m which contains
137 * the functions evaluated at the output x.
138 *
139 * ftol is a nonnegative input variable. termination
140 * occurs when both the actual and predicted relative
141 * reductions in the sum of squares are at most ftol.
142 * therefore, ftol measures the relative error desired
143 * in the sum of squares.
144 *
145 * xtol is a nonnegative input variable. termination
146 * occurs when the relative error between two consecutive
147 * iterates is at most xtol. therefore, xtol measures the
148 * relative error desired in the approximate solution.
149 *
150 * gtol is a nonnegative input variable. termination
151 * occurs when the cosine of the angle between fvec and
152 * any column of the jacobian is at most gtol in absolute
153 * value. therefore, gtol measures the orthogonality
154 * desired between the function vector and the columns
155 * of the jacobian.
156 *
157 * maxfev is a positive integer input variable. termination
158 * occurs when the number of calls to fcn is at least
159 * maxfev by the end of an iteration.
160 *
161 * epsfcn is an input variable used in determining a suitable
162 * step length for the forward-difference approximation. this
163 * approximation assumes that the relative errors in the
164 * functions are of the order of epsfcn. if epsfcn is less
165 * than the machine precision, it is assumed that the relative
166 * errors in the functions are of the order of the machine
167 * precision.
168 *
169 * diag is an array of length n. if mode = 1 (see
170 * below), diag is internally set. if mode = 2, diag
171 * must contain positive entries that serve as
172 * multiplicative scale factors for the variables.
173 *
174 * mode is an integer input variable. if mode = 1, the
175 * variables will be scaled internally. if mode = 2,
176 * the scaling is specified by the input diag. other
177 * values of mode are equivalent to mode = 1.
178 *
179 * factor is a positive input variable used in determining the
180 * initial step bound. this bound is set to the product of
181 * factor and the euclidean norm of diag*x if nonzero, or else
182 * to factor itself. in most cases factor should lie in the
183 * interval (.1,100.). 100. is a generally recommended value.
184 *
185 * nprint is an integer input variable that enables controlled
186 * printing of iterates if it is positive. in this case,
187 * fcn is called with iflag = 0 at the beginning of the first
188 * iteration and every nprint iterations thereafter and
189 * immediately prior to return, with x and fvec available
190 * for printing. if nprint is not positive, no special calls
191 * of fcn with iflag = 0 are made.
192 *
193 * info is an integer output variable. if the user has
194 * terminated execution, info is set to the (negative)
195 * value of iflag. see description of fcn. otherwise,
196 * info is set as follows.
197 *
198 * info = 0 improper input parameters.
199 *
200 * info = 1 both actual and predicted relative reductions
201 * in the sum of squares are at most ftol.
202 *
203 * info = 2 relative error between two consecutive iterates
204 * is at most xtol.
205 *
206 * info = 3 conditions for info = 1 and info = 2 both hold.
207 *
208 * info = 4 the cosine of the angle between fvec and any
209 * column of the jacobian is at most gtol in
210 * absolute value.
211 *
212 * info = 5 number of calls to fcn has reached or
213 * exceeded maxfev.
214 *
215 * info = 6 ftol is too small. no further reduction in
216 * the sum of squares is possible.
217 *
218 * info = 7 xtol is too small. no further improvement in
219 * the approximate solution x is possible.
220 *
221 * info = 8 gtol is too small. fvec is orthogonal to the
222 * columns of the jacobian to machine precision.
223 *
224 * nfev is an integer output variable set to the number of
225 * calls to fcn.
226 *
227 * fjac is an output m by n array. the upper n by n submatrix
228 * of fjac contains an upper triangular matrix r with
229 * diagonal elements of nonincreasing magnitude such that
230 *
231 * t t t
232 * p *(jac *jac)*p = r *r,
233 *
234 * where p is a permutation matrix and jac is the final
235 * calculated jacobian. column j of p is column ipvt(j)
236 * (see below) of the identity matrix. the lower trapezoidal
237 * part of fjac contains information generated during
238 * the computation of r.
239 *
240 * ldfjac is a positive integer input variable not less than m
241 * which specifies the leading dimension of the array fjac.
242 *
243 * ipvt is an integer output array of length n. ipvt
244 * defines a permutation matrix p such that jac*p = q*r,
245 * where jac is the final calculated jacobian, q is
246 * orthogonal (not stored), and r is upper triangular
247 * with diagonal elements of nonincreasing magnitude.
248 * column j of p is column ipvt(j) of the identity matrix.
249 *
250 * qtf is an output array of length n which contains
251 * the first n elements of the vector (q transpose)*fvec.
252 *
253 * wa1, wa2, and wa3 are work arrays of length n.
254 *
255 * wa4 is a work array of length m.
256 *
257 * subprograms called
258 *
259 * user-supplied ...... fcn
260 *
261 * minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
262 *
263 * fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
264 *
265 * argonne national laboratory. minpack project. march 1980.
266 * burton s. garbow, kenneth e. hillstrom, jorge j. more
267 *
268 * ********** */
269
270
271 int mpfit(mp_func funct, int m, int npar,
272 double *xall, mp_par *pars, mp_config *config, void *private_data,
273 mp_result *result)
274 {
275 mp_config conf;
276 int i, j, info, iflag, nfree, npegged, iter;
277 int qanylim = 0, qanypegged = 0;
278
279 int ij,jj,l;
280 double actred,delta,dirder,fnorm,fnorm1,gnorm, orignorm;
281 double par,pnorm,prered,ratio;
282 double sum,temp,temp1,temp2,temp3,xnorm, alpha;
283 static double one = 1.0;
284 static double p1 = 0.1;
285 static double p5 = 0.5;
286 static double p25 = 0.25;
287 static double p75 = 0.75;
288 static double p0001 = 1.0e-4;
289 static double zero = 0.0;
290 int nfev = 0;
291
292 double *step = 0, *dstep = 0, *llim = 0, *ulim = 0;
293 int *pfixed = 0, *mpside = 0, *ifree = 0, *qllim = 0, *qulim = 0;
294 int *ddebug = 0;
295 double *ddrtol = 0, *ddatol = 0;
296
297 double *fvec = 0, *qtf = 0;
298 double *x = 0, *xnew = 0, *fjac = 0, *diag = 0;
299 double *wa1 = 0, *wa2 = 0, *wa3 = 0, *wa4 = 0;
300 int *ipvt = 0;
301
302 int ldfjac;
303
304 /* Default configuration */
305 conf.ftol = 1e-10;
306 conf.xtol = 1e-10;
307 conf.gtol = 1e-10;
308 conf.stepfactor = 100.0;
309 conf.nprint = 1;
310 conf.epsfcn = MP_MACHEP0;
311 conf.maxiter = 200;
312 conf.douserscale = 0;
313 conf.maxfev = 0;
314 conf.covtol = 1e-14;
315 conf.nofinitecheck = 0;
316
317 if (config) {
318 /* Transfer any user-specified configurations */
319 if (config->ftol > 0) conf.ftol = config->ftol;
320 if (config->xtol > 0) conf.xtol = config->xtol;
321 if (config->gtol > 0) conf.gtol = config->gtol;
322 if (config->stepfactor > 0) conf.stepfactor = config->stepfactor;
323 if (config->nprint >= 0) conf.nprint = config->nprint;
324 if (config->epsfcn > 0) conf.epsfcn = config->epsfcn;
325 if (config->maxiter > 0) conf.maxiter = config->maxiter;
326 if (config->douserscale != 0) conf.douserscale = config->douserscale;
327 if (config->covtol > 0) conf.covtol = config->covtol;
328 if (config->nofinitecheck > 0) conf.nofinitecheck = config->nofinitecheck;
329 conf.maxfev = config->maxfev;
330 }
331
332 info = 0;
333 iflag = 0;
334 nfree = 0;
335 npegged = 0;
336
337 if (funct == 0) {
338 return MP_ERR_FUNC;
339 }
340
341 if ((m <= 0) || (xall == 0)) {
342 return MP_ERR_NPOINTS;
343 }
344
345 if (npar <= 0) {
346 return MP_ERR_NFREE;
347 }
348
349 fnorm = -1.0;
350 fnorm1 = -1.0;
351 xnorm = -1.0;
352 delta = 0.0;
353
354 /* FIXED parameters? */
355 mp_malloc(pfixed, int, npar);
356 if (pars) for (i=0; i<npar; i++) {
357 pfixed[i] = (pars[i].fixed)?1:0;
358 }
359
360 /* Finite differencing step, absolute and relative, and sidedness of deriv */
361 mp_malloc(step, double, npar);
362 mp_malloc(dstep, double, npar);
363 mp_malloc(mpside, int, npar);
364 mp_malloc(ddebug, int, npar);
365 mp_malloc(ddrtol, double, npar);
366 mp_malloc(ddatol, double, npar);
367 if (pars) for (i=0; i<npar; i++) {
368 step[i] = pars[i].step;
369 dstep[i] = pars[i].relstep;
370 mpside[i] = pars[i].side;
371 ddebug[i] = pars[i].deriv_debug;
372 ddrtol[i] = pars[i].deriv_reltol;
373 ddatol[i] = pars[i].deriv_abstol;
374 }
375
376 /* Finish up the free parameters */
377 nfree = 0;
378 mp_malloc(ifree, int, npar);
379 for (i=0, j=0; i<npar; i++) {
380 if (pfixed[i] == 0) {
381 nfree++;
382 ifree[j++] = i;
383 }
384 }
385 if (nfree == 0) {
386 info = MP_ERR_NFREE;
387 goto CLEANUP;
388 }
389
390 if (pars) {
391 for (i=0; i<npar; i++) {
392 if ( (pars[i].limited[0] && (xall[i] < pars[i].limits[0])) ||
393 (pars[i].limited[1] && (xall[i] > pars[i].limits[1])) ) {
394 info = MP_ERR_INITBOUNDS;
395 goto CLEANUP;
396 }
397 if ( (pars[i].fixed == 0) && pars[i].limited[0] && pars[i].limited[1] &&
398 (pars[i].limits[0] >= pars[i].limits[1])) {
399 info = MP_ERR_BOUNDS;
400 goto CLEANUP;
401 }
402 }
403
404 mp_malloc(qulim, int, nfree);
405 mp_malloc(qllim, int, nfree);
406 mp_malloc(ulim, double, nfree);
407 mp_malloc(llim, double, nfree);
408
409 for (i=0; i<nfree; i++) {
410 qllim[i] = pars[ifree[i]].limited[0];
411 qulim[i] = pars[ifree[i]].limited[1];
412 llim[i] = pars[ifree[i]].limits[0];
413 ulim[i] = pars[ifree[i]].limits[1];
414 if (qllim[i] || qulim[i]) qanylim = 1;
415 }
416 }
417
418 /* Sanity checking on input configuration */
419 if ((npar <= 0) || (conf.ftol <= 0) || (conf.xtol <= 0) ||
420 (conf.gtol <= 0) || (conf.maxiter < 0) ||
421 (conf.stepfactor <= 0)) {
422 info = MP_ERR_PARAM;
423 goto CLEANUP;
424 }
425
426 /* Ensure there are some degrees of freedom */
427 if (m < nfree) {
428 info = MP_ERR_DOF;
429 goto CLEANUP;
430 }
431
432 /* Allocate temporary storage */
433 mp_malloc(fvec, double, m);
434 mp_malloc(qtf, double, nfree);
435 mp_malloc(x, double, nfree);
436 mp_malloc(xnew, double, npar);
437 mp_malloc(fjac, double, m*nfree);
438 ldfjac = m;
439 mp_malloc(diag, double, npar);
440 mp_malloc(wa1, double, npar);
441 mp_malloc(wa2, double, npar);
442 mp_malloc(wa3, double, npar);
443 mp_malloc(wa4, double, m);
444 mp_malloc(ipvt, int, npar);
445
446 /* Evaluate user function with initial parameter values */
447 iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data);
448 nfev += 1;
449 if (iflag < 0) {
450 goto CLEANUP;
451 }
452
453 fnorm = mp_enorm(m, fvec);
454 orignorm = fnorm*fnorm;
455
456 /* Make a new copy */
457 for (i=0; i<npar; i++) {
458 xnew[i] = xall[i];
459 }
460
461 /* Transfer free parameters to 'x' */
462 for (i=0; i<nfree; i++) {
463 x[i] = xall[ifree[i]];
464 }
465
466 /* Initialize Levelberg-Marquardt parameter and iteration counter */
467
468 par = 0.0;
469 iter = 1;
470 for (i=0; i<nfree; i++) {
471 qtf[i] = 0;
472 }
473
474 /* Beginning of the outer loop */
475 OUTER_LOOP:
476 for (i=0; i<nfree; i++) {
477 xnew[ifree[i]] = x[i];
478 }
479
480 /* XXX call iterproc */
481
482 /* Calculate the jacobian matrix */
483 iflag = mp_fdjac2(funct, m, nfree, ifree, npar, xnew, fvec, fjac, ldfjac,
484 conf.epsfcn, wa4, private_data, &nfev,
485 step, dstep, mpside, qulim, ulim,
486 ddebug, ddrtol, ddatol);
487 if (iflag < 0) {
488 goto CLEANUP;
489 }
490
491 /* Determine if any of the parameters are pegged at the limits */
492 qanypegged = 0;
493 if (qanylim) {
494 for (j=0; j<nfree; j++) {
495 int lpegged = (qllim[j] && (x[j] == llim[j]));
496 int upegged = (qulim[j] && (x[j] == ulim[j]));
497 sum = 0;
498
499 if (lpegged || upegged) {
500 qanypegged = 1;
501 ij = j*ldfjac;
502 for (i=0; i<m; i++, ij++) {
503 sum += fvec[i] * fjac[ij];
504 }
505 }
506 if (lpegged && (sum > 0)) {
507 ij = j*ldfjac;
508 for (i=0; i<m; i++, ij++) fjac[ij] = 0;
509 }
510 if (upegged && (sum < 0)) {
511 ij = j*ldfjac;
512 for (i=0; i<m; i++, ij++) fjac[ij] = 0;
513 }
514 }
515 }
516
517 /* Compute the QR factorization of the jacobian */
518 mp_qrfac(m,nfree,fjac,ldfjac,1,ipvt,nfree,wa1,wa2,wa3);
519
520 /*
521 * on the first iteration and if mode is 1, scale according
522 * to the norms of the columns of the initial jacobian.
523 */
524 if (iter == 1) {
525 if (conf.douserscale == 0) {
526 for (j=0; j<nfree; j++) {
527 diag[ifree[j]] = wa2[j];
528 if (wa2[j] == zero ) {
529 diag[ifree[j]] = one;
530 }
531 }
532 }
533
534 /*
535 * on the first iteration, calculate the norm of the scaled x
536 * and initialize the step bound delta.
537 */
538 for (j=0; j<nfree; j++ ) {
539 wa3[j] = diag[ifree[j]] * x[j];
540 }
541
542 xnorm = mp_enorm(nfree, wa3);
543 delta = conf.stepfactor*xnorm;
544 if (delta == zero) delta = conf.stepfactor;
545 }
546
547 /*
548 * form (q transpose)*fvec and store the first n components in
549 * qtf.
550 */
551 for (i=0; i<m; i++ ) {
552 wa4[i] = fvec[i];
553 }
554
555 jj = 0;
556 for (j=0; j<nfree; j++ ) {
557 temp3 = fjac[jj];
558 if (temp3 != zero) {
559 sum = zero;
560 ij = jj;
561 for (i=j; i<m; i++ ) {
562 sum += fjac[ij] * wa4[i];
563 ij += 1; /* fjac[i+m*j] */
564 }
565 temp = -sum / temp3;
566 ij = jj;
567 for (i=j; i<m; i++ ) {
568 wa4[i] += fjac[ij] * temp;
569 ij += 1; /* fjac[i+m*j] */
570 }
571 }
572 fjac[jj] = wa1[j];
573 jj += m+1; /* fjac[j+m*j] */
574 qtf[j] = wa4[j];
575 }
576
577 /* ( From this point on, only the square matrix, consisting of the
578 triangle of R, is needed.) */
579
580
581 if (conf.nofinitecheck) {
582 /* Check for overflow. This should be a cheap test here since FJAC
583 has been reduced to a (small) square matrix, and the test is
584 O(N^2). */
585 int off = 0, nonfinite = 0;
586
587 for (j=0; j<nfree; j++) {
588 for (i=0; i<nfree; i++) {
589 if (mpfinite(fjac[off+i]) == 0) nonfinite = 1;
590 }
591 off += ldfjac;
592 }
593
594 if (nonfinite) {
595 info = MP_ERR_NAN;
596 goto CLEANUP;
597 }
598 }
599
600
601 /*
602 * compute the norm of the scaled gradient.
603 */
604 gnorm = zero;
605 if (fnorm != zero) {
606 jj = 0;
607 for (j=0; j<nfree; j++ ) {
608 l = ipvt[j];
609 if (wa2[l] != zero) {
610 sum = zero;
611 ij = jj;
612 for (i=0; i<=j; i++ ) {
613 sum += fjac[ij]*(qtf[i]/fnorm);
614 ij += 1; /* fjac[i+m*j] */
615 }
616 gnorm = mp_dmax1(gnorm,fabs(sum/wa2[l]));
617 }
618 jj += m;
619 }
620 }
621
622 /*
623 * test for convergence of the gradient norm.
624 */
625 if (gnorm <= conf.gtol) info = MP_OK_DIR;
626 if (info != 0) goto L300;
627 if (conf.maxiter == 0) goto L300;
628
629 /*
630 * rescale if necessary.
631 */
632 if (conf.douserscale == 0) {
633 for (j=0; j<nfree; j++ ) {
634 diag[ifree[j]] = mp_dmax1(diag[ifree[j]],wa2[j]);
635 }
636 }
637
638 /*
639 * beginning of the inner loop.
640 */
641 L200:
642 /*
643 * determine the levenberg-marquardt parameter.
644 */
645 mp_lmpar(nfree,fjac,ldfjac,ipvt,ifree,diag,qtf,delta,&par,wa1,wa2,wa3,wa4);
646 /*
647 * store the direction p and x + p. calculate the norm of p.
648 */
649 for (j=0; j<nfree; j++ ) {
650 wa1[j] = -wa1[j];
651 }
652
653 alpha = 1.0;
654 if (qanylim == 0) {
655 /* No parameter limits, so just move to new position WA2 */
656 for (j=0; j<nfree; j++ ) {
657 wa2[j] = x[j] + wa1[j];
658 }
659
660 } else {
661 /* Respect the limits. If a step were to go out of bounds, then
662 * we should take a step in the same direction but shorter distance.
663 * The step should take us right to the limit in that case.
664 */
665 for (j=0; j<nfree; j++) {
666 int lpegged = (qllim[j] && (x[j] <= llim[j]));
667 int upegged = (qulim[j] && (x[j] >= ulim[j]));
668 int dwa1 = fabs(wa1[j]) > MP_MACHEP0;
669
670 if (lpegged && (wa1[j] < 0)) wa1[j] = 0;
671 if (upegged && (wa1[j] > 0)) wa1[j] = 0;
672
673 if (dwa1 && qllim[j] && ((x[j] + wa1[j]) < llim[j])) {
674 alpha = mp_dmin1(alpha, (llim[j]-x[j])/wa1[j]);
675 }
676 if (dwa1 && qulim[j] && ((x[j] + wa1[j]) > ulim[j])) {
677 alpha = mp_dmin1(alpha, (ulim[j]-x[j])/wa1[j]);
678 }
679 }
680
681 /* Scale the resulting vector, advance to the next position */
682 for (j=0; j<nfree; j++) {
683 double sgnu, sgnl;
684 double ulim1, llim1;
685
686 wa1[j] = wa1[j] * alpha;
687 wa2[j] = x[j] + wa1[j];
688
689 /* Adjust the output values. If the step put us exactly
690 * on a boundary, make sure it is exact.
691 */
692 sgnu = (ulim[j] >= 0) ? (+1) : (-1);
693 sgnl = (llim[j] >= 0) ? (+1) : (-1);
694 ulim1 = ulim[j]*(1-sgnu*MP_MACHEP0) - ((ulim[j] == 0)?(MP_MACHEP0):0);
695 llim1 = llim[j]*(1+sgnl*MP_MACHEP0) + ((llim[j] == 0)?(MP_MACHEP0):0);
696
697 if (qulim[j] && (wa2[j] >= ulim1)) {
698 wa2[j] = ulim[j];
699 }
700 if (qllim[j] && (wa2[j] <= llim1)) {
701 wa2[j] = llim[j];
702 }
703 }
704
705 }
706
707 for (j=0; j<nfree; j++ ) {
708 wa3[j] = diag[ifree[j]]*wa1[j];
709 }
710
711 pnorm = mp_enorm(nfree,wa3);
712
713 /*
714 * on the first iteration, adjust the initial step bound.
715 */
716 if (iter == 1) {
717 delta = mp_dmin1(delta,pnorm);
718 }
719
720 /*
721 * evaluate the function at x + p and calculate its norm.
722 */
723 for (i=0; i<nfree; i++) {
724 xnew[ifree[i]] = wa2[i];
725 }
726
727 iflag = mp_call(funct, m, npar, xnew, wa4, 0, private_data);
728 nfev += 1;
729 if (iflag < 0) goto L300;
730
731 fnorm1 = mp_enorm(m,wa4);
732
733 /*
734 * compute the scaled actual reduction.
735 */
736 actred = -one;
737 if ((p1*fnorm1) < fnorm) {
738 temp = fnorm1/fnorm;
739 actred = one - temp * temp;
740 }
741
742 /*
743 * compute the scaled predicted reduction and
744 * the scaled directional derivative.
745 */
746 jj = 0;
747 for (j=0; j<nfree; j++ ) {
748 wa3[j] = zero;
749 l = ipvt[j];
750 temp = wa1[l];
751 ij = jj;
752 for (i=0; i<=j; i++ ) {
753 wa3[i] += fjac[ij]*temp;
754 ij += 1; /* fjac[i+m*j] */
755 }
756 jj += m;
757 }
758
759 /* Remember, alpha is the fraction of the full LM step actually
760 * taken
761 */
762
763 temp1 = mp_enorm(nfree,wa3)*alpha/fnorm;
764 temp2 = (sqrt(alpha*par)*pnorm)/fnorm;
765 prered = temp1*temp1 + (temp2*temp2)/p5;
766 dirder = -(temp1*temp1 + temp2*temp2);
767
768 /*
769 * compute the ratio of the actual to the predicted
770 * reduction.
771 */
772 ratio = zero;
773 if (prered != zero) {
774 ratio = actred/prered;
775 }
776
777 /*
778 * update the step bound.
779 */
780
781 if (ratio <= p25) {
782 if (actred >= zero) {
783 temp = p5;
784 } else {
785 temp = p5*dirder/(dirder + p5*actred);
786 }
787 if (((p1*fnorm1) >= fnorm)
788 || (temp < p1) ) {
789 temp = p1;
790 }
791 delta = temp*mp_dmin1(delta,pnorm/p1);
792 par = par/temp;
793 } else {
794 if ((par == zero) || (ratio >= p75) ) {
795 delta = pnorm/p5;
796 par = p5*par;
797 }
798 }
799
800 /*
801 * test for successful iteration.
802 */
803 if (ratio >= p0001) {
804
805 /*
806 * successful iteration. update x, fvec, and their norms.
807 */
808 for (j=0; j<nfree; j++ ) {
809 x[j] = wa2[j];
810 wa2[j] = diag[ifree[j]]*x[j];
811 }
812 for (i=0; i<m; i++ ) {
813 fvec[i] = wa4[i];
814 }
815 xnorm = mp_enorm(nfree,wa2);
816 fnorm = fnorm1;
817 iter += 1;
818 }
819
820 /*
821 * tests for convergence.
822 */
823 if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) &&
824 (p5*ratio <= one) ) {
825 info = MP_OK_CHI;
826 }
827 if (delta <= conf.xtol*xnorm) {
828 info = MP_OK_PAR;
829 }
830 if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && (p5*ratio <= one)
831 && ( info == 2) ) {
832 info = MP_OK_BOTH;
833 }
834 if (info != 0) {
835 goto L300;
836 }
837
838 /*
839 * tests for termination and stringent tolerances.
840 */
841 if ((conf.maxfev > 0) && (nfev >= conf.maxfev)) {
842 /* Too many function evaluations */
843 info = MP_MAXITER;
844 }
845 if (iter >= conf.maxiter) {
846 /* Too many iterations */
847 info = MP_MAXITER;
848 }
849 if ((fabs(actred) <= MP_MACHEP0) && (prered <= MP_MACHEP0) && (p5*ratio <= one) ) {
850 info = MP_FTOL;
851 }
852 if (delta <= MP_MACHEP0*xnorm) {
853 info = MP_XTOL;
854 }
855 if (gnorm <= MP_MACHEP0) {
856 info = MP_GTOL;
857 }
858 if (info != 0) {
859 goto L300;
860 }
861
862 /*
863 * end of the inner loop. repeat if iteration unsuccessful.
864 */
865 if (ratio < p0001) goto L200;
866 /*
867 * end of the outer loop.
868 */
869 goto OUTER_LOOP;
870
871 L300:
872 /*
873 * termination, either normal or user imposed.
874 */
875 if (iflag < 0) {
876 info = iflag;
877 }
878 iflag = 0;
879
880 for (i=0; i<nfree; i++) {
881 xall[ifree[i]] = x[i];
882 }
883
884 if ((conf.nprint > 0) && (info > 0)) {
885 iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data);
886 nfev += 1;
887 }
888
889 /* Compute number of pegged parameters */
890 npegged = 0;
891 if (pars) for (i=0; i<npar; i++) {
892 if ((pars[i].limited[0] && (pars[i].limits[0] == xall[i])) ||
893 (pars[i].limited[1] && (pars[i].limits[1] == xall[i]))) {
894 npegged ++;
895 }
896 }
897
898 /* Compute and return the covariance matrix and/or parameter errors */
899 if (result && (result->covar || result->xerror)) {
900 mp_covar(nfree, fjac, ldfjac, ipvt, conf.covtol, wa2);
901
902 if (result->covar) {
903 /* Zero the destination covariance array */
904 for (j=0; j<(npar*npar); j++) result->covar[j] = 0;
905
906 /* Transfer the covariance array */
907 for (j=0; j<nfree; j++) {
908 for (i=0; i<nfree; i++) {
909 result->covar[ifree[j]*npar+ifree[i]] = fjac[j*ldfjac+i];
910 }
911 }
912 }
913
914 if (result->xerror) {
915 for (j=0; j<npar; j++) result->xerror[j] = 0;
916
917 for (j=0; j<nfree; j++) {
918 double cc = fjac[j*ldfjac+j];
919 if (cc > 0) result->xerror[ifree[j]] = sqrt(cc);
920 }
921 }
922 }
923
924 if (result) {
925 strcpy(result->version, MPFIT_VERSION);
926 result->bestnorm = mp_dmax1(fnorm,fnorm1);
927 result->bestnorm *= result->bestnorm;
928 result->orignorm = orignorm;
929 result->status = info;
930 result->niter = iter;
931 result->nfev = nfev;
932 result->npar = npar;
933 result->nfree = nfree;
934 result->npegged = npegged;
935 result->nfunc = m;
936
937 /* Copy residuals if requested */
938 if (result->resid) {
939 for (j=0; j<m; j++) result->resid[j] = fvec[j];
940 }
941 }
942
943
944 CLEANUP:
945 if (fvec) free(fvec);
946 if (qtf) free(qtf);
947 if (x) free(x);
948 if (xnew) free(xnew);
949 if (fjac) free(fjac);
950 if (diag) free(diag);
951 if (wa1) free(wa1);
952 if (wa2) free(wa2);
953 if (wa3) free(wa3);
954 if (wa4) free(wa4);
955 if (ipvt) free(ipvt);
956 if (pfixed) free(pfixed);
957 if (step) free(step);
958 if (dstep) free(dstep);
959 if (mpside) free(mpside);
960 if (ddebug) free(ddebug);
961 if (ddrtol) free(ddrtol);
962 if (ddatol) free(ddatol);
963 if (ifree) free(ifree);
964 if (qllim) free(qllim);
965 if (qulim) free(qulim);
966 if (llim) free(llim);
967 if (ulim) free(ulim);
968
969
970 return info;
971 }
972
973
974 /************************fdjac2.c*************************/
975
976 static
977 int mp_fdjac2(mp_func funct,
978 int m, int n, int *ifree, int npar, double *x, double *fvec,
979 double *fjac, int ldfjac, double epsfcn,
980 double *wa, void *priv, int *nfev,
981 double *step, double *dstep, int *dside,
982 int *qulimited, double *ulimit,
983 int *ddebug, double *ddrtol, double *ddatol)
984 {
985 /*
986 * **********
987 *
988 * subroutine fdjac2
989 *
990 * this subroutine computes a forward-difference approximation
991 * to the m by n jacobian matrix associated with a specified
992 * problem of m functions in n variables.
993 *
994 * the subroutine statement is
995 *
996 * subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
997 *
998 * where
999 *
1000 * fcn is the name of the user-supplied subroutine which
1001 * calculates the functions. fcn must be declared
1002 * in an external statement in the user calling
1003 * program, and should be written as follows.
1004 *
1005 * subroutine fcn(m,n,x,fvec,iflag)
1006 * integer m,n,iflag
1007 * double precision x(n),fvec(m)
1008 * ----------
1009 * calculate the functions at x and
1010 * return this vector in fvec.
1011 * ----------
1012 * return
1013 * end
1014 *
1015 * the value of iflag should not be changed by fcn unless
1016 * the user wants to terminate execution of fdjac2.
1017 * in this case set iflag to a negative integer.
1018 *
1019 * m is a positive integer input variable set to the number
1020 * of functions.
1021 *
1022 * n is a positive integer input variable set to the number
1023 * of variables. n must not exceed m.
1024 *
1025 * x is an input array of length n.
1026 *
1027 * fvec is an input array of length m which must contain the
1028 * functions evaluated at x.
1029 *
1030 * fjac is an output m by n array which contains the
1031 * approximation to the jacobian matrix evaluated at x.
1032 *
1033 * ldfjac is a positive integer input variable not less than m
1034 * which specifies the leading dimension of the array fjac.
1035 *
1036 * iflag is an integer variable which can be used to terminate
1037 * the execution of fdjac2. see description of fcn.
1038 *
1039 * epsfcn is an input variable used in determining a suitable
1040 * step length for the forward-difference approximation. this
1041 * approximation assumes that the relative errors in the
1042 * functions are of the order of epsfcn. if epsfcn is less
1043 * than the machine precision, it is assumed that the relative
1044 * errors in the functions are of the order of the machine
1045 * precision.
1046 *
1047 * wa is a work array of length m.
1048 *
1049 * subprograms called
1050 *
1051 * user-supplied ...... fcn
1052 *
1053 * minpack-supplied ... dpmpar
1054 *
1055 * fortran-supplied ... dabs,dmax1,dsqrt
1056 *
1057 * argonne national laboratory. minpack project. march 1980.
1058 * burton s. garbow, kenneth e. hillstrom, jorge j. more
1059 *
1060 **********
1061 */
1062 int i,j,ij;
1063 int iflag = 0;
1064 double eps,h,temp;
1065 static double zero = 0.0;
1066 double **dvec = 0;
1067 int has_analytical_deriv = 0, has_numerical_deriv = 0;
1068 int has_debug_deriv = 0;
1069
1070 temp = mp_dmax1(epsfcn,MP_MACHEP0);
1071 eps = sqrt(temp);
1072 ij = 0;
1073 ldfjac = 0; /* Prevents compiler warning */
1074
1075 dvec = (double **) malloc(sizeof(double **)*npar);
1076 if (dvec == 0) return MP_ERR_MEMORY;
1077 for (j=0; j<npar; j++) dvec[j] = 0;
1078
1079 /* Initialize the Jacobian derivative matrix */
1080 for (j=0; j<(n*m); j++) fjac[j] = 0;
1081
1082 /* Check for which parameters need analytical derivatives and which
1083 need numerical ones */
1084 for (j=0; j<n; j++) { /* Loop through free parameters only */
1085 if (dside && dside[ifree[j]] == 3 && ddebug[ifree[j]] == 0) {
1086 /* Purely analytical derivatives */
1087 dvec[ifree[j]] = fjac + j*m;
1088 has_analytical_deriv = 1;
1089 } else if (dside && ddebug[ifree[j]] == 1) {
1090 /* Numerical and analytical derivatives as a debug cross-check */
1091 dvec[ifree[j]] = fjac + j*m;
1092 has_analytical_deriv = 1;
1093 has_numerical_deriv = 1;
1094 has_debug_deriv = 1;
1095 } else {
1096 has_numerical_deriv = 1;
1097 }
1098 }
1099
1100 /* If there are any parameters requiring analytical derivatives,
1101 then compute them first. */
1102 if (has_analytical_deriv) {
1103 iflag = mp_call(funct, m, npar, x, wa, dvec, priv);
1104 if (nfev) *nfev = *nfev + 1;
1105 if (iflag < 0 ) goto DONE;
1106 }
1107
1108 if (has_debug_deriv) {
1109 printf("FJAC DEBUG BEGIN\n");
1110 printf("# %10s %10s %10s %10s %10s %10s\n",
1111 "IPNT", "FUNC", "DERIV_U", "DERIV_N", "DIFF_ABS", "DIFF_REL");
1112 }
1113
1114 /* Any parameters requiring numerical derivatives */
1115 if (has_numerical_deriv) for (j=0; j<n; j++) { /* Loop thru free parms */
1116 int dsidei = (dside)?(dside[ifree[j]]):(0);
1117 int debug = ddebug[ifree[j]];
1118 double dr = ddrtol[ifree[j]], da = ddatol[ifree[j]];
1119
1120 /* Check for debugging */
1121 if (debug) {
1122 printf("FJAC PARM %d\n", ifree[j]);
1123 }
1124
1125 /* Skip parameters already done by user-computed partials */
1126 if (dside && dsidei == 3) continue;
1127
1128 temp = x[ifree[j]];
1129 h = eps * fabs(temp);
1130 if (step && step[ifree[j]] > 0) h = step[ifree[j]];
1131 if (dstep && dstep[ifree[j]] > 0) h = fabs(dstep[ifree[j]]*temp);
1132 if (h == zero) h = eps;
1133
1134 /* If negative step requested, or we are against the upper limit */
1135 if ((dside && dsidei == -1) ||
1136 (dside && dsidei == 0 &&
1137 qulimited && ulimit && qulimited[j] &&
1138 (temp > (ulimit[j]-h)))) {
1139 h = -h;
1140 }
1141
1142 x[ifree[j]] = temp + h;
1143 iflag = mp_call(funct, m, npar, x, wa, 0, priv);
1144 if (nfev) *nfev = *nfev + 1;
1145 if (iflag < 0 ) goto DONE;
1146 x[ifree[j]] = temp;
1147
1148 if (dsidei <= 1) {
1149 /* COMPUTE THE ONE-SIDED DERIVATIVE */
1150 if (! debug) {
1151 /* Non-debug path for speed */
1152 for (i=0; i<m; i++, ij++) {
1153 fjac[ij] = (wa[i] - fvec[i])/h; /* fjac[i+m*j] */
1154 }
1155 } else {
1156 /* Debug path for correctness */
1157 for (i=0; i<m; i++, ij++) {
1158 double fjold = fjac[ij];
1159 fjac[ij] = (wa[i] - fvec[i])/h; /* fjac[i+m*j] */
1160 if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) ||
1161 ((da != 0 || dr != 0) && (fabs(fjold-fjac[ij]) > da + fabs(fjold)*dr))) {
1162 printf(" %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n",
1163 i, fvec[i], fjold, fjac[ij], fjold-fjac[ij],
1164 (fjold == 0)?(0):((fjold-fjac[ij])/fjold));
1165 }
1166 }
1167 }
1168
1169 } else {
1170 /* COMPUTE THE TWO-SIDED DERIVATIVE */
1171 for (i=0; i<m; i++, ij++) {
1172 fjac[ij] = wa[i]; /* Store temp data: fjac[i+m*j] */
1173 }
1174
1175 /* Evaluate at x - h */
1176 x[ifree[j]] = temp - h;
1177 iflag = mp_call(funct, m, npar, x, wa, 0, priv);
1178 if (nfev) *nfev = *nfev + 1;
1179 if (iflag < 0 ) goto DONE;
1180 x[ifree[j]] = temp;
1181
1182 /* Now compute derivative as (f(x+h) - f(x-h))/(2h) */
1183 ij -= m;
1184 if (! debug ) {
1185 for (i=0; i<m; i++, ij++) {
1186 fjac[ij] = (fjac[ij] - wa[i])/(2*h); /* fjac[i+m*j] */
1187 }
1188 } else {
1189 for (i=0; i<m; i++, ij++) {
1190 double fjold = fjac[ij];
1191 fjac[ij] = (fjac[ij] - wa[i])/(2*h); /* fjac[i+m*j] */
1192 if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) ||
1193 ((da != 0 || dr != 0) && (fabs(fjold-fjac[ij]) > da + fabs(fjold)*dr))) {
1194 printf(" %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n",
1195 i, fvec[i], fjold, fjac[ij], fjold-fjac[ij],
1196 (fjold == 0)?(0):((fjold-fjac[ij])/fjold));
1197 }
1198 }
1199 }
1200
1201 }
1202 }
1203
1204 if (has_debug_deriv) {
1205 printf("FJAC DEBUG END\n");
1206 }
1207
1208 DONE:
1209 if (dvec) free(dvec);
1210 if (iflag < 0) return iflag;
1211 return 0;
1212 /*
1213 * last card of subroutine fdjac2.
1214 */
1215 }
1216
1217
1218 /************************qrfac.c*************************/
1219
1220 static
1221 void mp_qrfac(int m, int n, double *a, int lda,
1222 int pivot, int *ipvt, int lipvt,
1223 double *rdiag, double *acnorm, double *wa)
1224 {
1225 /*
1226 * **********
1227 *
1228 * subroutine qrfac
1229 *
1230 * this subroutine uses householder transformations with column
1231 * pivoting (optional) to compute a qr factorization of the
1232 * m by n matrix a. that is, qrfac determines an orthogonal
1233 * matrix q, a permutation matrix p, and an upper trapezoidal
1234 * matrix r with diagonal elements of nonincreasing magnitude,
1235 * such that a*p = q*r. the householder transformation for
1236 * column k, k = 1,2,...,min(m,n), is of the form
1237 *
1238 * t
1239 * i - (1/u(k))*u*u
1240 *
1241 * where u has zeros in the first k-1 positions. the form of
1242 * this transformation and the method of pivoting first
1243 * appeared in the corresponding linpack subroutine.
1244 *
1245 * the subroutine statement is
1246 *
1247 * subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
1248 *
1249 * where
1250 *
1251 * m is a positive integer input variable set to the number
1252 * of rows of a.
1253 *
1254 * n is a positive integer input variable set to the number
1255 * of columns of a.
1256 *
1257 * a is an m by n array. on input a contains the matrix for
1258 * which the qr factorization is to be computed. on output
1259 * the strict upper trapezoidal part of a contains the strict
1260 * upper trapezoidal part of r, and the lower trapezoidal
1261 * part of a contains a factored form of q (the non-trivial
1262 * elements of the u vectors described above).
1263 *
1264 * lda is a positive integer input variable not less than m
1265 * which specifies the leading dimension of the array a.
1266 *
1267 * pivot is a logical input variable. if pivot is set true,
1268 * then column pivoting is enforced. if pivot is set false,
1269 * then no column pivoting is done.
1270 *
1271 * ipvt is an integer output array of length lipvt. ipvt
1272 * defines the permutation matrix p such that a*p = q*r.
1273 * column j of p is column ipvt(j) of the identity matrix.
1274 * if pivot is false, ipvt is not referenced.
1275 *
1276 * lipvt is a positive integer input variable. if pivot is false,
1277 * then lipvt may be as small as 1. if pivot is true, then
1278 * lipvt must be at least n.
1279 *
1280 * rdiag is an output array of length n which contains the
1281 * diagonal elements of r.
1282 *
1283 * acnorm is an output array of length n which contains the
1284 * norms of the corresponding columns of the input matrix a.
1285 * if this information is not needed, then acnorm can coincide
1286 * with rdiag.
1287 *
1288 * wa is a work array of length n. if pivot is false, then wa
1289 * can coincide with rdiag.
1290 *
1291 * subprograms called
1292 *
1293 * minpack-supplied ... dpmpar,enorm
1294 *
1295 * fortran-supplied ... dmax1,dsqrt,min0
1296 *
1297 * argonne national laboratory. minpack project. march 1980.
1298 * burton s. garbow, kenneth e. hillstrom, jorge j. more
1299 *
1300 * **********
1301 */
1302 int i,ij,jj,j,jp1,k,kmax,minmn;
1303 double ajnorm,sum,temp;
1304 static double zero = 0.0;
1305 static double one = 1.0;
1306 static double p05 = 0.05;
1307
1308 lda = 0; /* Prevent compiler warning */
1309 lipvt = 0; /* Prevent compiler warning */
1310
1311 /*
1312 * compute the initial column norms and initialize several arrays.
1313 */
1314 ij = 0;
1315 for (j=0; j<n; j++) {
1316 acnorm[j] = mp_enorm(m,&a[ij]);
1317 rdiag[j] = acnorm[j];
1318 wa[j] = rdiag[j];
1319 if (pivot != 0)
1320 ipvt[j] = j;
1321 ij += m; /* m*j */
1322 }
1323 /*
1324 * reduce a to r with householder transformations.
1325 */
1326 minmn = mp_min0(m,n);
1327 for (j=0; j<minmn; j++) {
1328 if (pivot == 0)
1329 goto L40;
1330 /*
1331 * bring the column of largest norm into the pivot position.
1332 */
1333 kmax = j;
1334 for (k=j; k<n; k++)
1335 {
1336 if (rdiag[k] > rdiag[kmax])
1337 kmax = k;
1338 }
1339 if (kmax == j)
1340 goto L40;
1341
1342 ij = m * j;
1343 jj = m * kmax;
1344 for (i=0; i<m; i++)
1345 {
1346 temp = a[ij]; /* [i+m*j] */
1347 a[ij] = a[jj]; /* [i+m*kmax] */
1348 a[jj] = temp;
1349 ij += 1;
1350 jj += 1;
1351 }
1352 rdiag[kmax] = rdiag[j];
1353 wa[kmax] = wa[j];
1354 k = ipvt[j];
1355 ipvt[j] = ipvt[kmax];
1356 ipvt[kmax] = k;
1357
1358 L40:
1359 /*
1360 * compute the householder transformation to reduce the
1361 * j-th column of a to a multiple of the j-th unit vector.
1362 */
1363 jj = j + m*j;
1364 ajnorm = mp_enorm(m-j,&a[jj]);
1365 if (ajnorm == zero)
1366 goto L100;
1367 if (a[jj] < zero)
1368 ajnorm = -ajnorm;
1369 ij = jj;
1370 for (i=j; i<m; i++)
1371 {
1372 a[ij] /= ajnorm;
1373 ij += 1; /* [i+m*j] */
1374 }
1375 a[jj] += one;
1376 /*
1377 * apply the transformation to the remaining columns
1378 * and update the norms.
1379 */
1380 jp1 = j + 1;
1381 if (jp1 < n)
1382 {
1383 for (k=jp1; k<n; k++)
1384 {
1385 sum = zero;
1386 ij = j + m*k;
1387 jj = j + m*j;
1388 for (i=j; i<m; i++)
1389 {
1390 sum += a[jj]*a[ij];
1391 ij += 1; /* [i+m*k] */
1392 jj += 1; /* [i+m*j] */
1393 }
1394 temp = sum/a[j+m*j];
1395 ij = j + m*k;
1396 jj = j + m*j;
1397 for (i=j; i<m; i++)
1398 {
1399 a[ij] -= temp*a[jj];
1400 ij += 1; /* [i+m*k] */
1401 jj += 1; /* [i+m*j] */
1402 }
1403 if ((pivot != 0) && (rdiag[k] != zero))
1404 {
1405 temp = a[j+m*k]/rdiag[k];
1406 temp = mp_dmax1( zero, one-temp*temp );
1407 rdiag[k] *= sqrt(temp);
1408 temp = rdiag[k]/wa[k];
1409 if ((p05*temp*temp) <= MP_MACHEP0)
1410 {
1411 rdiag[k] = mp_enorm(m-j-1,&a[jp1+m*k]);
1412 wa[k] = rdiag[k];
1413 }
1414 }
1415 }
1416 }
1417
1418 L100:
1419 rdiag[j] = -ajnorm;
1420 }
1421 /*
1422 * last card of subroutine qrfac.
1423 */
1424 }
1425
1426 /************************qrsolv.c*************************/
1427
1428 static
1429 void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,
1430 double *qtb, double *x, double *sdiag, double *wa)
1431 {
1432 /*
1433 * **********
1434 *
1435 * subroutine qrsolv
1436 *
1437 * given an m by n matrix a, an n by n diagonal matrix d,
1438 * and an m-vector b, the problem is to determine an x which
1439 * solves the system
1440 *
1441 * a*x = b , d*x = 0 ,
1442 *
1443 * in the least squares sense.
1444 *
1445 * this subroutine completes the solution of the problem
1446 * if it is provided with the necessary information from the
1447 * qr factorization, with column pivoting, of a. that is, if
1448 * a*p = q*r, where p is a permutation matrix, q has orthogonal
1449 * columns, and r is an upper triangular matrix with diagonal
1450 * elements of nonincreasing magnitude, then qrsolv expects
1451 * the full upper triangle of r, the permutation matrix p,
1452 * and the first n components of (q transpose)*b. the system
1453 * a*x = b, d*x = 0, is then equivalent to
1454 *
1455 * t t
1456 * r*z = q *b , p *d*p*z = 0 ,
1457 *
1458 * where x = p*z. if this system does not have full rank,
1459 * then a least squares solution is obtained. on output qrsolv
1460 * also provides an upper triangular matrix s such that
1461 *
1462 * t t t
1463 * p *(a *a + d*d)*p = s *s .
1464 *
1465 * s is computed within qrsolv and may be of separate interest.
1466 *
1467 * the subroutine statement is
1468 *
1469 * subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
1470 *
1471 * where
1472 *
1473 * n is a positive integer input variable set to the order of r.
1474 *
1475 * r is an n by n array. on input the full upper triangle
1476 * must contain the full upper triangle of the matrix r.
1477 * on output the full upper triangle is unaltered, and the
1478 * strict lower triangle contains the strict upper triangle
1479 * (transposed) of the upper triangular matrix s.
1480 *
1481 * ldr is a positive integer input variable not less than n
1482 * which specifies the leading dimension of the array r.
1483 *
1484 * ipvt is an integer input array of length n which defines the
1485 * permutation matrix p such that a*p = q*r. column j of p
1486 * is column ipvt(j) of the identity matrix.
1487 *
1488 * diag is an input array of length n which must contain the
1489 * diagonal elements of the matrix d.
1490 *
1491 * qtb is an input array of length n which must contain the first
1492 * n elements of the vector (q transpose)*b.
1493 *
1494 * x is an output array of length n which contains the least
1495 * squares solution of the system a*x = b, d*x = 0.
1496 *
1497 * sdiag is an output array of length n which contains the
1498 * diagonal elements of the upper triangular matrix s.
1499 *
1500 * wa is a work array of length n.
1501 *
1502 * subprograms called
1503 *
1504 * fortran-supplied ... dabs,dsqrt
1505 *
1506 * argonne national laboratory. minpack project. march 1980.
1507 * burton s. garbow, kenneth e. hillstrom, jorge j. more
1508 *
1509 * **********
1510 */
1511 int i,ij,ik,kk,j,jp1,k,kp1,l,nsing;
1512 double cosx,cotan,qtbpj,sinx,sum,tanx,temp;
1513 static double zero = 0.0;
1514 static double p25 = 0.25;
1515 static double p5 = 0.5;
1516
1517 /*
1518 * copy r and (q transpose)*b to preserve input and initialize s.
1519 * in particular, save the diagonal elements of r in x.
1520 */
1521 kk = 0;
1522 for (j=0; j<n; j++) {
1523 ij = kk;
1524 ik = kk;
1525 for (i=j; i<n; i++)
1526 {
1527 r[ij] = r[ik];
1528 ij += 1; /* [i+ldr*j] */
1529 ik += ldr; /* [j+ldr*i] */
1530 }
1531 x[j] = r[kk];
1532 wa[j] = qtb[j];
1533 kk += ldr+1; /* j+ldr*j */
1534 }
1535
1536 /*
1537 * eliminate the diagonal matrix d using a givens rotation.
1538 */
1539 for (j=0; j<n; j++) {
1540 /*
1541 * prepare the row of d to be eliminated, locating the
1542 * diagonal element using p from the qr factorization.
1543 */
1544 l = ipvt[j];
1545 if (diag[l] == zero)
1546 goto L90;
1547 for (k=j; k<n; k++)
1548 sdiag[k] = zero;
1549 sdiag[j] = diag[l];
1550 /*
1551 * the transformations to eliminate the row of d
1552 * modify only a single element of (q transpose)*b
1553 * beyond the first n, which is initially zero.
1554 */
1555 qtbpj = zero;
1556 for (k=j; k<n; k++)
1557 {
1558 /*
1559 * determine a givens rotation which eliminates the
1560 * appropriate element in the current row of d.
1561 */
1562 if (sdiag[k] == zero)
1563 continue;
1564 kk = k + ldr * k;
1565 if (fabs(r[kk]) < fabs(sdiag[k]))
1566 {
1567 cotan = r[kk]/sdiag[k];
1568 sinx = p5/sqrt(p25+p25*cotan*cotan);
1569 cosx = sinx*cotan;
1570 }
1571 else
1572 {
1573 tanx = sdiag[k]/r[kk];
1574 cosx = p5/sqrt(p25+p25*tanx*tanx);
1575 sinx = cosx*tanx;
1576 }
1577 /*
1578 * compute the modified diagonal element of r and
1579 * the modified element of ((q transpose)*b,0).
1580 */
1581 r[kk] = cosx*r[kk] + sinx*sdiag[k];
1582 temp = cosx*wa[k] + sinx*qtbpj;
1583 qtbpj = -sinx*wa[k] + cosx*qtbpj;
1584 wa[k] = temp;
1585 /*
1586 * accumulate the tranformation in the row of s.
1587 */
1588 kp1 = k + 1;
1589 if (n > kp1)
1590 {
1591 ik = kk + 1;
1592 for (i=kp1; i<n; i++)
1593 {
1594 temp = cosx*r[ik] + sinx*sdiag[i];
1595 sdiag[i] = -sinx*r[ik] + cosx*sdiag[i];
1596 r[ik] = temp;
1597 ik += 1; /* [i+ldr*k] */
1598 }
1599 }
1600 }
1601 L90:
1602 /*
1603 * store the diagonal element of s and restore
1604 * the corresponding diagonal element of r.
1605 */
1606 kk = j + ldr*j;
1607 sdiag[j] = r[kk];
1608 r[kk] = x[j];
1609 }
1610 /*
1611 * solve the triangular system for z. if the system is
1612 * singular, then obtain a least squares solution.
1613 */
1614 nsing = n;
1615 for (j=0; j<n; j++) {
1616 if ((sdiag[j] == zero) && (nsing == n))
1617 nsing = j;
1618 if (nsing < n)
1619 wa[j] = zero;
1620 }
1621 if (nsing < 1)
1622 goto L150;
1623
1624 for (k=0; k<nsing; k++) {
1625 j = nsing - k - 1;
1626 sum = zero;
1627 jp1 = j + 1;
1628 if (nsing > jp1)
1629 {
1630 ij = jp1 + ldr * j;
1631 for (i=jp1; i<nsing; i++)
1632 {
1633 sum += r[ij]*wa[i];
1634 ij += 1; /* [i+ldr*j] */
1635 }
1636 }
1637 wa[j] = (wa[j] - sum)/sdiag[j];
1638 }
1639 L150:
1640 /*
1641 * permute the components of z back to components of x.
1642 */
1643 for (j=0; j<n; j++) {
1644 l = ipvt[j];
1645 x[l] = wa[j];
1646 }
1647 /*
1648 * last card of subroutine qrsolv.
1649 */
1650 }
1651
1652 /************************lmpar.c*************************/
1653
1654 static
1655 void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag,
1656 double *qtb, double delta, double *par, double *x,
1657 double *sdiag, double *wa1, double *wa2)
1658 {
1659 /* **********
1660 *
1661 * subroutine lmpar
1662 *
1663 * given an m by n matrix a, an n by n nonsingular diagonal
1664 * matrix d, an m-vector b, and a positive number delta,
1665 * the problem is to determine a value for the parameter
1666 * par such that if x solves the system
1667 *
1668 * a*x = b , sqrt(par)*d*x = 0 ,
1669 *
1670 * in the least squares sense, and dxnorm is the euclidean
1671 * norm of d*x, then either par is zero and
1672 *
1673 * (dxnorm-delta) .le. 0.1*delta ,
1674 *
1675 * or par is positive and
1676 *
1677 * abs(dxnorm-delta) .le. 0.1*delta .
1678 *
1679 * this subroutine completes the solution of the problem
1680 * if it is provided with the necessary information from the
1681 * qr factorization, with column pivoting, of a. that is, if
1682 * a*p = q*r, where p is a permutation matrix, q has orthogonal
1683 * columns, and r is an upper triangular matrix with diagonal
1684 * elements of nonincreasing magnitude, then lmpar expects
1685 * the full upper triangle of r, the permutation matrix p,
1686 * and the first n components of (q transpose)*b. on output
1687 * lmpar also provides an upper triangular matrix s such that
1688 *
1689 * t t t
1690 * p *(a *a + par*d*d)*p = s *s .
1691 *
1692 * s is employed within lmpar and may be of separate interest.
1693 *
1694 * only a few iterations are generally needed for convergence
1695 * of the algorithm. if, however, the limit of 10 iterations
1696 * is reached, then the output par will contain the best
1697 * value obtained so far.
1698 *
1699 * the subroutine statement is
1700 *
1701 * subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,
1702 * wa1,wa2)
1703 *
1704 * where
1705 *
1706 * n is a positive integer input variable set to the order of r.
1707 *
1708 * r is an n by n array. on input the full upper triangle
1709 * must contain the full upper triangle of the matrix r.
1710 * on output the full upper triangle is unaltered, and the
1711 * strict lower triangle contains the strict upper triangle
1712 * (transposed) of the upper triangular matrix s.
1713 *
1714 * ldr is a positive integer input variable not less than n
1715 * which specifies the leading dimension of the array r.
1716 *
1717 * ipvt is an integer input array of length n which defines the
1718 * permutation matrix p such that a*p = q*r. column j of p
1719 * is column ipvt(j) of the identity matrix.
1720 *
1721 * diag is an input array of length n which must contain the
1722 * diagonal elements of the matrix d.
1723 *
1724 * qtb is an input array of length n which must contain the first
1725 * n elements of the vector (q transpose)*b.
1726 *
1727 * delta is a positive input variable which specifies an upper
1728 * bound on the euclidean norm of d*x.
1729 *
1730 * par is a nonnegative variable. on input par contains an
1731 * initial estimate of the levenberg-marquardt parameter.
1732 * on output par contains the final estimate.
1733 *
1734 * x is an output array of length n which contains the least
1735 * squares solution of the system a*x = b, sqrt(par)*d*x = 0,
1736 * for the output par.
1737 *
1738 * sdiag is an output array of length n which contains the
1739 * diagonal elements of the upper triangular matrix s.
1740 *
1741 * wa1 and wa2 are work arrays of length n.
1742 *
1743 * subprograms called
1744 *
1745 * minpack-supplied ... dpmpar,mp_enorm,qrsolv
1746 *
1747 * fortran-supplied ... dabs,mp_dmax1,dmin1,dsqrt
1748 *
1749 * argonne national laboratory. minpack project. march 1980.
1750 * burton s. garbow, kenneth e. hillstrom, jorge j. more
1751 *
1752 * **********
1753 */
1754 int i,iter,ij,jj,j,jm1,jp1,k,l,nsing;
1755 double dxnorm,fp,gnorm,parc,parl,paru;
1756 double sum,temp;
1757 static double zero = 0.0;
1758 /* static double one = 1.0; */
1759 static double p1 = 0.1;
1760 static double p001 = 0.001;
1761
1762 /*
1763 * compute and store in x the gauss-newton direction. if the
1764 * jacobian is rank-deficient, obtain a least squares solution.
1765 */
1766 nsing = n;
1767 jj = 0;
1768 for (j=0; j<n; j++) {
1769 wa1[j] = qtb[j];
1770 if ((r[jj] == zero) && (nsing == n))
1771 nsing = j;
1772 if (nsing < n)
1773 wa1[j] = zero;
1774 jj += ldr+1; /* [j+ldr*j] */
1775 }
1776
1777 if (nsing >= 1) {
1778 for (k=0; k<nsing; k++)
1779 {
1780 j = nsing - k - 1;
1781 wa1[j] = wa1[j]/r[j+ldr*j];
1782 temp = wa1[j];
1783 jm1 = j - 1;
1784 if (jm1 >= 0)
1785 {
1786 ij = ldr * j;
1787 for (i=0; i<=jm1; i++)
1788 {
1789 wa1[i] -= r[ij]*temp;
1790 ij += 1;
1791 }
1792 }
1793 }
1794 }
1795
1796 for (j=0; j<n; j++) {
1797 l = ipvt[j];
1798 x[l] = wa1[j];
1799 }
1800 /*
1801 * initialize the iteration counter.
1802 * evaluate the function at the origin, and test
1803 * for acceptance of the gauss-newton direction.
1804 */
1805 iter = 0;
1806 for (j=0; j<n; j++)
1807 wa2[j] = diag[ifree[j]]*x[j];
1808 dxnorm = mp_enorm(n,wa2);
1809 fp = dxnorm - delta;
1810 if (fp <= p1*delta) {
1811 goto L220;
1812 }
1813 /*
1814 * if the jacobian is not rank deficient, the newton
1815 * step provides a lower bound, parl, for the zero of
1816 * the function. otherwise set this bound to zero.
1817 */
1818 parl = zero;
1819 if (nsing >= n) {
1820 for (j=0; j<n; j++)
1821 {
1822 l = ipvt[j];
1823 wa1[j] = diag[ifree[l]]*(wa2[l]/dxnorm);
1824 }
1825 jj = 0;
1826 for (j=0; j<n; j++)
1827 {
1828 sum = zero;
1829 jm1 = j - 1;
1830 if (jm1 >= 0)
1831 {
1832 ij = jj;
1833 for (i=0; i<=jm1; i++)
1834 {
1835 sum += r[ij]*wa1[i];
1836 ij += 1;
1837 }
1838 }
1839 wa1[j] = (wa1[j] - sum)/r[j+ldr*j];
1840 jj += ldr; /* [i+ldr*j] */
1841 }
1842 temp = mp_enorm(n,wa1);
1843 parl = ((fp/delta)/temp)/temp;
1844 }
1845 /*
1846 * calculate an upper bound, paru, for the zero of the function.
1847 */
1848 jj = 0;
1849 for (j=0; j<n; j++) {
1850 sum = zero;
1851 ij = jj;
1852 for (i=0; i<=j; i++)
1853 {
1854 sum += r[ij]*qtb[i];
1855 ij += 1;
1856 }
1857 l = ipvt[j];
1858 wa1[j] = sum/diag[ifree[l]];
1859 jj += ldr; /* [i+ldr*j] */
1860 }
1861 gnorm = mp_enorm(n,wa1);
1862 paru = gnorm/delta;
1863 if (paru == zero)
1864 paru = MP_DWARF/mp_dmin1(delta,p1);
1865 /*
1866 * if the input par lies outside of the interval (parl,paru),
1867 * set par to the closer endpoint.
1868 */
1869 *par = mp_dmax1( *par,parl);
1870 *par = mp_dmin1( *par,paru);
1871 if (*par == zero)
1872 *par = gnorm/dxnorm;
1873
1874 /*
1875 * beginning of an iteration.
1876 */
1877 L150:
1878 iter += 1;
1879 /*
1880 * evaluate the function at the current value of par.
1881 */
1882 if (*par == zero)
1883 *par = mp_dmax1(MP_DWARF,p001*paru);
1884 temp = sqrt( *par );
1885 for (j=0; j<n; j++)
1886 wa1[j] = temp*diag[ifree[j]];
1887 mp_qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2);
1888 for (j=0; j<n; j++)
1889 wa2[j] = diag[ifree[j]]*x[j];
1890 dxnorm = mp_enorm(n,wa2);
1891 temp = fp;
1892 fp = dxnorm - delta;
1893 /*
1894 * if the function is small enough, accept the current value
1895 * of par. also test for the exceptional cases where parl
1896 * is zero or the number of iterations has reached 10.
1897 */
1898 if ((fabs(fp) <= p1*delta)
1899 || ((parl == zero) && (fp <= temp) && (temp < zero))
1900 || (iter == 10))
1901 goto L220;
1902 /*
1903 * compute the newton correction.
1904 */
1905 for (j=0; j<n; j++) {
1906 l = ipvt[j];
1907 wa1[j] = diag[ifree[l]]*(wa2[l]/dxnorm);
1908 }
1909 jj = 0;
1910 for (j=0; j<n; j++) {
1911 wa1[j] = wa1[j]/sdiag[j];
1912 temp = wa1[j];
1913 jp1 = j + 1;
1914 if (jp1 < n)
1915 {
1916 ij = jp1 + jj;
1917 for (i=jp1; i<n; i++)
1918 {
1919 wa1[i] -= r[ij]*temp;
1920 ij += 1; /* [i+ldr*j] */
1921 }
1922 }
1923 jj += ldr; /* ldr*j */
1924 }
1925 temp = mp_enorm(n,wa1);
1926 parc = ((fp/delta)/temp)/temp;
1927 /*
1928 * depending on the sign of the function, update parl or paru.
1929 */
1930 if (fp > zero)
1931 parl = mp_dmax1(parl, *par);
1932 if (fp < zero)
1933 paru = mp_dmin1(paru, *par);
1934 /*
1935 * compute an improved estimate for par.
1936 */
1937 *par = mp_dmax1(parl, *par + parc);
1938 /*
1939 * end of an iteration.
1940 */
1941 goto L150;
1942
1943 L220:
1944 /*
1945 * termination.
1946 */
1947 if (iter == 0)
1948 *par = zero;
1949 /*
1950 * last card of subroutine lmpar.
1951 */
1952 }
1953
1954
1955 /************************enorm.c*************************/
1956
1957 static
1958 double mp_enorm(int n, double *x)
1959 {
1960 /*
1961 * **********
1962 *
1963 * function enorm
1964 *
1965 * given an n-vector x, this function calculates the
1966 * euclidean norm of x.
1967 *
1968 * the euclidean norm is computed by accumulating the sum of
1969 * squares in three different sums. the sums of squares for the
1970 * small and large components are scaled so that no overflows
1971 * occur. non-destructive underflows are permitted. underflows
1972 * and overflows do not occur in the computation of the unscaled
1973 * sum of squares for the intermediate components.
1974 * the definitions of small, intermediate and large components
1975 * depend on two constants, rdwarf and rgiant. the main
1976 * restrictions on these constants are that rdwarf**2 not
1977 * underflow and rgiant**2 not overflow. the constants
1978 * given here are suitable for every known computer.
1979 *
1980 * the function statement is
1981 *
1982 * double precision function enorm(n,x)
1983 *
1984 * where
1985 *
1986 * n is a positive integer input variable.
1987 *
1988 * x is an input array of length n.
1989 *
1990 * subprograms called
1991 *
1992 * fortran-supplied ... dabs,dsqrt
1993 *
1994 * argonne national laboratory. minpack project. march 1980.
1995 * burton s. garbow, kenneth e. hillstrom, jorge j. more
1996 *
1997 * **********
1998 */
1999 int i;
2000 double agiant,floatn,s1,s2,s3,xabs,x1max,x3max;
2001 double ans, temp;
2002 double rdwarf = MP_RDWARF;
2003 double rgiant = MP_RGIANT;
2004 static double zero = 0.0;
2005 static double one = 1.0;
2006
2007 s1 = zero;
2008 s2 = zero;
2009 s3 = zero;
2010 x1max = zero;
2011 x3max = zero;
2012 floatn = n;
2013 agiant = rgiant/floatn;
2014
2015 for (i=0; i<n; i++) {
2016 xabs = fabs(x[i]);
2017 if ((xabs > rdwarf) && (xabs < agiant))
2018 {
2019 /*
2020 * sum for intermediate components.
2021 */
2022 s2 += xabs*xabs;
2023 continue;
2024 }
2025
2026 if (xabs > rdwarf)
2027 {
2028 /*
2029 * sum for large components.
2030 */
2031 if (xabs > x1max)
2032 {
2033 temp = x1max/xabs;
2034 s1 = one + s1*temp*temp;
2035 x1max = xabs;
2036 }
2037 else
2038 {
2039 temp = xabs/x1max;
2040 s1 += temp*temp;
2041 }
2042 continue;
2043 }
2044 /*
2045 * sum for small components.
2046 */
2047 if (xabs > x3max)
2048 {
2049 temp = x3max/xabs;
2050 s3 = one + s3*temp*temp;
2051 x3max = xabs;
2052 }
2053 else
2054 {
2055 if (xabs != zero)
2056 {
2057 temp = xabs/x3max;
2058 s3 += temp*temp;
2059 }
2060 }
2061 }
2062 /*
2063 * calculation of norm.
2064 */
2065 if (s1 != zero) {
2066 temp = s1 + (s2/x1max)/x1max;
2067 ans = x1max*sqrt(temp);
2068 return(ans);
2069 }
2070 if (s2 != zero) {
2071 if (s2 >= x3max)
2072 temp = s2*(one+(x3max/s2)*(x3max*s3));
2073 else
2074 temp = x3max*((s2/x3max)+(x3max*s3));
2075 ans = sqrt(temp);
2076 }
2077 else
2078 {
2079 ans = x3max*sqrt(s3);
2080 }
2081 return(ans);
2082 /*
2083 * last card of function enorm.
2084 */
2085 }
2086
2087 /************************lmmisc.c*************************/
2088
2089 static
2090 double mp_dmax1(double a, double b)
2091 {
2092 if (a >= b)
2093 return(a);
2094 else
2095 return(b);
2096 }
2097
2098 static
2099 double mp_dmin1(double a, double b)
2100 {
2101 if (a <= b)
2102 return(a);
2103 else
2104 return(b);
2105 }
2106
2107 static
2108 int mp_min0(int a, int b)
2109 {
2110 if (a <= b)
2111 return(a);
2112 else
2113 return(b);
2114 }
2115
2116 /************************covar.c*************************/
2117 /*
2118 c **********
2119 c
2120 c subroutine covar
2121 c
2122 c given an m by n matrix a, the problem is to determine
2123 c the covariance matrix corresponding to a, defined as
2124 c
2125 c t
2126 c inverse(a *a) .
2127 c
2128 c this subroutine completes the solution of the problem
2129 c if it is provided with the necessary information from the
2130 c qr factorization, with column pivoting, of a. that is, if
2131 c a*p = q*r, where p is a permutation matrix, q has orthogonal
2132 c columns, and r is an upper triangular matrix with diagonal
2133 c elements of nonincreasing magnitude, then covar expects
2134 c the full upper triangle of r and the permutation matrix p.
2135 c the covariance matrix is then computed as
2136 c
2137 c t t
2138 c p*inverse(r *r)*p .
2139 c
2140 c if a is nearly rank deficient, it may be desirable to compute
2141 c the covariance matrix corresponding to the linearly independent
2142 c columns of a. to define the numerical rank of a, covar uses
2143 c the tolerance tol. if l is the largest integer such that
2144 c
2145 c abs(r(l,l)) .gt. tol*abs(r(1,1)) ,
2146 c
2147 c then covar computes the covariance matrix corresponding to
2148 c the first l columns of r. for k greater than l, column
2149 c and row ipvt(k) of the covariance matrix are set to zero.
2150 c
2151 c the subroutine statement is
2152 c
2153 c subroutine covar(n,r,ldr,ipvt,tol,wa)
2154 c
2155 c where
2156 c
2157 c n is a positive integer input variable set to the order of r.
2158 c
2159 c r is an n by n array. on input the full upper triangle must
2160 c contain the full upper triangle of the matrix r. on output
2161 c r contains the square symmetric covariance matrix.
2162 c
2163 c ldr is a positive integer input variable not less than n
2164 c which specifies the leading dimension of the array r.
2165 c
2166 c ipvt is an integer input array of length n which defines the
2167 c permutation matrix p such that a*p = q*r. column j of p
2168 c is column ipvt(j) of the identity matrix.
2169 c
2170 c tol is a nonnegative input variable used to define the
2171 c numerical rank of a in the manner described above.
2172 c
2173 c wa is a work array of length n.
2174 c
2175 c subprograms called
2176 c
2177 c fortran-supplied ... dabs
2178 c
2179 c argonne national laboratory. minpack project. august 1980.
2180 c burton s. garbow, kenneth e. hillstrom, jorge j. more
2181 c
2182 c **********
2183 */
2184
2185 static
2186 int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa)
2187 {
2188 int i, ii, j, jj, k, l;
2189 int kk, kj, ji, j0, k0, jj0;
2190 int sing;
2191 double one = 1.0, temp, tolr, zero = 0.0;
2192
2193 /*
2194 * form the inverse of r in the full upper triangle of r.
2195 */
2196
2197 #if 0
2198 for (j=0; j<n; j++) {
2199 for (i=0; i<n; i++) {
2200 printf("%f ", r[j*ldr+i]);
2201 }
2202 printf("\n");
2203 }
2204 #endif
2205
2206 tolr = tol*fabs(r[0]);
2207 l = -1;
2208 for (k=0; k<n; k++) {
2209 kk = k*ldr + k;
2210 if (fabs(r[kk]) <= tolr) break;
2211
2212 r[kk] = one/r[kk];
2213 for (j=0; j<k; j++) {
2214 kj = k*ldr + j;
2215 temp = r[kk] * r[kj];
2216 r[kj] = zero;
2217
2218 k0 = k*ldr; j0 = j*ldr;
2219 for (i=0; i<=j; i++) {
2220 r[k0+i] += (-temp*r[j0+i]);
2221 }
2222 }
2223 l = k;
2224 }
2225
2226 /*
2227 * Form the full upper triangle of the inverse of (r transpose)*r
2228 * in the full upper triangle of r
2229 */
2230
2231 if (l >= 0) {
2232 for (k=0; k <= l; k++) {
2233 k0 = k*ldr;
2234
2235 for (j=0; j<k; j++) {
2236 temp = r[k*ldr+j];
2237
2238 j0 = j*ldr;
2239 for (i=0; i<=j; i++) {
2240 r[j0+i] += temp*r[k0+i];
2241 }
2242 }
2243
2244 temp = r[k0+k];
2245 for (i=0; i<=k; i++) {
2246 r[k0+i] *= temp;
2247 }
2248 }
2249 }
2250
2251 /*
2252 * For the full lower triangle of the covariance matrix
2253 * in the strict lower triangle or and in wa
2254 */
2255 for (j=0; j<n; j++) {
2256 jj = ipvt[j];
2257 sing = (j > l);
2258 j0 = j*ldr;
2259 jj0 = jj*ldr;
2260 for (i=0; i<=j; i++) {
2261 ji = j0+i;
2262
2263 if (sing) r[ji] = zero;
2264 ii = ipvt[i];
2265 if (ii > jj) r[jj0+ii] = r[ji];
2266 if (ii < jj) r[ii*ldr+jj] = r[ji];
2267 }
2268 wa[jj] = r[j0+j];
2269 }
2270
2271 /*
2272 * Symmetrize the covariance matrix in r
2273 */
2274 for (j=0; j<n; j++) {
2275 j0 = j*ldr;
2276 for (i=0; i<j; i++) {
2277 r[j0+i] = r[i*ldr+j];
2278 }
2279 r[j0+j] = wa[j];
2280 }
2281
2282 #if 0
2283 for (j=0; j<n; j++) {
2284 for (i=0; i<n; i++) {
2285 printf("%f ", r[j*ldr+i]);
2286 }
2287 printf("\n");
2288 }
2289 #endif
2290
2291 return 0;
2292 }