static int
dogleg_preloop(const void * vtrust_state, void * vstate)
{
int status;
const gsl_multifit_nlinear_trust_state *trust_state =
(const gsl_multifit_nlinear_trust_state *) vtrust_state;
dogleg_state_t *state = (dogleg_state_t *) vstate;
const gsl_multifit_nlinear_parameters *params = trust_state->params;
double u;
double alpha; /* ||g||^2 / ||Jg||^2 */
/* initialize linear least squares solver */
status = (params->solver->init)(trust_state, trust_state->solver_state);
if (status)
return status;
/* prepare the linear solver to compute Gauss-Newton step */
status = (params->solver->presolve)(0.0, trust_state, trust_state->solver_state);
if (status)
return status;
/* solve: J dx_gn = -f for Gauss-Newton step */
status = (params->solver->solve)(trust_state->f,
state->dx_gn,
trust_state,
trust_state->solver_state);
if (status)
return status;
/* now calculate the steepest descent step */
/* compute workp = D^{-1} g and its norm */
gsl_vector_memcpy(state->workp, trust_state->g);
gsl_vector_div(state->workp, trust_state->diag);
state->norm_Dinvg = gsl_blas_dnrm2(state->workp);
/* compute workp = D^{-2} g */
gsl_vector_div(state->workp, trust_state->diag);
/* compute: workn = J D^{-2} g */
gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, state->workp, 0.0, state->workn);
state->norm_JDinv2g = gsl_blas_dnrm2(state->workn);
u = state->norm_Dinvg / state->norm_JDinv2g;
alpha = u * u;
/* dx_sd = -alpha D^{-2} g */
gsl_vector_memcpy(state->dx_sd, state->workp);
gsl_vector_scale(state->dx_sd, -alpha);
state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
state->norm_Dsd = scaled_enorm(trust_state->diag, state->dx_sd);
return GSL_SUCCESS;
}
static double
compute_delta (gsl_vector * diag, gsl_vector * x)
{
double Dx = scaled_enorm (diag, x);
double factor = 100;
return (Dx > 0) ? factor * Dx : factor;
}
static double
compute_delta (gsl_vector * diag, gsl_vector * x)
{
double Dx = scaled_enorm (diag, x);
double factor = 100; /* generally recommended value from MINPACK */
return (Dx > 0) ? factor * Dx : factor;
}
static double
dogleg_beta(const double t, const double delta,
const gsl_vector * diag, dogleg_state_t * state)
{
double beta;
double a, b, c;
/* compute: workp = t*dx_gn - dx_sd */
scaled_addition(t, state->dx_gn, -1.0, state->dx_sd, state->workp);
/* a = || D (t*dx_gn - dx_sd) ||^2 */
a = scaled_enorm(diag, state->workp);
a *= a;
/* workp = D^T D (t*dx_gn - dx_sd) */
gsl_vector_mul(state->workp, diag);
gsl_vector_mul(state->workp, diag);
/* b = 2 dx_sd^T D^T D (t*dx_gn - dx-sd) */
gsl_blas_ddot(state->dx_sd, state->workp, &b);
b *= 2.0;
/* c = || D dx_sd ||^2 - delta^2 = (||D dx_sd|| + delta) (||D dx_sd|| - delta) */
c = (state->norm_Dsd + delta) * (state->norm_Dsd - delta);
if (b > 0.0)
{
beta = (-2.0 * c) / (b + sqrt(b*b - 4.0*a*c));
}
else
{
beta = (-b + sqrt(b*b - 4.0*a*c)) / (2.0 * a);
}
return beta;
}
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;
}
static int
dogleg (const gsl_matrix * r, const gsl_vector * qtf,
const gsl_vector * diag, double delta,
gsl_vector * newton, gsl_vector * gradient, gsl_vector * p)
{
double qnorm, gnorm, sgnorm, bnorm, temp;
newton_direction (r, qtf, newton);
#ifdef DEBUG
printf("newton = "); gsl_vector_fprintf(stdout, newton, "%g"); printf("\n");
#endif
qnorm = scaled_enorm (diag, newton);
if (qnorm <= delta)
{
gsl_vector_memcpy (p, newton);
#ifdef DEBUG
printf("took newton (qnorm = %g <= delta = %g)\n", qnorm, delta);
#endif
return GSL_SUCCESS;
}
gradient_direction (r, qtf, diag, gradient);
#ifdef DEBUG
printf("grad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
#endif
gnorm = enorm (gradient);
if (gnorm == 0)
{
double alpha = delta / qnorm;
double beta = 0;
scaled_addition (alpha, newton, beta, gradient, p);
#ifdef DEBUG
printf("took scaled newton because gnorm = 0\n");
#endif
return GSL_SUCCESS;
}
minimum_step (gnorm, diag, gradient);
compute_Rg (r, gradient, p); /* Use p as temporary space to compute Rg */
#ifdef DEBUG
printf("mingrad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
printf("Rg = "); gsl_vector_fprintf(stdout, p, "%g"); printf("\n");
#endif
temp = enorm (p);
sgnorm = (gnorm / temp) / temp;
if (sgnorm > delta)
{
double alpha = 0;
double beta = delta;
scaled_addition (alpha, newton, beta, gradient, p);
#ifdef DEBUG
printf("took gradient\n");
#endif
return GSL_SUCCESS;
}
bnorm = enorm (qtf);
{
double bg = bnorm / gnorm;
double bq = bnorm / qnorm;
double dq = delta / qnorm;
double dq2 = dq * dq;
double sd = sgnorm / delta;
double sd2 = sd * sd;
double t1 = bg * bq * sd;
double u = t1 - dq;
double t2 = t1 - dq * sd2 + sqrt (u * u + (1-dq2) * (1 - sd2));
double alpha = dq * (1 - sd2) / t2;
double beta = (1 - alpha) * sgnorm;
#ifdef DEBUG
printf("bnorm = %g\n", bnorm);
printf("gnorm = %g\n", gnorm);
printf("qnorm = %g\n", qnorm);
printf("delta = %g\n", delta);
printf("alpha = %g beta = %g\n", alpha, beta);
printf("took scaled combination of newton and gradient\n");
#endif
scaled_addition (alpha, newton, beta, gradient, p);
}
return GSL_SUCCESS;
}
static int
iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale)
{
hybrid_state_t *state = (hybrid_state_t *) vstate;
const double fnorm = state->fnorm;
gsl_matrix *J = state->J;
gsl_matrix *q = state->q;
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 *df = state->df;
gsl_vector *qtdf = state->qtdf;
gsl_vector *rdx = state->rdx;
gsl_vector *w = state->w;
gsl_vector *v = state->v;
double prered, actred;
double pnorm, fnorm1, fnorm1p;
double ratio;
double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
/* Compute qtf = Q^T f */
compute_qtf (q, f, qtf);
/* Compute dogleg step */
dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
/* Take a trial step */
compute_trial_step (x, dx, state->x_trial);
pnorm = scaled_enorm (diag, dx);
if (state->iter == 1)
{
if (pnorm < state->delta)
{
state->delta = pnorm;
}
}
/* Evaluate function at x + p */
{
int status = GSL_MULTIROOT_FN_EVAL (func, x_trial, f_trial);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
/* Set df = f_trial - f */
compute_df (f_trial, f, df);
/* Compute the scaled actual reduction */
fnorm1 = enorm (f_trial);
actred = compute_actual_reduction (fnorm, fnorm1);
/* Compute rdx = R dx */
compute_rdx (r, dx, rdx);
/* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
fnorm1p = enorm_sum (qtf, rdx);
prered = compute_predicted_reduction (fnorm, fnorm1p);
/* Compute the ratio of the actual to predicted reduction */
if (prered > 0)
{
ratio = actred / prered;
}
else
{
ratio = 0;
}
/* Update the step bound */
if (ratio < p1)
{
state->ncsuc = 0;
state->ncfail++;
state->delta *= p5;
}
else
{
state->ncfail = 0;
state->ncsuc++;
if (ratio >= p5 || state->ncsuc > 1)
state->delta = GSL_MAX (state->delta, pnorm / p5);
if (fabs (ratio - 1) <= p1)
state->delta = pnorm / p5;
}
/* Test for successful iteration */
if (ratio >= p0001)
{
gsl_vector_memcpy (x, x_trial);
gsl_vector_memcpy (f, f_trial);
state->fnorm = fnorm1;
state->iter++;
}
/* Determine the progress of the iteration */
state->nslow1++;
if (actred >= p001)
state->nslow1 = 0;
if (actred >= p1)
state->nslow2 = 0;
if (state->ncfail == 2)
{
gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J) ;
state->nslow2++;
if (state->iter == 1)
{
if (scale)
compute_diag (J, diag);
state->delta = compute_delta (diag, x);
}
else
{
if (scale)
update_diag (J, diag);
}
/* Factorize J into QR decomposition */
gsl_linalg_QR_decomp (J, tau);
gsl_linalg_QR_unpack (J, tau, q, r);
return GSL_SUCCESS;
}
/* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|, v = D^2 dx/|dx| */
compute_qtf (q, df, qtdf);
compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
/* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
gsl_linalg_QR_update (q, r, w, v);
/* No progress as measured by jacobian evaluations */
if (state->nslow2 == 5)
{
return GSL_ENOPROGJ;
}
/* No progress as measured by function evaluations */
if (state->nslow1 == 10)
{
return GSL_ENOPROG;
}
return GSL_SUCCESS;
}
static int
trust_iterate(void *vstate, const gsl_vector *swts,
gsl_multilarge_nlinear_fdf *fdf, gsl_vector *x,
gsl_vector *f, gsl_vector *g, gsl_matrix *JTJ,
gsl_vector *dx)
{
int status;
trust_state_t *state = (trust_state_t *) vstate;
const gsl_multilarge_nlinear_parameters *params = &(state->params);
const gsl_multilarge_nlinear_trs *trs = params->trs;
/* collect all state parameters needed by low level methods */
const gsl_multilarge_nlinear_trust_state trust_state = { x, f, g, JTJ, state->diag,
swts, &(state->mu), params,
state->solver_state, fdf,
&(state->avratio) };
gsl_vector *x_trial = state->x_trial; /* trial x + dx */
gsl_vector *f_trial = state->f_trial; /* trial f(x + dx) */
double rho; /* ratio actual_reduction/predicted_reduction */
int foundstep = 0; /* found step dx */
int bad_steps = 0; /* consecutive rejected steps */
/* initialize trust region subproblem with this Jacobian */
status = (trs->preloop)(&trust_state, state->trs_state);
if (status)
return status;
/* loop until we find an acceptable step dx */
while (!foundstep)
{
/* calculate new step */
status = (trs->step)(&trust_state, state->delta, dx, state->trs_state);
/* occasionally the iterative methods (ie: CG Steihaug) can fail to find a step,
* so in this case skip rho calculation and count it as a rejected step */
if (status == GSL_SUCCESS)
{
/* compute x_trial = x + dx */
trust_trial_step(x, dx, x_trial);
/* compute f_trial = f(x + dx) */
status = gsl_multilarge_nlinear_eval_f(fdf, x_trial, swts, f_trial);
if (status)
return status;
/* check if step should be accepted or rejected */
status = trust_eval_step(&trust_state, f_trial, dx, &rho, state);
if (status == GSL_SUCCESS)
foundstep = 1;
#if 0 /*XXX*/
fprintf(stdout, "delta = %.12e |D dx| = %.12e |dx| = %.12e, dx0 = %.12e dx1 = %.12e |x_trial| = %.12e |f_trial| = %.12e rho = %.12e\n",
state->delta,
scaled_enorm(state->diag, dx),
gsl_blas_dnrm2(dx),
gsl_vector_get(dx, 0),
gsl_vector_get(dx, 1),
gsl_blas_dnrm2(x_trial),
gsl_blas_dnrm2(f_trial),
rho);
#endif
}
else
{
/* an iterative TRS method failed to find a step vector */
rho = -1.0;
}
/*
* update trust region radius: if rho is large,
* then the quadratic model is a good approximation
* to the objective function, enlarge trust region.
* If rho is small (or negative), the model function
* is a poor approximation so decrease trust region. This
* can happen even if the step is accepted.
*/
if (rho > 0.75)
state->delta *= params->factor_up;
else if (rho < 0.25)
state->delta /= params->factor_down;
if (foundstep)
{
/* step was accepted */
/* update x <- x + dx */
gsl_vector_memcpy(x, x_trial);
/* update f <- f(x + dx) */
gsl_vector_memcpy(f, f_trial);
/* compute new g = J^T f and J^T J */
status = gsl_multilarge_nlinear_eval_df(CblasTrans, x, f, f,
swts, params->h_df, params->fdtype,
fdf, g, JTJ, state->workn);
if (status)
return status;
/* update scaling matrix D */
if (JTJ != NULL)
(params->scale->update)(JTJ, state->diag);
/* step accepted, decrease LM parameter */
nielsen_accept(rho, &(state->mu), &(state->nu));
bad_steps = 0;
}
else
{
/* step rejected, increase LM parameter */
nielsen_reject(&(state->mu), &(state->nu));
/* if more than 15 consecutive rejected steps, report no progress */
if (++bad_steps > 15)
{
return GSL_ENOPROG;
}
}
}
return GSL_SUCCESS;
} /* trust_iterate() */
static int
set (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 *work1 = state->work1;
gsl_permutation *perm = state->perm;
int signum;
/* Evaluate function at x */
/* return immediately if evaluation raised error */
{
int status = GSL_MULTIFIT_FN_EVAL_F_DF (fdf, x, f, J);
if (status)
return status;
}
state->par = 0;
state->iter = 1;
state->fnorm = enorm (f);
gsl_vector_set_all (dx, 0.0);
/* store column norms in diag */
if (scale)
{
compute_diag (J, diag);
}
else
{
gsl_vector_set_all (diag, 1.0);
}
/* set delta to 100 |D x| or to 100 if |D x| is zero */
state->xnorm = scaled_enorm (diag, x);
state->delta = compute_delta (diag, x);
/* Factorize J into QR decomposition */
gsl_matrix_memcpy (r, J);
gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
gsl_vector_set_zero (state->rptdx);
gsl_vector_set_zero (state->w);
/* Zero the trial vector, as in the alloc function */
gsl_vector_set_zero (state->f_trial);
#ifdef DEBUG
printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
printf("tau = "); gsl_vector_fprintf(stdout, tau, "%g");
#endif
return GSL_SUCCESS;
}