localizem.nc

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

             * already known. */
            if (pos[1][l].set != LOC_ROBUST) {
                ret = trilaterate(i, j, k, dil, dlj, dkl, &x, &y);
                if (!ret) {
                    pos[1][l].set = LOC_ROBUST;
                    pos[1][l].x = x;
                    pos[1][l].y = y;
                    pos_num[1]++;
                }
                /* If there's a baseline ambiguity, store the solved position
                 * but return immediately without enqueuing the quad. */
                else if (ret == ERROR_BASELINE && pos[1][l].set == LOC_NONE) {
                    pos[1][l].set = LOC_WEAK;
                    pos[1][l].x = x;
                    pos[1][l].y = y;
                    UARTOutput(OUT_DEBUG, "Weak trilat of %d\n", nodes[l]);
                    return;
                }
                else {
                    UARTOutput(OUT_DEBUG, "Trilat of %d rejected\n",
                            nodes[l]);
                    return;
                }
            }
            UARTOutput(OUT_DEBUG, "RQ: %02d %02d %02d\n",
                    nodes[j], nodes[k], nodes[l]);
            
            /* Only use for further trilateration if node is not moving */
            if (!(info[nodes[l]].status & IS_MOVING))
                ring_enqueue(j, k, l);
        }
        else if (pos[1][l].set == LOC_NONE) {
            /* If the quad is not robust, we can still trilaterate, but
             * we don't use it for further localization (by not enqueuing
             * it). */
            ret = trilaterate(i, j, k, dil, dlj, dkl, &x, &y);
            if (!ret) {
                pos[1][l].set = LOC_WEAK;
                pos[1][l].x = x;
                pos[1][l].y = y;
                UARTOutput(OUT_DEBUG, "Very weak trilat of %d\n", nodes[l]);
            }
        }
    }
    
    /* The heart of the localization algorithm.  This function does
     * a breadth-first search into the graph by following robust
     * quads that overlap by three nodes.  Thus, by starting with
     * known positions of three nodes in the quad, we are able to
     * localize the fourth, and then continue the search until a
     * maximum number of nodes have been localized.  Calling
     * this function assumes that the queue has already been
     * initialized with at least one quad, and those quads in the
     * queue have three nodes with known positions.
     * 
     * Arguments:
     *      i           The origin node (present in all quads)
     */
    void quad_bfs(uint8_t i)
    {
        uint8_t v[3];
        uint8_t p, l;
        uint16_t djk, dij, dik, dkl, dlj, dil;
        
        /* Continue the search until no more quads are available */
        while (!ring_empty()) {
            ring_dequeue(v);
            /* Any quad we dequeue has already been localized when
             * it was enqueued.  Thus we immediately begin by
             * enumerating all quads that border the dequeued quad.
             * Consider the triangle composed of nodes in the quad
             * that are _not_ the origin node.  This triangle has
             * three edges.  One edge has already has all its bordering
             * quads fully enumerated in previous passes.  The other
             * two edges (adjacent to the most recently localized node)
             * have not, so we enumerate all their bordering quads. */

            /* Loop through the two un-enumerated edges */
            for (p=0; p<2; p++) {
                /* Get the first three edges of any bordering quad.
                 * These edges are present in _all_ bordering quads. */
                djk = GET_D_ORIG(v[!p],v[2]);
                dij = GET_D(i,v[!p]);
                dik = GET_D(i,v[2]);
                /* Loop through all nodes as the possible fourth node
                 * in the quad. */
                for (l=0; l<norm_count; l++) {
                    /* Fourth node must be different than the first three */
                    if (l==v[2] || l==v[!p])
                        continue;
                    dkl = GET_D(l,v[2]);
                    dlj = GET_D(l,v[!p]);
                    /* Fourth node must form a fully-connected quad */
                    if (dkl==0 || dlj==0)
                        continue;
                    dil = GET_D(i,l);
                    if (dil == 0)
                        continue;
                    /* Check the quad for robustness, localize the
                     * fourth node if possible, and enqueue it. */
                    check_quad_and_enqueue(i, v[!p], v[2], l,
                            djk, dij, dik, dkl, dlj, dil);

                    /* Let any pending tasks run */
                    TOSH_flush_tasks();
                }
                /* Since we've enumerated all quads bordering this edge,
                 * we clear it to avoid searching the same edge in the
                 * future. */
                CLEAR_D(v[!p],v[2]);
            }
            /* Abort the search early if all nodes have been localized */
            if (pos_num[1] == norm_count)
                break;
        }
    }

    /* The starting stage of the localization algorithm.  Picks a
     * robust quad as a starting point and then continues the
     * localization from there.  Other starting points are also
     * considered in case they yield more localizations. */
    void FindQuads()
    {
        uint8_t j, k, l;
        uint8_t i = norm_count;

        UARTOutput(OUT_DEBUG, "Localizing...\n");

        pos_num[0] = 0;
        ring_init();

        /* Loop through all neighboring nodes as a possible second node */
        for (j=0; j<norm_count; j++) {
            uint16_t dij = GET_D(i,j);
            /* Only consider nodes with a distance to the origin */
            if (dij == 0)
                continue;
            /* Only consider static nodes */
            if (info[nodes[j]].status & IS_MOVING)
                continue;
            /* Loop through all nodes as a possible third node */
            for (k=0; k<norm_count; k++) {
                uint16_t djk = GET_D(j,k);
                uint16_t dik = GET_D(i,k);
                /* Only consider third nodes that form a fully-connected
                 * triangle with the origin and second node. */
                if (k==j || djk==0 || dik==0)
                    continue;
                if (info[nodes[k]].status & IS_MOVING)
                    continue;
                /* The triangle must also be robust */
                if (!is_robust(dij,dik,djk))
                    continue;
                /* Three nodes are enough to define the coordinate system,
                 * so we do it. */
                init_pos(i, j, k, dij, dik, djk);

                /* Loop through all nodes as a possible fourth node */
                for (l=0; l<norm_count; l++) {
                    uint16_t dkl = GET_D(k,l);
                    uint16_t dlj, dil;
                    /* Only consider fourth nodes that form a fully-connected
                     * quad with the origin, first, and second nodes. */
                    if (l==k || l==j || dkl==0)
                        continue;
                    dlj = GET_D(l,j);
                    if (dlj == 0)
                        continue;
                    dil = GET_D(i,l);
                    if (dil == 0)
                        continue;
                    /* Check the quad for robustness, localize it, and
                     * enqueue if possible. */
                    check_quad_and_enqueue(i, j, k, l,
                            djk, dij, dik, dkl, dlj, dil);
                }
                /* Since we've now enumerated all quads that contain
                 * edge j-k, we remove it from future consideration. */
                CLEAR_D(j,k);

                /* Do the breadth-first search, localizing as many
                 * nodes as possible with this starting point. */
                if (!ring_empty())
                    quad_bfs(i);

                /* Update the localization state and abort if we've
                 * localized all nodes. */
                update_pos();
                if (pos_num[0] == norm_count)
                    break;
            }
            if (pos_num[0] == norm_count)
                break;
        }

        /* Give other tasks a chance to run. */
        TOSH_flush_tasks();

        /* Print localization results */
        UARTOutput(OUT_INFO, "Localized %d of %d neighbors:\n",
                pos_num[0], norm_count);
        if (pos_num[0] > 0) {
            for (l=0; l<norm_count; l++) {
                if (pos[0][l].set) {
                    UARTOutput(OUT_INFO, "  %02d: x=%5.1f  y=%5.1f ",
                            nodes[l], pos[0][l].x/DISTANCE_MULT_US,
                            pos[0][l].y/DISTANCE_MULT_US);
                    if (pos[0][l].set == 2)
                        UARTOutput(OUT_INFO, "*");
                    if (info[nodes[l]].status & IS_MOVING) {
                        UARTOutput(OUT_INFO, "m");
                    }
                    UARTOutput(OUT_INFO, "\n");
                }
            }
        }
        UARTOutput(OUT_INFO, "done\n");
    }


    /* The following section is for localization of mobile nodes. */

    uint8_t opt_node;
    uint16_t * opt_dist;

    /* Given a mobile node and an array of distances to that mobile
     * node, localize it using least-squares. */
    command result_t LocalizeMoving.TrackMoving(uint8_t node, uint16_t * dist)
    {
        float fret;
        int n, i;
        /* Determine the index of the node in our internal list. */
        for (i=0; i<norm_count; i++) {
            if (nodes[i] == node) {
                opt_node = i;
                break;
            }
        }
        if (i == norm_count) {
            UARTOutput(OUT_ERROR, "Failed to find localized node %d\n", node);
            return FAIL;
        }
        
        /* Count the number of distance measurements which we can
         * actually use (we must know the position of the node from
         * which the distance was measured). */
        n = 0;
        for (i=0; i<norm_count; i++) {
            if (pos[0][i].set == LOC_ROBUST && dist[nodes[i]] != 0)
                n++;
        }
        if (dist[nodes[norm_count]] != 0)
            n++;

        /* We require at least three useful distances in order to do
         * the localizatin (other the least-squares solution is
         * degenerated). */
        if (n < 3) {
            UARTOutput(OUT_ERROR, "Not enough reference nodes for tracking\n");
            return FAIL;
        }
        
        opt_dist = dist;
        /* Fill in the "p" array with our initial guess for the parameters
         * being optimized by the least-squares solver. */
        if (pos[0][opt_node].set) {
            m.nl.p[0] = pos[0][opt_node].x;
            m.nl.p[1] = pos[0][opt_node].y;
        }
        else {
            m.nl.p[0] = 10;
            m.nl.p[1] = 10;
        }
        /* Do the least-squares minimization.  The minimization uses
         * the callbacks, below, to actually evaluate the function. */
        call NonLinMin.FindMin(m.nl.p, 2, 0.001, &fret,
                m.nl.g, m.nl.h, m.nl.xi);

        /* Print the results */
        UARTOutput(OUT_INFO, "Optimized, sumsq=%9.1f, n=%d:\n", fret, n);
        UARTOutput(OUT_INFO, "  %02d: x=%5.1f  y=%5.1f\n", nodes[opt_node],
                m.nl.p[0]/DISTANCE_MULT_US, m.nl.p[1]/DISTANCE_MULT_US);
        UARTOutput(OUT_INFO, "opt done\n");
        return SUCCESS;
    }

    /* Callback used by NonLinMin.FindMin() to evaluate the function
     * to be minimized.  For the peculiarities of its usage, see the
     * comments in NonLinMin.nc. */
    event float NonLinMin.Func(float p[], float x, float xi[])
    {
        float sumsq = 0.0;
        float xdif, ydif, x_1=0, y_1=0;
        uint8_t j, n;
        uint16_t d;

        TOSH_flush_tasks();

        /* The current estimate of the parameters */
        if (xi) {
            x_1 = p[0]+ x*xi[0];
            y_1 = p[1]+ x*xi[1];
        }
        else {
            x_1 = p[0];
            y_1 = p[1];
        }

        /* Sum the square errors between the measured distances and
         * the distances computed by the current position estimate. */
        n = 0;
        for (j=0; j<norm_count; j++) {
            if (pos[0][j].set != LOC_ROBUST)
                continue;
            d = opt_dist[nodes[j]];
            if (d == 0)
                continue;
            xdif = x_1 - pos[0][j].x;
            ydif = y_1 - pos[0][j].y;
            sumsq += square(sqrt(square(xdif)+square(ydif)) - d);
            n++;
        }
        d = opt_dist[nodes[norm_count]];
        if (d != 0) {
            sumsq += square(sqrt(square(x_1)+square(y_1)) - d);
            n++;
        }
        return sumsq;
    }
    
    /* Callback used by NonLinMin.FindMin() to evaluate the partial
     * derivatives of the function to be minimized.  For the
     * peculiarities of its usage, see the comments in NonLinMin.nc. */
    event float NonLinMin.DFunc(float p[], float x, float xi[], float df[])
    {
        float sum = 0.0;
        float xdif, ydif, x_1=0, y_1=0, dx, dy, gam;
        uint8_t j, n;
        uint16_t d;

        TOSH_flush_tasks();

        /* The current estimate of the parameters */
        if (xi) {
            x_1 = p[0]+ x*xi[0];
            y_1 = p[1]+ x*xi[1];
        }
        else {
            x_1 = p[0];
            y_1 = p[1];
        }

        /* Evaluate the derivative of the sum of square errors with
         * respect to the two parameters. */
        n = 0;
        dx = 0.0;
        dy = 0.0;
        for (j=0; j<norm_count; j++) {
            if (pos[0][j].set != LOC_ROBUST)
                continue;
            d = opt_dist[nodes[j]];
            if (d == 0)
                continue;
            xdif = x_1 - pos[0][j].x;
            ydif = y_1 - pos[0][j].y;
            if (xdif == 0.0 && ydif == 0.0)
                UARTOutput(OUT_ERROR, "Divide by zero\n");
            gam = 2.0*(1.0 - d/sqrt(square(xdif)+square(ydif)));
            dx += gam*xdif;
            dy += gam*ydif;
            n++;
        }
        d = opt_dist[nodes[norm_count]];
        if (d != 0) {
            if (x_1 == 0.0 && y_1 == 0.0)
                UARTOutput(OUT_ERROR, "Divide by zero\n");
            gam = 2.0*(1.0 - d/sqrt(square(x_1)+square(y_1)));
            dx += gam*x_1;
            dy += gam*y_1;
            n++;
        }
        
        /* Return either the derivatives themselves or a linear
         * combination of them. */
        if (df) {
            df[0] = dx;
            df[1] = dy;
        }
        else if (xi) {
            sum += dx*xi[0] + dy*xi[1];
        }
        return sum;
    }

    default event int LocalizeStatic.GetDistances(uint8_t o[MAX_NEIGHBORS+1],
            struct NodeInfo ** nodeinfo,
            uint16_t dist[MAX_NEIGHBORS+1][MAX_NEIGHBORS+1])
    {
        return 0;
    }

    /* Do the static localization.  This task can potentially
     * take a _long_ time to run since there's a lot of computation.
     * Thus, throughout these operations we sprinkle TOSH_flush_tasks()
     * to allow short running tasks to cut in and do their thing before
     * proceeding. */
    command result_t LocalizeStatic.LocalizeStaticNodes()
    {
        /* Get matrix of distance measurements. */
        norm_count = signal LocalizeStatic.GetDistances(nodes, &info, d_norm);
        /* Let pending tasks go */
        TOSH_flush_tasks();
        /* Do the static localization */
        FindQuads();

        return SUCCESS;
    }

}