| /* |
| * MINPACK-1 Least Squares Fitting Library |
| * |
| * Original public domain version by B. Garbow, K. Hillstrom, J. More' |
| * (Argonne National Laboratory, MINPACK project, March 1980) |
| * See the file DISCLAIMER for copyright information. |
| * |
| * Tranlation to C Language by S. Moshier (moshier.net) |
| * |
| * Enhancements and packaging by C. Markwardt |
| * (comparable to IDL fitting routine MPFIT |
| * see http://cow.physics.wisc.edu/~craigm/idl/idl.html) |
| */ |
| |
| /* Main mpfit library routines (double precision) |
| $Id: mpfit.c,v 1.24 2013/04/23 18:37:38 craigm Exp $ |
| */ |
| |
| #include <stdio.h> |
| #include <stdlib.h> |
| #include <math.h> |
| #include <string.h> |
| #include "mpfit.h" |
| |
| /* Forward declarations of functions in this module */ |
| static int mp_fdjac2(mp_func funct, |
| int m, int n, int *ifree, int npar, double *x, double *fvec, |
| double *fjac, int ldfjac, double epsfcn, |
| double *wa, void *priv, int *nfev, |
| double *step, double *dstep, int *dside, |
| int *qulimited, double *ulimit, |
| int *ddebug, double *ddrtol, double *ddatol, |
| double *wa2, double **dvecptr); |
| static void mp_qrfac(int m, int n, double *a, int lda, |
| int pivot, int *ipvt, int lipvt, |
| double *rdiag, double *acnorm, double *wa); |
| static void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, |
| double *qtb, double *x, double *sdiag, double *wa); |
| static void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag, |
| double *qtb, double delta, double *par, double *x, |
| double *sdiag, double *wa1, double *wa2); |
| static double mp_enorm(int n, double *x); |
| static double mp_dmax1(double a, double b); |
| static double mp_dmin1(double a, double b); |
| static int mp_min0(int a, int b); |
| static int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa); |
| |
| /* Macro to call user function */ |
| #define mp_call(funct, m, n, x, fvec, dvec, priv) (*(funct))(m,n,x,fvec,dvec,priv) |
| |
| /* Macro to safely allocate memory */ |
| #define mp_malloc(dest,type,size) \ |
| dest = (type *) malloc( sizeof(type)*size ); \ |
| if (dest == 0) { \ |
| info = MP_ERR_MEMORY; \ |
| goto CLEANUP; \ |
| } else { \ |
| int _k; \ |
| for (_k=0; _k<(size); _k++) dest[_k] = 0; \ |
| } |
| |
| /* |
| * ********** |
| * |
| * subroutine mpfit |
| * |
| * the purpose of mpfit is to minimize the sum of the squares of |
| * m nonlinear functions in n variables by a modification of |
| * the levenberg-marquardt algorithm. the user must provide a |
| * subroutine which calculates the functions. the jacobian is |
| * then calculated by a finite-difference approximation. |
| * |
| * mp_funct funct - function to be minimized |
| * int m - number of data points |
| * int npar - number of fit parameters |
| * double *xall - array of n initial parameter values |
| * upon return, contains adjusted parameter values |
| * mp_par *pars - array of npar structures specifying constraints; |
| * or 0 (null pointer) for unconstrained fitting |
| * [ see README and mpfit.h for definition & use of mp_par] |
| * mp_config *config - pointer to structure which specifies the |
| * configuration of mpfit(); or 0 (null pointer) |
| * if the default configuration is to be used. |
| * See README and mpfit.h for definition and use |
| * of config. |
| * void *private - any private user data which is to be passed directly |
| * to funct without modification by mpfit(). |
| * mp_result *result - pointer to structure, which upon return, contains |
| * the results of the fit. The user should zero this |
| * structure. If any of the array values are to be |
| * returned, the user should allocate storage for them |
| * and assign the corresponding pointer in *result. |
| * Upon return, *result will be updated, and |
| * any of the non-null arrays will be filled. |
| * |
| * |
| * FORTRAN DOCUMENTATION BELOW |
| * |
| * |
| * the subroutine statement is |
| * |
| * subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, |
| * diag,mode,factor,nprint,info,nfev,fjac, |
| * ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) |
| * |
| * where |
| * |
| * fcn is the name of the user-supplied subroutine which |
| * calculates the functions. fcn must be declared |
| * in an external statement in the user calling |
| * program, and should be written as follows. |
| * |
| * subroutine fcn(m,n,x,fvec,iflag) |
| * integer m,n,iflag |
| * double precision x(n),fvec(m) |
| * ---------- |
| * calculate the functions at x and |
| * return this vector in fvec. |
| * ---------- |
| * return |
| * end |
| * |
| * the value of iflag should not be changed by fcn unless |
| * the user wants to terminate execution of lmdif. |
| * in this case set iflag to a negative integer. |
| * |
| * m is a positive integer input variable set to the number |
| * of functions. |
| * |
| * n is a positive integer input variable set to the number |
| * of variables. n must not exceed m. |
| * |
| * x is an array of length n. on input x must contain |
| * an initial estimate of the solution vector. on output x |
| * contains the final estimate of the solution vector. |
| * |
| * fvec is an output array of length m which contains |
| * the functions evaluated at the output x. |
| * |
| * ftol is a nonnegative input variable. termination |
| * occurs when both the actual and predicted relative |
| * reductions in the sum of squares are at most ftol. |
| * therefore, ftol measures the relative error desired |
| * in the sum of squares. |
| * |
| * xtol is a nonnegative input variable. termination |
| * occurs when the relative error between two consecutive |
| * iterates is at most xtol. therefore, xtol measures the |
| * relative error desired in the approximate solution. |
| * |
| * gtol is a nonnegative input variable. termination |
| * occurs when the cosine of the angle between fvec and |
| * any column of the jacobian is at most gtol in absolute |
| * value. therefore, gtol measures the orthogonality |
| * desired between the function vector and the columns |
| * of the jacobian. |
| * |
| * maxfev is a positive integer input variable. termination |
| * occurs when the number of calls to fcn is at least |
| * maxfev by the end of an iteration. |
| * |
| * epsfcn is an input variable used in determining a suitable |
| * step length for the forward-difference approximation. this |
| * approximation assumes that the relative errors in the |
| * functions are of the order of epsfcn. if epsfcn is less |
| * than the machine precision, it is assumed that the relative |
| * errors in the functions are of the order of the machine |
| * precision. |
| * |
| * diag is an array of length n. if mode = 1 (see |
| * below), diag is internally set. if mode = 2, diag |
| * must contain positive entries that serve as |
| * multiplicative scale factors for the variables. |
| * |
| * mode is an integer input variable. if mode = 1, the |
| * variables will be scaled internally. if mode = 2, |
| * the scaling is specified by the input diag. other |
| * values of mode are equivalent to mode = 1. |
| * |
| * factor is a positive input variable used in determining the |
| * initial step bound. this bound is set to the product of |
| * factor and the euclidean norm of diag*x if nonzero, or else |
| * to factor itself. in most cases factor should lie in the |
| * interval (.1,100.). 100. is a generally recommended value. |
| * |
| * nprint is an integer input variable that enables controlled |
| * printing of iterates if it is positive. in this case, |
| * fcn is called with iflag = 0 at the beginning of the first |
| * iteration and every nprint iterations thereafter and |
| * immediately prior to return, with x and fvec available |
| * for printing. if nprint is not positive, no special calls |
| * of fcn with iflag = 0 are made. |
| * |
| * info is an integer output variable. if the user has |
| * terminated execution, info is set to the (negative) |
| * value of iflag. see description of fcn. otherwise, |
| * info is set as follows. |
| * |
| * info = 0 improper input parameters. |
| * |
| * info = 1 both actual and predicted relative reductions |
| * in the sum of squares are at most ftol. |
| * |
| * info = 2 relative error between two consecutive iterates |
| * is at most xtol. |
| * |
| * info = 3 conditions for info = 1 and info = 2 both hold. |
| * |
| * info = 4 the cosine of the angle between fvec and any |
| * column of the jacobian is at most gtol in |
| * absolute value. |
| * |
| * info = 5 number of calls to fcn has reached or |
| * exceeded maxfev. |
| * |
| * info = 6 ftol is too small. no further reduction in |
| * the sum of squares is possible. |
| * |
| * info = 7 xtol is too small. no further improvement in |
| * the approximate solution x is possible. |
| * |
| * info = 8 gtol is too small. fvec is orthogonal to the |
| * columns of the jacobian to machine precision. |
| * |
| * nfev is an integer output variable set to the number of |
| * calls to fcn. |
| * |
| * fjac is an output m by n array. the upper n by n submatrix |
| * of fjac contains an upper triangular matrix r with |
| * diagonal elements of nonincreasing magnitude such that |
| * |
| * t t t |
| * p *(jac *jac)*p = r *r, |
| * |
| * where p is a permutation matrix and jac is the final |
| * calculated jacobian. column j of p is column ipvt(j) |
| * (see below) of the identity matrix. the lower trapezoidal |
| * part of fjac contains information generated during |
| * the computation of r. |
| * |
| * ldfjac is a positive integer input variable not less than m |
| * which specifies the leading dimension of the array fjac. |
| * |
| * ipvt is an integer output array of length n. ipvt |
| * defines a permutation matrix p such that jac*p = q*r, |
| * where jac is the final calculated jacobian, q is |
| * orthogonal (not stored), and r is upper triangular |
| * with diagonal elements of nonincreasing magnitude. |
| * column j of p is column ipvt(j) of the identity matrix. |
| * |
| * qtf is an output array of length n which contains |
| * the first n elements of the vector (q transpose)*fvec. |
| * |
| * wa1, wa2, and wa3 are work arrays of length n. |
| * |
| * wa4 is a work array of length m. |
| * |
| * subprograms called |
| * |
| * user-supplied ...... fcn |
| * |
| * minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac |
| * |
| * fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod |
| * |
| * argonne national laboratory. minpack project. march 1980. |
| * burton s. garbow, kenneth e. hillstrom, jorge j. more |
| * |
| * ********** */ |
| |
| |
| int mpfit(mp_func funct, int m, int npar, |
| double *xall, mp_par *pars, mp_config *config, void *private_data, |
| mp_result *result) |
| { |
| mp_config conf; |
| int i, j, info, iflag, nfree, npegged, iter; |
| int qanylim = 0; |
| |
| int ij,jj,l; |
| double actred,delta,dirder,fnorm,fnorm1,gnorm, orignorm; |
| double par,pnorm,prered,ratio; |
| double sum,temp,temp1,temp2,temp3,xnorm, alpha; |
| static double one = 1.0; |
| static double p1 = 0.1; |
| static double p5 = 0.5; |
| static double p25 = 0.25; |
| static double p75 = 0.75; |
| static double p0001 = 1.0e-4; |
| static double zero = 0.0; |
| int nfev = 0; |
| |
| double *step = 0, *dstep = 0, *llim = 0, *ulim = 0; |
| int *pfixed = 0, *mpside = 0, *ifree = 0, *qllim = 0, *qulim = 0; |
| int *ddebug = 0; |
| double *ddrtol = 0, *ddatol = 0; |
| |
| double *fvec = 0, *qtf = 0; |
| double *x = 0, *xnew = 0, *fjac = 0, *diag = 0; |
| double *wa1 = 0, *wa2 = 0, *wa3 = 0, *wa4 = 0; |
| double **dvecptr = 0; |
| int *ipvt = 0; |
| |
| int ldfjac; |
| |
| /* Default configuration */ |
| conf.ftol = 1e-10; |
| conf.xtol = 1e-10; |
| conf.gtol = 1e-10; |
| conf.stepfactor = 100.0; |
| conf.nprint = 1; |
| conf.epsfcn = MP_MACHEP0; |
| conf.maxiter = 200; |
| conf.douserscale = 0; |
| conf.maxfev = 0; |
| conf.covtol = 1e-14; |
| conf.nofinitecheck = 0; |
| |
| if (config) { |
| /* Transfer any user-specified configurations */ |
| if (config->ftol > 0) conf.ftol = config->ftol; |
| if (config->xtol > 0) conf.xtol = config->xtol; |
| if (config->gtol > 0) conf.gtol = config->gtol; |
| if (config->stepfactor > 0) conf.stepfactor = config->stepfactor; |
| if (config->nprint >= 0) conf.nprint = config->nprint; |
| if (config->epsfcn > 0) conf.epsfcn = config->epsfcn; |
| if (config->maxiter > 0) conf.maxiter = config->maxiter; |
| if (config->maxiter == MP_NO_ITER) conf.maxiter = 0; |
| if (config->douserscale != 0) conf.douserscale = config->douserscale; |
| if (config->covtol > 0) conf.covtol = config->covtol; |
| if (config->nofinitecheck > 0) conf.nofinitecheck = config->nofinitecheck; |
| conf.maxfev = config->maxfev; |
| } |
| |
| info = MP_ERR_INPUT; /* = 0 */ |
| iflag = 0; |
| nfree = 0; |
| npegged = 0; |
| |
| /* Basic error checking */ |
| if (funct == 0) { |
| return MP_ERR_FUNC; |
| } |
| |
| if ((m <= 0) || (xall == 0)) { |
| return MP_ERR_NPOINTS; |
| } |
| |
| if (npar <= 0) { |
| return MP_ERR_NFREE; |
| } |
| |
| fnorm = -1.0; |
| fnorm1 = -1.0; |
| xnorm = -1.0; |
| delta = 0.0; |
| |
| /* FIXED parameters? */ |
| mp_malloc(pfixed, int, npar); |
| if (pars) for (i=0; i<npar; i++) { |
| pfixed[i] = (pars[i].fixed)?1:0; |
| } |
| |
| /* Finite differencing step, absolute and relative, and sidedness of deriv */ |
| mp_malloc(step, double, npar); |
| mp_malloc(dstep, double, npar); |
| mp_malloc(mpside, int, npar); |
| mp_malloc(ddebug, int, npar); |
| mp_malloc(ddrtol, double, npar); |
| mp_malloc(ddatol, double, npar); |
| if (pars) for (i=0; i<npar; i++) { |
| step[i] = pars[i].step; |
| dstep[i] = pars[i].relstep; |
| mpside[i] = pars[i].side; |
| ddebug[i] = pars[i].deriv_debug; |
| ddrtol[i] = pars[i].deriv_reltol; |
| ddatol[i] = pars[i].deriv_abstol; |
| } |
| |
| /* Finish up the free parameters */ |
| nfree = 0; |
| mp_malloc(ifree, int, npar); |
| for (i=0, j=0; i<npar; i++) { |
| if (pfixed[i] == 0) { |
| nfree++; |
| ifree[j++] = i; |
| } |
| } |
| if (nfree == 0) { |
| info = MP_ERR_NFREE; |
| goto CLEANUP; |
| } |
| |
| if (pars) { |
| for (i=0; i<npar; i++) { |
| if ( (pars[i].limited[0] && (xall[i] < pars[i].limits[0])) || |
| (pars[i].limited[1] && (xall[i] > pars[i].limits[1])) ) { |
| info = MP_ERR_INITBOUNDS; |
| goto CLEANUP; |
| } |
| if ( (pars[i].fixed == 0) && pars[i].limited[0] && pars[i].limited[1] && |
| (pars[i].limits[0] >= pars[i].limits[1])) { |
| info = MP_ERR_BOUNDS; |
| goto CLEANUP; |
| } |
| } |
| |
| mp_malloc(qulim, int, nfree); |
| mp_malloc(qllim, int, nfree); |
| mp_malloc(ulim, double, nfree); |
| mp_malloc(llim, double, nfree); |
| |
| for (i=0; i<nfree; i++) { |
| qllim[i] = pars[ifree[i]].limited[0]; |
| qulim[i] = pars[ifree[i]].limited[1]; |
| llim[i] = pars[ifree[i]].limits[0]; |
| ulim[i] = pars[ifree[i]].limits[1]; |
| if (qllim[i] || qulim[i]) qanylim = 1; |
| } |
| } |
| |
| /* Sanity checking on input configuration */ |
| if ((npar <= 0) || (conf.ftol <= 0) || (conf.xtol <= 0) || |
| (conf.gtol <= 0) || (conf.maxiter < 0) || |
| (conf.stepfactor <= 0)) { |
| info = MP_ERR_PARAM; |
| goto CLEANUP; |
| } |
| |
| /* Ensure there are some degrees of freedom */ |
| if (m < nfree) { |
| info = MP_ERR_DOF; |
| goto CLEANUP; |
| } |
| |
| /* Allocate temporary storage */ |
| mp_malloc(fvec, double, m); |
| mp_malloc(qtf, double, nfree); |
| mp_malloc(x, double, nfree); |
| mp_malloc(xnew, double, npar); |
| mp_malloc(fjac, double, m*nfree); |
| ldfjac = m; |
| mp_malloc(diag, double, npar); |
| mp_malloc(wa1, double, npar); |
| mp_malloc(wa2, double, npar); |
| mp_malloc(wa3, double, npar); |
| mp_malloc(wa4, double, m); |
| mp_malloc(ipvt, int, npar); |
| mp_malloc(dvecptr, double *, npar); |
| |
| /* Evaluate user function with initial parameter values */ |
| iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data); |
| nfev += 1; |
| if (iflag < 0) { |
| goto CLEANUP; |
| } |
| |
| fnorm = mp_enorm(m, fvec); |
| orignorm = fnorm*fnorm; |
| |
| /* Make a new copy */ |
| for (i=0; i<npar; i++) { |
| xnew[i] = xall[i]; |
| } |
| |
| /* Transfer free parameters to 'x' */ |
| for (i=0; i<nfree; i++) { |
| x[i] = xall[ifree[i]]; |
| } |
| |
| /* Initialize Levelberg-Marquardt parameter and iteration counter */ |
| |
| par = 0.0; |
| iter = 1; |
| for (i=0; i<nfree; i++) { |
| qtf[i] = 0; |
| } |
| |
| /* Beginning of the outer loop */ |
| OUTER_LOOP: |
| for (i=0; i<nfree; i++) { |
| xnew[ifree[i]] = x[i]; |
| } |
| |
| /* XXX call iterproc */ |
| |
| /* Calculate the jacobian matrix */ |
| iflag = mp_fdjac2(funct, m, nfree, ifree, npar, xnew, fvec, fjac, ldfjac, |
| conf.epsfcn, wa4, private_data, &nfev, |
| step, dstep, mpside, qulim, ulim, |
| ddebug, ddrtol, ddatol, wa2, dvecptr); |
| if (iflag < 0) { |
| goto CLEANUP; |
| } |
| |
| /* Determine if any of the parameters are pegged at the limits */ |
| if (qanylim) { |
| for (j=0; j<nfree; j++) { |
| int lpegged = (qllim[j] && (x[j] == llim[j])); |
| int upegged = (qulim[j] && (x[j] == ulim[j])); |
| sum = 0; |
| |
| /* If the parameter is pegged at a limit, compute the gradient |
| direction */ |
| if (lpegged || upegged) { |
| ij = j*ldfjac; |
| for (i=0; i<m; i++, ij++) { |
| sum += fvec[i] * fjac[ij]; |
| } |
| } |
| /* If pegged at lower limit and gradient is toward negative then |
| reset gradient to zero */ |
| if (lpegged && (sum > 0)) { |
| ij = j*ldfjac; |
| for (i=0; i<m; i++, ij++) fjac[ij] = 0; |
| } |
| /* If pegged at upper limit and gradient is toward positive then |
| reset gradient to zero */ |
| if (upegged && (sum < 0)) { |
| ij = j*ldfjac; |
| for (i=0; i<m; i++, ij++) fjac[ij] = 0; |
| } |
| } |
| } |
| |
| /* Compute the QR factorization of the jacobian */ |
| mp_qrfac(m,nfree,fjac,ldfjac,1,ipvt,nfree,wa1,wa2,wa3); |
| |
| /* |
| * on the first iteration and if mode is 1, scale according |
| * to the norms of the columns of the initial jacobian. |
| */ |
| if (iter == 1) { |
| if (conf.douserscale == 0) { |
| for (j=0; j<nfree; j++) { |
| diag[ifree[j]] = wa2[j]; |
| if (wa2[j] == zero ) { |
| diag[ifree[j]] = one; |
| } |
| } |
| } |
| |
| /* |
| * on the first iteration, calculate the norm of the scaled x |
| * and initialize the step bound delta. |
| */ |
| for (j=0; j<nfree; j++ ) { |
| wa3[j] = diag[ifree[j]] * x[j]; |
| } |
| |
| xnorm = mp_enorm(nfree, wa3); |
| delta = conf.stepfactor*xnorm; |
| if (delta == zero) delta = conf.stepfactor; |
| } |
| |
| /* |
| * form (q transpose)*fvec and store the first n components in |
| * qtf. |
| */ |
| for (i=0; i<m; i++ ) { |
| wa4[i] = fvec[i]; |
| } |
| |
| jj = 0; |
| for (j=0; j<nfree; j++ ) { |
| temp3 = fjac[jj]; |
| if (temp3 != zero) { |
| sum = zero; |
| ij = jj; |
| for (i=j; i<m; i++ ) { |
| sum += fjac[ij] * wa4[i]; |
| ij += 1; /* fjac[i+m*j] */ |
| } |
| temp = -sum / temp3; |
| ij = jj; |
| for (i=j; i<m; i++ ) { |
| wa4[i] += fjac[ij] * temp; |
| ij += 1; /* fjac[i+m*j] */ |
| } |
| } |
| fjac[jj] = wa1[j]; |
| jj += m+1; /* fjac[j+m*j] */ |
| qtf[j] = wa4[j]; |
| } |
| |
| /* ( From this point on, only the square matrix, consisting of the |
| triangle of R, is needed.) */ |
| |
| |
| if (conf.nofinitecheck) { |
| /* Check for overflow. This should be a cheap test here since FJAC |
| has been reduced to a (small) square matrix, and the test is |
| O(N^2). */ |
| int off = 0, nonfinite = 0; |
| |
| for (j=0; j<nfree; j++) { |
| for (i=0; i<nfree; i++) { |
| if (mpfinite(fjac[off+i]) == 0) nonfinite = 1; |
| } |
| off += ldfjac; |
| } |
| |
| if (nonfinite) { |
| info = MP_ERR_NAN; |
| goto CLEANUP; |
| } |
| } |
| |
| |
| /* |
| * compute the norm of the scaled gradient. |
| */ |
| gnorm = zero; |
| if (fnorm != zero) { |
| jj = 0; |
| for (j=0; j<nfree; j++ ) { |
| l = ipvt[j]; |
| if (wa2[l] != zero) { |
| sum = zero; |
| ij = jj; |
| for (i=0; i<=j; i++ ) { |
| sum += fjac[ij]*(qtf[i]/fnorm); |
| ij += 1; /* fjac[i+m*j] */ |
| } |
| gnorm = mp_dmax1(gnorm,fabs(sum/wa2[l])); |
| } |
| jj += m; |
| } |
| } |
| |
| /* |
| * test for convergence of the gradient norm. |
| */ |
| if (gnorm <= conf.gtol) info = MP_OK_DIR; |
| if (info != 0) goto L300; |
| if (conf.maxiter == 0) { |
| info = MP_MAXITER; |
| goto L300; |
| } |
| |
| /* |
| * rescale if necessary. |
| */ |
| if (conf.douserscale == 0) { |
| for (j=0; j<nfree; j++ ) { |
| diag[ifree[j]] = mp_dmax1(diag[ifree[j]],wa2[j]); |
| } |
| } |
| |
| /* |
| * beginning of the inner loop. |
| */ |
| L200: |
| /* |
| * determine the levenberg-marquardt parameter. |
| */ |
| mp_lmpar(nfree,fjac,ldfjac,ipvt,ifree,diag,qtf,delta,&par,wa1,wa2,wa3,wa4); |
| /* |
| * store the direction p and x + p. calculate the norm of p. |
| */ |
| for (j=0; j<nfree; j++ ) { |
| wa1[j] = -wa1[j]; |
| } |
| |
| alpha = 1.0; |
| if (qanylim == 0) { |
| /* No parameter limits, so just move to new position WA2 */ |
| for (j=0; j<nfree; j++ ) { |
| wa2[j] = x[j] + wa1[j]; |
| } |
| |
| } else { |
| /* Respect the limits. If a step were to go out of bounds, then |
| * we should take a step in the same direction but shorter distance. |
| * The step should take us right to the limit in that case. |
| */ |
| for (j=0; j<nfree; j++) { |
| int lpegged = (qllim[j] && (x[j] <= llim[j])); |
| int upegged = (qulim[j] && (x[j] >= ulim[j])); |
| int dwa1 = fabs(wa1[j]) > MP_MACHEP0; |
| |
| if (lpegged && (wa1[j] < 0)) wa1[j] = 0; |
| if (upegged && (wa1[j] > 0)) wa1[j] = 0; |
| |
| if (dwa1 && qllim[j] && ((x[j] + wa1[j]) < llim[j])) { |
| alpha = mp_dmin1(alpha, (llim[j]-x[j])/wa1[j]); |
| } |
| if (dwa1 && qulim[j] && ((x[j] + wa1[j]) > ulim[j])) { |
| alpha = mp_dmin1(alpha, (ulim[j]-x[j])/wa1[j]); |
| } |
| } |
| |
| /* Scale the resulting vector, advance to the next position */ |
| for (j=0; j<nfree; j++) { |
| double sgnu, sgnl; |
| double ulim1, llim1; |
| |
| wa1[j] = wa1[j] * alpha; |
| wa2[j] = x[j] + wa1[j]; |
| |
| /* Adjust the output values. If the step put us exactly |
| * on a boundary, make sure it is exact. |
| */ |
| sgnu = (ulim[j] >= 0) ? (+1) : (-1); |
| sgnl = (llim[j] >= 0) ? (+1) : (-1); |
| ulim1 = ulim[j]*(1-sgnu*MP_MACHEP0) - ((ulim[j] == 0)?(MP_MACHEP0):0); |
| llim1 = llim[j]*(1+sgnl*MP_MACHEP0) + ((llim[j] == 0)?(MP_MACHEP0):0); |
| |
| if (qulim[j] && (wa2[j] >= ulim1)) { |
| wa2[j] = ulim[j]; |
| } |
| if (qllim[j] && (wa2[j] <= llim1)) { |
| wa2[j] = llim[j]; |
| } |
| } |
| |
| } |
| |
| for (j=0; j<nfree; j++ ) { |
| wa3[j] = diag[ifree[j]]*wa1[j]; |
| } |
| |
| pnorm = mp_enorm(nfree,wa3); |
| |
| /* |
| * on the first iteration, adjust the initial step bound. |
| */ |
| if (iter == 1) { |
| delta = mp_dmin1(delta,pnorm); |
| } |
| |
| /* |
| * evaluate the function at x + p and calculate its norm. |
| */ |
| for (i=0; i<nfree; i++) { |
| xnew[ifree[i]] = wa2[i]; |
| } |
| |
| iflag = mp_call(funct, m, npar, xnew, wa4, 0, private_data); |
| nfev += 1; |
| if (iflag < 0) goto L300; |
| |
| fnorm1 = mp_enorm(m,wa4); |
| |
| /* |
| * compute the scaled actual reduction. |
| */ |
| actred = -one; |
| if ((p1*fnorm1) < fnorm) { |
| temp = fnorm1/fnorm; |
| actred = one - temp * temp; |
| } |
| |
| /* |
| * compute the scaled predicted reduction and |
| * the scaled directional derivative. |
| */ |
| jj = 0; |
| for (j=0; j<nfree; j++ ) { |
| wa3[j] = zero; |
| l = ipvt[j]; |
| temp = wa1[l]; |
| ij = jj; |
| for (i=0; i<=j; i++ ) { |
| wa3[i] += fjac[ij]*temp; |
| ij += 1; /* fjac[i+m*j] */ |
| } |
| jj += m; |
| } |
| |
| /* Remember, alpha is the fraction of the full LM step actually |
| * taken |
| */ |
| |
| temp1 = mp_enorm(nfree,wa3)*alpha/fnorm; |
| temp2 = (sqrt(alpha*par)*pnorm)/fnorm; |
| prered = temp1*temp1 + (temp2*temp2)/p5; |
| dirder = -(temp1*temp1 + temp2*temp2); |
| |
| /* |
| * compute the ratio of the actual to the predicted |
| * reduction. |
| */ |
| ratio = zero; |
| if (prered != zero) { |
| ratio = actred/prered; |
| } |
| |
| /* |
| * update the step bound. |
| */ |
| |
| if (ratio <= p25) { |
| if (actred >= zero) { |
| temp = p5; |
| } else { |
| temp = p5*dirder/(dirder + p5*actred); |
| } |
| if (((p1*fnorm1) >= fnorm) |
| || (temp < p1) ) { |
| temp = p1; |
| } |
| delta = temp*mp_dmin1(delta,pnorm/p1); |
| par = par/temp; |
| } else { |
| if ((par == zero) || (ratio >= p75) ) { |
| delta = pnorm/p5; |
| par = p5*par; |
| } |
| } |
| |
| /* |
| * test for successful iteration. |
| */ |
| if (ratio >= p0001) { |
| |
| /* |
| * successful iteration. update x, fvec, and their norms. |
| */ |
| for (j=0; j<nfree; j++ ) { |
| x[j] = wa2[j]; |
| wa2[j] = diag[ifree[j]]*x[j]; |
| } |
| for (i=0; i<m; i++ ) { |
| fvec[i] = wa4[i]; |
| } |
| xnorm = mp_enorm(nfree,wa2); |
| fnorm = fnorm1; |
| iter += 1; |
| } |
| |
| /* |
| * tests for convergence. |
| */ |
| if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && |
| (p5*ratio <= one) ) { |
| info = MP_OK_CHI; |
| } |
| if (delta <= conf.xtol*xnorm) { |
| info = MP_OK_PAR; |
| } |
| if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && (p5*ratio <= one) |
| && ( info == 2) ) { |
| info = MP_OK_BOTH; |
| } |
| if (info != 0) { |
| goto L300; |
| } |
| |
| /* |
| * tests for termination and stringent tolerances. |
| */ |
| if ((conf.maxfev > 0) && (nfev >= conf.maxfev)) { |
| /* Too many function evaluations */ |
| info = MP_MAXITER; |
| } |
| if (iter >= conf.maxiter) { |
| /* Too many iterations */ |
| info = MP_MAXITER; |
| } |
| if ((fabs(actred) <= MP_MACHEP0) && (prered <= MP_MACHEP0) && (p5*ratio <= one) ) { |
| info = MP_FTOL; |
| } |
| if (delta <= MP_MACHEP0*xnorm) { |
| info = MP_XTOL; |
| } |
| if (gnorm <= MP_MACHEP0) { |
| info = MP_GTOL; |
| } |
| if (info != 0) { |
| goto L300; |
| } |
| |
| /* |
| * end of the inner loop. repeat if iteration unsuccessful. |
| */ |
| if (ratio < p0001) goto L200; |
| /* |
| * end of the outer loop. |
| */ |
| goto OUTER_LOOP; |
| |
| L300: |
| /* |
| * termination, either normal or user imposed. |
| */ |
| if (iflag < 0) { |
| info = iflag; |
| } |
| iflag = 0; |
| |
| for (i=0; i<nfree; i++) { |
| xall[ifree[i]] = x[i]; |
| } |
| |
| if ((conf.nprint > 0) && (info > 0)) { |
| iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data); |
| nfev += 1; |
| } |
| |
| /* Compute number of pegged parameters */ |
| npegged = 0; |
| if (pars) for (i=0; i<npar; i++) { |
| if ((pars[i].limited[0] && (pars[i].limits[0] == xall[i])) || |
| (pars[i].limited[1] && (pars[i].limits[1] == xall[i]))) { |
| npegged ++; |
| } |
| } |
| |
| /* Compute and return the covariance matrix and/or parameter errors */ |
| if (result && (result->covar || result->xerror)) { |
| mp_covar(nfree, fjac, ldfjac, ipvt, conf.covtol, wa2); |
| |
| if (result->covar) { |
| /* Zero the destination covariance array */ |
| for (j=0; j<(npar*npar); j++) result->covar[j] = 0; |
| |
| /* Transfer the covariance array */ |
| for (j=0; j<nfree; j++) { |
| for (i=0; i<nfree; i++) { |
| result->covar[ifree[j]*npar+ifree[i]] = fjac[j*ldfjac+i]; |
| } |
| } |
| } |
| |
| if (result->xerror) { |
| for (j=0; j<npar; j++) result->xerror[j] = 0; |
| |
| for (j=0; j<nfree; j++) { |
| double cc = fjac[j*ldfjac+j]; |
| if (cc > 0) result->xerror[ifree[j]] = sqrt(cc); |
| } |
| } |
| } |
| |
| if (result) { |
| strcpy(result->version, MPFIT_VERSION); |
| result->bestnorm = mp_dmax1(fnorm,fnorm1); |
| result->bestnorm *= result->bestnorm; |
| result->orignorm = orignorm; |
| result->status = info; |
| result->niter = iter; |
| result->nfev = nfev; |
| result->npar = npar; |
| result->nfree = nfree; |
| result->npegged = npegged; |
| result->nfunc = m; |
| |
| /* Copy residuals if requested */ |
| if (result->resid) { |
| for (j=0; j<m; j++) result->resid[j] = fvec[j]; |
| } |
| } |
| |
| |
| CLEANUP: |
| if (fvec) free(fvec); |
| if (qtf) free(qtf); |
| if (x) free(x); |
| if (xnew) free(xnew); |
| if (fjac) free(fjac); |
| if (diag) free(diag); |
| if (wa1) free(wa1); |
| if (wa2) free(wa2); |
| if (wa3) free(wa3); |
| if (wa4) free(wa4); |
| if (ipvt) free(ipvt); |
| if (pfixed) free(pfixed); |
| if (step) free(step); |
| if (dstep) free(dstep); |
| if (mpside) free(mpside); |
| if (ddebug) free(ddebug); |
| if (ddrtol) free(ddrtol); |
| if (ddatol) free(ddatol); |
| if (ifree) free(ifree); |
| if (qllim) free(qllim); |
| if (qulim) free(qulim); |
| if (llim) free(llim); |
| if (ulim) free(ulim); |
| if (dvecptr) free(dvecptr); |
| |
| return info; |
| } |
| |
| |
| /************************fdjac2.c*************************/ |
| |
| static |
| int mp_fdjac2(mp_func funct, |
| int m, int n, int *ifree, int npar, double *x, double *fvec, |
| double *fjac, int ldfjac, double epsfcn, |
| double *wa, void *priv, int *nfev, |
| double *step, double *dstep, int *dside, |
| int *qulimited, double *ulimit, |
| int *ddebug, double *ddrtol, double *ddatol, |
| double *wa2, double **dvec) |
| { |
| /* |
| * ********** |
| * |
| * subroutine fdjac2 |
| * |
| * this subroutine computes a forward-difference approximation |
| * to the m by n jacobian matrix associated with a specified |
| * problem of m functions in n variables. |
| * |
| * the subroutine statement is |
| * |
| * subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) |
| * |
| * where |
| * |
| * fcn is the name of the user-supplied subroutine which |
| * calculates the functions. fcn must be declared |
| * in an external statement in the user calling |
| * program, and should be written as follows. |
| * |
| * subroutine fcn(m,n,x,fvec,iflag) |
| * integer m,n,iflag |
| * double precision x(n),fvec(m) |
| * ---------- |
| * calculate the functions at x and |
| * return this vector in fvec. |
| * ---------- |
| * return |
| * end |
| * |
| * the value of iflag should not be changed by fcn unless |
| * the user wants to terminate execution of fdjac2. |
| * in this case set iflag to a negative integer. |
| * |
| * m is a positive integer input variable set to the number |
| * of functions. |
| * |
| * n is a positive integer input variable set to the number |
| * of variables. n must not exceed m. |
| * |
| * x is an input array of length n. |
| * |
| * fvec is an input array of length m which must contain the |
| * functions evaluated at x. |
| * |
| * fjac is an output m by n array which contains the |
| * approximation to the jacobian matrix evaluated at x. |
| * |
| * ldfjac is a positive integer input variable not less than m |
| * which specifies the leading dimension of the array fjac. |
| * |
| * iflag is an integer variable which can be used to terminate |
| * the execution of fdjac2. see description of fcn. |
| * |
| * epsfcn is an input variable used in determining a suitable |
| * step length for the forward-difference approximation. this |
| * approximation assumes that the relative errors in the |
| * functions are of the order of epsfcn. if epsfcn is less |
| * than the machine precision, it is assumed that the relative |
| * errors in the functions are of the order of the machine |
| * precision. |
| * |
| * wa is a work array of length m. |
| * |
| * subprograms called |
| * |
| * user-supplied ...... fcn |
| * |
| * minpack-supplied ... dpmpar |
| * |
| * fortran-supplied ... dabs,dmax1,dsqrt |
| * |
| * argonne national laboratory. minpack project. march 1980. |
| * burton s. garbow, kenneth e. hillstrom, jorge j. more |
| * |
| ********** |
| */ |
| int i,j,ij; |
| int iflag = 0; |
| double eps,h,temp; |
| static double zero = 0.0; |
| int has_analytical_deriv = 0, has_numerical_deriv = 0; |
| int has_debug_deriv = 0; |
| |
| temp = mp_dmax1(epsfcn,MP_MACHEP0); |
| eps = sqrt(temp); |
| ij = 0; |
| ldfjac = 0; /* Prevent compiler warning */ |
| if (ldfjac){} /* Prevent compiler warning */ |
| |
| for (j=0; j<npar; j++) dvec[j] = 0; |
| |
| /* Initialize the Jacobian derivative matrix */ |
| for (j=0; j<(n*m); j++) fjac[j] = 0; |
| |
| /* Check for which parameters need analytical derivatives and which |
| need numerical ones */ |
| for (j=0; j<n; j++) { /* Loop through free parameters only */ |
| if (dside && dside[ifree[j]] == 3 && ddebug[ifree[j]] == 0) { |
| /* Purely analytical derivatives */ |
| dvec[ifree[j]] = fjac + j*m; |
| has_analytical_deriv = 1; |
| } else if (dside && ddebug[ifree[j]] == 1) { |
| /* Numerical and analytical derivatives as a debug cross-check */ |
| dvec[ifree[j]] = fjac + j*m; |
| has_analytical_deriv = 1; |
| has_numerical_deriv = 1; |
| has_debug_deriv = 1; |
| } else { |
| has_numerical_deriv = 1; |
| } |
| } |
| |
| /* If there are any parameters requiring analytical derivatives, |
| then compute them first. */ |
| if (has_analytical_deriv) { |
| iflag = mp_call(funct, m, npar, x, wa, dvec, priv); |
| if (nfev) *nfev = *nfev + 1; |
| if (iflag < 0 ) goto DONE; |
| } |
| |
| if (has_debug_deriv) { |
| printf("FJAC DEBUG BEGIN\n"); |
| printf("# %10s %10s %10s %10s %10s %10s\n", |
| "IPNT", "FUNC", "DERIV_U", "DERIV_N", "DIFF_ABS", "DIFF_REL"); |
| } |
| |
| /* Any parameters requiring numerical derivatives */ |
| if (has_numerical_deriv) for (j=0; j<n; j++) { /* Loop thru free parms */ |
| int dsidei = (dside)?(dside[ifree[j]]):(0); |
| int debug = ddebug[ifree[j]]; |
| double dr = ddrtol[ifree[j]], da = ddatol[ifree[j]]; |
| |
| /* Check for debugging */ |
| if (debug) { |
| printf("FJAC PARM %d\n", ifree[j]); |
| } |
| |
| /* Skip parameters already done by user-computed partials */ |
| if (dside && dsidei == 3) continue; |
| |
| temp = x[ifree[j]]; |
| h = eps * fabs(temp); |
| if (step && step[ifree[j]] > 0) h = step[ifree[j]]; |
| if (dstep && dstep[ifree[j]] > 0) h = fabs(dstep[ifree[j]]*temp); |
| if (h == zero) h = eps; |
| |
| /* If negative step requested, or we are against the upper limit */ |
| if ((dside && dsidei == -1) || |
| (dside && dsidei == 0 && |
| qulimited && ulimit && qulimited[j] && |
| (temp > (ulimit[j]-h)))) { |
| h = -h; |
| } |
| |
| x[ifree[j]] = temp + h; |
| iflag = mp_call(funct, m, npar, x, wa, 0, priv); |
| if (nfev) *nfev = *nfev + 1; |
| if (iflag < 0 ) goto DONE; |
| x[ifree[j]] = temp; |
| |
| if (dsidei <= 1) { |
| /* COMPUTE THE ONE-SIDED DERIVATIVE */ |
| if (! debug) { |
| /* Non-debug path for speed */ |
| for (i=0; i<m; i++, ij++) { |
| fjac[ij] = (wa[i] - fvec[i])/h; /* fjac[i+m*j] */ |
| } |
| } else { |
| /* Debug path for correctness */ |
| for (i=0; i<m; i++, ij++) { |
| double fjold = fjac[ij]; |
| fjac[ij] = (wa[i] - fvec[i])/h; /* fjac[i+m*j] */ |
| if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) || |
| ((da != 0 || dr != 0) && (fabs(fjold-fjac[ij]) > da + fabs(fjold)*dr))) { |
| printf(" %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n", |
| i, fvec[i], fjold, fjac[ij], fjold-fjac[ij], |
| (fjold == 0)?(0):((fjold-fjac[ij])/fjold)); |
| } |
| } |
| } /* end debugging */ |
| |
| } else { /* dside > 2 */ |
| /* COMPUTE THE TWO-SIDED DERIVATIVE */ |
| for (i=0; i<m; i++) { |
| wa2[i] = wa[i]; |
| } |
| |
| /* Evaluate at x - h */ |
| x[ifree[j]] = temp - h; |
| iflag = mp_call(funct, m, npar, x, wa, 0, priv); |
| if (nfev) *nfev = *nfev + 1; |
| if (iflag < 0 ) goto DONE; |
| x[ifree[j]] = temp; |
| |
| /* Now compute derivative as (f(x+h) - f(x-h))/(2h) */ |
| if (! debug ) { |
| /* Non-debug path for speed */ |
| for (i=0; i<m; i++, ij++) { |
| fjac[ij] = (fjac[ij] - wa[i])/(2*h); /* fjac[i+m*j] */ |
| } |
| } else { |
| /* Debug path for correctness */ |
| for (i=0; i<m; i++, ij++) { |
| double fjold = fjac[ij]; |
| fjac[ij] = (wa2[i] - wa[i])/(2*h); /* fjac[i+m*j] */ |
| if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) || |
| ((da != 0 || dr != 0) && (fabs(fjold-fjac[ij]) > da + fabs(fjold)*dr))) { |
| printf(" %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n", |
| i, fvec[i], fjold, fjac[ij], fjold-fjac[ij], |
| (fjold == 0)?(0):((fjold-fjac[ij])/fjold)); |
| } |
| } |
| } /* end debugging */ |
| |
| } /* if (dside > 2) */ |
| } /* if (has_numerical_derivative) */ |
| |
| if (has_debug_deriv) { |
| printf("FJAC DEBUG END\n"); |
| } |
| |
| DONE: |
| if (iflag < 0) return iflag; |
| return 0; |
| /* |
| * last card of subroutine fdjac2. |
| */ |
| } |
| |
| |
| /************************qrfac.c*************************/ |
| |
| static |
| void mp_qrfac(int m, int n, double *a, int lda, |
| int pivot, int *ipvt, int lipvt, |
| double *rdiag, double *acnorm, double *wa) |
| { |
| /* |
| * ********** |
| * |
| * subroutine qrfac |
| * |
| * this subroutine uses householder transformations with column |
| * pivoting (optional) to compute a qr factorization of the |
| * m by n matrix a. that is, qrfac determines an orthogonal |
| * matrix q, a permutation matrix p, and an upper trapezoidal |
| * matrix r with diagonal elements of nonincreasing magnitude, |
| * such that a*p = q*r. the householder transformation for |
| * column k, k = 1,2,...,min(m,n), is of the form |
| * |
| * t |
| * i - (1/u(k))*u*u |
| * |
| * where u has zeros in the first k-1 positions. the form of |
| * this transformation and the method of pivoting first |
| * appeared in the corresponding linpack subroutine. |
| * |
| * the subroutine statement is |
| * |
| * subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) |
| * |
| * where |
| * |
| * m is a positive integer input variable set to the number |
| * of rows of a. |
| * |
| * n is a positive integer input variable set to the number |
| * of columns of a. |
| * |
| * a is an m by n array. on input a contains the matrix for |
| * which the qr factorization is to be computed. on output |
| * the strict upper trapezoidal part of a contains the strict |
| * upper trapezoidal part of r, and the lower trapezoidal |
| * part of a contains a factored form of q (the non-trivial |
| * elements of the u vectors described above). |
| * |
| * lda is a positive integer input variable not less than m |
| * which specifies the leading dimension of the array a. |
| * |
| * pivot is a logical input variable. if pivot is set true, |
| * then column pivoting is enforced. if pivot is set false, |
| * then no column pivoting is done. |
| * |
| * ipvt is an integer output array of length lipvt. ipvt |
| * defines the permutation matrix p such that a*p = q*r. |
| * column j of p is column ipvt(j) of the identity matrix. |
| * if pivot is false, ipvt is not referenced. |
| * |
| * lipvt is a positive integer input variable. if pivot is false, |
| * then lipvt may be as small as 1. if pivot is true, then |
| * lipvt must be at least n. |
| * |
| * rdiag is an output array of length n which contains the |
| * diagonal elements of r. |
| * |
| * acnorm is an output array of length n which contains the |
| * norms of the corresponding columns of the input matrix a. |
| * if this information is not needed, then acnorm can coincide |
| * with rdiag. |
| * |
| * wa is a work array of length n. if pivot is false, then wa |
| * can coincide with rdiag. |
| * |
| * subprograms called |
| * |
| * minpack-supplied ... dpmpar,enorm |
| * |
| * fortran-supplied ... dmax1,dsqrt,min0 |
| * |
| * argonne national laboratory. minpack project. march 1980. |
| * burton s. garbow, kenneth e. hillstrom, jorge j. more |
| * |
| * ********** |
| */ |
| int i,ij,jj,j,jp1,k,kmax,minmn; |
| double ajnorm,sum,temp; |
| static double zero = 0.0; |
| static double one = 1.0; |
| static double p05 = 0.05; |
| |
| lda = 0; /* Prevent compiler warning */ |
| lipvt = 0; /* Prevent compiler warning */ |
| if (lda) {} /* Prevent compiler warning */ |
| if (lipvt) {} /* Prevent compiler warning */ |
| |
| /* |
| * compute the initial column norms and initialize several arrays. |
| */ |
| ij = 0; |
| for (j=0; j<n; j++) { |
| acnorm[j] = mp_enorm(m,&a[ij]); |
| rdiag[j] = acnorm[j]; |
| wa[j] = rdiag[j]; |
| if (pivot != 0) |
| ipvt[j] = j; |
| ij += m; /* m*j */ |
| } |
| /* |
| * reduce a to r with householder transformations. |
| */ |
| minmn = mp_min0(m,n); |
| for (j=0; j<minmn; j++) { |
| if (pivot == 0) |
| goto L40; |
| /* |
| * bring the column of largest norm into the pivot position. |
| */ |
| kmax = j; |
| for (k=j; k<n; k++) |
| { |
| if (rdiag[k] > rdiag[kmax]) |
| kmax = k; |
| } |
| if (kmax == j) |
| goto L40; |
| |
| ij = m * j; |
| jj = m * kmax; |
| for (i=0; i<m; i++) |
| { |
| temp = a[ij]; /* [i+m*j] */ |
| a[ij] = a[jj]; /* [i+m*kmax] */ |
| a[jj] = temp; |
| ij += 1; |
| jj += 1; |
| } |
| rdiag[kmax] = rdiag[j]; |
| wa[kmax] = wa[j]; |
| k = ipvt[j]; |
| ipvt[j] = ipvt[kmax]; |
| ipvt[kmax] = k; |
| |
| L40: |
| /* |
| * compute the householder transformation to reduce the |
| * j-th column of a to a multiple of the j-th unit vector. |
| */ |
| jj = j + m*j; |
| ajnorm = mp_enorm(m-j,&a[jj]); |
| if (ajnorm == zero) |
| goto L100; |
| if (a[jj] < zero) |
| ajnorm = -ajnorm; |
| ij = jj; |
| for (i=j; i<m; i++) |
| { |
| a[ij] /= ajnorm; |
| ij += 1; /* [i+m*j] */ |
| } |
| a[jj] += one; |
| /* |
| * apply the transformation to the remaining columns |
| * and update the norms. |
| */ |
| jp1 = j + 1; |
| if (jp1 < n) |
| { |
| for (k=jp1; k<n; k++) |
| { |
| sum = zero; |
| ij = j + m*k; |
| jj = j + m*j; |
| for (i=j; i<m; i++) |
| { |
| sum += a[jj]*a[ij]; |
| ij += 1; /* [i+m*k] */ |
| jj += 1; /* [i+m*j] */ |
| } |
| temp = sum/a[j+m*j]; |
| ij = j + m*k; |
| jj = j + m*j; |
| for (i=j; i<m; i++) |
| { |
| a[ij] -= temp*a[jj]; |
| ij += 1; /* [i+m*k] */ |
| jj += 1; /* [i+m*j] */ |
| } |
| if ((pivot != 0) && (rdiag[k] != zero)) |
| { |
| temp = a[j+m*k]/rdiag[k]; |
| temp = mp_dmax1( zero, one-temp*temp ); |
| rdiag[k] *= sqrt(temp); |
| temp = rdiag[k]/wa[k]; |
| if ((p05*temp*temp) <= MP_MACHEP0) |
| { |
| rdiag[k] = mp_enorm(m-j-1,&a[jp1+m*k]); |
| wa[k] = rdiag[k]; |
| } |
| } |
| } |
| } |
| |
| L100: |
| rdiag[j] = -ajnorm; |
| } |
| /* |
| * last card of subroutine qrfac. |
| */ |
| } |
| |
| /************************qrsolv.c*************************/ |
| |
| static |
| void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, |
| double *qtb, double *x, double *sdiag, double *wa) |
| { |
| /* |
| * ********** |
| * |
| * subroutine qrsolv |
| * |
| * given an m by n matrix a, an n by n diagonal matrix d, |
| * and an m-vector b, the problem is to determine an x which |
| * solves the system |
| * |
| * a*x = b , d*x = 0 , |
| * |
| * in the least squares sense. |
| * |
| * this subroutine completes the solution of the problem |
| * if it is provided with the necessary information from the |
| * qr factorization, with column pivoting, of a. that is, if |
| * a*p = q*r, where p is a permutation matrix, q has orthogonal |
| * columns, and r is an upper triangular matrix with diagonal |
| * elements of nonincreasing magnitude, then qrsolv expects |
| * the full upper triangle of r, the permutation matrix p, |
| * and the first n components of (q transpose)*b. the system |
| * a*x = b, d*x = 0, is then equivalent to |
| * |
| * t t |
| * r*z = q *b , p *d*p*z = 0 , |
| * |
| * where x = p*z. if this system does not have full rank, |
| * then a least squares solution is obtained. on output qrsolv |
| * also provides an upper triangular matrix s such that |
| * |
| * t t t |
| * p *(a *a + d*d)*p = s *s . |
| * |
| * s is computed within qrsolv and may be of separate interest. |
| * |
| * the subroutine statement is |
| * |
| * subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa) |
| * |
| * where |
| * |
| * n is a positive integer input variable set to the order of r. |
| * |
| * r is an n by n array. on input the full upper triangle |
| * must contain the full upper triangle of the matrix r. |
| * on output the full upper triangle is unaltered, and the |
| * strict lower triangle contains the strict upper triangle |
| * (transposed) of the upper triangular matrix s. |
| * |
| * ldr is a positive integer input variable not less than n |
| * which specifies the leading dimension of the array r. |
| * |
| * ipvt is an integer input array of length n which defines the |
| * permutation matrix p such that a*p = q*r. column j of p |
| * is column ipvt(j) of the identity matrix. |
| * |
| * diag is an input array of length n which must contain the |
| * diagonal elements of the matrix d. |
| * |
| * qtb is an input array of length n which must contain the first |
| * n elements of the vector (q transpose)*b. |
| * |
| * x is an output array of length n which contains the least |
| * squares solution of the system a*x = b, d*x = 0. |
| * |
| * sdiag is an output array of length n which contains the |
| * diagonal elements of the upper triangular matrix s. |
| * |
| * wa is a work array of length n. |
| * |
| * subprograms called |
| * |
| * fortran-supplied ... dabs,dsqrt |
| * |
| * argonne national laboratory. minpack project. march 1980. |
| * burton s. garbow, kenneth e. hillstrom, jorge j. more |
| * |
| * ********** |
| */ |
| int i,ij,ik,kk,j,jp1,k,kp1,l,nsing; |
| double cosx,cotan,qtbpj,sinx,sum,tanx,temp; |
| static double zero = 0.0; |
| static double p25 = 0.25; |
| static double p5 = 0.5; |
| |
| /* |
| * copy r and (q transpose)*b to preserve input and initialize s. |
| * in particular, save the diagonal elements of r in x. |
| */ |
| kk = 0; |
| for (j=0; j<n; j++) { |
| ij = kk; |
| ik = kk; |
| for (i=j; i<n; i++) |
| { |
| r[ij] = r[ik]; |
| ij += 1; /* [i+ldr*j] */ |
| ik += ldr; /* [j+ldr*i] */ |
| } |
| x[j] = r[kk]; |
| wa[j] = qtb[j]; |
| kk += ldr+1; /* j+ldr*j */ |
| } |
| |
| /* |
| * eliminate the diagonal matrix d using a givens rotation. |
| */ |
| for (j=0; j<n; j++) { |
| /* |
| * prepare the row of d to be eliminated, locating the |
| * diagonal element using p from the qr factorization. |
| */ |
| l = ipvt[j]; |
| if (diag[l] == zero) |
| goto L90; |
| for (k=j; k<n; k++) |
| sdiag[k] = zero; |
| sdiag[j] = diag[l]; |
| /* |
| * the transformations to eliminate the row of d |
| * modify only a single element of (q transpose)*b |
| * beyond the first n, which is initially zero. |
| */ |
| qtbpj = zero; |
| for (k=j; k<n; k++) |
| { |
| /* |
| * determine a givens rotation which eliminates the |
| * appropriate element in the current row of d. |
| */ |
| if (sdiag[k] == zero) |
| continue; |
| kk = k + ldr * k; |
| if (fabs(r[kk]) < fabs(sdiag[k])) |
| { |
| cotan = r[kk]/sdiag[k]; |
| sinx = p5/sqrt(p25+p25*cotan*cotan); |
| cosx = sinx*cotan; |
| } |
| else |
| { |
| tanx = sdiag[k]/r[kk]; |
| cosx = p5/sqrt(p25+p25*tanx*tanx); |
| sinx = cosx*tanx; |
| } |
| /* |
| * compute the modified diagonal element of r and |
| * the modified element of ((q transpose)*b,0). |
| */ |
| r[kk] = cosx*r[kk] + sinx*sdiag[k]; |
| temp = cosx*wa[k] + sinx*qtbpj; |
| qtbpj = -sinx*wa[k] + cosx*qtbpj; |
| wa[k] = temp; |
| /* |
| * accumulate the tranformation in the row of s. |
| */ |
| kp1 = k + 1; |
| if (n > kp1) |
| { |
| ik = kk + 1; |
| for (i=kp1; i<n; i++) |
| { |
| temp = cosx*r[ik] + sinx*sdiag[i]; |
| sdiag[i] = -sinx*r[ik] + cosx*sdiag[i]; |
| r[ik] = temp; |
| ik += 1; /* [i+ldr*k] */ |
| } |
| } |
| } |
| L90: |
| /* |
| * store the diagonal element of s and restore |
| * the corresponding diagonal element of r. |
| */ |
| kk = j + ldr*j; |
| sdiag[j] = r[kk]; |
| r[kk] = x[j]; |
| } |
| /* |
| * solve the triangular system for z. if the system is |
| * singular, then obtain a least squares solution. |
| */ |
| nsing = n; |
| for (j=0; j<n; j++) { |
| if ((sdiag[j] == zero) && (nsing == n)) |
| nsing = j; |
| if (nsing < n) |
| wa[j] = zero; |
| } |
| if (nsing < 1) |
| goto L150; |
| |
| for (k=0; k<nsing; k++) { |
| j = nsing - k - 1; |
| sum = zero; |
| jp1 = j + 1; |
| if (nsing > jp1) |
| { |
| ij = jp1 + ldr * j; |
| for (i=jp1; i<nsing; i++) |
| { |
| sum += r[ij]*wa[i]; |
| ij += 1; /* [i+ldr*j] */ |
| } |
| } |
| wa[j] = (wa[j] - sum)/sdiag[j]; |
| } |
| L150: |
| /* |
| * permute the components of z back to components of x. |
| */ |
| for (j=0; j<n; j++) { |
| l = ipvt[j]; |
| x[l] = wa[j]; |
| } |
| /* |
| * last card of subroutine qrsolv. |
| */ |
| } |
| |
| /************************lmpar.c*************************/ |
| |
| static |
| void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag, |
| double *qtb, double delta, double *par, double *x, |
| double *sdiag, double *wa1, double *wa2) |
| { |
| /* ********** |
| * |
| * subroutine lmpar |
| * |
| * given an m by n matrix a, an n by n nonsingular diagonal |
| * matrix d, an m-vector b, and a positive number delta, |
| * the problem is to determine a value for the parameter |
| * par such that if x solves the system |
| * |
| * a*x = b , sqrt(par)*d*x = 0 , |
| * |
| * in the least squares sense, and dxnorm is the euclidean |
| * norm of d*x, then either par is zero and |
| * |
| * (dxnorm-delta) .le. 0.1*delta , |
| * |
| * or par is positive and |
| * |
| * abs(dxnorm-delta) .le. 0.1*delta . |
| * |
| * this subroutine completes the solution of the problem |
| * if it is provided with the necessary information from the |
| * qr factorization, with column pivoting, of a. that is, if |
| * a*p = q*r, where p is a permutation matrix, q has orthogonal |
| * columns, and r is an upper triangular matrix with diagonal |
| * elements of nonincreasing magnitude, then lmpar expects |
| * the full upper triangle of r, the permutation matrix p, |
| * and the first n components of (q transpose)*b. on output |
| * lmpar also provides an upper triangular matrix s such that |
| * |
| * t t t |
| * p *(a *a + par*d*d)*p = s *s . |
| * |
| * s is employed within lmpar and may be of separate interest. |
| * |
| * only a few iterations are generally needed for convergence |
| * of the algorithm. if, however, the limit of 10 iterations |
| * is reached, then the output par will contain the best |
| * value obtained so far. |
| * |
| * the subroutine statement is |
| * |
| * subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, |
| * wa1,wa2) |
| * |
| * where |
| * |
| * n is a positive integer input variable set to the order of r. |
| * |
| * r is an n by n array. on input the full upper triangle |
| * must contain the full upper triangle of the matrix r. |
| * on output the full upper triangle is unaltered, and the |
| * strict lower triangle contains the strict upper triangle |
| * (transposed) of the upper triangular matrix s. |
| * |
| * ldr is a positive integer input variable not less than n |
| * which specifies the leading dimension of the array r. |
| * |
| * ipvt is an integer input array of length n which defines the |
| * permutation matrix p such that a*p = q*r. column j of p |
| * is column ipvt(j) of the identity matrix. |
| * |
| * diag is an input array of length n which must contain the |
| * diagonal elements of the matrix d. |
| * |
| * qtb is an input array of length n which must contain the first |
| * n elements of the vector (q transpose)*b. |
| * |
| * delta is a positive input variable which specifies an upper |
| * bound on the euclidean norm of d*x. |
| * |
| * par is a nonnegative variable. on input par contains an |
| * initial estimate of the levenberg-marquardt parameter. |
| * on output par contains the final estimate. |
| * |
| * x is an output array of length n which contains the least |
| * squares solution of the system a*x = b, sqrt(par)*d*x = 0, |
| * for the output par. |
| * |
| * sdiag is an output array of length n which contains the |
| * diagonal elements of the upper triangular matrix s. |
| * |
| * wa1 and wa2 are work arrays of length n. |
| * |
| * subprograms called |
| * |
| * minpack-supplied ... dpmpar,mp_enorm,qrsolv |
| * |
| * fortran-supplied ... dabs,mp_dmax1,dmin1,dsqrt |
| * |
| * argonne national laboratory. minpack project. march 1980. |
| * burton s. garbow, kenneth e. hillstrom, jorge j. more |
| * |
| * ********** |
| */ |
| int i,iter,ij,jj,j,jm1,jp1,k,l,nsing; |
| double dxnorm,fp,gnorm,parc,parl,paru; |
| double sum,temp; |
| static double zero = 0.0; |
| /* static double one = 1.0; */ |
| static double p1 = 0.1; |
| static double p001 = 0.001; |
| |
| /* |
| * compute and store in x the gauss-newton direction. if the |
| * jacobian is rank-deficient, obtain a least squares solution. |
| */ |
| nsing = n; |
| jj = 0; |
| for (j=0; j<n; j++) { |
| wa1[j] = qtb[j]; |
| if ((r[jj] == zero) && (nsing == n)) |
| nsing = j; |
| if (nsing < n) |
| wa1[j] = zero; |
| jj += ldr+1; /* [j+ldr*j] */ |
| } |
| |
| if (nsing >= 1) { |
| for (k=0; k<nsing; k++) |
| { |
| j = nsing - k - 1; |
| wa1[j] = wa1[j]/r[j+ldr*j]; |
| temp = wa1[j]; |
| jm1 = j - 1; |
| if (jm1 >= 0) |
| { |
| ij = ldr * j; |
| for (i=0; i<=jm1; i++) |
| { |
| wa1[i] -= r[ij]*temp; |
| ij += 1; |
| } |
| } |
| } |
| } |
| |
| for (j=0; j<n; j++) { |
| l = ipvt[j]; |
| x[l] = wa1[j]; |
| } |
| /* |
| * initialize the iteration counter. |
| * evaluate the function at the origin, and test |
| * for acceptance of the gauss-newton direction. |
| */ |
| iter = 0; |
| for (j=0; j<n; j++) |
| wa2[j] = diag[ifree[j]]*x[j]; |
| dxnorm = mp_enorm(n,wa2); |
| fp = dxnorm - delta; |
| if (fp <= p1*delta) { |
| goto L220; |
| } |
| /* |
| * if the jacobian is not rank deficient, the newton |
| * step provides a lower bound, parl, for the zero of |
| * the function. otherwise set this bound to zero. |
| */ |
| parl = zero; |
| if (nsing >= n) { |
| for (j=0; j<n; j++) |
| { |
| l = ipvt[j]; |
| wa1[j] = diag[ifree[l]]*(wa2[l]/dxnorm); |
| } |
| jj = 0; |
| for (j=0; j<n; j++) |
| { |
| sum = zero; |
| jm1 = j - 1; |
| if (jm1 >= 0) |
| { |
| ij = jj; |
| for (i=0; i<=jm1; i++) |
| { |
| sum += r[ij]*wa1[i]; |
| ij += 1; |
| } |
| } |
| wa1[j] = (wa1[j] - sum)/r[j+ldr*j]; |
| jj += ldr; /* [i+ldr*j] */ |
| } |
| temp = mp_enorm(n,wa1); |
| parl = ((fp/delta)/temp)/temp; |
| } |
| /* |
| * calculate an upper bound, paru, for the zero of the function. |
| */ |
| jj = 0; |
| for (j=0; j<n; j++) { |
| sum = zero; |
| ij = jj; |
| for (i=0; i<=j; i++) |
| { |
| sum += r[ij]*qtb[i]; |
| ij += 1; |
| } |
| l = ipvt[j]; |
| wa1[j] = sum/diag[ifree[l]]; |
| jj += ldr; /* [i+ldr*j] */ |
| } |
| gnorm = mp_enorm(n,wa1); |
| paru = gnorm/delta; |
| if (paru == zero) |
| paru = MP_DWARF/mp_dmin1(delta,p1); |
| /* |
| * if the input par lies outside of the interval (parl,paru), |
| * set par to the closer endpoint. |
| */ |
| *par = mp_dmax1( *par,parl); |
| *par = mp_dmin1( *par,paru); |
| if (*par == zero) |
| *par = gnorm/dxnorm; |
| |
| /* |
| * beginning of an iteration. |
| */ |
| L150: |
| iter += 1; |
| /* |
| * evaluate the function at the current value of par. |
| */ |
| if (*par == zero) |
| *par = mp_dmax1(MP_DWARF,p001*paru); |
| temp = sqrt( *par ); |
| for (j=0; j<n; j++) |
| wa1[j] = temp*diag[ifree[j]]; |
| mp_qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2); |
| for (j=0; j<n; j++) |
| wa2[j] = diag[ifree[j]]*x[j]; |
| dxnorm = mp_enorm(n,wa2); |
| temp = fp; |
| fp = dxnorm - delta; |
| /* |
| * if the function is small enough, accept the current value |
| * of par. also test for the exceptional cases where parl |
| * is zero or the number of iterations has reached 10. |
| */ |
| if ((fabs(fp) <= p1*delta) |
| || ((parl == zero) && (fp <= temp) && (temp < zero)) |
| || (iter == 10)) |
| goto L220; |
| /* |
| * compute the newton correction. |
| */ |
| for (j=0; j<n; j++) { |
| l = ipvt[j]; |
| wa1[j] = diag[ifree[l]]*(wa2[l]/dxnorm); |
| } |
| jj = 0; |
| for (j=0; j<n; j++) { |
| wa1[j] = wa1[j]/sdiag[j]; |
| temp = wa1[j]; |
| jp1 = j + 1; |
| if (jp1 < n) |
| { |
| ij = jp1 + jj; |
| for (i=jp1; i<n; i++) |
| { |
| wa1[i] -= r[ij]*temp; |
| ij += 1; /* [i+ldr*j] */ |
| } |
| } |
| jj += ldr; /* ldr*j */ |
| } |
| temp = mp_enorm(n,wa1); |
| parc = ((fp/delta)/temp)/temp; |
| /* |
| * depending on the sign of the function, update parl or paru. |
| */ |
| if (fp > zero) |
| parl = mp_dmax1(parl, *par); |
| if (fp < zero) |
| paru = mp_dmin1(paru, *par); |
| /* |
| * compute an improved estimate for par. |
| */ |
| *par = mp_dmax1(parl, *par + parc); |
| /* |
| * end of an iteration. |
| */ |
| goto L150; |
| |
| L220: |
| /* |
| * termination. |
| */ |
| if (iter == 0) |
| *par = zero; |
| /* |
| * last card of subroutine lmpar. |
| */ |
| } |
| |
| |
| /************************enorm.c*************************/ |
| |
| static |
| double mp_enorm(int n, double *x) |
| { |
| /* |
| * ********** |
| * |
| * function enorm |
| * |
| * given an n-vector x, this function calculates the |
| * euclidean norm of x. |
| * |
| * the euclidean norm is computed by accumulating the sum of |
| * squares in three different sums. the sums of squares for the |
| * small and large components are scaled so that no overflows |
| * occur. non-destructive underflows are permitted. underflows |
| * and overflows do not occur in the computation of the unscaled |
| * sum of squares for the intermediate components. |
| * the definitions of small, intermediate and large components |
| * depend on two constants, rdwarf and rgiant. the main |
| * restrictions on these constants are that rdwarf**2 not |
| * underflow and rgiant**2 not overflow. the constants |
| * given here are suitable for every known computer. |
| * |
| * the function statement is |
| * |
| * double precision function enorm(n,x) |
| * |
| * where |
| * |
| * n is a positive integer input variable. |
| * |
| * x is an input array of length n. |
| * |
| * subprograms called |
| * |
| * fortran-supplied ... dabs,dsqrt |
| * |
| * argonne national laboratory. minpack project. march 1980. |
| * burton s. garbow, kenneth e. hillstrom, jorge j. more |
| * |
| * ********** |
| */ |
| int i; |
| double agiant,floatn,s1,s2,s3,xabs,x1max,x3max; |
| double ans, temp; |
| double rdwarf = MP_RDWARF; |
| double rgiant = MP_RGIANT; |
| static double zero = 0.0; |
| static double one = 1.0; |
| |
| s1 = zero; |
| s2 = zero; |
| s3 = zero; |
| x1max = zero; |
| x3max = zero; |
| floatn = n; |
| agiant = rgiant/floatn; |
| |
| for (i=0; i<n; i++) { |
| xabs = fabs(x[i]); |
| if ((xabs > rdwarf) && (xabs < agiant)) |
| { |
| /* |
| * sum for intermediate components. |
| */ |
| s2 += xabs*xabs; |
| continue; |
| } |
| |
| if (xabs > rdwarf) |
| { |
| /* |
| * sum for large components. |
| */ |
| if (xabs > x1max) |
| { |
| temp = x1max/xabs; |
| s1 = one + s1*temp*temp; |
| x1max = xabs; |
| } |
| else |
| { |
| temp = xabs/x1max; |
| s1 += temp*temp; |
| } |
| continue; |
| } |
| /* |
| * sum for small components. |
| */ |
| if (xabs > x3max) |
| { |
| temp = x3max/xabs; |
| s3 = one + s3*temp*temp; |
| x3max = xabs; |
| } |
| else |
| { |
| if (xabs != zero) |
| { |
| temp = xabs/x3max; |
| s3 += temp*temp; |
| } |
| } |
| } |
| /* |
| * calculation of norm. |
| */ |
| if (s1 != zero) { |
| temp = s1 + (s2/x1max)/x1max; |
| ans = x1max*sqrt(temp); |
| return(ans); |
| } |
| if (s2 != zero) { |
| if (s2 >= x3max) |
| temp = s2*(one+(x3max/s2)*(x3max*s3)); |
| else |
| temp = x3max*((s2/x3max)+(x3max*s3)); |
| ans = sqrt(temp); |
| } |
| else |
| { |
| ans = x3max*sqrt(s3); |
| } |
| return(ans); |
| /* |
| * last card of function enorm. |
| */ |
| } |
| |
| /************************lmmisc.c*************************/ |
| |
| static |
| double mp_dmax1(double a, double b) |
| { |
| if (a >= b) |
| return(a); |
| else |
| return(b); |
| } |
| |
| static |
| double mp_dmin1(double a, double b) |
| { |
| if (a <= b) |
| return(a); |
| else |
| return(b); |
| } |
| |
| static |
| int mp_min0(int a, int b) |
| { |
| if (a <= b) |
| return(a); |
| else |
| return(b); |
| } |
| |
| /************************covar.c*************************/ |
| /* |
| c ********** |
| c |
| c subroutine covar |
| c |
| c given an m by n matrix a, the problem is to determine |
| c the covariance matrix corresponding to a, defined as |
| c |
| c t |
| c inverse(a *a) . |
| c |
| c this subroutine completes the solution of the problem |
| c if it is provided with the necessary information from the |
| c qr factorization, with column pivoting, of a. that is, if |
| c a*p = q*r, where p is a permutation matrix, q has orthogonal |
| c columns, and r is an upper triangular matrix with diagonal |
| c elements of nonincreasing magnitude, then covar expects |
| c the full upper triangle of r and the permutation matrix p. |
| c the covariance matrix is then computed as |
| c |
| c t t |
| c p*inverse(r *r)*p . |
| c |
| c if a is nearly rank deficient, it may be desirable to compute |
| c the covariance matrix corresponding to the linearly independent |
| c columns of a. to define the numerical rank of a, covar uses |
| c the tolerance tol. if l is the largest integer such that |
| c |
| c abs(r(l,l)) .gt. tol*abs(r(1,1)) , |
| c |
| c then covar computes the covariance matrix corresponding to |
| c the first l columns of r. for k greater than l, column |
| c and row ipvt(k) of the covariance matrix are set to zero. |
| c |
| c the subroutine statement is |
| c |
| c subroutine covar(n,r,ldr,ipvt,tol,wa) |
| c |
| c where |
| c |
| c n is a positive integer input variable set to the order of r. |
| c |
| c r is an n by n array. on input the full upper triangle must |
| c contain the full upper triangle of the matrix r. on output |
| c r contains the square symmetric covariance matrix. |
| c |
| c ldr is a positive integer input variable not less than n |
| c which specifies the leading dimension of the array r. |
| c |
| c ipvt is an integer input array of length n which defines the |
| c permutation matrix p such that a*p = q*r. column j of p |
| c is column ipvt(j) of the identity matrix. |
| c |
| c tol is a nonnegative input variable used to define the |
| c numerical rank of a in the manner described above. |
| c |
| c wa is a work array of length n. |
| c |
| c subprograms called |
| c |
| c fortran-supplied ... dabs |
| c |
| c argonne national laboratory. minpack project. august 1980. |
| c burton s. garbow, kenneth e. hillstrom, jorge j. more |
| c |
| c ********** |
| */ |
| |
| static |
| int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa) |
| { |
| int i, ii, j, jj, k, l; |
| int kk, kj, ji, j0, k0, jj0; |
| int sing; |
| double one = 1.0, temp, tolr, zero = 0.0; |
| |
| /* |
| * form the inverse of r in the full upper triangle of r. |
| */ |
| |
| #if 0 |
| for (j=0; j<n; j++) { |
| for (i=0; i<n; i++) { |
| printf("%f ", r[j*ldr+i]); |
| } |
| printf("\n"); |
| } |
| #endif |
| |
| tolr = tol*fabs(r[0]); |
| l = -1; |
| for (k=0; k<n; k++) { |
| kk = k*ldr + k; |
| if (fabs(r[kk]) <= tolr) break; |
| |
| r[kk] = one/r[kk]; |
| for (j=0; j<k; j++) { |
| kj = k*ldr + j; |
| temp = r[kk] * r[kj]; |
| r[kj] = zero; |
| |
| k0 = k*ldr; j0 = j*ldr; |
| for (i=0; i<=j; i++) { |
| r[k0+i] += (-temp*r[j0+i]); |
| } |
| } |
| l = k; |
| } |
| |
| /* |
| * Form the full upper triangle of the inverse of (r transpose)*r |
| * in the full upper triangle of r |
| */ |
| |
| if (l >= 0) { |
| for (k=0; k <= l; k++) { |
| k0 = k*ldr; |
| |
| for (j=0; j<k; j++) { |
| temp = r[k*ldr+j]; |
| |
| j0 = j*ldr; |
| for (i=0; i<=j; i++) { |
| r[j0+i] += temp*r[k0+i]; |
| } |
| } |
| |
| temp = r[k0+k]; |
| for (i=0; i<=k; i++) { |
| r[k0+i] *= temp; |
| } |
| } |
| } |
| |
| /* |
| * For the full lower triangle of the covariance matrix |
| * in the strict lower triangle or and in wa |
| */ |
| for (j=0; j<n; j++) { |
| jj = ipvt[j]; |
| sing = (j > l); |
| j0 = j*ldr; |
| jj0 = jj*ldr; |
| for (i=0; i<=j; i++) { |
| ji = j0+i; |
| |
| if (sing) r[ji] = zero; |
| ii = ipvt[i]; |
| if (ii > jj) r[jj0+ii] = r[ji]; |
| if (ii < jj) r[ii*ldr+jj] = r[ji]; |
| } |
| wa[jj] = r[j0+j]; |
| } |
| |
| /* |
| * Symmetrize the covariance matrix in r |
| */ |
| for (j=0; j<n; j++) { |
| j0 = j*ldr; |
| for (i=0; i<j; i++) { |
| r[j0+i] = r[i*ldr+j]; |
| } |
| r[j0+j] = wa[j]; |
| } |
| |
| #if 0 |
| for (j=0; j<n; j++) { |
| for (i=0; i<n; i++) { |
| printf("%f ", r[j*ldr+i]); |
| } |
| printf("\n"); |
| } |
| #endif |
| |
| return 0; |
| } |