nonlinminm.nc

无线传感器网络中的节点定位算法。详见ReadMe文件。在TinyOS上实现的节点定位算法。

/**
 * Non-linear minimization module
 * David Moore <[e-mail]>
 *
 * 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;
    }
}