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

   1 static int
   2 iterate (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
   3 {
   4   lmder_state_t *state = (lmder_state_t *) vstate;
   5
   6   gsl_matrix *r = state->r;
   7   gsl_vector *tau = state->tau;
   8   gsl_vector *diag = state->diag;
   9   gsl_vector *qtf = state->qtf;
  10   gsl_vector *x_trial = state->x_trial;
  11   gsl_vector *f_trial = state->f_trial;
  12   gsl_vector *rptdx = state->rptdx;
  13   gsl_vector *newton = state->newton;
  14   gsl_vector *gradient = state->gradient;
  15   gsl_vector *sdiag = state->sdiag;
  16   gsl_vector *w = state->w;
  17   gsl_vector *work1 = state->work1;
  18   gsl_permutation *perm = state->perm;
  19
  20   double prered, actred;
  21   double pnorm, fnorm1, fnorm1p, gnorm;
  22   double ratio;
  23   double dirder;
  24
  25   int iter = 0;
  26
  27   double p1 = 0.1, p25 = 0.25, p5 = 0.5, p75 = 0.75, p0001 = 0.0001;
  28
  29   if (state->fnorm == 0.0)
  30     {
  31       return GSL_SUCCESS;
  32     }
  33
  34   /* Compute qtf = Q^T f */
  35
  36   gsl_vector_memcpy (qtf, f);
  37   gsl_linalg_QR_QTvec (r, tau, qtf);
  38
  39   /* Compute norm of scaled gradient */
  40
  41   compute_gradient_direction (r, perm, qtf, diag, gradient);
  42
  43   {
  44     size_t iamax = gsl_blas_idamax (gradient);
  45
  46     gnorm = fabs(gsl_vector_get (gradient, iamax) / state->fnorm);
  47   }
  48
  49   /* Determine the Levenberg-Marquardt parameter */
  50
  51 lm_iteration:
  52
  53   iter++ ;
  54
  55   {
  56     int status = lmpar (r, perm, qtf, diag, state->delta, &(state->par), newton, gradient, sdiag, dx, w);
  57     if (status)
  58       return status;
  59   }
  60
  61   /* Take a trial step */
  62
  63   gsl_vector_scale (dx, -1.0); /* reverse the step to go downhill */
  64
  65   compute_trial_step (x, dx, state->x_trial);
  66
  67   pnorm = scaled_enorm (diag, dx);
  68
  69   if (state->iter == 1)
  70     {
  71       if (pnorm < state->delta)
  72         {
  73 #ifdef DEBUG
  74           printf("set delta = pnorm = %g\n" , pnorm);
  75 #endif
  76           state->delta = pnorm;
  77         }
  78     }
  79
  80   /* Evaluate function at x + p */
  81   /* return immediately if evaluation raised error */
  82   {
  83     int status = GSL_MULTIFIT_FN_EVAL_F (fdf, x_trial, f_trial);
  84     if (status)
  85       return status;
  86   }
  87
  88   fnorm1 = enorm (f_trial);
  89
  90   /* Compute the scaled actual reduction */
  91
  92   actred = compute_actual_reduction (state->fnorm, fnorm1);
  93
  94 #ifdef DEBUG
  95   printf("lmiterate: fnorm = %g fnorm1 = %g  actred = %g\n", state->fnorm, fnorm1, actred);
  96   printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
  97   printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
  98   printf("dx = "); gsl_vector_fprintf(stdout, dx, "%g");
  99 #endif
 100
 101   /* Compute rptdx = R P^T dx, noting that |J dx| = |R P^T dx| */
 102
 103   compute_rptdx (r, perm, dx, rptdx);
 104
 105 #ifdef DEBUG
 106   printf("rptdx = "); gsl_vector_fprintf(stdout, rptdx, "%g");
 107 #endif
 108
 109   fnorm1p = enorm (rptdx);
 110
 111   /* Compute the scaled predicted reduction = |J dx|^2 + 2 par |D dx|^2 */
 112
 113   {
 114     double t1 = fnorm1p / state->fnorm;
 115     double t2 = (sqrt(state->par) * pnorm) / state->fnorm;
 116
 117     prered = t1 * t1 + t2 * t2 / p5;
 118     dirder = -(t1 * t1 + t2 * t2);
 119   }
 120
 121   /* compute the ratio of the actual to predicted reduction */
 122
 123   if (prered > 0)
 124     {
 125       ratio = actred / prered;
 126     }
 127   else
 128     {
 129       ratio = 0;
 130     }
 131
 132 #ifdef DEBUG
 133   printf("lmiterate: prered = %g dirder = %g ratio = %g\n", prered, dirder,ratio);
 134 #endif
 135
 136
 137   /* update the step bound */
 138
 139   if (ratio > p25)
 140     {
 141 #ifdef DEBUG
 142       printf("ratio > p25\n");
 143 #endif
 144       if (state->par == 0 || ratio >= p75)
 145         {
 146           state->delta = pnorm / p5;
 147           state->par *= p5;
 148 #ifdef DEBUG
 149           printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
 150 #endif
 151         }
 152     }
 153   else
 154     {
 155       double temp = (actred >= 0) ? p5 : p5*dirder / (dirder + p5 * actred);
 156
 157 #ifdef DEBUG
 158       printf("ratio < p25\n");
 159 #endif
 160
 161       if (p1 * fnorm1 >= state->fnorm || temp < p1 )
 162         {
 163           temp = p1;
 164         }
 165
 166       state->delta = temp * GSL_MIN_DBL (state->delta, pnorm/p1);
 167
 168       state->par /= temp;
 169 #ifdef DEBUG
 170       printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
 171 #endif
 172     }
 173
 174
 175   /* test for successful iteration, termination and stringent tolerances */
 176
 177   if (ratio >= p0001)
 178     {
 179       gsl_vector_memcpy (x, x_trial);
 180       gsl_vector_memcpy (f, f_trial);
 181
 182       /* return immediately if evaluation raised error */
 183       {
 184         int status = GSL_MULTIFIT_FN_EVAL_DF (fdf, x_trial, J);
 185         if (status)
 186           return status;
 187       }
 188
 189       /* wa2_j  = diag_j * x_j */
 190       state->xnorm = scaled_enorm(diag, x);
 191       state->fnorm = fnorm1;
 192       state->iter++;
 193
 194       /* Rescale if necessary */
 195
 196       if (scale)
 197         {
 198           update_diag (J, diag);
 199         }
 200
 201       {
 202         int signum;
 203         gsl_matrix_memcpy (r, J);
 204         gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
 205       }
 206
 207       return GSL_SUCCESS;
 208     }
 209   else if (fabs(actred) <= GSL_DBL_EPSILON  && prered <= GSL_DBL_EPSILON
 210            && p5 * ratio <= 1.0)
 211     {
 212       return GSL_ETOLF ;
 213     }
 214   else if (state->delta <= GSL_DBL_EPSILON * state->xnorm)
 215     {
 216       return GSL_ETOLX;
 217     }
 218   else if (gnorm <= GSL_DBL_EPSILON)
 219     {
 220       return GSL_ETOLG;
 221     }
 222   else if (iter < 10)
 223     {
 224       /* Repeat inner loop if unsuccessful */
 225       goto lm_iteration;
 226     }
 227
 228   return GSL_CONTINUE;
 229 }