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_permute_vector_double.h" 21 22 #include "gsl_multifit__qrsolv.c" 23 24 static size_t 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 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 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 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 static void 187 compute_gradient_direction (const gsl_matrix * r, const gsl_permutation * p, 188 const gsl_vector * qtf, const gsl_vector * diag, 189 gsl_vector * g) 190 { 191 const size_t n = r->size2; 192 193 size_t i, j; 194 195 for (j = 0; j < n; j++) 196 { 197 double sum = 0; 198 199 for (i = 0; i <= j; i++) 200 { 201 sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i); 202 } 203 204 { 205 size_t pj = gsl_permutation_get (p, j); 206 double dpj = gsl_vector_get (diag, pj); 207 208 gsl_vector_set (g, j, sum / dpj); 209 } 210 } 211 } 212 213 static int 214 lmpar (gsl_matrix * r, const gsl_permutation * perm, const gsl_vector * qtf, 215 const gsl_vector * diag, double delta, double * par_inout, 216 gsl_vector * newton, gsl_vector * gradient, gsl_vector * sdiag, 217 gsl_vector * x, gsl_vector * w) 218 { 219 double dxnorm, gnorm, fp, fp_old, par_lower, par_upper, par_c; 220 221 double par = *par_inout; 222 223 size_t iter = 0; 224 225 #ifdef DEBUG 226 printf("ENTERING lmpar\n"); 227 #endif 228 229 230 compute_newton_direction (r, perm, qtf, newton); 231 232 #ifdef DEBUG 233 printf ("newton = "); 234 gsl_vector_fprintf (stdout, newton, "%g"); 235 printf ("\n"); 236 237 printf ("diag = "); 238 gsl_vector_fprintf (stdout, diag, "%g"); 239 printf ("\n"); 240 #endif 241 242 /* Evaluate the function at the origin and test for acceptance of 243 the Gauss-Newton direction. */ 244 245 dxnorm = scaled_enorm (diag, newton); 246 247 fp = dxnorm - delta; 248 249 #ifdef DEBUG 250 printf ("dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp); 251 #endif 252 253 if (fp <= 0.1 * delta) 254 { 255 gsl_vector_memcpy (x, newton); 256 #ifdef DEBUG 257 printf ("took newton (fp = %g, delta = %g)\n", fp, delta); 258 #endif 259 260 *par_inout = 0; 261 262 return GSL_SUCCESS; 263 } 264 265 #ifdef DEBUG 266 printf ("r = "); 267 gsl_matrix_fprintf (stdout, r, "%g"); 268 printf ("\n"); 269 270 printf ("newton = "); 271 gsl_vector_fprintf (stdout, newton, "%g"); 272 printf ("\n"); 273 274 printf ("dxnorm = %g\n", dxnorm); 275 #endif 276 277 278 compute_newton_bound (r, newton, dxnorm, perm, diag, w); 279 280 #ifdef DEBUG 281 printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d"); 282 283 printf ("diag = "); 284 gsl_vector_fprintf (stdout, diag, "%g"); 285 printf ("\n"); 286 287 printf ("w = "); 288 gsl_vector_fprintf (stdout, w, "%g"); 289 printf ("\n"); 290 #endif 291 292 293 { 294 double wnorm = enorm (w); 295 double phider = wnorm * wnorm; 296 297 /* w == zero if r rank-deficient, 298 then set lower bound to zero form MINPACK, lmder.f 299 Hans E. Plesser 2002-02-25 (hans.plesser@itf.nlh.no) */ 300 if ( wnorm > 0 ) 301 par_lower = fp / (delta * phider); 302 else 303 par_lower = 0.0; 304 } 305 306 #ifdef DEBUG 307 printf("par = %g\n", par ); 308 printf("par_lower = %g\n", par_lower); 309 #endif 310 311 compute_gradient_direction (r, perm, qtf, diag, gradient); 312 313 gnorm = enorm (gradient); 314 315 #ifdef DEBUG 316 printf("gradient = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n"); 317 printf("gnorm = %g\n", gnorm); 318 #endif 319 320 par_upper = gnorm / delta; 321 322 if (par_upper == 0) 323 { 324 par_upper = GSL_DBL_MIN / GSL_MIN_DBL(delta, 0.1); 325 } 326 327 #ifdef DEBUG 328 printf("par_upper = %g\n", par_upper); 329 #endif 330 331 if (par > par_upper) 332 { 333 #ifdef DEBUG 334 printf("set par to par_upper\n"); 335 #endif 336 337 par = par_upper; 338 } 339 else if (par < par_lower) 340 { 341 #ifdef DEBUG 342 printf("set par to par_lower\n"); 343 #endif 344 345 par = par_lower; 346 } 347 348 if (par == 0) 349 { 350 par = gnorm / dxnorm; 351 #ifdef DEBUG 352 printf("set par to gnorm/dxnorm = %g\n", par); 353 #endif 354 355 } 356 357 /* Beginning of iteration */ 358 359 iteration: 360 361 iter++; 362 363 #ifdef DEBUG 364 printf("lmpar iteration = %d\n", iter); 365 #endif 366 367 #ifdef BRIANSFIX 368 /* Seems like this is described in the paper but not in the MINPACK code */ 369 370 if (par < par_lower || par > par_upper) 371 { 372 par = GSL_MAX_DBL (0.001 * par_upper, sqrt(par_lower * par_upper)); 373 } 374 #endif 375 376 /* Evaluate the function at the current value of par */ 377 378 if (par == 0) 379 { 380 par = GSL_MAX_DBL (0.001 * par_upper, GSL_DBL_MIN); 381 #ifdef DEBUG 382 printf("par = 0, set par to = %g\n", par); 383 #endif 384 385 } 386 387 /* Compute the least squares solution of [ R P x - Q^T f, sqrt(par) D x] 388 for A = Q R P^T */ 389 390 #ifdef DEBUG 391 printf ("calling qrsolv with par = %g\n", par); 392 #endif 393 394 { 395 double sqrt_par = sqrt(par); 396 397 qrsolv (r, perm, sqrt_par, diag, qtf, x, sdiag, w); 398 } 399 400 dxnorm = scaled_enorm (diag, x); 401 402 fp_old = fp; 403 404 fp = dxnorm - delta; 405 406 #ifdef DEBUG 407 printf ("After qrsolv dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp); 408 printf ("sdiag = ") ; gsl_vector_fprintf(stdout, sdiag, "%g"); printf("\n"); 409 printf ("x = ") ; gsl_vector_fprintf(stdout, x, "%g"); printf("\n"); 410 printf ("r = ") ; gsl_matrix_fprintf(stdout, r, "%g"); printf("\nXXX\n"); 411 #endif 412 413 /* If the function is small enough, accept the current value of par */ 414 415 if (fabs (fp) <= 0.1 * delta) 416 goto line220; 417 418 if (par_lower == 0 && fp <= fp_old && fp_old < 0) 419 goto line220; 420 421 /* Check for maximum number of iterations */ 422 423 if (iter == 10) 424 goto line220; 425 426 /* Compute the Newton correction */ 427 428 compute_newton_correction (r, sdiag, perm, x, dxnorm, diag, w); 429 430 #ifdef DEBUG 431 printf ("newton_correction = "); 432 gsl_vector_fprintf(stdout, w, "%g"); printf("\n"); 433 #endif 434 435 { 436 double wnorm = enorm (w); 437 par_c = fp / (delta * wnorm * wnorm); 438 } 439 440 #ifdef DEBUG 441 printf("fp = %g\n", fp); 442 printf("par_lower = %g\n", par_lower); 443 printf("par_upper = %g\n", par_upper); 444 printf("par_c = %g\n", par_c); 445 #endif 446 447 448 /* Depending on the sign of the function, update par_lower or par_upper */ 449 450 if (fp > 0) 451 { 452 if (par > par_lower) 453 { 454 par_lower = par; 455 #ifdef DEBUG 456 printf("fp > 0: set par_lower = par = %g\n", par); 457 #endif 458 459 } 460 } 461 else if (fp < 0) 462 { 463 if (par < par_upper) 464 { 465 #ifdef DEBUG 466 printf("fp < 0: set par_upper = par = %g\n", par); 467 #endif 468 par_upper = par; 469 } 470 } 471 472 /* Compute an improved estimate for par */ 473 474 #ifdef DEBUG 475 printf("improved estimate par = MAX(%g, %g) \n", par_lower, par+par_c); 476 #endif 477 478 par = GSL_MAX_DBL (par_lower, par + par_c); 479 480 #ifdef DEBUG 481 printf("improved estimate par = %g \n", par); 482 #endif 483 484 485 goto iteration; 486 487 line220: 488 489 #ifdef DEBUG 490 printf("LEAVING lmpar, par = %g\n", par); 491 #endif 492 493 *par_inout = par; 494 495 return GSL_SUCCESS; 496 } 497 498 499 500