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;
}
int trustregion(NLP1* nlp, std::ostream *fout,
SymmetricMatrix& H, ColumnVector& search_dir,
ColumnVector& sx, real& TR_size, real& step_length,
real stpmax, real stpmin)
{
/****************************************************************************
* subroutine trustregion
*
* Purpose
* find a step which satisfies the Goldstein-Armijo line search conditions
*
* Compute the dogleg step
* Compute the predicted reduction, pred, of the quadratic model
* Compute the actual reduction, ared
* IF ared/pred > eta
* THEN x_vec = x_vec + d_vec
* TR_size >= TR_size
* Compute the gradient g_vec at the new point
* ELSE TR_size < ||d_vec||
*
* Parameters
* nlp --> pointer to nonlinear problem object
*
* search_dir --> Vector of length n which specifies the
* newton direction on input. On output it will
* contain the step
*
* step_length <-- is a nonnegative variable.
* On output step_length contains the step size taken
*
* ftol --> default Value = 1.e-4
* ftol should be smaller than 5.e-1
* suggested value = 1.e-4 for newton methods
* = 1.e-1 for more exact line searches
* xtol --> default Value = 2.2e-16
* gtol --> default Value = 0.9
* gtol should be greater than 1.e-4
*
* termination occurs when the sufficient decrease
* condition and the directional derivative condition are
* satisfied.
*
* stpmin and TR_size are nonnegative input variables which
* specify lower and upper bounds for the step.
* stpmin Default Value = 1.e-9
*
* Initial version Juan Meza November 1994
*
*
*****************************************************************************/
// Local variables
int n = nlp->getDim();
bool debug = nlp->getDebug();
bool modeOverride = nlp->getModeOverride();
ColumnVector tgrad(n), newton_dir(n), tvec(n), xc(n), xtrial(n);
real fvalue, fplus, dnorm;
real eta1 = .001;
real eta2 = .1;
real eta3 = .75;
real rho_k;
int iter = 0;
int iter_max = 100;
real dd1, dd2;
real ared, pred;
int dog_step;
real TR_MAX = stpmax;
static char *steps[] = {"C", "D", "N", "B"};
static bool accept;
//
// Initialize variables
//
fvalue = nlp->getF();
xc = nlp->getXc();
step_length = 1.0;
tgrad = nlp->getGrad();
newton_dir = search_dir;
if (debug) {
*fout << "\n***************************************";
*fout << "***************************************\n";
*fout << "\nComputeStep using trustregion\n";
*fout << "\tStep ||step|| ared pred TR_size \n";
}
while (iter < iter_max) {
iter++;
//
// Compute the dogleg step
//
search_dir = newton_dir;
dog_step = dogleg(nlp, fout, H, tgrad, search_dir, sx, dnorm, TR_size, stpmax);
step_length = dnorm;
//
// Compute pred = -g'd - 1/2 d'Hd
//
dd1 = Dot(tgrad,search_dir);
tvec = H * search_dir;
dd2 = Dot(search_dir,tvec);
pred = -dd1 - dd2/2.0;
//
// Compute objective function at trial point
//
xtrial = xc + search_dir;
if (modeOverride) {
nlp->setX(xtrial);
nlp->eval();
fplus = nlp->getF();
}
else
fplus = nlp->evalF(xtrial);
ared = fvalue - fplus;
//
// Should we take this step ?
//
rho_k = ared/pred;
accept = false;
if (rho_k >= eta1) accept = true;
//
// Update the trust region
//
if (accept) {
//
// Do the standard updating
//
if (rho_k <= eta2) {
// Model is just sort of bad
// New trust region will be TR_size/2
if (debug) {
*fout << "trustregion: rho_k = " << e(rho_k,14,4)
<< " eta2 = " << e(eta2,14,4) << "\n";
}
TR_size = step_length / 2.0;
if (TR_size < stpmin) {
*fout << "***** Trust region too small to continue.\n";
nlp->setX(xc);
nlp->setF(fvalue);
nlp->setGrad(tgrad);
return(-1);
}
}
else if ((eta3 <= rho_k) && (rho_k <= (2.0 - eta3))) {
// Model is PRETTY good
// Double trust region
if (debug) {
*fout << "trustregion: rho_k = " << e(rho_k,14,4)
<< " eta3 = " << e(eta3,14,4) << "\n";
}
TR_size = min(2.0*TR_size, TR_MAX);
}
else {
//
// All other cases
//
TR_size = max(2.0*step_length,TR_size);
TR_size = min(TR_size, TR_MAX);
if (debug) {
*fout << "trustregion: rho_k = " << e(rho_k,14,4) << "\n";
}
}
}
else {
// Model is REALLY bad
//
TR_size = step_length/10.0;
if (debug) {
*fout << "trustregion: rho_k = " << e(rho_k,14,4) << "n"
<< " eta1 = " << e(eta1,14,4) << "\n";
}
if (TR_size < stpmin) {
*fout << "***** Trust region too small to continue.\n";
nlp->setX(xc);
nlp->setF(fvalue);
nlp->setGrad(tgrad);
return(-1);
}
}
//
// Accept/Reject Step
//
if (accept) {
//
// Update x, f, and grad
//
if (!modeOverride) {
nlp->setX(xtrial);
nlp->setF(fplus);
nlp->evalG();
}
if (debug) {
*fout << "\t Step ||step|| ared pred TR_size \n";
*fout << "Accept " << steps[dog_step] << e(step_length,14,4)
<< e(ared,14,4) << e(pred,14,4) << e(TR_size,14,4) << "\n";
}
return(dog_step);
}
else {
//
// Reject step
//
if (debug) {
*fout << "\t Step ||step|| ared pred TR_size \n";
*fout << "Reject " << steps[dog_step] << e(step_length,14,4)
<< e(ared,14,4) << e(pred,14,4) << e(TR_size,14,4) << "\n";
}
}
}
nlp->setX(xc);
nlp->setF(fvalue);
nlp->setGrad(tgrad);
return(-1); // Too many iterations
}
/* Subroutine */ int hybrj(minpack_funcder_nn fcn, void *p, int n, double *x, double *
fvec, double *fjac, int ldfjac, double xtol, int
maxfev, double *diag, int mode, double factor, int
nprint, int *nfev, int *njev, double *r,
int lr, double *qtf, double *wa1, double *wa2,
double *wa3, double *wa4)
{
/* Initialized data */
#define p1 .1
#define p5 .5
#define p001 .001
#define p0001 1e-4
/* System generated locals */
int fjac_dim1, fjac_offset;
double d1, d2;
/* Local variables */
int i, j, l, jm1, iwa[1];
double sum;
int sing;
int iter;
double temp;
int iflag;
double delta;
int jeval;
int ncsuc;
double ratio;
double fnorm;
double pnorm, xnorm, fnorm1;
int nslow1, nslow2;
int ncfail;
double actred, epsmch, prered;
int info;
/* ********** */
/* subroutine hybrj */
/* the purpose of hybrj is to find a zero of a system of */
/* n nonlinear functions in n variables by a modification */
/* of the powell hybrid method. the user must provide a */
/* subroutine which calculates the functions and the jacobian. */
/* the subroutine statement is */
/* subroutine hybrj(fcn,n,x,fvec,fjac,ldfjac,xtol,maxfev,diag, */
/* mode,factor,nprint,info,nfev,njev,r,lr,qtf, */
/* wa1,wa2,wa3,wa4) */
/* where */
/* fcn is the name of the user-supplied subroutine which */
/* calculates the functions and the jacobian. fcn must */
/* be declared in an external statement in the user */
/* calling program, and should be written as follows. */
/* subroutine fcn(n,x,fvec,fjac,ldfjac,iflag) */
/* integer n,ldfjac,iflag */
/* double precision x(n),fvec(n),fjac(ldfjac,n) */
/* ---------- */
/* if iflag = 1 calculate the functions at x and */
/* return this vector in fvec. do not alter fjac. */
/* if iflag = 2 calculate the jacobian at x and */
/* return this matrix in fjac. do not alter fvec. */
/* --------- */
/* return */
/* end */
/* the value of iflag should not be changed by fcn unless */
/* the user wants to terminate execution of hybrj. */
/* in this case set iflag to a negative integer. */
/* n is a positive integer input variable set to the number */
/* of functions and variables. */
/* x is an array of length n. on input x must contain */
/* an initial estimate of the solution vector. on output x */
/* contains the final estimate of the solution vector. */
/* fvec is an output array of length n which contains */
/* the functions evaluated at the output x. */
/* fjac is an output n by n array which contains the */
/* orthogonal matrix q produced by the qr factorization */
/* of the final approximate jacobian. */
/* ldfjac is a positive integer input variable not less than n */
/* which specifies the leading dimension of the array fjac. */
/* xtol is a nonnegative input variable. termination */
/* occurs when the relative error between two consecutive */
/* iterates is at most xtol. */
/* maxfev is a positive integer input variable. termination */
/* occurs when the number of calls to fcn with iflag = 1 */
/* has reached maxfev. */
/* diag is an array of length n. if mode = 1 (see */
/* below), diag is internally set. if mode = 2, diag */
/* must contain positive entries that serve as */
/* multiplicative scale factors for the variables. */
/* mode is an integer input variable. if mode = 1, the */
/* variables will be scaled internally. if mode = 2, */
/* the scaling is specified by the input diag. other */
/* values of mode are equivalent to mode = 1. */
/* factor is a positive input variable used in determining the */
/* initial step bound. this bound is set to the product of */
/* factor and the euclidean norm of diag*x if nonzero, or else */
/* to factor itself. in most cases factor should lie in the */
/* interval (.1,100.). 100. is a generally recommended value. */
/* nprint is an integer input variable that enables controlled */
/* printing of iterates if it is positive. in this case, */
/* fcn is called with iflag = 0 at the beginning of the first */
/* iteration and every nprint iterations thereafter and */
/* immediately prior to return, with x and fvec available */
/* for printing. fvec and fjac should not be altered. */
/* if nprint is not positive, no special calls of fcn */
/* with iflag = 0 are made. */
/* info is an integer output variable. if the user has */
/* terminated execution, info is set to the (negative) */
/* value of iflag. see description of fcn. otherwise, */
/* info is set as follows. */
/* info = 0 improper input parameters. */
/* info = 1 relative error between two consecutive iterates */
/* is at most xtol. */
/* info = 2 number of calls to fcn with iflag = 1 has */
/* reached maxfev. */
/* info = 3 xtol is too small. no further improvement in */
/* the approximate solution x is possible. */
/* info = 4 iteration is not making good progress, as */
/* measured by the improvement from the last */
/* five jacobian evaluations. */
/* info = 5 iteration is not making good progress, as */
/* measured by the improvement from the last */
/* ten iterations. */
/* nfev is an integer output variable set to the number of */
/* calls to fcn with iflag = 1. */
/* njev is an integer output variable set to the number of */
/* calls to fcn with iflag = 2. */
/* r is an output array of length lr which contains the */
/* upper triangular matrix produced by the qr factorization */
/* of the final approximate jacobian, stored rowwise. */
/* lr is a positive integer input variable not less than */
/* (n*(n+1))/2. */
/* qtf is an output array of length n which contains */
/* the vector (q transpose)*fvec. */
/* wa1, wa2, wa3, and wa4 are work arrays of length n. */
/* subprograms called */
/* user-supplied ...... fcn */
/* minpack-supplied ... dogleg,dpmpar,enorm, */
/* qform,qrfac,r1mpyq,r1updt */
/* fortran-supplied ... dabs,dmax1,dmin1,mod */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* Parameter adjustments */
--wa4;
--wa3;
--wa2;
--wa1;
--qtf;
--diag;
--fvec;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--r;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || ldfjac < n || xtol < 0. || maxfev <= 0 || factor <=
0. || lr < n * (n + 1) / 2) {
goto TERMINATE;
}
if (mode == 2) {
for (j = 1; j <= n; ++j) {
if (diag[j] <= 0.) {
goto TERMINATE;
}
}
}
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 1);
*nfev = 1;
if (iflag < 0) {
goto TERMINATE;
}
fnorm = enorm(n, &fvec[1]);
/* initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
/* beginning of the outer loop. */
for (;;) {
jeval = TRUE_;
/* calculate the jacobian matrix. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 2);
++(*njev);
if (iflag < 0) {
goto TERMINATE;
}
/* compute the qr factorization of the jacobian. */
qrfac(n, n, &fjac[fjac_offset], ldfjac, FALSE_, iwa, 1,
&wa1[1], &wa2[1], &wa3[1]);
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter == 1) {
if (mode != 2) {
for (j = 1; j <= n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
}
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
for (j = 1; j <= n; ++j) {
wa3[j] = diag[j] * x[j];
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
}
/* form (q transpose)*fvec and store in qtf. */
for (i = 1; i <= n; ++i) {
qtf[i] = fvec[i];
}
for (j = 1; j <= n; ++j) {
if (fjac[j + j * fjac_dim1] != 0.) {
sum = 0.;
for (i = j; i <= n; ++i) {
sum += fjac[i + j * fjac_dim1] * qtf[i];
}
temp = -sum / fjac[j + j * fjac_dim1];
for (i = j; i <= n; ++i) {
qtf[i] += fjac[i + j * fjac_dim1] * temp;
}
}
}
/* copy the triangular factor of the qr factorization into r. */
sing = FALSE_;
for (j = 1; j <= n; ++j) {
l = j;
jm1 = j - 1;
if (jm1 >= 1) {
for (i = 1; i <= jm1; ++i) {
r[l] = fjac[i + j * fjac_dim1];
l = l + n - i;
}
}
r[l] = wa1[j];
if (wa1[j] == 0.) {
sing = TRUE_;
}
}
/* accumulate the orthogonal factor in fjac. */
qform(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]);
/* rescale if necessary. */
if (mode != 2) {
for (j = 1; j <= n; ++j) {
/* Computing MAX */
d1 = diag[j], d2 = wa2[j];
diag[j] = max(d1,d2);
}
}
/* beginning of the inner loop. */
for (;;) {
/* if requested, call fcn to enable printing of iterates. */
if (nprint > 0) {
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
if (iflag < 0) {
goto TERMINATE;
}
}
/* determine the direction p. */
dogleg(n, &r[1], lr, &diag[1], &qtf[1], delta, &wa1[1], &wa2[1], &wa3[1]);
/* store the direction p and x + p. calculate the norm of p. */
for (j = 1; j <= n; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, n, &wa2[1], &wa4[1], &fjac[fjac_offset], ldfjac, 1);
++(*nfev);
if (iflag < 0) {
goto TERMINATE;
}
fnorm1 = enorm(n, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (fnorm1 < fnorm) {
/* Computing 2nd power */
d1 = fnorm1 / fnorm;
actred = 1. - d1 * d1;
}
/* compute the scaled predicted reduction. */
l = 1;
for (i = 1; i <= n; ++i) {
sum = 0.;
for (j = i; j <= n; ++j) {
sum += r[l] * wa1[j];
++l;
}
wa3[i] = qtf[i] + sum;
}
temp = enorm(n, &wa3[1]);
prered = 0.;
if (temp < fnorm) {
/* Computing 2nd power */
d1 = temp / fnorm;
prered = 1. - d1 * d1;
}
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered > 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio < p1) {
ncsuc = 0;
++ncfail;
delta = p5 * delta;
} else {
ncfail = 0;
++ncsuc;
if (ratio >= p5 || ncsuc > 1) {
/* Computing MAX */
d1 = pnorm / p5;
delta = max(delta,d1);
}
if (fabs(ratio - 1.) <= p1) {
delta = pnorm / p5;
}
}
/* test for successful iteration. */
if (ratio >= p0001) {
/* successful iteration. update x, fvec, and their norms. */
for (j = 1; j <= n; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
fvec[j] = wa4[j];
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
}
/* determine the progress of the iteration. */
++nslow1;
if (actred >= p001) {
nslow1 = 0;
}
if (jeval) {
++nslow2;
}
if (actred >= p1) {
nslow2 = 0;
}
/* test for convergence. */
if (delta <= xtol * xnorm || fnorm == 0.) {
info = 1;
}
if (info != 0) {
goto TERMINATE;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 2;
}
/* Computing MAX */
d1 = p1 * delta;
if (p1 * max(d1,pnorm) <= epsmch * xnorm) {
info = 3;
}
if (nslow2 == 5) {
info = 4;
}
if (nslow1 == 10) {
info = 5;
}
if (info != 0) {
goto TERMINATE;
}
/* criterion for recalculating jacobian. */
if (ncfail == 2) {
goto TERMINATE_INNER_LOOP;
}
/* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
for (j = 1; j <= n; ++j) {
sum = 0.;
for (i = 1; i <= n; ++i) {
sum += fjac[i + j * fjac_dim1] * wa4[i];
}
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= p0001) {
qtf[j] = sum;
}
}
/* compute the qr factorization of the updated jacobian. */
r1updt(n, n, &r[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing);
r1mpyq(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]);
r1mpyq(1, n, &qtf[1], 1, &wa2[1], &wa3[1]);
/* end of the inner loop. */
jeval = FALSE_;
}
TERMINATE_INNER_LOOP:
;
/* end of the outer loop. */
}
TERMINATE:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
if (nprint > 0) {
(*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
return info;
/* last card of subroutine hybrj. */
} /* hybrj_ */
