localizem.nc

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

/*
 * LocalizeM.nc
 * David Moore <[e-mail]>
 *
 * Module for localizing sensor nodes given distance measurements
 * between the nodes.  The are two primary algorithms in use here:
 *
 * Static nodes are localized using the principle of robust quads.
 *
 * Mobile nodes are localized using least-squares.
 * 
 *
 * 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 LocalizeM {
    provides {
        interface LocalizeStatic;
        interface LocalizeMoving;
    }
    uses {
        interface NonLinMin;
    }
}

/* The threshold for triangle robustness in units of length:
 * sound-microseconds.  Ideally, this value should be about three
 * standard deviations of the measurement noise of the system. */
#define ROBUST_THRESH        90

implementation {
    /* Distances between each pair of nodes.  This array does not
     * need to be complete.  Any element set to zero is assumed
     * to be not present.  Since each pair appears twice in this
     * 2-D array, we use half for temporary storage and the other
     * half for storage of the original values. */
    uint16_t d_norm[MAX_NEIGHBORS+1][MAX_NEIGHBORS+1];

    /* Maps node indices for our internal use to node indices used
     * by external modules in the system. */
    uint8_t nodes[MAX_NEIGHBORS+1];

    /* Pointer to other information about each node (provided by
     * the GetDistances() callback). */
    struct NodeInfo * info;

    /* Number of nodes available for localization. */
    uint8_t norm_count;

    /* Head/tail pointers for the robust quad queue */
#ifdef PLATFORM_PC
#define QUEUE_LEN 800
    uint16_t qhead, qtail;
#else
#define QUEUE_LEN 200
    uint8_t qhead, qtail;
#endif
    /* A chunk of temporary memory which we either use for a queue
     * of robust quads or storage needed by the least-squares
     * solver. */
    union {
        /* The queue of robust quads */
        struct quad {
            uint8_t v[3];   // 3 vertices of the quad (4th is always
                            // the cluster origin)
        } q[QUEUE_LEN];

        struct nonlin {
            float p[MAX_NEIGHBORS*2];
            float g[MAX_NEIGHBORS*2];
            float h[MAX_NEIGHBORS*2];
            float xi[MAX_NEIGHBORS*2];
            uint8_t i[MAX_NEIGHBORS+1];
        } nl;
    } m;

    /* Array of localized positions.  There are two copies since we have
     * to make several passes until the best set of localizations is
     * found. */
    struct node_pos {
        uint8_t set;        // whether a position is set
        double x, y;        // the localized position
    } pos[2][MAX_NEIGHBORS];

    /* Number of localized positions in pos[] */
    uint8_t pos_num[2];


#define MIN(x,y)    (((x)<(y))?(x):(y))
#define MAX(x,y)    (((x)>(y))?(x):(y))

    /* Given the lengths of the three sides of a triangle, return 1
     * if the triangle is robust. */
    uint8_t is_robust(uint16_t a, uint16_t b, uint16_t c)
    {
        double min, d, e;
        double costh;

        if (a <= b && a <= c)
            min = a, d = b, e = c;
        else if (b <= a && b <= c)
            min = b, d = a, e = c;
        else
            min = c, d = a, e = b;

        costh = (square(d)+square(e)-square(min))/(2.0*d*e);
        if (min*(1-square(costh)) < ROBUST_THRESH)
            return 0;
        else
            return 1;
    }

    /* Initialize the ring-buffer holding a queue of robust quads */
    void ring_init()
    {
        qhead = qtail = 0;
    }

    /* Return 1 if the queue is empty */
    uint8_t ring_empty()
    {
        return (qhead == qtail);
    }

    /* Enqueue an element (3 vertices) onto the quad queue. */
    result_t ring_enqueue(uint8_t i, uint8_t j, uint8_t k)
    {
        if ((qhead+1) == qtail || (qhead==(QUEUE_LEN-1) && qtail==0)) {
            UARTOutput(OUT_ERROR, "ring_enqueue(): queue is full\n");
            return FAIL;
        }
        m.q[qhead].v[0] = i;
        m.q[qhead].v[1] = j;
        m.q[qhead].v[2] = k;
        qhead++;
        if (qhead == QUEUE_LEN)
            qhead = 0;
        return SUCCESS;
    }

    /* Dequeue an element (3 vertices) from the quad queue. */
    result_t ring_dequeue(uint8_t * v)
    {
        if (ring_empty())
            return FAIL;

        v[0] = m.q[qtail].v[0];
        v[1] = m.q[qtail].v[1];
        v[2] = m.q[qtail].v[2];
        qtail++;
        if (qtail == QUEUE_LEN)
            qtail = 0;
        return SUCCESS;
    }

#define ERROR_NONINTERSECT  0xfe
#define ERROR_BASELINE      0xff
    
/* Since the origin of the cluster is always at (0,0) and we don't
 * want to waste storage for that, this macro provides a convenience
 * for reading the coordinates of an arbitrary node. */
#define GET_POS(_i,_x,_y) \
    do { \
        if ((_i) == norm_count) { \
            _x = 0; \
            _y = 0; \
        } else { \
            _x = pos[1][_i].x; \
            _y = pos[1][_i].y; \
        } \
    } while (0)

    /* Solves for the position of a node using the known positions
     * of three nodes and three distances to those nodes.  The
     * technique here is quite simplified -- the distances and positions
     * of the first two nodes define two circles that intersect (in
     * the normal case) at two points.  We choose one of these two
     * points as the answer based on the distance to third node.
     * This is just a helper function for trilaterate() which does
     * it more accurately.
     *
     * Arguments:
     *      i1, i2, i3      Indices of the three known nodes
     *      r1, r2, r3      Distances to the three nodes
     *      x, y            Return pointers for the solved position
     */
    uint8_t trilaterate1(uint8_t i1, uint8_t i2, uint8_t i3,
            uint16_t r1, uint16_t r2, uint16_t r3,
            double * x, double * y)
    {
        double dsq, gam, xa, ya, xb, yb, xt1, xt2, yt1, yt2, d1, d2;
        double x_1, y_1, x_2, y_2, x_3, y_3;
        uint8_t ret;

        /* Get the positions of the three known nodes */
        GET_POS(i1, x_1, y_1);
        GET_POS(i2, x_2, y_2);
        GET_POS(i3, x_3, y_3);
        
        /* Find the two intersection points of the two circles. */
        dsq = square(x_2-x_1) + square(y_2-y_1);
        gam = (square((double)r2+r1) - dsq)*(dsq - square((double)r2-r1));
        if (gam < 0) {
            UARTOutput(OUT_DEBUG, "Nonintersecting circles\n");
            return ERROR_NONINTERSECT;
        }
        gam = sqrt(gam);
        xa = -(square(r2)-square(r1))*(x_2-x_1)/(2.0*dsq) + (x_1+x_2)*0.5;
        ya = -(square(r2)-square(r1))*(y_2-y_1)/(2.0*dsq) + (y_1+y_2)*0.5;
        xb = (y_2-y_1)*gam/(2.0*dsq);
        yb = (x_2-x_1)*gam/(2.0*dsq);
        xt1 = xa - xb;
        xt2 = xa + xb;
        yt1 = ya + yb;
        yt2 = ya - yb;
        
        /* Disambiguate between the two points using the third node. */
        d1 = sqrt(square(xt1-x_3) + square(yt1-y_3));
        d2 = sqrt(square(xt2-x_3) + square(yt2-y_3));
        if (fabs(d1-r3) < fabs(d2-r3)) {
            *x = xt1;
            *y = yt1;
        }
        else {
            *x = xt2;
            *y = yt2;
        }

        /* The three nodes of known position define a triangle in 2-space.
         * Consider the three sides of this triangle, and extend the
         * length of each edge to infinity.  This divides the plane
         * into 7 distinct regions.  The position we computed above is
         * in one of these 7 regions.  The point of all the computation
         * below is to compute which region.  We do this by finding
         * "which side" we are on of each edge of the triangle.  This is
         * done for each of the three edges and the result is a 3-bit
         * bitmask where each bit tells us which side we are on.  This
         * is the return value of the function.
         *
         * We also check if the solved position is "close" to one of the
         * edges (within 2 standard deviations of the measurement noise).
         * If so, we return an error that indicates potential for
         * localization ambiguity.
         */
        gam = (x_2-x_1)*(*y-y_1) - (y_2-y_1)*(*x-x_1);
        if (fabs(gam)/sqrt(dsq) < 2*ROBUST_THRESH)
            return ERROR_BASELINE;
        ret = (gam > 0);

        gam = (x_3-x_1)*(*y-y_1) - (y_3-y_1)*(*x-x_1);
        dsq = square(x_3-x_1) + square(y_3-y_1);
        if (fabs(gam)/sqrt(dsq) < 2*ROBUST_THRESH)
            return ERROR_BASELINE;
        ret |= (gam > 0) << 1;

        gam = (x_3-x_2)*(*y-y_2) - (y_3-y_2)*(*x-x_2);
        dsq = square(x_3-x_2) + square(y_3-y_2);
        if (fabs(gam)/sqrt(dsq) < 2*ROBUST_THRESH)
            return ERROR_BASELINE;
        ret |= (gam > 0) << 2;
        return ret;
    }

    /* Solves for the position of a node using the known positions
     * of three nodes and three distances to those nodes.  This
     * function basically evaluates trilaterate1() thrice, finding
     * the centroid of the three positions returned by trilaterate1().
     *
     * Arguments:
     *      i1, i2, i3      Indices of the three known nodes
     *      r1, r2, r3      Distances to the three nodes
     *      x, y            Return pointers for the solved position
     */
    uint8_t trilaterate(uint8_t i1, uint8_t i2, uint8_t i3,
            uint16_t r1, uint16_t r2, uint16_t r3,
            double * x, double * y)
    {
        double xt, yt;
        uint8_t ret, bl, bad=0;
        ret = trilaterate1(i1, i2, i3, r1, r2, r3, x, y);
        if (ret == ERROR_NONINTERSECT)
            return ERROR_NONINTERSECT;
        if (ret == ERROR_BASELINE)
            bad = 1;
        bl = ((ret >> 1) & 1) | ((ret << 1) & 2) | (~ret & 4);
        ret = trilaterate1(i1, i3, i2, r1, r3, r2, &xt, &yt);
        if (ret == ERROR_NONINTERSECT)
            return ERROR_NONINTERSECT;
        /* In addition to checking for a baseline error, we make sure
         * that each trilateration solves for a position in the same
         * region as the last (see comments in trilaterate1() about the
         * 7 possible regions).  That's what all this bitshifting
         * nonsense is about. */
        if (ret == ERROR_BASELINE || ret != bl)
            bad = 1;
        *x += xt;
        *y += yt;
        bl = ((~bl >> 2) & 1) | (~bl & 2) | ((~bl << 2) & 4);
        ret = trilaterate1(i2, i3, i1, r2, r3, r1, &xt, &yt);
        if (ret == ERROR_NONINTERSECT)
            return ERROR_NONINTERSECT;
        if (ret == ERROR_BASELINE || ret != bl)
            bad = 1;
        *x += xt;
        *y += yt;
        *x /= 3.0;
        *y /= 3.0;
        /* In the case of baseline errors, we still solve for the
         * localization, but we note the error for higher-level
         * processing. */
        if (bad)
            return ERROR_BASELINE;

        return 0;
    }

#define LOC_NONE    0
#define LOC_ROBUST  1
#define LOC_WEAK    2

#define GET_D(i,j)  (((i)<(j))?d_norm[i][j]:d_norm[j][i])
#define GET_D_ORIG(i,j)  (((i)>(j))?d_norm[i][j]:d_norm[j][i])
#define CLEAR_D(i,j)  (((i)<(j))?(d_norm[i][j]=0):(d_norm[j][i]=0))

    /* Initialize the first few three nodes of a localization.  This
     * assumes they have already been checked for robustness and
     * connectivity.  We merely solve for their underconstrained
     * positions, thus defining a local coordinate system.
     *
     * Arguments:
     *      i, j, k         The indices of the origin and two other nodes
     *      dij, dik, djk   Distances between nodes
     */
    void init_pos(uint8_t i, uint8_t j, uint8_t k,
            uint16_t dij, uint16_t dik, uint16_t djk)
    {
        double alpha;
        uint8_t l;

        /* Reset the position array */
        for (l=0; l<MAX_NEIGHBORS; l++) {
            pos[1][l].set = LOC_NONE;
        }

        /* The first node defines (0,0) implicitly */
        
        /* The second node defines the x-axis */
        pos[1][j].set = LOC_ROBUST;
        pos[1][j].x = dij;
        pos[1][j].y = 0.0;

        /* The third node defines the positive direction of the y-axis.
         * Its position is computed using law of cosines. */
        alpha = (square(dij)+square(dik)-square(djk))/(2.0*dij*dik);
        pos[1][k].set = LOC_ROBUST;
        pos[1][k].x = alpha*dik;                    /* dik * cos(th) */
        alpha = 1 - square(alpha);
        if (alpha < 0)
            UARTOutput(OUT_WARNING, "Warning: negative square root\n");
        pos[1][k].y = sqrt(alpha)*dik;    /* dik * sin(th) */

        pos_num[1] = 2;
        
    }

    /* Check the temporary list of positions with the best-so-far
     * list of positions.  If the temporary list is better, use it
     * instead. */
    void update_pos() {
        if (pos_num[1] > pos_num[0]) {
            /* If the temporary list has more localized positions,
             * replace the best-so-far list. */
            pos_num[0] = pos_num[1];
            memcpy(pos[0], pos[1], sizeof(pos[1]));
            UARTOutput(OUT_DEBUG, "Solved for %d (best)\n", pos_num[1]);
        }
        else
            UARTOutput(OUT_DEBUG, "Solved for %d\n", pos_num[1]);
    }

    /* Given a new quad that we've encountered in the breadth-first
     * search:
     *  1) check if it's robust
     *  2) if so, trilaterate the unknown vertex and enqueue the
     *      quad for later use in the BFS.
     * If the quad is not robust, we can still trilaterate the unknown
     * vertex, but we don't enqueue it.
     * Also, if the trilateration is found to have a baseline ambiguity
     * we don't enqueue it.
     *
     * Arguments:
     *      i,j,k,l         The four vertices of the quad
     *      djk, dij, etc.  The six distances of the quad
     */
    void check_quad_and_enqueue(uint8_t i, uint8_t j, uint8_t k, uint8_t l,
            uint16_t djk, uint16_t dij, uint16_t dik,
            uint16_t dkl, uint16_t dlj, uint16_t dil)
    {
        double x, y;
        uint8_t ret;

        /* Check if three of the quads triangles are robust.  The fourth
         * triangle has already been checked in earlier stages of the
         * algorithm. */
        if (is_robust(dij,dil,dlj) &&
                is_robust(dik,dil,dkl) && is_robust(djk,dkl,dlj)) {
            /* Only bother trilaterating if the node's position isn't