## Diff of /lmdif.c [000000] .. [2a97a8]  Maximize  Restore

### Switch to unified view

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