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() */