486 lines
13 KiB
C
486 lines
13 KiB
C
|
|
/* Multi-dimentional minizer using Variable Metric method */
|
|
/* This is good for smoother, well behaved functions. */
|
|
|
|
/* Code is an original expression of the algorithms decsribed in */
|
|
/* "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */
|
|
/* S.A.Teukolsky & W.T.Vetterling. */
|
|
|
|
/*
|
|
* Copyright 2000, 2006, 2007, 2017 Graeme W. Gill
|
|
* All rights reserved.
|
|
*
|
|
* This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
|
|
* see the License.txt file for licencing details.
|
|
*/
|
|
|
|
/* Note that all arrays are indexed from 0 */
|
|
|
|
#include "numsup.h"
|
|
#include "powell.h"
|
|
#include "varmet.h"
|
|
|
|
#undef VDEBUG /* Varmet debug */
|
|
#undef LDEBUG /* Line min debug */
|
|
#undef PLOTL /* Plot line search response */
|
|
|
|
#if defined(VDEBUG)
|
|
# undef VDBG
|
|
# define VDBG(xxx) printf xxx ;
|
|
#else
|
|
# undef VDBG
|
|
# define VDBG(xxx)
|
|
#endif
|
|
|
|
#if defined(LDEBUG)
|
|
# undef LDBG
|
|
# define LDBG(xxx) printf xxx ;
|
|
#else
|
|
# undef LDBG
|
|
# define LDBG(xxx)
|
|
#endif
|
|
|
|
|
|
#define FMAX(A,B) ((A) > (B) ? (A) : (B))
|
|
#define EPS 1.0e-10 /* Machine precision. */
|
|
#define XTOL (4 * EPS) /* X value stop value */
|
|
#define MAXLEN 100.0 /* Maximum step length */
|
|
|
|
void linesearch(int di, double cpold[], double fpold, double g[], double p[], double cpnew[],
|
|
double *pfp, double maxstep, double (*func)(void *fdata, double tp[]), void *fdata);
|
|
|
|
static double fake_dfunc(void *fdata, double (*func)(void *fdata, double tp[]), int di,
|
|
double dp[], double tp[]);
|
|
|
|
/* return 0 on sucess, 1 on failure due to excessive itterions */
|
|
/* Result will be in cp */
|
|
/* Note that we could use gradient in line minimiser, */
|
|
/* but haven't bothered yet. */
|
|
int varmet(
|
|
double *rv, /* If not NULL, return the residual error */
|
|
int di, /* Dimentionality */
|
|
double cp[], /* Initial starting point */
|
|
double s[], /* Size of initial search area */
|
|
double ftol, /* relative function value change tolleranc to stop on */
|
|
//double xtol, /* relative cp change tolleranc to stop on */
|
|
//double gtol, /* Gradient value tollerance to stop on */
|
|
int maxit, /* Maximum iterations allowed */
|
|
double (*func)(void *fdata, double tp[]), /* Error function to evaluate */
|
|
double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient function to evaluate */
|
|
void *fdata /* Opaque data needed by function */
|
|
) {
|
|
int iter, fails;
|
|
double fp, sumsq, maxstep;
|
|
double *sdir, sumsdir; /* Search direction */
|
|
double *dp, *lastdp;
|
|
double **hessian; /* Hessian matrix */
|
|
double *hlastdp; /* Hessian times lastdp */
|
|
double *cpnew; /* new cp value from linemin */
|
|
double *dels; /* Delta's from each step */
|
|
double test;
|
|
|
|
double den, fac, fad, fae;
|
|
double sumdg;
|
|
int i, j;
|
|
|
|
double xtol = XTOL; /* relative cp change tolleranc to stop on */
|
|
|
|
double pfp = 1e38, stopth, curdel;
|
|
|
|
sdir = dvector(0, di-1);
|
|
dp = dvector(0, di-1);
|
|
lastdp = dvector(0, di-1);
|
|
hessian = dmatrix(0, di-1, 0, di-1);
|
|
hlastdp = dvector(0, di-1);
|
|
cpnew = dvector(0, di-1);
|
|
dels = dvector(0, di-1);
|
|
|
|
VDBG((" doing partial deriv.\n"));
|
|
if (dfunc != NULL)
|
|
fp = (*dfunc)(fdata, dp, cp);
|
|
else
|
|
fp = fake_dfunc(fdata, func, di, dp, cp);
|
|
|
|
if (fp == DFUNC_NRV)
|
|
fp = (*func)(fdata, cp);
|
|
|
|
/* Initial line direction and pde squared */
|
|
sumsq = 0.0;
|
|
for (i = 0; i < di ;i++) {
|
|
sdir[i] = -dp[i];
|
|
sumsq += cp[i] * cp[i];
|
|
}
|
|
|
|
VDBG((" initial fp %f dp %s\n", fp, debPdv(di, dp)));
|
|
|
|
/* Initialize inverse Hessian to unity */
|
|
for (i = 0; i < di ;i++) {
|
|
for (j = 0; j < di ; j++) {
|
|
if (i == j)
|
|
hessian[i][j] = 1.0;
|
|
else
|
|
hessian[i][j] = 0.0;
|
|
}
|
|
}
|
|
|
|
/* Maximum line search step size */
|
|
maxstep = MAXLEN * FMAX(sqrt(sumsq), (double)di);
|
|
VDBG((" maxstep %g\n", maxstep));
|
|
|
|
/* Until we give up */
|
|
for (fails = iter = 0; fails < di && iter < maxit; iter++) {
|
|
|
|
/* Search in direction sdir */
|
|
VDBG((" it %d: doing line search\n",iter));
|
|
linesearch(di, cp, fp, dp, sdir, cpnew, &fp, maxstep, func, fdata);
|
|
|
|
for (i = 0; i < di; i++) {
|
|
sdir[i] = cpnew[i] - cp[i]; /* Compare search direction, */
|
|
cp[i] = cpnew[i]; /* and the current value. */
|
|
}
|
|
|
|
/* Check the relative change in x, and stop if it's below */
|
|
/* the machine precision */
|
|
for (test = 0.0, i = 0 ; i < di; i++) {
|
|
double tt = fabs(sdir[i]) / FMAX(fabs(cp[i]), 1.0);
|
|
if (tt > test)
|
|
test = tt;
|
|
}
|
|
|
|
if (test < xtol) {
|
|
// VDBG((" converged because test %g < xtol %g\n",test,xtol));
|
|
VDBG((" failed to make progres (iter %d < maxit %d): test %g < xtol %g\n",iter,maxit,test,xtol));
|
|
|
|
/* Try a move in the dp direction */
|
|
sumsq = 0.0;
|
|
for (i = 0; i < di ;i++) {
|
|
sdir[i] = -dp[i];
|
|
sumsq += cp[i] * cp[i];
|
|
}
|
|
|
|
maxstep = MAXLEN * FMAX(sqrt(sumsq), (double)di);
|
|
|
|
VDBG((" retrying\n"));
|
|
|
|
fails++;
|
|
|
|
continue;
|
|
// break;
|
|
}
|
|
fails = 0;
|
|
VDBG((" not converged because test %g >= xtol %g\n",test,xtol));
|
|
|
|
/* Check relative change in function value */
|
|
curdel = fabs(pfp - fp);
|
|
dels[iter % di] = curdel;
|
|
|
|
if (iter > di) { /* Enough to compute a moving average del */
|
|
double avgdel;
|
|
|
|
for (avgdel = 0.0, i = 0 ; i < di; i++)
|
|
avgdel += dels[i] * dels[i];
|
|
avgdel = sqrt(avgdel/(double)di); /* Moving RMS average */
|
|
|
|
stopth = ftol * 0.5 * (fabs(pfp) + fabs(fp) + DBL_EPSILON);
|
|
|
|
/* If average relative change is below threshold */
|
|
if (avgdel <= stopth) {
|
|
VDBG(("Reached stop tollerance avgdel %g <= stopth %g\n",avgdel,stopth))
|
|
break;
|
|
} else {
|
|
VDBG(("Not stopping because avgdel %g > stopth %g\n",avgdel,stopth))
|
|
}
|
|
}
|
|
|
|
pfp = fp;
|
|
|
|
for (i = 0; i < di; i++) /* Save previous partial deriv. */
|
|
lastdp[i] = dp[i];
|
|
|
|
VDBG((" it %d: doing partial deriv\n",iter));
|
|
if (dfunc != NULL)
|
|
(*dfunc)(fdata, dp, cp); /* and get the new gradient. */
|
|
else
|
|
fake_dfunc(fdata, func, di, dp, cp);
|
|
|
|
#ifdef NEVER /* This doesn't seem useful */
|
|
/* Check the largest relative gradient, and stop if it's below */
|
|
/* our stopping tollerance */
|
|
den = FMAX(fp, 1.0);
|
|
for (test = 0.0, i = 0; i < di; i++) {
|
|
double tt = fabs(dp[i]) * FMAX(fabs(cp[i]),1.0) / den;
|
|
if (tt > test)
|
|
test = tt;
|
|
}
|
|
|
|
if (test < gtol) {
|
|
VDBG((" converged because test %g < ftol %g\n",test,gtol));
|
|
break;
|
|
}
|
|
VDBG((" Not converged because test %g >= ftol %g\n",test,gtol));
|
|
#endif
|
|
|
|
for (i = 0 ; i < di; i++)
|
|
lastdp[i] = dp[i] - lastdp[i]; /* Compute diference of gradients, */
|
|
|
|
for (i = 0; i < di; i++) { /* and difernce times current matrix. */
|
|
hlastdp[i] = 0.0;
|
|
for (j = 0; j < di; j++)
|
|
hlastdp[i] += hessian[i][j] * lastdp[j];
|
|
}
|
|
|
|
/* Calculate dot products for the denominator */
|
|
fac = fae = sumdg = sumsdir = 0.0;
|
|
for (i = 0; i < di; i++) {
|
|
fac += lastdp[i] * sdir[i];
|
|
fae += lastdp[i] * hlastdp[i];
|
|
sumdg += lastdp[i] * lastdp[i];
|
|
sumsdir += sdir[i] * sdir[i];
|
|
}
|
|
if (fac > sqrt(EPS * sumdg * sumsdir)) { /* Skip update if fac not sufficiently posive */
|
|
fac = 1.0/fac;
|
|
fad = 1.0/fae;
|
|
/* The vector that makes BFGS different from DFP: */
|
|
for (i = 0; i < di;i++)
|
|
lastdp[i] = fac * sdir[i] - fad * hlastdp[i];
|
|
for (i = 0; i < di;i++) { /* The BFGS updating formula: */
|
|
for (j = i; j < di; j++) {
|
|
hessian[i][j] += fac * sdir[i] * sdir[j]
|
|
- fad * hlastdp[i] * hlastdp[j] + fae * lastdp[i] * lastdp[j];
|
|
hessian[j][i] = hessian[i][j];
|
|
}
|
|
}
|
|
}
|
|
for (i = 0; i < di; i++) { /* Now calculate the next direction to go, */
|
|
sdir[i] = 0.0;
|
|
for (j = 0; j < di; j++)
|
|
sdir[i] -= hessian[i][j] * dp[j];
|
|
}
|
|
}
|
|
|
|
VDBG((" Returning %g\n",fp));
|
|
|
|
free_dvector(dels, 0, di-1);
|
|
free_dvector(cpnew, 0, di-1);
|
|
free_dvector(hlastdp, 0, di-1);
|
|
free_dmatrix(hessian, 0, di-1, 0, di-1);
|
|
free_dvector(lastdp, 0, di-1);
|
|
free_dvector(dp, 0, di-1);
|
|
free_dvector(sdir, 0, di-1);
|
|
if (rv != NULL)
|
|
*rv = fp;
|
|
return 0;
|
|
}
|
|
|
|
/* -------------------------------------- */
|
|
|
|
/* Numerical approximation to gradient */
|
|
static double fake_dfunc(void *fdata, double (*func)(void *fdata, double tp[]), int di,
|
|
double dp[], double tp[]) {
|
|
double rv, h, temp;
|
|
int i;
|
|
|
|
rv = (*func)(fdata, tp);
|
|
|
|
for (i = 0; i < di; i++) {
|
|
double drv;
|
|
|
|
temp = tp[i];
|
|
|
|
h = 1e-8 * fabs(temp);
|
|
if (h == 0.0)
|
|
h = 1e-8;
|
|
tp[i] = temp + h; /* Add delta */
|
|
h = tp[i] - temp; /* Actual delta with fp precision limits */
|
|
drv = (*func)(fdata, tp);
|
|
dp[i] = (drv - rv)/h;
|
|
tp[i] = temp; /* Restore value */
|
|
}
|
|
return rv;
|
|
}
|
|
|
|
/* -------------------------------------- */
|
|
|
|
#ifdef PLOTL /* Plot the step direction */
|
|
# include "aconfig.h"
|
|
# include "plot.h"
|
|
#endif
|
|
|
|
#define ALPHA 1.0e-4 /* Ensures sufficient decrease in function value. */
|
|
#define LXTOL 1.0e-7 /* [1e-7] Min step length termination criteria on linesearch. */
|
|
|
|
void linesearch(
|
|
int di,
|
|
double cpold[], /* Incoming (old) current value */
|
|
double fpold, /* Incomming current function value */
|
|
double dp[], /* Partial derivative */
|
|
double sdir[], /* Search direction */
|
|
double cpnew[], /* Return (new) value */
|
|
double *pfp, /* Return objective value */
|
|
double maxstep,
|
|
double (*func)(void *fdata, double tp[]),
|
|
void *fdata
|
|
) {
|
|
double sum, slope;
|
|
double slen, slen_2 = 0.0, slen_min;
|
|
double test, fp_2 = 0.0;
|
|
int i;
|
|
|
|
for (sum = 0.0, i = 0; i < di; i++)
|
|
sum += sdir[i] * sdir[i];
|
|
sum = sqrt(sum);
|
|
|
|
LDBG((" fpold %f, sdir length %f\n",fpold,sum));
|
|
|
|
if (sum > maxstep) {
|
|
LDBG((" sdir scaled to maxstep %f\n",maxstep));
|
|
for (i = 0; i < di; i++)
|
|
sdir[i] *= maxstep/sum; /* Scale if attempted step is too big. */
|
|
}
|
|
for (slope = 0.0, i = 0; i < di; i++)
|
|
slope += dp[i] * sdir[i];
|
|
|
|
LDBG((" dp . sdir = slope = %f\n",slope));
|
|
|
|
if (slope >= 0.0) { /* Hmm. */
|
|
warning("varmet:linesearch: slope is >= 0");
|
|
#ifdef PLOTL
|
|
goto do_return;
|
|
#else
|
|
return;
|
|
#endif
|
|
}
|
|
|
|
/* Compute largest ratio of sdir component to max(current value,1.0) */
|
|
for (test = 0.0, i = 0;i < di; i++) {
|
|
double tt = fabs(sdir[i])/FMAX(fabs(cpold[i]), 1.0);
|
|
if (tt > test)
|
|
test = tt;
|
|
}
|
|
|
|
slen_min = LXTOL/test; /* min. step length termination criteria */
|
|
slen = 1.0; /* Try full step */
|
|
|
|
/* Start of iteration loop. */
|
|
for (;;) {
|
|
double slen_t;
|
|
|
|
LDBG((" top of loop: slen %f slen_min %g slen_2 %f\n",slen, slen_min, slen_2));
|
|
|
|
/* trial point */
|
|
for (i = 0; i < di;i++)
|
|
cpnew[i] = cpold[i] + slen * sdir[i];
|
|
|
|
*pfp = (*func)(fdata, cpnew);
|
|
LDBG((" func %f, fpold %f %s\n",*pfp, fpold, *pfp < fpold ? "BETTER" : "same/worse"));
|
|
|
|
if (slen < slen_min) {
|
|
LDBG((" return slen %f fp %f because slen %g < slen_min %g (cpold)\n",slen_2, fp_2, slen, slen_min));
|
|
for (i = 0; i < di; i++)
|
|
cpnew[i] = cpold[i];
|
|
*pfp = fp_2;
|
|
slen = slen_min;
|
|
|
|
#ifdef PLOTL
|
|
goto do_return;
|
|
#else
|
|
return;
|
|
#endif
|
|
|
|
} else if (*pfp <= (fpold + ALPHA * slen * slope)) {
|
|
LDBG((" return slen %f fp %f because func %f <= %f = fpold %f + %f * slen %f * slope %f (cpnew)\n",slen, *pfp, *pfp, fpold + ALPHA * slen * slope, fpold, ALPHA, slen, slope));
|
|
#ifdef PLOTL
|
|
goto do_return;
|
|
#else
|
|
return;
|
|
#endif
|
|
|
|
/* Backtracking */
|
|
} else {
|
|
LDBG((" slen %g >= slen_min %g\n",slen, slen_min));
|
|
|
|
LDBG((" backtracking:\n"));
|
|
|
|
/* First time through model as a quadratic */
|
|
if (slen == 1.0) {
|
|
slen_t = -slope/(2.0 * (*pfp - fpold - slope));
|
|
LDBG((" 1: slen_t %f = -slope %f/(2.0 * (*pfp %f - fpold %f - slope %f)\n",slen_t,-slope, *pfp, fpold, slope));
|
|
|
|
/* 2nd and subsequent times through model as a cubic */
|
|
} else {
|
|
double aa, bb;
|
|
double rhs_1, rhs_2;
|
|
|
|
LDBG((" 2:\n"));
|
|
|
|
/* Components of cubic solution */
|
|
rhs_1 = *pfp - fpold - slen * slope;
|
|
rhs_2 = fp_2 - fpold - slen_2 * slope;
|
|
aa = (rhs_1/(slen * slen) - rhs_2/(slen_2 * slen_2))/(slen - slen_2);
|
|
bb = (-slen_2 * rhs_1/(slen * slen)+slen * rhs_2/(slen_2 * slen_2))/(slen - slen_2);
|
|
LDBG((" 2: rhs_1 %f rhs_2 %f aa %f bb %f)\n",rhs_1,rhs_2,aa,bb));
|
|
|
|
/* Denominator is zero */
|
|
if (aa == 0.0) {
|
|
slen_t = -slope/(2.0 * bb);
|
|
LDBG((" 3: aa == 0, slen_t %f = -slope %f/(2.0 * bb %f)\n",slen_t,-slope, bb));
|
|
} else {
|
|
double dd = bb * bb - 3.0 * aa * slope;
|
|
LDBG((" 2: dd == %f)\n",dd));
|
|
if (dd < 0.0) {
|
|
slen_t = 0.5 * slen;
|
|
LDBG((" 4: dd %f < 0, slen_t %f = 0.5 * slen %f)\n",dd, slen_t, slen));
|
|
} else if (bb <= 0.0) {
|
|
slen_t = (-bb + sqrt(dd))/(3.0 * aa);
|
|
LDBG((" 5: bb %f <= 0.0, slen_t %f = (-bb %f + sqrt(dd %f))/(3.0 * aa %f)\n",bb, slen_t, -bb, dd, aa));
|
|
} else {
|
|
slen_t = -slope/(bb + sqrt(dd));
|
|
LDBG((" 6: bb %f > 0.0, slen_t %f = -slope %f /(bb %f + sqrt(dd %f)\n",bb, slen_t, -slope, bb, dd));
|
|
}
|
|
}
|
|
if (slen_t > 0.5 * slen) {
|
|
LDBG((" 7: slen_t %f > (0.5 * slen %f = %f)\n",slen_t, slen, 0.5 * slen));
|
|
slen_t = 0.5 * slen;
|
|
}
|
|
}
|
|
}
|
|
fp_2 = *pfp;
|
|
slen_2 = slen;
|
|
slen = FMAX(slen_t, 0.1 * slen);
|
|
}
|
|
|
|
#ifdef PLOTL /* Plot the step direction */
|
|
do_return:;
|
|
{
|
|
# define RES 51
|
|
double x1, x2;
|
|
double xx[RES];
|
|
double y1[RES];
|
|
int j;
|
|
|
|
/* Plot range */
|
|
x1 = log10(0.00001);
|
|
x2 = log10(1.0);
|
|
|
|
printf("Computing plot points:\n");
|
|
for (j = 0; j < RES; j++) {
|
|
double cp[24];
|
|
double vv = j/(RES-1.0) * (x2 - x1) + x1;
|
|
double vl;
|
|
|
|
vl = pow(10.0, vv);
|
|
|
|
for (i = 0; i < di;i++)
|
|
cp[i] = cpold[i] + vl * sdir[i];
|
|
|
|
xx[j] = vv;
|
|
y1[j] = (*func)(fdata, cp);
|
|
}
|
|
printf(" log10 step = %f:\n",log10(slen));
|
|
do_plot(xx, y1, NULL, NULL, RES);
|
|
}
|
|
#endif /* PLOTL */
|
|
|
|
}
|
|
|