home *** CD-ROM | disk | FTP | other *** search
/ Power-Programmierung / CD2.mdf / c / library / dos / math / cephes / eigen / lmdif.c < prev   
Encoding:
C/C++ Source or Header  |  1992-11-17  |  40.5 KB  |  1,793 lines
  1. /************************lmdif*************************/
  2.  
  3. /*
  4.  * Solves or minimizes the sum of squares of m nonlinear
  5.  * functions of n variables.
  6.  *
  7.  * From public domain Fortran version
  8.  * of Argonne National Laboratories MINPACK
  9.  *
  10.  * C translation by Steve Moshier
  11.  */
  12.  
  13. /****************Sample main program******************/
  14. #define BUG 0
  15. #define N 4
  16. #define M 4
  17. double ftol = 1.0e-14;
  18. double xtol = 1.0e-14;
  19. double gtol = 1.0e-14;
  20. double epsfcn = 1.0e-15;
  21. double factor = 0.1;
  22. double x[N] = {0.0};
  23. double fvec[M] = {0.0};
  24. double diag[N] = {0.0};
  25. double fjac[M*N] = {0.0};
  26. double qtf[N] = {0.0};
  27. double wa1[N] = {0.0};
  28. double wa2[N] = {0.0};
  29. double wa3[N] = {0.0};
  30. double wa4[M] = {0.0};
  31. int ipvt[N] = {0};
  32. int maxfev = 200 * (N+1);
  33. /* resolution of arithmetic */
  34. double MACHEP = 1.2e-16;
  35. extern double MACHEP;
  36. /* smallest nonzero number */
  37. double DWARF = 1.0e-38;
  38. extern double DWARF;
  39.  
  40. int fcn();
  41. char *infmsg[] = {
  42. "improper input parameters",
  43. "the relative error in the sum of squares is at most tol",
  44. "the relative error between x and the solution is at most tol",
  45. "conditions for info = 1 and info = 2 both hold",
  46. "fvec is orthogonal to the columns of the jacobian to machine precision",
  47. "number of calls to fcn has reached or exceeded 200*(n+1)",
  48. "tol is too small. no further reduction in the sum of squares is possible",
  49. "tol too small. no further improvement in approximate solution x possible"
  50. };
  51. double enorm();
  52.  
  53. main()
  54. {
  55. int m,n,info;
  56. /*
  57. *    fcn is the name of the user-supplied subroutine which
  58. *      calculates the functions. fcn must be declared
  59. *      in an external statement in the user calling
  60. *      program, and should be written as follows.
  61. *
  62. *      subroutine fcn(m,n,x,fvec,iflag)
  63. *      integer m,n,iflag
  64. *      double precision x(n),fvec(m)
  65. *      ----------
  66. *      calculate the functions at x and
  67. *      return this vector in fvec.
  68. *      ----------
  69. *      return
  70. *      end
  71. *
  72. *      the value of iflag should not be changed by fcn unless
  73. *      the user wants to terminate execution of lmdif1.
  74. *      in this case set iflag to a negative integer.
  75. *
  76. *    m is a positive integer input variable set to the number
  77. *      of functions.
  78. *
  79. *    n is a positive integer input variable set to the number
  80. *      of variables. n must not exceed m.
  81. *
  82. *    x is an array of length n. on input x must contain
  83. *      an initial estimate of the solution vector. on output x
  84. *      contains the final estimate of the solution vector.
  85. *
  86. *    fvec is an output array of length m which contains
  87. *      the functions evaluated at the output x.
  88. *
  89. *
  90. *     argonne national laboratory. minpack project. march 1980.
  91. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  92. *
  93. *     **********
  94. */
  95. int mode,nfev,nprint;
  96. int iflag, ldfjac;
  97. static double zero = 0.0;
  98.  
  99. n = N;
  100. m = M;
  101. fcn(m, n, x, fvec, &iflag);
  102. printf( "initial x\n" );
  103. pmat( 1, n, x ); /* display 1 by n matrix */
  104. printf( "initial function\n" );
  105. pmat( 1, m, fvec );
  106.  
  107. /* Call lmdif. */
  108. ldfjac = m;
  109. mode = 1;
  110. nprint = 1;
  111. info = 0;
  112.  
  113. lmdif(m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
  114.  diag,mode,factor,nprint,&info,&nfev,fjac,
  115.  ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4);
  116.  
  117. printf( "%d function evalutations\n", nfev );
  118. /* display solution and function vector */
  119. printf( "x\n" );
  120. pmat( 1, n, x );
  121. printf( "fvec\n" );
  122. pmat( 1, m, fvec );
  123. printf( "function norm = %.15e\n", enorm(m, fvec) );
  124. /* display info returned by lmdif */
  125. if(info >= 8)
  126.     info = 4;
  127. printf( "%s\n", infmsg[info] );
  128. }
  129.  
  130.  
  131. /***********Sample of user supplied function****************
  132.  * m = number of functions
  133.  * n = number of variables
  134.  * x = vector of function arguments
  135.  * fvec = vector of function values
  136.  * iflag = error return variable
  137.  */
  138. fcn(m,n,x,fvec,iflag)
  139. int m,n;
  140. int *iflag;
  141. double x[],fvec[];
  142. {
  143. double temp;
  144. double exp(), sin(), log();
  145.  
  146. /* an arbitrary test function: */
  147. fvec[0] = 1.0 - 0.3 * x[0] + 0.9 * x[1] - 1.7 * x[2] +log(1.5+x[3]);
  148. fvec[1] = sin(-4.0*x[0] ) - 3.0 * x[1] + 0.1 * x[2] + x[3]*x[3];
  149. temp = (x[2] + 2.0) * x[2] * x[1];
  150. fvec[2] = 0.5 * x[1] - sin( x[2] + 1.0 ) + temp + .3 * x[3];
  151. fvec[3] = x[0]*x[1] + x[1]*x[2] + x[0]*x[2] - x[3]*x[3];
  152. }
  153. /*********************** lmdif.c ****************************/
  154. #define BUG 0
  155.  
  156. extern double MACHEP;
  157.  
  158. lmdif(m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
  159.  diag,mode,factor,nprint,info,nfev,fjac,
  160.  ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
  161. int m,n,maxfev,mode,nprint,ldfjac;
  162. int *info, *nfev;
  163. double ftol, xtol, gtol, epsfcn, factor;
  164. double x[], fvec[], diag[], fjac[], qtf[];
  165. double wa1[], wa2[], wa3[], wa4[];
  166. int ipvt[];
  167. {
  168. /*
  169. *     **********
  170. *
  171. *     subroutine lmdif
  172. *
  173. *     the purpose of lmdif is to minimize the sum of the squares of
  174. *     m nonlinear functions in n variables by a modification of
  175. *     the levenberg-marquardt algorithm. the user must provide a
  176. *     subroutine which calculates the functions. the jacobian is
  177. *     then calculated by a forward-difference approximation.
  178. *
  179. *     the subroutine statement is
  180. *
  181. *    subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
  182. *             diag,mode,factor,nprint,info,nfev,fjac,
  183. *             ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
  184. *
  185. *     where
  186. *
  187. *    fcn is the name of the user-supplied subroutine which
  188. *      calculates the functions. fcn must be declared
  189. *      in an external statement in the user calling
  190. *      program, and should be written as follows.
  191. *
  192. *      subroutine fcn(m,n,x,fvec,iflag)
  193. *      integer m,n,iflag
  194. *      double precision x(n),fvec(m)
  195. *      ----------
  196. *      calculate the functions at x and
  197. *      return this vector in fvec.
  198. *      ----------
  199. *      return
  200. *      end
  201. *
  202. *      the value of iflag should not be changed by fcn unless
  203. *      the user wants to terminate execution of lmdif.
  204. *      in this case set iflag to a negative integer.
  205. *
  206. *    m is a positive integer input variable set to the number
  207. *      of functions.
  208. *
  209. *    n is a positive integer input variable set to the number
  210. *      of variables. n must not exceed m.
  211. *
  212. *    x is an array of length n. on input x must contain
  213. *      an initial estimate of the solution vector. on output x
  214. *      contains the final estimate of the solution vector.
  215. *
  216. *    fvec is an output array of length m which contains
  217. *      the functions evaluated at the output x.
  218. *
  219. *    ftol is a nonnegative input variable. termination
  220. *      occurs when both the actual and predicted relative
  221. *      reductions in the sum of squares are at most ftol.
  222. *      therefore, ftol measures the relative error desired
  223. *      in the sum of squares.
  224. *
  225. *    xtol is a nonnegative input variable. termination
  226. *      occurs when the relative error between two consecutive
  227. *      iterates is at most xtol. therefore, xtol measures the
  228. *      relative error desired in the approximate solution.
  229. *
  230. *    gtol is a nonnegative input variable. termination
  231. *      occurs when the cosine of the angle between fvec and
  232. *      any column of the jacobian is at most gtol in absolute
  233. *      value. therefore, gtol measures the orthogonality
  234. *      desired between the function vector and the columns
  235. *      of the jacobian.
  236. *
  237. *    maxfev is a positive integer input variable. termination
  238. *      occurs when the number of calls to fcn is at least
  239. *      maxfev by the end of an iteration.
  240. *
  241. *    epsfcn is an input variable used in determining a suitable
  242. *      step length for the forward-difference approximation. this
  243. *      approximation assumes that the relative errors in the
  244. *      functions are of the order of epsfcn. if epsfcn is less
  245. *      than the machine precision, it is assumed that the relative
  246. *      errors in the functions are of the order of the machine
  247. *      precision.
  248. *
  249. *    diag is an array of length n. if mode = 1 (see
  250. *      below), diag is internally set. if mode = 2, diag
  251. *      must contain positive entries that serve as
  252. *      multiplicative scale factors for the variables.
  253. *
  254. *    mode is an integer input variable. if mode = 1, the
  255. *      variables will be scaled internally. if mode = 2,
  256. *      the scaling is specified by the input diag. other
  257. *      values of mode are equivalent to mode = 1.
  258. *
  259. *    factor is a positive input variable used in determining the
  260. *      initial step bound. this bound is set to the product of
  261. *      factor and the euclidean norm of diag*x if nonzero, or else
  262. *      to factor itself. in most cases factor should lie in the
  263. *      interval (.1,100.). 100. is a generally recommended value.
  264. *
  265. *    nprint is an integer input variable that enables controlled
  266. *      printing of iterates if it is positive. in this case,
  267. *      fcn is called with iflag = 0 at the beginning of the first
  268. *      iteration and every nprint iterations thereafter and
  269. *      immediately prior to return, with x and fvec available
  270. *      for printing. if nprint is not positive, no special calls
  271. *      of fcn with iflag = 0 are made.
  272. *
  273. *    info is an integer output variable. if the user has
  274. *      terminated execution, info is set to the (negative)
  275. *      value of iflag. see description of fcn. otherwise,
  276. *      info is set as follows.
  277. *
  278. *      info = 0  improper input parameters.
  279. *
  280. *      info = 1  both actual and predicted relative reductions
  281. *            in the sum of squares are at most ftol.
  282. *
  283. *      info = 2  relative error between two consecutive iterates
  284. *            is at most xtol.
  285. *
  286. *      info = 3  conditions for info = 1 and info = 2 both hold.
  287. *
  288. *      info = 4  the cosine of the angle between fvec and any
  289. *            column of the jacobian is at most gtol in
  290. *            absolute value.
  291. *
  292. *      info = 5  number of calls to fcn has reached or
  293. *            exceeded maxfev.
  294. *
  295. *      info = 6  ftol is too small. no further reduction in
  296. *            the sum of squares is possible.
  297. *
  298. *      info = 7  xtol is too small. no further improvement in
  299. *            the approximate solution x is possible.
  300. *
  301. *      info = 8  gtol is too small. fvec is orthogonal to the
  302. *            columns of the jacobian to machine precision.
  303. *
  304. *    nfev is an integer output variable set to the number of
  305. *      calls to fcn.
  306. *
  307. *    fjac is an output m by n array. the upper n by n submatrix
  308. *      of fjac contains an upper triangular matrix r with
  309. *      diagonal elements of nonincreasing magnitude such that
  310. *
  311. *         t     t       t
  312. *        p *(jac *jac)*p = r *r,
  313. *
  314. *      where p is a permutation matrix and jac is the final
  315. *      calculated jacobian. column j of p is column ipvt(j)
  316. *      (see below) of the identity matrix. the lower trapezoidal
  317. *      part of fjac contains information generated during
  318. *      the computation of r.
  319. *
  320. *    ldfjac is a positive integer input variable not less than m
  321. *      which specifies the leading dimension of the array fjac.
  322. *
  323. *    ipvt is an integer output array of length n. ipvt
  324. *      defines a permutation matrix p such that jac*p = q*r,
  325. *      where jac is the final calculated jacobian, q is
  326. *      orthogonal (not stored), and r is upper triangular
  327. *      with diagonal elements of nonincreasing magnitude.
  328. *      column j of p is column ipvt(j) of the identity matrix.
  329. *
  330. *    qtf is an output array of length n which contains
  331. *      the first n elements of the vector (q transpose)*fvec.
  332. *
  333. *    wa1, wa2, and wa3 are work arrays of length n.
  334. *
  335. *    wa4 is a work array of length m.
  336. *
  337. *     subprograms called
  338. *
  339. *    user-supplied ...... fcn
  340. *
  341. *    minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
  342. *
  343. *    fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
  344. *
  345. *     argonne national laboratory. minpack project. march 1980.
  346. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  347. *
  348. *     **********
  349. */
  350. int i,iflag,ij,jj,iter,j,l;
  351. double actred,delta,dirder,fnorm,fnorm1,gnorm;
  352. double par,pnorm,prered,ratio;
  353. double sum,temp,temp1,temp2,temp3,xnorm;
  354. double enorm(), fabs(), dmax1(), dmin1(), sqrt();
  355. int fcn();    /* user supplied function */
  356. static double one = 1.0;
  357. static double p1 = 0.1;
  358. static double p5 = 0.5;
  359. static double p25 = 0.25;
  360. static double p75 = 0.75;
  361. static double p0001 = 1.0e-4;
  362. static double zero = 0.0;
  363. static double p05 = 0.05;
  364.  
  365. *info = 0;
  366. iflag = 0;
  367. *nfev = 0;
  368. /*
  369. *     check the input parameters for errors.
  370. */
  371. if( (n <= 0) || (m < n) || (ldfjac < m) || (ftol < zero)
  372.     || (xtol < zero) || (gtol < zero) || (maxfev <= 0)
  373.     || (factor <= zero) )
  374.     goto L300;
  375.  
  376. if( mode == 2 )
  377.     { /* scaling by diag[] */
  378.     for( j=0; j<n; j++ )
  379.         {
  380.         if( diag[j] <= 0.0 )
  381.             goto L300;
  382.         }    
  383.     }
  384. #if BUG
  385. printf( "lmdif\n" );
  386. #endif
  387. /*
  388. *     evaluate the function at the starting point
  389. *     and calculate its norm.
  390. */
  391. iflag = 1;
  392. fcn(m,n,x,fvec,&iflag);
  393. *nfev = 1;
  394. if(iflag < 0)
  395.     goto L300;
  396. fnorm = enorm(m,fvec);
  397. /*
  398. *     initialize levenberg-marquardt parameter and iteration counter.
  399. */
  400. par = zero;
  401. iter = 1;
  402. /*
  403. *     beginning of the outer loop.
  404. */
  405.  
  406. L30:
  407.  
  408. /*
  409. *     calculate the jacobian matrix.
  410. */
  411. iflag = 2;
  412. fdjac2(m,n,x,fvec,fjac,ldfjac,&iflag,epsfcn,wa4);
  413. *nfev += n;
  414. if(iflag < 0)
  415.     goto L300;
  416. /*
  417. *     if requested, call fcn to enable printing of iterates.
  418. */
  419. if( nprint > 0 )
  420.     {
  421.     iflag = 0;
  422.     if(mod(iter-1,nprint) == 0)
  423.         {
  424.         fcn(m,n,x,fvec,&iflag);
  425.         if(iflag < 0)
  426.             goto L300;
  427.         printf( "fnorm %.15e\n", enorm(m,fvec) );
  428.         }
  429.     }
  430. /*
  431. *     compute the qr factorization of the jacobian.
  432. */
  433. qrfac(m,n,fjac,ldfjac,1,ipvt,n,wa1,wa2,wa3);
  434. /*
  435. *     on the first iteration and if mode is 1, scale according
  436. *     to the norms of the columns of the initial jacobian.
  437. */
  438. if(iter == 1)
  439.     {
  440.     if(mode != 2)
  441.         {
  442.         for( j=0; j<n; j++ )
  443.             {
  444.             diag[j] = wa2[j];
  445.             if( wa2[j] == zero )
  446.                 diag[j] = one;
  447.             }
  448.         }
  449.  
  450. /*
  451. *     on the first iteration, calculate the norm of the scaled x
  452. *     and initialize the step bound delta.
  453. */
  454.     for( j=0; j<n; j++ )
  455.         wa3[j] = diag[j] * x[j];
  456.  
  457.     xnorm = enorm(n,wa3);
  458.     delta = factor*xnorm;
  459.     if(delta == zero)
  460.         delta = factor;
  461.     }
  462.  
  463. /*
  464. *     form (q transpose)*fvec and store the first n components in
  465. *     qtf.
  466. */
  467. for( i=0; i<m; i++ )
  468.     wa4[i] = fvec[i];
  469. jj = 0;
  470. for( j=0; j<n; j++ )
  471.     {
  472.     temp3 = fjac[jj];
  473.     if(temp3 != zero)
  474.         {
  475.         sum = zero;
  476.         ij = jj;
  477.         for( i=j; i<m; i++ )
  478.             {
  479.             sum += fjac[ij] * wa4[i];
  480.             ij += 1;    /* fjac[i+m*j] */
  481.             }
  482.         temp = -sum / temp3;
  483.         ij = jj;
  484.         for( i=j; i<m; i++ )
  485.             {
  486.             wa4[i] += fjac[ij] * temp;
  487.             ij += 1;    /* fjac[i+m*j] */
  488.             }
  489.         }
  490.     fjac[jj] = wa1[j];
  491.     jj += m+1;    /* fjac[j+m*j] */
  492.     qtf[j] = wa4[j];
  493.     }
  494.  
  495. /*
  496. *     compute the norm of the scaled gradient.
  497. */
  498.  gnorm = zero;
  499.  if(fnorm != zero)
  500.     {
  501.     jj = 0;
  502.     for( j=0; j<n; j++ )
  503.         {
  504.         l = ipvt[j];
  505.         if(wa2[l] != zero)
  506.                 {
  507.             sum = zero;
  508.             ij = jj;
  509.             for( i=0; i<=j; i++ )
  510.                 {
  511.                 sum += fjac[ij]*(qtf[i]/fnorm);
  512.                 ij += 1; /* fjac[i+m*j] */
  513.                 }
  514.             gnorm = dmax1(gnorm,fabs(sum/wa2[l]));
  515.             }
  516.         jj += m;
  517.         }
  518.     }
  519.  
  520. /*
  521. *     test for convergence of the gradient norm.
  522. */
  523.  if(gnorm <= gtol)
  524.      *info = 4;
  525.  if( *info != 0)
  526.      goto L300;
  527. /*
  528. *     rescale if necessary.
  529. */
  530.  if(mode != 2)
  531.     {
  532.     for( j=0; j<n; j++ )
  533.         diag[j] = dmax1(diag[j],wa2[j]);
  534.     }
  535.  
  536. /*
  537. *     beginning of the inner loop.
  538. */
  539. L200:
  540. /*
  541. *        determine the levenberg-marquardt parameter.
  542. */
  543. lmpar(n,fjac,ldfjac,ipvt,diag,qtf,delta,&par,wa1,wa2,wa3,wa4);
  544. /*
  545. *        store the direction p and x + p. calculate the norm of p.
  546. */
  547. for( j=0; j<n; j++ )
  548.     {
  549.        wa1[j] = -wa1[j];
  550.        wa2[j] = x[j] + wa1[j];
  551.        wa3[j] = diag[j]*wa1[j];
  552.     }
  553. pnorm = enorm(n,wa3);
  554. /*
  555. *        on the first iteration, adjust the initial step bound.
  556. */
  557. if(iter == 1)
  558.     delta = dmin1(delta,pnorm);
  559. /*
  560. *        evaluate the function at x + p and calculate its norm.
  561. */
  562. iflag = 1;
  563. fcn(m,n,wa2,wa4,&iflag);
  564. *nfev += 1;
  565. if(iflag < 0)
  566.     goto L300;
  567. fnorm1 = enorm(m,wa4);
  568. #if BUG 
  569. printf( "pnorm %.10e  fnorm1 %.10e\n", pnorm, fnorm1 );
  570. #endif
  571. /*
  572. *        compute the scaled actual reduction.
  573. */
  574. actred = -one;
  575. if( (p1*fnorm1) < fnorm)
  576.     {
  577.     temp = fnorm1/fnorm;
  578.     actred = one - temp * temp;
  579.     }
  580. /*
  581. *        compute the scaled predicted reduction and
  582. *        the scaled directional derivative.
  583. */
  584. jj = 0;
  585. for( j=0; j<n; j++ )
  586.     {
  587.     wa3[j] = zero;
  588.     l = ipvt[j];
  589.     temp = wa1[l];
  590.     ij = jj;
  591.     for( i=0; i<=j; i++ )
  592.         {
  593.         wa3[i] += fjac[ij]*temp;
  594.         ij += 1; /* fjac[i+m*j] */
  595.         }
  596.     jj += m;
  597.     }
  598. temp1 = enorm(n,wa3)/fnorm;
  599. temp2 = (sqrt(par)*pnorm)/fnorm;
  600. prered = temp1*temp1 + (temp2*temp2)/p5;
  601. dirder = -(temp1*temp1 + temp2*temp2);
  602. /*
  603. *        compute the ratio of the actual to the predicted
  604. *        reduction.
  605. */
  606. ratio = zero;
  607. if(prered != zero)
  608.     ratio = actred/prered;
  609. /*
  610. *        update the step bound.
  611. */
  612. if(ratio <= p25)
  613.     {
  614.     if(actred >= zero)
  615.         temp = p5;
  616.     else
  617.         temp = p5*dirder/(dirder + p5*actred);
  618.     if( ((p1*fnorm1) >= fnorm)
  619.     || (temp < p1) )
  620.         temp = p1;
  621.        delta = temp*dmin1(delta,pnorm/p1);
  622.        par = par/temp;
  623.     }
  624. else
  625.     {
  626.     if( (par == zero) || (ratio >= p75) )
  627.         {
  628.         delta = pnorm/p5;
  629.         par = p5*par;
  630.         }
  631.     }
  632. /*
  633. *        test for successful iteration.
  634. */
  635. if(ratio >= p0001)
  636.     {
  637. /*
  638. *        successful iteration. update x, fvec, and their norms.
  639. */
  640.     for( j=0; j<n; j++ )
  641.         {
  642.         x[j] = wa2[j];
  643.         wa2[j] = diag[j]*x[j];
  644.         }
  645.     for( i=0; i<m; i++ )
  646.         fvec[i] = wa4[i];
  647.     xnorm = enorm(n,wa2);
  648.     fnorm = fnorm1;
  649.     iter += 1;
  650.     }
  651. /*
  652. *        tests for convergence.
  653. */
  654. if( (fabs(actred) <= ftol)
  655.   && (prered <= ftol)
  656.   && (p5*ratio <= one) )
  657.     *info = 1;
  658. if(delta <= xtol*xnorm)
  659.     *info = 2;
  660. if( (fabs(actred) <= ftol)
  661.   && (prered <= ftol)
  662.   && (p5*ratio <= one)
  663.   && ( *info == 2) )
  664.     *info = 3;
  665. if( *info != 0)
  666.     goto L300;
  667. /*
  668. *        tests for termination and stringent tolerances.
  669. */
  670. if( *nfev >= maxfev)
  671.     *info = 5;
  672. if( (fabs(actred) <= MACHEP)
  673.   && (prered <= MACHEP)
  674.   && (p5*ratio <= one) )
  675.     *info = 6;
  676. if(delta <= MACHEP*xnorm)
  677.     *info = 7;
  678. if(gnorm <= MACHEP)
  679.     *info = 8;
  680. if( *info != 0)
  681.     goto L300;
  682. /*
  683. *        end of the inner loop. repeat if iteration unsuccessful.
  684. */
  685. if(ratio < p0001)
  686.     goto L200;
  687. /*
  688. *     end of the outer loop.
  689. */
  690. goto L30;
  691.     
  692. L300:
  693. /*
  694. *     termination, either normal or user imposed.
  695. */
  696. if(iflag < 0)
  697.     *info = iflag;
  698. iflag = 0;
  699. if(nprint > 0)
  700.     fcn(m,n,x,fvec,&iflag);
  701. /*
  702.       last card of subroutine lmdif.
  703. */
  704. }
  705. /************************lmpar.c*************************/
  706.  
  707. #define BUG 0
  708.  
  709. lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,wa1,wa2)
  710. int n,ldr;
  711. int ipvt[];
  712. double delta;
  713. double *par;
  714. double r[],diag[],qtb[],x[],sdiag[],wa1[],wa2[];
  715. {
  716. /*     **********
  717. *
  718. *     subroutine lmpar
  719. *
  720. *     given an m by n matrix a, an n by n nonsingular diagonal
  721. *     matrix d, an m-vector b, and a positive number delta,
  722. *     the problem is to determine a value for the parameter
  723. *     par such that if x solves the system
  724. *
  725. *        a*x = b ,      sqrt(par)*d*x = 0 ,
  726. *
  727. *     in the least squares sense, and dxnorm is the euclidean
  728. *     norm of d*x, then either par is zero and
  729. *
  730. *        (dxnorm-delta) .le. 0.1*delta ,
  731. *
  732. *     or par is positive and
  733. *
  734. *        abs(dxnorm-delta) .le. 0.1*delta .
  735. *
  736. *     this subroutine completes the solution of the problem
  737. *     if it is provided with the necessary information from the
  738. *     qr factorization, with column pivoting, of a. that is, if
  739. *     a*p = q*r, where p is a permutation matrix, q has orthogonal
  740. *     columns, and r is an upper triangular matrix with diagonal
  741. *     elements of nonincreasing magnitude, then lmpar expects
  742. *     the full upper triangle of r, the permutation matrix p,
  743. *     and the first n components of (q transpose)*b. on output
  744. *     lmpar also provides an upper triangular matrix s such that
  745. *
  746. *         t     t             t
  747. *        p *(a *a + par*d*d)*p = s *s .
  748. *
  749. *     s is employed within lmpar and may be of separate interest.
  750. *
  751. *     only a few iterations are generally needed for convergence
  752. *     of the algorithm. if, however, the limit of 10 iterations
  753. *     is reached, then the output par will contain the best
  754. *     value obtained so far.
  755. *
  756. *     the subroutine statement is
  757. *
  758. *    subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,
  759. *             wa1,wa2)
  760. *
  761. *     where
  762. *
  763. *    n is a positive integer input variable set to the order of r.
  764. *
  765. *    r is an n by n array. on input the full upper triangle
  766. *      must contain the full upper triangle of the matrix r.
  767. *      on output the full upper triangle is unaltered, and the
  768. *      strict lower triangle contains the strict upper triangle
  769. *      (transposed) of the upper triangular matrix s.
  770. *
  771. *    ldr is a positive integer input variable not less than n
  772. *      which specifies the leading dimension of the array r.
  773. *
  774. *    ipvt is an integer input array of length n which defines the
  775. *      permutation matrix p such that a*p = q*r. column j of p
  776. *      is column ipvt(j) of the identity matrix.
  777. *
  778. *    diag is an input array of length n which must contain the
  779. *      diagonal elements of the matrix d.
  780. *
  781. *    qtb is an input array of length n which must contain the first
  782. *      n elements of the vector (q transpose)*b.
  783. *
  784. *    delta is a positive input variable which specifies an upper
  785. *      bound on the euclidean norm of d*x.
  786. *
  787. *    par is a nonnegative variable. on input par contains an
  788. *      initial estimate of the levenberg-marquardt parameter.
  789. *      on output par contains the final estimate.
  790. *
  791. *    x is an output array of length n which contains the least
  792. *      squares solution of the system a*x = b, sqrt(par)*d*x = 0,
  793. *      for the output par.
  794. *
  795. *    sdiag is an output array of length n which contains the
  796. *      diagonal elements of the upper triangular matrix s.
  797. *
  798. *    wa1 and wa2 are work arrays of length n.
  799. *
  800. *     subprograms called
  801. *
  802. *    minpack-supplied ... dpmpar,enorm,qrsolv
  803. *
  804. *    fortran-supplied ... dabs,dmax1,dmin1,dsqrt
  805. *
  806. *     argonne national laboratory. minpack project. march 1980.
  807. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  808. *
  809. *     **********
  810. */
  811. int i,iter,ij,jj,j,jm1,jp1,k,l,nsing;
  812. double dxnorm,fp,gnorm,parc,parl,paru;
  813. double sum,temp;
  814. double enorm(), fabs(), dmax1(), dmin1(), sqrt();
  815. static double zero = 0.0;
  816. static double one = 1.0;
  817. static double p1 = 0.1;
  818. static double p001 = 0.001;
  819. extern double MACHEP;
  820. extern double DWARF;
  821.  
  822. #if BUG
  823. printf( "lmpar\n" );
  824. #endif
  825. /*
  826. *     compute and store in x the gauss-newton direction. if the
  827. *     jacobian is rank-deficient, obtain a least squares solution.
  828. */
  829. nsing = n;
  830. jj = 0;
  831. for( j=0; j<n; j++ )
  832.     {
  833.     wa1[j] = qtb[j];
  834.     if( (r[jj] == zero) && (nsing == n) )
  835.         nsing = j;
  836.     if(nsing < n)
  837.         wa1[j] = zero;
  838.     jj += ldr+1; /* [j+ldr*j] */
  839.     }
  840. #if BUG
  841. printf( "nsing %d ", nsing );
  842. #endif
  843. if(nsing >= 1)
  844.     {
  845.     for( k=0; k<nsing; k++ )
  846.         {
  847.         j = nsing - k - 1;
  848.         wa1[j] = wa1[j]/r[j+ldr*j];
  849.         temp = wa1[j];
  850.         jm1 = j - 1;
  851.         if(jm1 >= 0)
  852.             {
  853.             ij = ldr * j;
  854.             for( i=0; i<=jm1; i++ )
  855.                 {
  856.                 wa1[i] -= r[ij]*temp;
  857.                 ij += 1;
  858.                 }
  859.             }
  860.         }
  861.     }
  862.  
  863. for( j=0; j<n; j++ )
  864.     {
  865.     l = ipvt[j];
  866.     x[l] = wa1[j];
  867.     }
  868. /*
  869. *     initialize the iteration counter.
  870. *     evaluate the function at the origin, and test
  871. *     for acceptance of the gauss-newton direction.
  872. */
  873. iter = 0;
  874. for( j=0; j<n; j++ )
  875.     wa2[j] = diag[j]*x[j];
  876. dxnorm = enorm(n,wa2);
  877. fp = dxnorm - delta;
  878. if(fp <= p1*delta)
  879.     {
  880. #if BUG
  881.     printf( "going to L220\n" );
  882. #endif
  883.     goto L220;
  884.     }
  885. /*
  886. *     if the jacobian is not rank deficient, the newton
  887. *     step provides a lower bound, parl, for the zero of
  888. *     the function. otherwise set this bound to zero.
  889. */
  890. parl = zero;
  891. if(nsing >= n)
  892.     {
  893.     for( j=0; j<n; j++ )
  894.         {
  895.         l = ipvt[j];
  896.         wa1[j] = diag[l]*(wa2[l]/dxnorm);
  897.         }
  898.     jj = 0;
  899.     for( j=0; j<n; j++ )
  900.         {
  901.         sum = zero;
  902.         jm1 = j - 1;
  903.         if(jm1 >= 0)
  904.             {
  905.             ij = jj;
  906.             for( i=0; i<=jm1; i++ )
  907.                 {
  908.                 sum += r[ij]*wa1[i];
  909.                 ij += 1;
  910.                 }
  911.             }
  912.         wa1[j] = (wa1[j] - sum)/r[j+ldr*j];
  913.         jj += ldr; /* [i+ldr*j] */
  914.         }
  915.     temp = enorm(n,wa1);
  916.     parl = ((fp/delta)/temp)/temp;
  917.     }
  918. /*
  919. *     calculate an upper bound, paru, for the zero of the function.
  920. */
  921. jj = 0;
  922. for( j=0; j<n; j++ )
  923.     {
  924.     sum = zero;
  925.     ij = jj;
  926.     for( i=0; i<=j; i++ )
  927.         {
  928.         sum += r[ij]*qtb[i];
  929.         ij += 1;
  930.         }
  931.     l = ipvt[j];
  932.     wa1[j] = sum/diag[l];
  933.     jj += ldr; /* [i+ldr*j] */
  934.     }
  935. gnorm = enorm(n,wa1);
  936. paru = gnorm/delta;
  937. if(paru == zero)
  938.     paru = DWARF/dmin1(delta,p1);
  939. /*
  940. *     if the input par lies outside of the interval (parl,paru),
  941. *     set par to the closer endpoint.
  942. */
  943. *par = dmax1( *par,parl);
  944. *par = dmin1( *par,paru);
  945. if( *par == zero)
  946.     *par = gnorm/dxnorm;
  947. #if BUG
  948. printf( "parl %.4e  par %.4e  paru %.4e\n", parl, *par, paru );
  949. #endif
  950. /*
  951. *     beginning of an iteration.
  952. */
  953. L150:
  954. iter += 1;
  955. /*
  956. *     evaluate the function at the current value of par.
  957. */
  958. if( *par == zero)
  959.     *par = dmax1(DWARF,p001*paru);
  960. temp = sqrt( *par );
  961. for( j=0; j<n; j++ )
  962.     wa1[j] = temp*diag[j];
  963. qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2);
  964. for( j=0; j<n; j++ )
  965.     wa2[j] = diag[j]*x[j];
  966. dxnorm = enorm(n,wa2);
  967. temp = fp;
  968. fp = dxnorm - delta;
  969. /*
  970. *     if the function is small enough, accept the current value
  971. *     of par. also test for the exceptional cases where parl
  972. *     is zero or the number of iterations has reached 10.
  973. */
  974. if( (fabs(fp) <= p1*delta)
  975.  || ((parl == zero) && (fp <= temp) && (temp < zero))
  976.  || (iter == 10) )
  977.     goto L220;
  978. /*
  979. *     compute the newton correction.
  980. */
  981. for( j=0; j<n; j++ )
  982.     {
  983.     l = ipvt[j];
  984.     wa1[j] = diag[l]*(wa2[l]/dxnorm);
  985.     }
  986. jj = 0;
  987. for( j=0; j<n; j++ )
  988.     {
  989.     wa1[j] = wa1[j]/sdiag[j];
  990.     temp = wa1[j];
  991.     jp1 = j + 1;
  992.     if(jp1 < n)
  993.         {
  994.         ij = jp1 + jj;
  995.         for( i=jp1; i<n; i++ )
  996.             {
  997.             wa1[i] -= r[ij]*temp;
  998.             ij += 1; /* [i+ldr*j] */
  999.             }
  1000.         }
  1001.     jj += ldr; /* ldr*j */
  1002.     }
  1003. temp = enorm(n,wa1);
  1004. parc = ((fp/delta)/temp)/temp;
  1005. /*
  1006. *     depending on the sign of the function, update parl or paru.
  1007. */
  1008. if(fp > zero)
  1009.     parl = dmax1(parl, *par);
  1010. if(fp < zero)
  1011.     paru = dmin1(paru, *par);
  1012. /*
  1013. *     compute an improved estimate for par.
  1014. */
  1015. *par = dmax1(parl, *par + parc);
  1016. /*
  1017. *     end of an iteration.
  1018. */
  1019. goto L150;
  1020.  
  1021. L220:
  1022. /*
  1023. *     termination.
  1024. */
  1025. if(iter == 0)
  1026.     *par = zero;
  1027. /*
  1028. *     last card of subroutine lmpar.
  1029. */
  1030. }
  1031. /************************qrfac.c*************************/
  1032.  
  1033. #define BUG 0
  1034.  
  1035. qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
  1036. int m,n,lda,lipvt;
  1037. int ipvt[];
  1038. int pivot;
  1039. double a[],rdiag[],acnorm[],wa[];
  1040. {
  1041. /*
  1042. *     **********
  1043. *
  1044. *     subroutine qrfac
  1045. *
  1046. *     this subroutine uses householder transformations with column
  1047. *     pivoting (optional) to compute a qr factorization of the
  1048. *     m by n matrix a. that is, qrfac determines an orthogonal
  1049. *     matrix q, a permutation matrix p, and an upper trapezoidal
  1050. *     matrix r with diagonal elements of nonincreasing magnitude,
  1051. *     such that a*p = q*r. the householder transformation for
  1052. *     column k, k = 1,2,...,min(m,n), is of the form
  1053. *
  1054. *                t
  1055. *        i - (1/u(k))*u*u
  1056. *
  1057. *     where u has zeros in the first k-1 positions. the form of
  1058. *     this transformation and the method of pivoting first
  1059. *     appeared in the corresponding linpack subroutine.
  1060. *
  1061. *     the subroutine statement is
  1062. *
  1063. *    subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
  1064. *
  1065. *     where
  1066. *
  1067. *    m is a positive integer input variable set to the number
  1068. *      of rows of a.
  1069. *
  1070. *    n is a positive integer input variable set to the number
  1071. *      of columns of a.
  1072. *
  1073. *    a is an m by n array. on input a contains the matrix for
  1074. *      which the qr factorization is to be computed. on output
  1075. *      the strict upper trapezoidal part of a contains the strict
  1076. *      upper trapezoidal part of r, and the lower trapezoidal
  1077. *      part of a contains a factored form of q (the non-trivial
  1078. *      elements of the u vectors described above).
  1079. *
  1080. *    lda is a positive integer input variable not less than m
  1081. *      which specifies the leading dimension of the array a.
  1082. *
  1083. *    pivot is a logical input variable. if pivot is set true,
  1084. *      then column pivoting is enforced. if pivot is set false,
  1085. *      then no column pivoting is done.
  1086. *
  1087. *    ipvt is an integer output array of length lipvt. ipvt
  1088. *      defines the permutation matrix p such that a*p = q*r.
  1089. *      column j of p is column ipvt(j) of the identity matrix.
  1090. *      if pivot is false, ipvt is not referenced.
  1091. *
  1092. *    lipvt is a positive integer input variable. if pivot is false,
  1093. *      then lipvt may be as small as 1. if pivot is true, then
  1094. *      lipvt must be at least n.
  1095. *
  1096. *    rdiag is an output array of length n which contains the
  1097. *      diagonal elements of r.
  1098. *
  1099. *    acnorm is an output array of length n which contains the
  1100. *      norms of the corresponding columns of the input matrix a.
  1101. *      if this information is not needed, then acnorm can coincide
  1102. *      with rdiag.
  1103. *
  1104. *    wa is a work array of length n. if pivot is false, then wa
  1105. *      can coincide with rdiag.
  1106. *
  1107. *     subprograms called
  1108. *
  1109. *    minpack-supplied ... dpmpar,enorm
  1110. *
  1111. *    fortran-supplied ... dmax1,dsqrt,min0
  1112. *
  1113. *     argonne national laboratory. minpack project. march 1980.
  1114. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  1115. *
  1116. *     **********
  1117. */
  1118. int i,ij,jj,j,jp1,k,kmax,minmn;
  1119. double ajnorm,sum,temp;
  1120. static double zero = 0.0;
  1121. static double one = 1.0;
  1122. static double p05 = 0.05;
  1123. extern double MACHEP;
  1124. double enorm(), dmax1(), sqrt();
  1125. /*
  1126. *     compute the initial column norms and initialize several arrays.
  1127. */
  1128. ij = 0;
  1129. for( j=0; j<n; j++ )
  1130.     {
  1131.     acnorm[j] = enorm(m,&a[ij]);
  1132.     rdiag[j] = acnorm[j];
  1133.     wa[j] = rdiag[j];
  1134.     if(pivot != 0)
  1135.         ipvt[j] = j;
  1136.     ij += m; /* m*j */
  1137.     }
  1138. #if BUG
  1139. printf( "qrfac\n" );
  1140. #endif
  1141. /*
  1142. *     reduce a to r with householder transformations.
  1143. */
  1144. minmn = min0(m,n);
  1145. for( j=0; j<minmn; j++ )
  1146. {
  1147. if(pivot == 0)
  1148.     goto L40;
  1149. /*
  1150. *     bring the column of largest norm into the pivot position.
  1151. */
  1152. kmax = j;
  1153. for( k=j; k<n; k++ )
  1154.     {
  1155.     if(rdiag[k] > rdiag[kmax])
  1156.         kmax = k;
  1157.     }
  1158. if(kmax == j)
  1159.     goto L40;
  1160.  
  1161. ij = m * j;
  1162. jj = m * kmax;
  1163. for( i=0; i<m; i++ )
  1164.     {
  1165.     temp = a[ij]; /* [i+m*j] */
  1166.     a[ij] = a[jj]; /* [i+m*kmax] */
  1167.     a[jj] = temp;
  1168.     ij += 1;
  1169.     jj += 1;
  1170.     }
  1171. rdiag[kmax] = rdiag[j];
  1172. wa[kmax] = wa[j];
  1173. k = ipvt[j];
  1174. ipvt[j] = ipvt[kmax];
  1175. ipvt[kmax] = k;
  1176.  
  1177. L40:
  1178. /*
  1179. *     compute the householder transformation to reduce the
  1180. *     j-th column of a to a multiple of the j-th unit vector.
  1181. */
  1182. jj = j + m*j;
  1183. ajnorm = enorm(m-j,&a[jj]);
  1184. if(ajnorm == zero)
  1185.     goto L100;
  1186. if(a[jj] < zero)
  1187.     ajnorm = -ajnorm;
  1188. ij = jj;
  1189. for( i=j; i<m; i++ )
  1190.     {
  1191.     a[ij] /= ajnorm;
  1192.     ij += 1; /* [i+m*j] */
  1193.     }
  1194. a[jj] += one;
  1195. /*
  1196. *     apply the transformation to the remaining columns
  1197. *     and update the norms.
  1198. */
  1199. jp1 = j + 1;
  1200. if(jp1 < n )
  1201. {
  1202. for( k=jp1; k<n; k++ )
  1203.     {
  1204.     sum = zero;
  1205.     ij = j + m*k;
  1206.     jj = j + m*j;
  1207.     for( i=j; i<m; i++ )
  1208.         {
  1209.         sum += a[jj]*a[ij];
  1210.         ij += 1; /* [i+m*k] */
  1211.         jj += 1; /* [i+m*j] */
  1212.         }
  1213.     temp = sum/a[j+m*j];
  1214.     ij = j + m*k;
  1215.     jj = j + m*j;
  1216.     for( i=j; i<m; i++ )
  1217.         {
  1218.         a[ij] -= temp*a[jj];
  1219.         ij += 1; /* [i+m*k] */
  1220.         jj += 1; /* [i+m*j] */
  1221.         }
  1222.     if( (pivot != 0) && (rdiag[k] != zero) )
  1223.         {
  1224.         temp = a[j+m*k]/rdiag[k];
  1225.         temp = dmax1( zero, one-temp*temp );
  1226.         rdiag[k] *= sqrt(temp);
  1227.         temp = rdiag[k]/wa[k];
  1228.         if( (p05*temp*temp) <= MACHEP)
  1229.             {
  1230.             rdiag[k] = enorm(m-j-1,&a[jp1+m*k]);
  1231.             wa[k] = rdiag[k];
  1232.             }
  1233.         }
  1234.     }
  1235. }
  1236.  
  1237. L100:
  1238.     rdiag[j] = -ajnorm;
  1239. }
  1240. /*
  1241. *     last card of subroutine qrfac.
  1242. */
  1243. }
  1244. /************************qrsolv.c*************************/
  1245.  
  1246. #define BUG 0
  1247.  
  1248. qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
  1249. int n,ldr;
  1250. int ipvt[];
  1251. double r[],diag[],qtb[],x[],sdiag[],wa[];
  1252. {
  1253. /*
  1254. *     **********
  1255. *
  1256. *     subroutine qrsolv
  1257. *
  1258. *     given an m by n matrix a, an n by n diagonal matrix d,
  1259. *     and an m-vector b, the problem is to determine an x which
  1260. *     solves the system
  1261. *
  1262. *        a*x = b ,      d*x = 0 ,
  1263. *
  1264. *     in the least squares sense.
  1265. *
  1266. *     this subroutine completes the solution of the problem
  1267. *     if it is provided with the necessary information from the
  1268. *     qr factorization, with column pivoting, of a. that is, if
  1269. *     a*p = q*r, where p is a permutation matrix, q has orthogonal
  1270. *     columns, and r is an upper triangular matrix with diagonal
  1271. *     elements of nonincreasing magnitude, then qrsolv expects
  1272. *     the full upper triangle of r, the permutation matrix p,
  1273. *     and the first n components of (q transpose)*b. the system
  1274. *     a*x = b, d*x = 0, is then equivalent to
  1275. *
  1276. *           t       t
  1277. *        r*z = q *b ,  p *d*p*z = 0 ,
  1278. *
  1279. *     where x = p*z. if this system does not have full rank,
  1280. *     then a least squares solution is obtained. on output qrsolv
  1281. *     also provides an upper triangular matrix s such that
  1282. *
  1283. *         t     t         t
  1284. *        p *(a *a + d*d)*p = s *s .
  1285. *
  1286. *     s is computed within qrsolv and may be of separate interest.
  1287. *
  1288. *     the subroutine statement is
  1289. *
  1290. *    subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
  1291. *
  1292. *     where
  1293. *
  1294. *    n is a positive integer input variable set to the order of r.
  1295. *
  1296. *    r is an n by n array. on input the full upper triangle
  1297. *      must contain the full upper triangle of the matrix r.
  1298. *      on output the full upper triangle is unaltered, and the
  1299. *      strict lower triangle contains the strict upper triangle
  1300. *      (transposed) of the upper triangular matrix s.
  1301. *
  1302. *    ldr is a positive integer input variable not less than n
  1303. *      which specifies the leading dimension of the array r.
  1304. *
  1305. *    ipvt is an integer input array of length n which defines the
  1306. *      permutation matrix p such that a*p = q*r. column j of p
  1307. *      is column ipvt(j) of the identity matrix.
  1308. *
  1309. *    diag is an input array of length n which must contain the
  1310. *      diagonal elements of the matrix d.
  1311. *
  1312. *    qtb is an input array of length n which must contain the first
  1313. *      n elements of the vector (q transpose)*b.
  1314. *
  1315. *    x is an output array of length n which contains the least
  1316. *      squares solution of the system a*x = b, d*x = 0.
  1317. *
  1318. *    sdiag is an output array of length n which contains the
  1319. *      diagonal elements of the upper triangular matrix s.
  1320. *
  1321. *    wa is a work array of length n.
  1322. *
  1323. *     subprograms called
  1324. *
  1325. *    fortran-supplied ... dabs,dsqrt
  1326. *
  1327. *     argonne national laboratory. minpack project. march 1980.
  1328. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  1329. *
  1330. *     **********
  1331. */
  1332. int i,ij,ik,kk,j,jp1,k,kp1,l,nsing;
  1333. double cos,cotan,qtbpj,sin,sum,tan,temp;
  1334. static double zero = 0.0;
  1335. static double p25 = 0.25;
  1336. static double p5 = 0.5;
  1337. double fabs(), sqrt();
  1338.  
  1339. /*
  1340. *     copy r and (q transpose)*b to preserve input and initialize s.
  1341. *     in particular, save the diagonal elements of r in x.
  1342. */
  1343. kk = 0;
  1344. for( j=0; j<n; j++ )
  1345.     {
  1346.     ij = kk;
  1347.     ik = kk;
  1348.     for( i=j; i<n; i++ )
  1349.         {
  1350.         r[ij] = r[ik];
  1351.         ij += 1;   /* [i+ldr*j] */
  1352.         ik += ldr; /* [j+ldr*i] */
  1353.         }
  1354.     x[j] = r[kk];
  1355.     wa[j] = qtb[j];
  1356.     kk += ldr+1; /* j+ldr*j */
  1357.     }
  1358. #if BUG
  1359. printf( "qrsolv\n" );
  1360. #endif
  1361. /*
  1362. *     eliminate the diagonal matrix d using a givens rotation.
  1363. */
  1364. for( j=0; j<n; j++ )
  1365. {
  1366. /*
  1367. *     prepare the row of d to be eliminated, locating the
  1368. *     diagonal element using p from the qr factorization.
  1369. */
  1370. l = ipvt[j];
  1371. if(diag[l] == zero)
  1372.     goto L90;
  1373. for( k=j; k<n; k++ )
  1374.     sdiag[k] = zero;
  1375. sdiag[j] = diag[l];
  1376. /*
  1377. *     the transformations to eliminate the row of d
  1378. *     modify only a single element of (q transpose)*b
  1379. *     beyond the first n, which is initially zero.
  1380. */
  1381. qtbpj = zero;
  1382. for( k=j; k<n; k++ )
  1383.     {
  1384. /*
  1385. *        determine a givens rotation which eliminates the
  1386. *        appropriate element in the current row of d.
  1387. */
  1388.     if(sdiag[k] == zero)
  1389.         continue;
  1390.     kk = k + ldr * k;
  1391.     if(fabs(r[kk]) < fabs(sdiag[k]))
  1392.         {
  1393.         cotan = r[kk]/sdiag[k];
  1394.         sin = p5/sqrt(p25+p25*cotan*cotan);
  1395.         cos = sin*cotan;
  1396.         }
  1397.     else
  1398.         {
  1399.         tan = sdiag[k]/r[kk];
  1400.         cos = p5/sqrt(p25+p25*tan*tan);
  1401.         sin = cos*tan;
  1402.         }
  1403. /*
  1404. *        compute the modified diagonal element of r and
  1405. *        the modified element of ((q transpose)*b,0).
  1406. */
  1407.     r[kk] = cos*r[kk] + sin*sdiag[k];
  1408.     temp = cos*wa[k] + sin*qtbpj;
  1409.     qtbpj = -sin*wa[k] + cos*qtbpj;
  1410.     wa[k] = temp;
  1411. /*
  1412. *        accumulate the tranformation in the row of s.
  1413. */
  1414.     kp1 = k + 1;
  1415.     if( n > kp1 )
  1416.         {
  1417.         ik = kk + 1;
  1418.         for( i=kp1; i<n; i++ )
  1419.             {
  1420.             temp = cos*r[ik] + sin*sdiag[i];
  1421.             sdiag[i] = -sin*r[ik] + cos*sdiag[i];
  1422.             r[ik] = temp;
  1423.             ik += 1; /* [i+ldr*k] */
  1424.             }
  1425.         }
  1426.     }
  1427. L90:
  1428. /*
  1429. *     store the diagonal element of s and restore
  1430. *     the corresponding diagonal element of r.
  1431. */
  1432.     kk = j + ldr*j;
  1433.     sdiag[j] = r[kk];
  1434.     r[kk] = x[j];
  1435. }
  1436. /*
  1437. *     solve the triangular system for z. if the system is
  1438. *     singular, then obtain a least squares solution.
  1439. */
  1440. nsing = n;
  1441. for( j=0; j<n; j++ )
  1442.     {
  1443.     if( (sdiag[j] == zero) && (nsing == n) )
  1444.         nsing = j;
  1445.     if(nsing < n)
  1446.         wa[j] = zero;
  1447.     }
  1448. if(nsing < 1)
  1449.     goto L150;
  1450.  
  1451. for( k=0; k<nsing; k++ )
  1452.     {
  1453.     j = nsing - k - 1;
  1454.     sum = zero;
  1455.     jp1 = j + 1;
  1456.     if(nsing > jp1)
  1457.         {
  1458.         ij = jp1 + ldr * j;
  1459.         for( i=jp1; i<nsing; i++ )
  1460.             {
  1461.             sum += r[ij]*wa[i];
  1462.             ij += 1; /* [i+ldr*j] */
  1463.             }
  1464.         }
  1465.     wa[j] = (wa[j] - sum)/sdiag[j];
  1466.     }
  1467. L150:
  1468. /*
  1469. *     permute the components of z back to components of x.
  1470. */
  1471. for( j=0; j<n; j++ )
  1472.     {
  1473.     l = ipvt[j];
  1474.     x[l] = wa[j];
  1475.     }
  1476. /*
  1477. *     last card of subroutine qrsolv.
  1478. */
  1479. }
  1480. /************************enorm.c*************************/
  1481.  
  1482. double enorm(n,x)
  1483. int n;
  1484. double x[];
  1485. {
  1486. /*
  1487. *     **********
  1488. *
  1489. *     function enorm
  1490. *
  1491. *     given an n-vector x, this function calculates the
  1492. *     euclidean norm of x.
  1493. *
  1494. *     the euclidean norm is computed by accumulating the sum of
  1495. *     squares in three different sums. the sums of squares for the
  1496. *     small and large components are scaled so that no overflows
  1497. *     occur. non-destructive underflows are permitted. underflows
  1498. *     and overflows do not occur in the computation of the unscaled
  1499. *     sum of squares for the intermediate components.
  1500. *     the definitions of small, intermediate and large components
  1501. *     depend on two constants, rdwarf and rgiant. the main
  1502. *     restrictions on these constants are that rdwarf**2 not
  1503. *     underflow and rgiant**2 not overflow. the constants
  1504. *     given here are suitable for every known computer.
  1505. *
  1506. *     the function statement is
  1507. *
  1508. *    double precision function enorm(n,x)
  1509. *
  1510. *     where
  1511. *
  1512. *    n is a positive integer input variable.
  1513. *
  1514. *    x is an input array of length n.
  1515. *
  1516. *     subprograms called
  1517. *
  1518. *    fortran-supplied ... dabs,dsqrt
  1519. *
  1520. *     argonne national laboratory. minpack project. march 1980.
  1521. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  1522. *
  1523. *     **********
  1524. */
  1525. int i;
  1526. double agiant,floatn,s1,s2,s3,xabs,x1max,x3max;
  1527. double ans, temp;
  1528. static double rdwarf = 3.834e-20;
  1529. static double rgiant = 1.304e19;
  1530. static double zero = 0.0;
  1531. static double one = 1.0;
  1532. double fabs(), sqrt();
  1533.  
  1534. s1 = zero;
  1535. s2 = zero;
  1536. s3 = zero;
  1537. x1max = zero;
  1538. x3max = zero;
  1539. floatn = n;
  1540. agiant = rgiant/floatn;
  1541.  
  1542. for( i=0; i<n; i++ )
  1543. {
  1544. xabs = fabs(x[i]);
  1545. if( (xabs > rdwarf) && (xabs < agiant) )
  1546.     {
  1547. /*
  1548. *        sum for intermediate components.
  1549. */
  1550.     s2 += xabs*xabs;
  1551.     continue;
  1552.     }
  1553.  
  1554. if(xabs > rdwarf)
  1555.     {
  1556. /*
  1557. *           sum for large components.
  1558. */
  1559.     if(xabs > x1max)
  1560.         {
  1561.         temp = x1max/xabs;
  1562.         s1 = one + s1*temp*temp;
  1563.         x1max = xabs;
  1564.         }
  1565.     else
  1566.         {
  1567.         temp = xabs/x1max;
  1568.         s1 += temp*temp;
  1569.         }
  1570.     continue;
  1571.     }
  1572. /*
  1573. *           sum for small components.
  1574. */
  1575. if(xabs > x3max)
  1576.     {
  1577.     temp = x3max/xabs;
  1578.     s3 = one + s3*temp*temp;
  1579.     x3max = xabs;
  1580.     }
  1581. else    
  1582.     {
  1583.     if(xabs != zero)
  1584.         {
  1585.         temp = xabs/x3max;
  1586.         s3 += temp*temp;
  1587.         }
  1588.     }
  1589. }
  1590. /*
  1591. *     calculation of norm.
  1592. */
  1593. if(s1 != zero)
  1594.     {
  1595.     temp = s1 + (s2/x1max)/x1max;
  1596.     ans = x1max*sqrt(temp);
  1597.     return(ans);
  1598.     }
  1599. if(s2 != zero)
  1600.     {
  1601.     if(s2 >= x3max)
  1602.         temp = s2*(one+(x3max/s2)*(x3max*s3));
  1603.     else
  1604.         temp = x3max*((s2/x3max)+(x3max*s3));
  1605.     ans = sqrt(temp);
  1606.     }
  1607. else
  1608.     {
  1609.     ans = x3max*sqrt(s3);
  1610.     }
  1611. return(ans);
  1612. /*
  1613. *     last card of function enorm.
  1614. */
  1615. }
  1616.  
  1617. /************************fdjac2.c*************************/
  1618.  
  1619. #define BUG 0
  1620.  
  1621. fdjac2(m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
  1622. int m,n,ldfjac;
  1623. int *iflag;
  1624. double epsfcn;
  1625. double x[],fvec[],fjac[],wa[];
  1626. {
  1627. /*
  1628. *     **********
  1629. *
  1630. *     subroutine fdjac2
  1631. *
  1632. *     this subroutine computes a forward-difference approximation
  1633. *     to the m by n jacobian matrix associated with a specified
  1634. *     problem of m functions in n variables.
  1635. *
  1636. *     the subroutine statement is
  1637. *
  1638. *    subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
  1639. *
  1640. *     where
  1641. *
  1642. *    fcn is the name of the user-supplied subroutine which
  1643. *      calculates the functions. fcn must be declared
  1644. *      in an external statement in the user calling
  1645. *      program, and should be written as follows.
  1646. *
  1647. *      subroutine fcn(m,n,x,fvec,iflag)
  1648. *      integer m,n,iflag
  1649. *      double precision x(n),fvec(m)
  1650. *      ----------
  1651. *      calculate the functions at x and
  1652. *      return this vector in fvec.
  1653. *      ----------
  1654. *      return
  1655. *      end
  1656. *
  1657. *      the value of iflag should not be changed by fcn unless
  1658. *      the user wants to terminate execution of fdjac2.
  1659. *      in this case set iflag to a negative integer.
  1660. *
  1661. *    m is a positive integer input variable set to the number
  1662. *      of functions.
  1663. *
  1664. *    n is a positive integer input variable set to the number
  1665. *      of variables. n must not exceed m.
  1666. *
  1667. *    x is an input array of length n.
  1668. *
  1669. *    fvec is an input array of length m which must contain the
  1670. *      functions evaluated at x.
  1671. *
  1672. *    fjac is an output m by n array which contains the
  1673. *      approximation to the jacobian matrix evaluated at x.
  1674. *
  1675. *    ldfjac is a positive integer input variable not less than m
  1676. *      which specifies the leading dimension of the array fjac.
  1677. *
  1678. *    iflag is an integer variable which can be used to terminate
  1679. *      the execution of fdjac2. see description of fcn.
  1680. *
  1681. *    epsfcn is an input variable used in determining a suitable
  1682. *      step length for the forward-difference approximation. this
  1683. *      approximation assumes that the relative errors in the
  1684. *      functions are of the order of epsfcn. if epsfcn is less
  1685. *      than the machine precision, it is assumed that the relative
  1686. *      errors in the functions are of the order of the machine
  1687. *      precision.
  1688. *
  1689. *    wa is a work array of length m.
  1690. *
  1691. *     subprograms called
  1692. *
  1693. *    user-supplied ...... fcn
  1694. *
  1695. *    minpack-supplied ... dpmpar
  1696. *
  1697. *    fortran-supplied ... dabs,dmax1,dsqrt
  1698. *
  1699. *     argonne national laboratory. minpack project. march 1980.
  1700. *     burton s. garbow, kenneth e. hillstrom, jorge j. more
  1701. *
  1702.       **********
  1703. */
  1704. int i,j,ij;
  1705. double eps,h,temp;
  1706. double fabs(), dmax1(), sqrt();
  1707. static double zero = 0.0;
  1708. extern double MACHEP;
  1709.  
  1710. temp = dmax1(epsfcn,MACHEP);
  1711. eps = sqrt(temp);
  1712. #if BUG
  1713. printf( "fdjac2\n" );
  1714. #endif
  1715. ij = 0;
  1716. for( j=0; j<n; j++ )
  1717.     {
  1718.     temp = x[j];
  1719.     h = eps * fabs(temp);
  1720.     if(h == zero)
  1721.         h = eps;
  1722.     x[j] = temp + h;
  1723.     fcn(m,n,x,wa,iflag);
  1724.     if( *iflag < 0)
  1725.         return;
  1726.     x[j] = temp;
  1727.     for( i=0; i<m; i++ )
  1728.         {
  1729.         fjac[ij] = (wa[i] - fvec[i])/h;
  1730.         ij += 1;    /* fjac[i+m*j] */
  1731.         }
  1732.     }
  1733. #if BUG
  1734. pmat( m, n, fjac );
  1735. #endif
  1736. /*
  1737. *     last card of subroutine fdjac2.
  1738. */
  1739. }
  1740. /************************lmmisc.c*************************/
  1741.  
  1742. double dmax1(a,b)
  1743. double a,b;
  1744. {
  1745. if( a >= b )
  1746.     return(a);
  1747. else
  1748.     return(b);
  1749. }
  1750.  
  1751. double dmin1(a,b)
  1752. double a,b;
  1753. {
  1754. if( a <= b )
  1755.     return(a);
  1756. else
  1757.     return(b);
  1758. }
  1759.  
  1760. int min0(a,b)
  1761. int a,b;
  1762. {
  1763. if( a <= b )
  1764.     return(a);
  1765. else
  1766.     return(b);
  1767. }
  1768.  
  1769. int mod( k, m )
  1770. int k, m;
  1771. {
  1772. return( k % m );
  1773. }
  1774.  
  1775.  
  1776. pmat( m, n, y  )
  1777. int m, n;
  1778. double y[];
  1779. {
  1780. int i, j, k;
  1781.  
  1782. k = 0;
  1783. for( i=0; i<m; i++ )
  1784.     {
  1785.     for( j=0; j<n; j++ )
  1786.         {
  1787.         printf( "%.5e ", y[k] );
  1788.         k += 1;
  1789.         }
  1790.     printf( "\n" );
  1791.     }
  1792. }    
  1793.