htswanalysis/MACS/lib/gsl/gsl-1.11/multifit/lmpar.c

   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
  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