static int lmniel_set(void *vstate, const gsl_vector *swts, gsl_multifit_function_fdf *fdf, gsl_vector *x, gsl_vector *f, gsl_vector *dx) { int status; lmniel_state_t *state = (lmniel_state_t *) vstate; const size_t p = x->size; size_t i; /* initialize counters for function and Jacobian evaluations */ fdf->nevalf = 0; fdf->nevaldf = 0; /* evaluate function and Jacobian at x and apply weight transform */ status = gsl_multifit_eval_wf(fdf, x, swts, f); if (status) return status; if (fdf->df) status = gsl_multifit_eval_wdf(fdf, x, swts, state->J); else status = gsl_multifit_fdfsolver_dif_df(x, swts, fdf, f, state->J); if (status) return status; /* compute rhs = -J^T f */ gsl_blas_dgemv(CblasTrans, -1.0, state->J, f, 0.0, state->rhs); #if SCALE gsl_vector_set_zero(state->diag); #else gsl_vector_set_all(state->diag, 1.0); #endif /* set default parameters */ state->nu = 2; #if SCALE state->mu = state->tau; #else /* compute mu_0 = tau * max(diag(J^T J)) */ state->mu = -1.0; for (i = 0; i < p; ++i) { gsl_vector_view c = gsl_matrix_column(state->J, i); double result; /* (J^T J)_{ii} */ gsl_blas_ddot(&c.vector, &c.vector, &result); state->mu = GSL_MAX(state->mu, result); } state->mu *= state->tau; #endif return GSL_SUCCESS; } /* lmniel_set() */
static int iterate (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale) { lmder_state_t *state = (lmder_state_t *) vstate; gsl_matrix *r = state->r; gsl_vector *tau = state->tau; gsl_vector *diag = state->diag; gsl_vector *qtf = state->qtf; gsl_vector *x_trial = state->x_trial; gsl_vector *f_trial = state->f_trial; gsl_vector *rptdx = state->rptdx; gsl_vector *newton = state->newton; gsl_vector *gradient = state->gradient; gsl_vector *sdiag = state->sdiag; gsl_vector *w = state->w; gsl_vector *work1 = state->work1; gsl_permutation *perm = state->perm; double prered, actred; double pnorm, fnorm1, fnorm1p, gnorm; double ratio; double dirder; int iter = 0; double p1 = 0.1, p25 = 0.25, p5 = 0.5, p75 = 0.75, p0001 = 0.0001; if (state->fnorm == 0.0) { return GSL_SUCCESS; } /* Compute qtf = Q^T f */ gsl_vector_memcpy (qtf, f); gsl_linalg_QR_QTvec (r, tau, qtf); /* Compute norm of scaled gradient */ compute_gradient_direction (r, perm, qtf, diag, gradient); { size_t iamax = gsl_blas_idamax (gradient); gnorm = fabs(gsl_vector_get (gradient, iamax) / state->fnorm); } /* Determine the Levenberg-Marquardt parameter */ lm_iteration: iter++ ; { int status = lmpar (r, perm, qtf, diag, state->delta, &(state->par), newton, gradient, sdiag, dx, w); if (status) return status; } /* Take a trial step */ gsl_vector_scale (dx, -1.0); /* reverse the step to go downhill */ compute_trial_step (x, dx, state->x_trial); pnorm = scaled_enorm (diag, dx); if (state->iter == 1) { if (pnorm < state->delta) { #ifdef DEBUG printf("set delta = pnorm = %g\n" , pnorm); #endif state->delta = pnorm; } } /* Evaluate function at x + p */ /* return immediately if evaluation raised error */ { int status = GSL_MULTIFIT_FN_EVAL_F (fdf, x_trial, f_trial); if (status) return status; } fnorm1 = enorm (f_trial); /* Compute the scaled actual reduction */ actred = compute_actual_reduction (state->fnorm, fnorm1); #ifdef DEBUG printf("lmiterate: fnorm = %g fnorm1 = %g actred = %g\n", state->fnorm, fnorm1, actred); printf("r = "); gsl_matrix_fprintf(stdout, r, "%g"); printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d"); printf("dx = "); gsl_vector_fprintf(stdout, dx, "%g"); #endif /* Compute rptdx = R P^T dx, noting that |J dx| = |R P^T dx| */ compute_rptdx (r, perm, dx, rptdx); #ifdef DEBUG printf("rptdx = "); gsl_vector_fprintf(stdout, rptdx, "%g"); #endif fnorm1p = enorm (rptdx); /* Compute the scaled predicted reduction = |J dx|^2 + 2 par |D dx|^2 */ { double t1 = fnorm1p / state->fnorm; double t2 = (sqrt(state->par) * pnorm) / state->fnorm; prered = t1 * t1 + t2 * t2 / p5; dirder = -(t1 * t1 + t2 * t2); } /* compute the ratio of the actual to predicted reduction */ if (prered > 0) { ratio = actred / prered; } else { ratio = 0; } #ifdef DEBUG printf("lmiterate: prered = %g dirder = %g ratio = %g\n", prered, dirder,ratio); #endif /* update the step bound */ if (ratio > p25) { #ifdef DEBUG printf("ratio > p25\n"); #endif if (state->par == 0 || ratio >= p75) { state->delta = pnorm / p5; state->par *= p5; #ifdef DEBUG printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par); #endif } } else { double temp = (actred >= 0) ? p5 : p5*dirder / (dirder + p5 * actred); #ifdef DEBUG printf("ratio < p25\n"); #endif if (p1 * fnorm1 >= state->fnorm || temp < p1 ) { temp = p1; } state->delta = temp * GSL_MIN_DBL (state->delta, pnorm/p1); state->par /= temp; #ifdef DEBUG printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par); #endif } /* test for successful iteration, termination and stringent tolerances */ if (ratio >= p0001) { gsl_vector_memcpy (x, x_trial); gsl_vector_memcpy (f, f_trial); /* return immediately if evaluation raised error */ { int status; if (fdf->df) status = GSL_MULTIFIT_FN_EVAL_DF (fdf, x_trial, J); else status = gsl_multifit_fdfsolver_dif_df(x_trial, fdf, f_trial, J); if (status) return status; } /* wa2_j = diag_j * x_j */ state->xnorm = scaled_enorm(diag, x); state->fnorm = fnorm1; state->iter++; /* Rescale if necessary */ if (scale) { update_diag (J, diag); } { int signum; gsl_matrix_memcpy (r, J); gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1); } return GSL_SUCCESS; } else if (fabs(actred) <= GSL_DBL_EPSILON && prered <= GSL_DBL_EPSILON && p5 * ratio <= 1.0) { return GSL_ETOLF ; } else if (state->delta <= GSL_DBL_EPSILON * state->xnorm) { return GSL_ETOLX; } else if (gnorm <= GSL_DBL_EPSILON) { return GSL_ETOLG; } else if (iter < 10) { /* Repeat inner loop if unsuccessful */ goto lm_iteration; } return GSL_ENOPROG; }
/** * C++ version of gsl_multifit_fdfsolver_dif_df(). * @param x The current position * @param fdf The fdfsolver_fdf object * @param f The function values at the current position * @param J The apprximate Jacobian [return] * @return Error code on failure. */ inline static int dif_df( vector const& x, function_fdf* fdf, vector const& f, matrix& J ){ return gsl_multifit_fdfsolver_dif_df( x.get(), fdf, f.get(), J.get() ); }
static int lmniel_iterate(void *vstate, const gsl_vector *swts, gsl_multifit_function_fdf *fdf, gsl_vector *x, gsl_vector *f, gsl_vector *dx) { int status; lmniel_state_t *state = (lmniel_state_t *) vstate; gsl_matrix *J = state->J; /* Jacobian J(x) */ gsl_matrix *A = state->A; /* J^T J */ gsl_vector *rhs = state->rhs; /* -g = -J^T f */ gsl_vector *x_trial = state->x_trial; /* trial x + dx */ gsl_vector *f_trial = state->f_trial; /* trial f(x + dx) */ gsl_vector *diag = state->diag; /* diag(D) */ double dF; /* F(x) - F(x + dx) */ double dL; /* L(0) - L(dx) */ int foundstep = 0; /* found step dx */ /* compute A = J^T J */ status = gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, J, J, 0.0, A); if (status) return status; #if SCALE lmniel_update_diag(J, diag); #endif /* loop until we find an acceptable step dx */ while (!foundstep) { /* solve (A + mu*I) dx = g */ status = lmniel_calc_dx(state->mu, A, rhs, dx, state); if (status) return status; /* compute x_trial = x + dx */ lmniel_trial_step(x, dx, x_trial); /* compute f(x + dx) */ status = gsl_multifit_eval_wf(fdf, x_trial, swts, f_trial); if (status) return status; /* compute dF = F(x) - F(x + dx) */ dF = lmniel_calc_dF(f, f_trial); /* compute dL = L(0) - L(dx) = dx^T (mu*dx - g) */ dL = lmniel_calc_dL(state->mu, diag, dx, rhs); /* check that rho = dF/dL > 0 */ if ((dL > 0.0) && (dF >= 0.0)) { /* reduction in error, step acceptable */ double tmp; /* update LM parameter mu */ tmp = 2.0 * (dF / dL) - 1.0; tmp = 1.0 - tmp*tmp*tmp; state->mu *= GSL_MAX(LM_ONE_THIRD, tmp); state->nu = 2; /* compute J <- J(x + dx) */ if (fdf->df) status = gsl_multifit_eval_wdf(fdf, x_trial, swts, J); else status = gsl_multifit_fdfsolver_dif_df(x_trial, swts, fdf, f_trial, J); if (status) return status; /* update x <- x + dx */ gsl_vector_memcpy(x, x_trial); /* update f <- f(x + dx) */ gsl_vector_memcpy(f, f_trial); /* compute new rhs = -J^T f */ gsl_blas_dgemv(CblasTrans, -1.0, J, f, 0.0, rhs); foundstep = 1; } else { long nu2; /* step did not reduce error, reject step */ state->mu *= state->nu; nu2 = state->nu << 1; /* 2*nu */ if (nu2 <= state->nu) { gsl_vector_view d = gsl_matrix_diagonal(A); /* * nu has wrapped around / overflown, reset mu and nu * to original values and break to force another iteration */ /*GSL_ERROR("nu parameter has overflown", GSL_EOVRFLW);*/ state->nu = 2; state->mu = state->tau * gsl_vector_max(&d.vector); break; } state->nu = nu2; } } /* while (!foundstep) */ return GSL_SUCCESS; } /* lmniel_iterate() */