1 #include "cminpack.h"
2 #include <math.h>
3 #include "cminpackP.h"
4
5 __cminpack_attr__
__cminpack_func__(lmdif)6 int __cminpack_func__(lmdif)(__cminpack_decl_fcn_mn__ void *p, int m, int n, real *x,
7 real *fvec, real ftol, real xtol, real
8 gtol, int maxfev, real epsfcn, real *diag, int
9 mode, real factor, int nprint, int *
10 nfev, real *fjac, int ldfjac, int *ipvt, real *
11 qtf, real *wa1, real *wa2, real *wa3, real *
12 wa4)
13 {
14 /* Initialized data */
15
16 #define p1 .1
17 #define p5 .5
18 #define p25 .25
19 #define p75 .75
20 #define p0001 1e-4
21
22 /* System generated locals */
23 real d1, d2;
24
25 /* Local variables */
26 int i, j, l;
27 real par, sum;
28 int iter;
29 real temp, temp1, temp2;
30 int iflag;
31 real delta = 0.;
32 real ratio;
33 real fnorm, gnorm;
34 real pnorm, xnorm = 0., fnorm1, actred, dirder, epsmch, prered;
35 int info;
36
37 /* ********** */
38
39 /* subroutine lmdif */
40
41 /* the purpose of lmdif is to minimize the sum of the squares of */
42 /* m nonlinear functions in n variables by a modification of */
43 /* the levenberg-marquardt algorithm. the user must provide a */
44 /* subroutine which calculates the functions. the jacobian is */
45 /* then calculated by a forward-difference approximation. */
46
47 /* the subroutine statement is */
48
49 /* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, */
50 /* diag,mode,factor,nprint,info,nfev,fjac, */
51 /* ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) */
52
53 /* where */
54
55 /* fcn is the name of the user-supplied subroutine which */
56 /* calculates the functions. fcn must be declared */
57 /* in an external statement in the user calling */
58 /* program, and should be written as follows. */
59
60 /* subroutine fcn(m,n,x,fvec,iflag) */
61 /* integer m,n,iflag */
62 /* double precision x(n),fvec(m) */
63 /* ---------- */
64 /* calculate the functions at x and */
65 /* return this vector in fvec. */
66 /* ---------- */
67 /* return */
68 /* end */
69
70 /* the value of iflag should not be changed by fcn unless */
71 /* the user wants to terminate execution of lmdif. */
72 /* in this case set iflag to a negative integer. */
73
74 /* m is a positive integer input variable set to the number */
75 /* of functions. */
76
77 /* n is a positive integer input variable set to the number */
78 /* of variables. n must not exceed m. */
79
80 /* x is an array of length n. on input x must contain */
81 /* an initial estimate of the solution vector. on output x */
82 /* contains the final estimate of the solution vector. */
83
84 /* fvec is an output array of length m which contains */
85 /* the functions evaluated at the output x. */
86
87 /* ftol is a nonnegative input variable. termination */
88 /* occurs when both the actual and predicted relative */
89 /* reductions in the sum of squares are at most ftol. */
90 /* therefore, ftol measures the relative error desired */
91 /* in the sum of squares. */
92
93 /* xtol is a nonnegative input variable. termination */
94 /* occurs when the relative error between two consecutive */
95 /* iterates is at most xtol. therefore, xtol measures the */
96 /* relative error desired in the approximate solution. */
97
98 /* gtol is a nonnegative input variable. termination */
99 /* occurs when the cosine of the angle between fvec and */
100 /* any column of the jacobian is at most gtol in absolute */
101 /* value. therefore, gtol measures the orthogonality */
102 /* desired between the function vector and the columns */
103 /* of the jacobian. */
104
105 /* maxfev is a positive integer input variable. termination */
106 /* occurs when the number of calls to fcn is at least */
107 /* maxfev by the end of an iteration. */
108
109 /* epsfcn is an input variable used in determining a suitable */
110 /* step length for the forward-difference approximation. this */
111 /* approximation assumes that the relative errors in the */
112 /* functions are of the order of epsfcn. if epsfcn is less */
113 /* than the machine precision, it is assumed that the relative */
114 /* errors in the functions are of the order of the machine */
115 /* precision. */
116
117 /* diag is an array of length n. if mode = 1 (see */
118 /* below), diag is internally set. if mode = 2, diag */
119 /* must contain positive entries that serve as */
120 /* multiplicative scale factors for the variables. */
121
122 /* mode is an integer input variable. if mode = 1, the */
123 /* variables will be scaled internally. if mode = 2, */
124 /* the scaling is specified by the input diag. other */
125 /* values of mode are equivalent to mode = 1. */
126
127 /* factor is a positive input variable used in determining the */
128 /* initial step bound. this bound is set to the product of */
129 /* factor and the euclidean norm of diag*x if nonzero, or else */
130 /* to factor itself. in most cases factor should lie in the */
131 /* interval (.1,100.). 100. is a generally recommended value. */
132
133 /* nprint is an integer input variable that enables controlled */
134 /* printing of iterates if it is positive. in this case, */
135 /* fcn is called with iflag = 0 at the beginning of the first */
136 /* iteration and every nprint iterations thereafter and */
137 /* immediately prior to return, with x and fvec available */
138 /* for printing. if nprint is not positive, no special calls */
139 /* of fcn with iflag = 0 are made. */
140
141 /* info is an integer output variable. if the user has */
142 /* terminated execution, info is set to the (negative) */
143 /* value of iflag. see description of fcn. otherwise, */
144 /* info is set as follows. */
145
146 /* info = 0 improper input parameters. */
147
148 /* info = 1 both actual and predicted relative reductions */
149 /* in the sum of squares are at most ftol. */
150
151 /* info = 2 relative error between two consecutive iterates */
152 /* is at most xtol. */
153
154 /* info = 3 conditions for info = 1 and info = 2 both hold. */
155
156 /* info = 4 the cosine of the angle between fvec and any */
157 /* column of the jacobian is at most gtol in */
158 /* absolute value. */
159
160 /* info = 5 number of calls to fcn has reached or */
161 /* exceeded maxfev. */
162
163 /* info = 6 ftol is too small. no further reduction in */
164 /* the sum of squares is possible. */
165
166 /* info = 7 xtol is too small. no further improvement in */
167 /* the approximate solution x is possible. */
168
169 /* info = 8 gtol is too small. fvec is orthogonal to the */
170 /* columns of the jacobian to machine precision. */
171
172 /* nfev is an integer output variable set to the number of */
173 /* calls to fcn. */
174
175 /* fjac is an output m by n array. the upper n by n submatrix */
176 /* of fjac contains an upper triangular matrix r with */
177 /* diagonal elements of nonincreasing magnitude such that */
178
179 /* t t t */
180 /* p *(jac *jac)*p = r *r, */
181
182 /* where p is a permutation matrix and jac is the final */
183 /* calculated jacobian. column j of p is column ipvt(j) */
184 /* (see below) of the identity matrix. the lower trapezoidal */
185 /* part of fjac contains information generated during */
186 /* the computation of r. */
187
188 /* ldfjac is a positive integer input variable not less than m */
189 /* which specifies the leading dimension of the array fjac. */
190
191 /* ipvt is an integer output array of length n. ipvt */
192 /* defines a permutation matrix p such that jac*p = q*r, */
193 /* where jac is the final calculated jacobian, q is */
194 /* orthogonal (not stored), and r is upper triangular */
195 /* with diagonal elements of nonincreasing magnitude. */
196 /* column j of p is column ipvt(j) of the identity matrix. */
197
198 /* qtf is an output array of length n which contains */
199 /* the first n elements of the vector (q transpose)*fvec. */
200
201 /* wa1, wa2, and wa3 are work arrays of length n. */
202
203 /* wa4 is a work array of length m. */
204
205 /* subprograms called */
206
207 /* user-supplied ...... fcn */
208
209 /* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac */
210
211 /* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod */
212
213 /* argonne national laboratory. minpack project. march 1980. */
214 /* burton s. garbow, kenneth e. hillstrom, jorge j. more */
215
216 /* ********** */
217
218 /* epsmch is the machine precision. */
219
220 epsmch = __cminpack_func__(dpmpar)(1);
221
222 info = 0;
223 iflag = 0;
224 *nfev = 0;
225
226 /* check the input parameters for errors. */
227
228 if (n <= 0 || m < n || ldfjac < m || ftol < 0. || xtol < 0. ||
229 gtol < 0. || maxfev <= 0 || factor <= 0.) {
230 goto TERMINATE;
231 }
232 if (mode == 2) {
233 for (j = 0; j < n; ++j) {
234 if (diag[j] <= 0.) {
235 goto TERMINATE;
236 }
237 }
238 }
239
240 /* evaluate the function at the starting point */
241 /* and calculate its norm. */
242
243 iflag = fcn_mn(p, m, n, x, fvec, 1);
244 *nfev = 1;
245 if (iflag < 0) {
246 goto TERMINATE;
247 }
248 fnorm = __cminpack_enorm__(m, fvec);
249
250 /* initialize levenberg-marquardt parameter and iteration counter. */
251
252 par = 0.;
253 iter = 1;
254
255 /* beginning of the outer loop. */
256
257 for (;;) {
258
259 /* calculate the jacobian matrix. */
260
261 iflag = __cminpack_func__(fdjac2)(__cminpack_param_fcn_mn__ p, m, n, x, fvec, fjac, ldfjac,
262 epsfcn, wa4);
263 *nfev += n;
264 if (iflag < 0) {
265 goto TERMINATE;
266 }
267
268 /* if requested, call fcn to enable printing of iterates. */
269
270 if (nprint > 0) {
271 iflag = 0;
272 if ((iter - 1) % nprint == 0) {
273 iflag = fcn_mn(p, m, n, x, fvec, 0);
274 }
275 if (iflag < 0) {
276 goto TERMINATE;
277 }
278 }
279
280 /* compute the qr factorization of the jacobian. */
281
282 __cminpack_func__(qrfac)(m, n, fjac, ldfjac, TRUE_, ipvt, n,
283 wa1, wa2, wa3);
284
285 /* on the first iteration and if mode is 1, scale according */
286 /* to the norms of the columns of the initial jacobian. */
287
288 if (iter == 1) {
289 if (mode != 2) {
290 for (j = 0; j < n; ++j) {
291 diag[j] = wa2[j];
292 if (wa2[j] == 0.) {
293 diag[j] = 1.;
294 }
295 }
296 }
297
298 /* on the first iteration, calculate the norm of the scaled x */
299 /* and initialize the step bound delta. */
300
301 for (j = 0; j < n; ++j) {
302 wa3[j] = diag[j] * x[j];
303 }
304 xnorm = __cminpack_enorm__(n, wa3);
305 delta = factor * xnorm;
306 if (delta == 0.) {
307 delta = factor;
308 }
309 }
310
311 /* form (q transpose)*fvec and store the first n components in */
312 /* qtf. */
313
314 for (i = 0; i < m; ++i) {
315 wa4[i] = fvec[i];
316 }
317 for (j = 0; j < n; ++j) {
318 if (fjac[j + j * ldfjac] != 0.) {
319 sum = 0.;
320 for (i = j; i < m; ++i) {
321 sum += fjac[i + j * ldfjac] * wa4[i];
322 }
323 temp = -sum / fjac[j + j * ldfjac];
324 for (i = j; i < m; ++i) {
325 wa4[i] += fjac[i + j * ldfjac] * temp;
326 }
327 }
328 fjac[j + j * ldfjac] = wa1[j];
329 qtf[j] = wa4[j];
330 }
331
332 /* compute the norm of the scaled gradient. */
333
334 gnorm = 0.;
335 if (fnorm != 0.) {
336 for (j = 0; j < n; ++j) {
337 l = ipvt[j]-1;
338 if (wa2[l] != 0.) {
339 sum = 0.;
340 for (i = 0; i <= j; ++i) {
341 sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm);
342 }
343 /* Computing MAX */
344 d1 = fabs(sum / wa2[l]);
345 gnorm = max(gnorm,d1);
346 }
347 }
348 }
349
350 /* test for convergence of the gradient norm. */
351
352 if (gnorm <= gtol) {
353 info = 4;
354 }
355 if (info != 0) {
356 goto TERMINATE;
357 }
358
359 /* rescale if necessary. */
360
361 if (mode != 2) {
362 for (j = 0; j < n; ++j) {
363 /* Computing MAX */
364 d1 = diag[j], d2 = wa2[j];
365 diag[j] = max(d1,d2);
366 }
367 }
368
369 /* beginning of the inner loop. */
370
371 do {
372
373 /* determine the levenberg-marquardt parameter. */
374
375 __cminpack_func__(lmpar)(n, fjac, ldfjac, ipvt, diag, qtf, delta,
376 &par, wa1, wa2, wa3, wa4);
377
378 /* store the direction p and x + p. calculate the norm of p. */
379
380 for (j = 0; j < n; ++j) {
381 wa1[j] = -wa1[j];
382 wa2[j] = x[j] + wa1[j];
383 wa3[j] = diag[j] * wa1[j];
384 }
385 pnorm = __cminpack_enorm__(n, wa3);
386
387 /* on the first iteration, adjust the initial step bound. */
388
389 if (iter == 1) {
390 delta = min(delta,pnorm);
391 }
392
393 /* evaluate the function at x + p and calculate its norm. */
394
395 iflag = fcn_mn(p, m, n, wa2, wa4, 1);
396 ++(*nfev);
397 if (iflag < 0) {
398 goto TERMINATE;
399 }
400 fnorm1 = __cminpack_enorm__(m, wa4);
401
402 /* compute the scaled actual reduction. */
403
404 actred = -1.;
405 if (p1 * fnorm1 < fnorm) {
406 /* Computing 2nd power */
407 d1 = fnorm1 / fnorm;
408 actred = 1. - d1 * d1;
409 }
410
411 /* compute the scaled predicted reduction and */
412 /* the scaled directional derivative. */
413
414 for (j = 0; j < n; ++j) {
415 wa3[j] = 0.;
416 l = ipvt[j]-1;
417 temp = wa1[l];
418 for (i = 0; i <= j; ++i) {
419 wa3[i] += fjac[i + j * ldfjac] * temp;
420 }
421 }
422 temp1 = __cminpack_enorm__(n, wa3) / fnorm;
423 temp2 = (sqrt(par) * pnorm) / fnorm;
424 prered = temp1 * temp1 + temp2 * temp2 / p5;
425 dirder = -(temp1 * temp1 + temp2 * temp2);
426
427 /* compute the ratio of the actual to the predicted */
428 /* reduction. */
429
430 ratio = 0.;
431 if (prered != 0.) {
432 ratio = actred / prered;
433 }
434
435 /* update the step bound. */
436
437 if (ratio <= p25) {
438 if (actred >= 0.) {
439 temp = p5;
440 } else {
441 temp = p5 * dirder / (dirder + p5 * actred);
442 }
443 if (p1 * fnorm1 >= fnorm || temp < p1) {
444 temp = p1;
445 }
446 /* Computing MIN */
447 d1 = pnorm / p1;
448 delta = temp * min(delta,d1);
449 par /= temp;
450 } else {
451 if (par == 0. || ratio >= p75) {
452 delta = pnorm / p5;
453 par = p5 * par;
454 }
455 }
456
457 /* test for successful iteration. */
458
459 if (ratio >= p0001) {
460
461 /* successful iteration. update x, fvec, and their norms. */
462
463 for (j = 0; j < n; ++j) {
464 x[j] = wa2[j];
465 wa2[j] = diag[j] * x[j];
466 }
467 for (i = 0; i < m; ++i) {
468 fvec[i] = wa4[i];
469 }
470 xnorm = __cminpack_enorm__(n, wa2);
471 fnorm = fnorm1;
472 ++iter;
473 }
474
475 /* tests for convergence. */
476
477 if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1.) {
478 info = 1;
479 }
480 if (delta <= xtol * xnorm) {
481 info = 2;
482 }
483 if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1. && info == 2) {
484 info = 3;
485 }
486 if (info != 0) {
487 goto TERMINATE;
488 }
489
490 /* tests for termination and stringent tolerances. */
491
492 if (*nfev >= maxfev) {
493 info = 5;
494 }
495 if (fabs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) {
496 info = 6;
497 }
498 if (delta <= epsmch * xnorm) {
499 info = 7;
500 }
501 if (gnorm <= epsmch) {
502 info = 8;
503 }
504 if (info != 0) {
505 goto TERMINATE;
506 }
507
508 /* end of the inner loop. repeat if iteration unsuccessful. */
509
510 } while (ratio < p0001);
511
512 /* end of the outer loop. */
513
514 }
515 TERMINATE:
516
517 /* termination, either normal or user imposed. */
518
519 if (iflag < 0) {
520 info = iflag;
521 }
522 if (nprint > 0) {
523 fcn_mn(p, m, n, x, fvec, 0);
524 }
525 return info;
526
527 /* last card of subroutine lmdif. */
528
529 } /* lmdif_ */
530
531