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