1 /* multifit/lmpar.c
2 *
3 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 */
19
20 #include <gsl/gsl_permute_vector_double.h>
21
22 #include "qrsolv.c"
23
24 static size_t
count_nsing(const gsl_matrix * r)25 count_nsing (const gsl_matrix * r)
26 {
27 /* Count the number of nonsingular entries. Returns the index of the
28 first entry which is singular. */
29
30 size_t n = r->size2;
31 size_t i;
32
33 for (i = 0; i < n; i++)
34 {
35 double rii = gsl_matrix_get (r, i, i);
36
37 if (rii == 0)
38 {
39 break;
40 }
41 }
42
43 return i;
44 }
45
46
47 static void
compute_newton_direction(const gsl_matrix * r,const gsl_permutation * perm,const gsl_vector * qtf,gsl_vector * x)48 compute_newton_direction (const gsl_matrix * r, const gsl_permutation * perm,
49 const gsl_vector * qtf, gsl_vector * x)
50 {
51
52 /* Compute and store in x the Gauss-Newton direction. If the
53 Jacobian is rank-deficient then obtain a least squares
54 solution. */
55
56 const size_t n = r->size2;
57 size_t i, j, nsing;
58
59 for (i = 0 ; i < n ; i++)
60 {
61 double qtfi = gsl_vector_get (qtf, i);
62 gsl_vector_set (x, i, qtfi);
63 }
64
65 nsing = count_nsing (r);
66
67 #ifdef DEBUG
68 printf("nsing = %d\n", nsing);
69 printf("r = "); gsl_matrix_fprintf(stdout, r, "%g"); printf("\n");
70 printf("qtf = "); gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
71 #endif
72
73 for (i = nsing; i < n; i++)
74 {
75 gsl_vector_set (x, i, 0.0);
76 }
77
78 if (nsing > 0)
79 {
80 for (j = nsing; j > 0 && j--;)
81 {
82 double rjj = gsl_matrix_get (r, j, j);
83 double temp = gsl_vector_get (x, j) / rjj;
84
85 gsl_vector_set (x, j, temp);
86
87 for (i = 0; i < j; i++)
88 {
89 double rij = gsl_matrix_get (r, i, j);
90 double xi = gsl_vector_get (x, i);
91 gsl_vector_set (x, i, xi - rij * temp);
92 }
93 }
94 }
95
96 gsl_permute_vector_inverse (perm, x);
97 }
98
99 static void
compute_newton_correction(const gsl_matrix * r,const gsl_vector * sdiag,const gsl_permutation * p,gsl_vector * x,double dxnorm,const gsl_vector * diag,gsl_vector * w)100 compute_newton_correction (const gsl_matrix * r, const gsl_vector * sdiag,
101 const gsl_permutation * p, gsl_vector * x,
102 double dxnorm,
103 const gsl_vector * diag, gsl_vector * w)
104 {
105 size_t n = r->size2;
106 size_t i, j;
107
108 for (i = 0; i < n; i++)
109 {
110 size_t pi = gsl_permutation_get (p, i);
111
112 double dpi = gsl_vector_get (diag, pi);
113 double xpi = gsl_vector_get (x, pi);
114
115 gsl_vector_set (w, i, dpi * (dpi * xpi) / dxnorm);
116 }
117
118 for (j = 0; j < n; j++)
119 {
120 double sj = gsl_vector_get (sdiag, j);
121 double wj = gsl_vector_get (w, j);
122
123 double tj = wj / sj;
124
125 gsl_vector_set (w, j, tj);
126
127 for (i = j + 1; i < n; i++)
128 {
129 double rij = gsl_matrix_get (r, i, j);
130 double wi = gsl_vector_get (w, i);
131
132 gsl_vector_set (w, i, wi - rij * tj);
133 }
134 }
135 }
136
137 static void
compute_newton_bound(const gsl_matrix * r,const gsl_vector * x,double dxnorm,const gsl_permutation * perm,const gsl_vector * diag,gsl_vector * w)138 compute_newton_bound (const gsl_matrix * r, const gsl_vector * x,
139 double dxnorm, const gsl_permutation * perm,
140 const gsl_vector * diag, gsl_vector * w)
141 {
142 /* If the jacobian is not rank-deficient then the Newton step
143 provides a lower bound for the zero of the function. Otherwise
144 set this bound to zero. */
145
146 size_t n = r->size2;
147
148 size_t i, j;
149
150 size_t nsing = count_nsing (r);
151
152 if (nsing < n)
153 {
154 gsl_vector_set_zero (w);
155 return;
156 }
157
158 for (i = 0; i < n; i++)
159 {
160 size_t pi = gsl_permutation_get (perm, i);
161
162 double dpi = gsl_vector_get (diag, pi);
163 double xpi = gsl_vector_get (x, pi);
164
165 gsl_vector_set (w, i, dpi * (dpi * xpi / dxnorm));
166 }
167
168 for (j = 0; j < n; j++)
169 {
170 double sum = 0;
171
172 for (i = 0; i < j; i++)
173 {
174 sum += gsl_matrix_get (r, i, j) * gsl_vector_get (w, i);
175 }
176
177 {
178 double rjj = gsl_matrix_get (r, j, j);
179 double wj = gsl_vector_get (w, j);
180
181 gsl_vector_set (w, j, (wj - sum) / rjj);
182 }
183 }
184 }
185
186 /* compute scaled gradient g = D^{-1} J^T f (see More' eq 7.2) */
187 static void
compute_gradient_direction(const gsl_matrix * r,const gsl_permutation * p,const gsl_vector * qtf,const gsl_vector * diag,gsl_vector * g)188 compute_gradient_direction (const gsl_matrix * r, const gsl_permutation * p,
189 const gsl_vector * qtf, const gsl_vector * diag,
190 gsl_vector * g)
191 {
192 const size_t n = r->size2;
193
194 size_t i, j;
195
196 for (j = 0; j < n; j++)
197 {
198 double sum = 0;
199
200 for (i = 0; i <= j; i++)
201 {
202 sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
203 }
204
205 {
206 size_t pj = gsl_permutation_get (p, j);
207 double dpj = gsl_vector_get (diag, pj);
208
209 gsl_vector_set (g, j, sum / dpj);
210 }
211 }
212 }
213
214 /* compute gradient g = J^T f */
215 static void
compute_gradient(const gsl_matrix * r,const gsl_vector * qtf,gsl_vector * g)216 compute_gradient (const gsl_matrix * r, const gsl_vector * qtf,
217 gsl_vector * g)
218 {
219 const size_t n = r->size2;
220
221 size_t i, j;
222
223 for (j = 0; j < n; j++)
224 {
225 double sum = 0;
226
227 for (i = 0; i <= j; i++)
228 {
229 sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
230 }
231
232 gsl_vector_set (g, j, sum);
233 }
234 }
235
236 static int
lmpar(gsl_matrix * r,const gsl_permutation * perm,const gsl_vector * qtf,const gsl_vector * diag,double delta,double * par_inout,gsl_vector * newton,gsl_vector * gradient,gsl_vector * sdiag,gsl_vector * x,gsl_vector * w)237 lmpar (gsl_matrix * r, const gsl_permutation * perm, const gsl_vector * qtf,
238 const gsl_vector * diag, double delta, double * par_inout,
239 gsl_vector * newton, gsl_vector * gradient, gsl_vector * sdiag,
240 gsl_vector * x, gsl_vector * w)
241 {
242 double dxnorm, gnorm, fp, fp_old, par_lower, par_upper, par_c;
243
244 double par = *par_inout;
245
246 size_t iter = 0;
247
248 #ifdef DEBUG
249 printf("ENTERING lmpar\n");
250 #endif
251
252
253 compute_newton_direction (r, perm, qtf, newton);
254
255 #ifdef DEBUG
256 printf ("newton = ");
257 gsl_vector_fprintf (stdout, newton, "%g");
258 printf ("\n");
259
260 printf ("diag = ");
261 gsl_vector_fprintf (stdout, diag, "%g");
262 printf ("\n");
263 #endif
264
265 /* Evaluate the function at the origin and test for acceptance of
266 the Gauss-Newton direction. */
267
268 dxnorm = scaled_enorm (diag, newton);
269
270 fp = dxnorm - delta;
271
272 #ifdef DEBUG
273 printf ("dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
274 #endif
275
276 if (fp <= 0.1 * delta)
277 {
278 gsl_vector_memcpy (x, newton);
279 #ifdef DEBUG
280 printf ("took newton (fp = %g, delta = %g)\n", fp, delta);
281 #endif
282
283 *par_inout = 0;
284
285 return GSL_SUCCESS;
286 }
287
288 #ifdef DEBUG
289 printf ("r = ");
290 gsl_matrix_fprintf (stdout, r, "%g");
291 printf ("\n");
292
293 printf ("newton = ");
294 gsl_vector_fprintf (stdout, newton, "%g");
295 printf ("\n");
296
297 printf ("dxnorm = %g\n", dxnorm);
298 #endif
299
300
301 compute_newton_bound (r, newton, dxnorm, perm, diag, w);
302
303 #ifdef DEBUG
304 printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
305
306 printf ("diag = ");
307 gsl_vector_fprintf (stdout, diag, "%g");
308 printf ("\n");
309
310 printf ("w = ");
311 gsl_vector_fprintf (stdout, w, "%g");
312 printf ("\n");
313 #endif
314
315
316 {
317 double wnorm = enorm (w);
318 double phider = wnorm * wnorm;
319
320 /* w == zero if r rank-deficient,
321 then set lower bound to zero form MINPACK, lmder.f
322 Hans E. Plesser 2002-02-25 (hans.plesser@itf.nlh.no) */
323 if ( wnorm > 0 )
324 par_lower = fp / (delta * phider);
325 else
326 par_lower = 0.0;
327 }
328
329 #ifdef DEBUG
330 printf("par = %g\n", par );
331 printf("par_lower = %g\n", par_lower);
332 #endif
333
334 compute_gradient_direction (r, perm, qtf, diag, gradient);
335
336 gnorm = enorm (gradient);
337
338 #ifdef DEBUG
339 printf("gradient = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
340 printf("gnorm = %g\n", gnorm);
341 #endif
342
343 par_upper = gnorm / delta;
344
345 if (par_upper == 0)
346 {
347 par_upper = GSL_DBL_MIN / GSL_MIN_DBL(delta, 0.1);
348 }
349
350 #ifdef DEBUG
351 printf("par_upper = %g\n", par_upper);
352 #endif
353
354 if (par > par_upper)
355 {
356 #ifdef DEBUG
357 printf("set par to par_upper\n");
358 #endif
359
360 par = par_upper;
361 }
362 else if (par < par_lower)
363 {
364 #ifdef DEBUG
365 printf("set par to par_lower\n");
366 #endif
367
368 par = par_lower;
369 }
370
371 if (par == 0)
372 {
373 par = gnorm / dxnorm;
374 #ifdef DEBUG
375 printf("set par to gnorm/dxnorm = %g\n", par);
376 #endif
377
378 }
379
380 /* Beginning of iteration */
381
382 iteration:
383
384 iter++;
385
386 #ifdef DEBUG
387 printf("lmpar iteration = %d\n", iter);
388 #endif
389
390 #ifdef BRIANSFIX
391 /* Seems like this is described in the paper but not in the MINPACK code */
392
393 if (par < par_lower || par > par_upper)
394 {
395 par = GSL_MAX_DBL (0.001 * par_upper, sqrt(par_lower * par_upper));
396 }
397 #endif
398
399 /* Evaluate the function at the current value of par */
400
401 if (par == 0)
402 {
403 par = GSL_MAX_DBL (0.001 * par_upper, GSL_DBL_MIN);
404 #ifdef DEBUG
405 printf("par = 0, set par to = %g\n", par);
406 #endif
407
408 }
409
410 /* Compute the least squares solution of [ R P x - Q^T f, sqrt(par) D x]
411 for A = Q R P^T */
412
413 #ifdef DEBUG
414 printf ("calling qrsolv with par = %g\n", par);
415 #endif
416
417 {
418 double sqrt_par = sqrt(par);
419
420 qrsolv (r, perm, sqrt_par, diag, qtf, x, sdiag, w);
421 }
422
423 dxnorm = scaled_enorm (diag, x);
424
425 fp_old = fp;
426
427 fp = dxnorm - delta;
428
429 #ifdef DEBUG
430 printf ("After qrsolv dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
431 printf ("sdiag = ") ; gsl_vector_fprintf(stdout, sdiag, "%g"); printf("\n");
432 printf ("x = ") ; gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
433 printf ("r = ") ; gsl_matrix_fprintf(stdout, r, "%g"); printf("\nXXX\n");
434 #endif
435
436 /* If the function is small enough, accept the current value of par */
437
438 if (fabs (fp) <= 0.1 * delta)
439 goto line220;
440
441 if (par_lower == 0 && fp <= fp_old && fp_old < 0)
442 goto line220;
443
444 /* Check for maximum number of iterations */
445
446 if (iter == 10)
447 goto line220;
448
449 /* Compute the Newton correction */
450
451 compute_newton_correction (r, sdiag, perm, x, dxnorm, diag, w);
452
453 #ifdef DEBUG
454 printf ("newton_correction = ");
455 gsl_vector_fprintf(stdout, w, "%g"); printf("\n");
456 #endif
457
458 {
459 double wnorm = enorm (w);
460 par_c = fp / (delta * wnorm * wnorm);
461 }
462
463 #ifdef DEBUG
464 printf("fp = %g\n", fp);
465 printf("par_lower = %g\n", par_lower);
466 printf("par_upper = %g\n", par_upper);
467 printf("par_c = %g\n", par_c);
468 #endif
469
470
471 /* Depending on the sign of the function, update par_lower or par_upper */
472
473 if (fp > 0)
474 {
475 if (par > par_lower)
476 {
477 par_lower = par;
478 #ifdef DEBUG
479 printf("fp > 0: set par_lower = par = %g\n", par);
480 #endif
481
482 }
483 }
484 else if (fp < 0)
485 {
486 if (par < par_upper)
487 {
488 #ifdef DEBUG
489 printf("fp < 0: set par_upper = par = %g\n", par);
490 #endif
491 par_upper = par;
492 }
493 }
494
495 /* Compute an improved estimate for par */
496
497 #ifdef DEBUG
498 printf("improved estimate par = MAX(%g, %g) \n", par_lower, par+par_c);
499 #endif
500
501 par = GSL_MAX_DBL (par_lower, par + par_c);
502
503 #ifdef DEBUG
504 printf("improved estimate par = %g \n", par);
505 #endif
506
507
508 goto iteration;
509
510 line220:
511
512 #ifdef DEBUG
513 printf("LEAVING lmpar, par = %g\n", par);
514 #endif
515
516 *par_inout = par;
517
518 return GSL_SUCCESS;
519 }
520
521
522
523