/** * Non-linear minimization module * David Moore <dcm@csail.mit.edu> * * This module provides non-linear minimization of an arbitrary n-dimensional * function using the Polak-Ribiere conjugate gradient method.  This is * similar to the straightforward steepest descent method, but is more robust. * * The code herein was based on the reference implementation provided by * _Numerical Recipes in C: The Art of Scientific Computing_ pp. 420-425 * (ISBN 0-521-43108-5). * * * Copyright (C) 2004  Massachusetts Institute of Technology * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA. */module NonLinMinM {    provides {        interface NonLinMin[uint8_t id];    }}implementation {    void dlinmin(uint8_t id, float p[], float xi[], int n, float * fret);    float mnbrak(uint8_t id, float p[], float xi[], float * ax, float * bx,            float * cx, float * fp);    float dbrent(uint8_t id, float p[], float xi[], float ax, float bx, float cx,            float tol, float * xmin, float * fx);    default event float NonLinMin.Func[uint8_t id](float p[], float x,            float xi[])    {        return 0.0;    }    default event float NonLinMin.DFunc[uint8_t id](float p[], float x,            float xi[], float df[])    {        return 0.0;    }#define EPS 0.1#define ITMAX   200    command void NonLinMin.FindMin[uint8_t id](float p[], int n, float ftol,            float * fret, float g[], float h[], float xi[])    {        int j, its;        float gg, gam, fp, dgg;                *fret = fp = signal NonLinMin.Func[id](p, 0, NULL);        signal NonLinMin.DFunc[id](p, 0, NULL, xi);        for (j=0; j<n; j++) {            g[j] = -xi[j];            xi[j] = h[j] = g[j];        }        for (its=0; its<ITMAX; its++) {            dlinmin(id, p, xi, n, fret);            if (2.0 * fabs(*fret-fp) <= ftol*(fabs(*fret)+fabs(fp)+EPS))                return;            fp = *fret;            signal NonLinMin.DFunc[id](p, 0, NULL, xi);            dgg = gg = 0.0;            for (j=0; j<n; j++) {                gg += g[j]*g[j];                dgg += (xi[j]+g[j])*xi[j];            }            if (gg == 0.0)                return;            gam = dgg/gg;            for (j=0; j<n; j++) {                g[j] = -xi[j];                xi[j] = h[j] = g[j] + gam*h[j];            }        }        UARTOutput(OUT_ERROR, "FindMin: Too many iterations\n");    }#define TOL 2.0e-2    void dlinmin(uint8_t id, float p[], float xi[], int n, float * fret)    {        int j;        float xx, xmin, bx, ax;        *fret = mnbrak(id, p, xi, &ax, &xx, &bx, fret);        *fret = dbrent(id, p, xi, ax, xx, bx, TOL, &xmin, fret);        for (j=0; j<n; j++) {            p[j] += xmin*xi[j];        }    }#define SWAP(a,b)   do { dum=(a); (a)=(b); (b)=dum; } while(0)#define SIGN(a,b)   ((b>=0)?fabs(a):-fabs(a))#define FMAX(a,b)   ((a>b)?a:b)#define GOLD 1.618034#define GLIMIT  100.0#define TINY    1.0e-20#define ZEPS    1.0e-10        float mnbrak(uint8_t id, float p[], float xi[], float * ax, float * bx,            float * cx, float * fp)    {        float ulim, u, r, q, fu, dum;        float fa, fb, fc;        *ax = 0.0;        *bx = 1.0;        if (fp)            fa = *fp;        else            fa = signal NonLinMin.Func[id](p, 0, NULL);        fb = signal NonLinMin.Func[id](p, *bx, xi);        if (fb > fa) {            SWAP(*ax, *bx);            SWAP(fb, fa);        }        *cx = (*bx)+GOLD*(*bx-*ax);        fc = signal NonLinMin.Func[id](p, *cx, xi);        while (fb > fc) {            r = (*bx-*ax)*(fb-fc);            q = (*bx-*cx)*(fb-fa);            u = (*bx)-((*bx-*cx)*q - (*bx-*ax)*r)/(2.0*SIGN(FMAX(fabs(q-r),TINY),q-r));            ulim = (*bx)+GLIMIT*(*cx-*bx);            if ((*bx-u)*(u-*cx) > 0.0) {                fu = signal NonLinMin.Func[id](p, u, xi);                if (fu < fc) {                    *ax = *bx;                    *bx = u;                    return fu;                }                else if (fu > fb) {                    *cx = u;                    return fb;                }                u = *cx + GOLD*(*cx-*bx);                fu = signal NonLinMin.Func[id](p, u, xi);            }            else if ((*cx-u)*(u-ulim) > 0.0) {                fu = signal NonLinMin.Func[id](p, u, xi);                if (fu < fc) {                    *bx = *cx;                    *cx = u;                    u = *cx + GOLD*(*cx-*bx);                    fb = fc;                    fc = fu;                    fu = signal NonLinMin.Func[id](p, u, xi);                }            }            else if ((u-ulim)*(ulim-*cx) >= 0.0) {                u = ulim;                fu = signal NonLinMin.Func[id](p, u, xi);            }            else {                u = *cx + GOLD*(*cx-*bx);                fu = signal NonLinMin.Func[id](p, u, xi);            }            *ax = *bx;            *bx = *cx;            *cx = u;            fa = fb;            fb = fc;            fc = fu;        }        return fb;    }    float dbrent(uint8_t id, float p[], float xi[], float ax, float bx, float cx,            float tol, float * xmin, float * fbx)    {        int iter, ok1, ok2;        float a, b, d=0, d1, d2, du, dv, dw, dx, e=0.0;        float fu, fv, fw, fx, olde, tol1, tol2, u, u1, u2, v, w, x, xm;        a = (ax < cx ? ax : cx);        b = (ax > cx ? ax : cx);        x = w = v = bx;        if (fbx)            fw = fv = fx = *fbx;        else            fw = fv = fx = signal NonLinMin.Func[id](p, x, xi);        dw = dv = dx = signal NonLinMin.DFunc[id](p, x, xi, NULL);        for (iter = 0; iter < ITMAX; iter++) {            xm = 0.5*(a+b);            tol1 = tol*fabs(x)+ZEPS;            tol2 = 2.0*tol1;            if (fabs(x-xm) <= (tol2-0.5*(b-a))) {                *xmin = x;                return fx;            }            if (fabs(e) > tol1) {                d1 = 2.0*(b-a);                d2 = d1;                if (dw != dx) d1=(w-x)*dx/(dx-dw);                if (dv != dx) d2=(v-x)*dx/(dx-dv);                u1 = x+d1;                u2 = x+d2;                ok1 = (a-u1)*(u1-b) > 0.0 && dx*d1 <= 0.0;                ok2 = (a-u2)*(u2-b) > 0.0 && dx*d2 <= 0.0;                olde = e;                e = d;                if (ok1 || ok2) {                    if (ok1 && ok2)                        d = (fabs(d1) < fabs(d2) ? d1 : d2);                    else if (ok1)                        d = d1;                    else                        d = d2;                    if (fabs(d) <= fabs(0.5*olde)) {                        u = x+d;                        if (u-a < tol2 || b-u < tol2)                            d = SIGN(tol1, xm-x);                    }                    else {                        d = 0.5*(e=(dx >= 0.0 ? a-x : b-x));                    }                }                else {                    d = 0.5*(e=(dx >= 0.0 ? a-x : b-x));                }            }            else {                d = 0.5*(e=(dx >= 0.0 ? a-x : b-x));            }            if (fabs(d) >= tol1) {                u = x+d;                fu = signal NonLinMin.Func[id](p, u, xi);            }            else {                u = x+SIGN(tol1, d);                fu = signal NonLinMin.Func[id](p, u, xi);                if (fu > fx) {                    *xmin = x;                    return fx;                }            }            du = signal NonLinMin.DFunc[id](p, u, xi, NULL);            if (fu <= fx) {                if (u >= x) a=x; else b=x;                v = w;                fv = fw;                dv = dw;                w = x;                fw = fx;                dw = dx;                x = u;                fx = fu;                dx = du;            }            else {                if (u < x) a=u; else b=u;                if (fu <= fw || w == x) {                    v = w;                    fv = fw;                    dv = dw;                    w = u;                    fw = fu;                    dw = du;                }                else if (fu < fv || v == x || v == w) {                    v = u;                    fv = fu;                    dv = du;                }            }        }        UARTOutput(OUT_ERROR, "dbrent: Too many iterations\n");        return 0.0;    }}