beacmanagem.nc

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

                filt[node].vel = v;

                /* Record the distance in the moving node tracker
                 * if necessary */
                if (neighbors[node].status & IS_MOVING)
                    new_tracked_dist(timestamp, node, SELF, a);
            }
            else {
                /* If the distance is static, force the velocity to zero. */
                filt[node].vel = 0;
            }

            /* Update step for the covariance */
            P[0] = (1.0 - K[0])*newP[0];
            P[1] = (1.0 - K[0])*newP[1];
            P[2] = -K[1]*newP[0] + newP[2];
            P[3] = -K[1]*newP[1] + newP[3];

            filt[node].meas_time = timestamp;
            filt[node].bad_count = 0;
        }
        else {
            /* Reject the measurement as an outlier */
            filt[node].bad_count++;
            if (neighbors[node].status & IS_MOVING)
                new_tracked_dist(timestamp, node, SELF, OUTLIER_DIST);
        }

        if (moving)
            UARTOutput(OUT_DEBUG, "filt:%02u,meas=%u,fdist=%d,fvel=%d,mdist=%.2f,clk=%u moving\n",
                    node, x, GET_DIST(SELF, node), filt[node].vel, mdist, timestamp);
        else
            UARTOutput(OUT_DEBUG, "filt:%02u,meas=%u,fdist=%d,fvel=%d,mdist=%.2f,clk=%u\n",
                    node, x, GET_DIST(SELF, node), filt[node].vel, mdist, timestamp);
        
        return SUCCESS;
    }

    /* Set the time at which we will perform the next static localization */
    command result_t BeacManage.SetLocTime(uint16_t t)
    {
        next_loc_time = t;
        return SUCCESS;
    }

    /* Stores a new relay packet from a neighbor that contains its
     * measurements to its neighbors. */
    command result_t BeacManage.NewRelay(uint8_t * id,
            uint8_t * buf, uint8_t len, uint16_t timediff)
    {
        uint8_t node, i;
        uint8_t stride;

        /* The first 4 bytes contain the ID of the neighbor.  If we don't
         * have a measurement to it yet, ignore the relayed data. */
        node = node_find(id);
        if (node == EMPTY)
            return FAIL;

        /* Clear the aging flag */
        neighbors[node].status &= ~OLD_RELAY;
        /* Indicate the relayed data is present */
        neighbors[node].status |= RELAY_DATA;

        /* Initialize the distances to zero (indicating no measurement) */
        for (i=0; i <= MAX_NEIGHBORS; i++)
            SET_DIST(node, i, 0);

        /* The rest of the packet contains either (ID,distance) pairs (for
         * static nodes) or (ID,distance,time) triples (for moving nodes).
         * Futhermore, moving nodes may repeat several distances to the
         * same node if they were recorded at different times. */
        for ( ;len > 0; len-=stride, buf+=stride) {
            int16_t d;

            /* Store the distance */
            d = *(int16_t *)(buf+4);

            /* The high bit of distance tells us if a time is present
             * or not. */
            if (!(d >> 15))
                stride = 8;
            else
                stride = 6;
            d &= 0x7fff;

            i = node_find(buf);
            if (i == EMPTY)
                continue;

            /* If a time is provided and the distance is moving,
             * add it to our list of distances to moving nodes for
             * tracking purposes. */
            if (stride == 8) {
                if ((neighbors[i].status & IS_MOVING) &&
                        !(neighbors[node].status & IS_MOVING))
                    new_tracked_dist(*(uint16_t *)(buf+6) + timediff, i,
                            node, d);
            }

            /* Record the distance for static purposes. */
            SET_DIST(node, i, d);
        }
        return SUCCESS;
    }

    /* Copy a (ID, distance, timestamp) triple into an outgoing radio
     * packet. */
    uint8_t copy_info(uint8_t ** buf, uint8_t size, uint16_t len, uint8_t * id,
            uint16_t dist, uint16_t ts)
    {
        if (size + 8 > len) {
            UARTOutput(OUT_WARNING, "Warning: Relay packet truncated\n");
            return size;
        }
        copy_id(*buf, id);
        dist &= 0x7fff;
        *(int16_t *)(*buf+4) = dist;
        *(uint16_t *)(*buf+6) = ts;
        *buf += 8;
        return size + 8;
    }

    /* Copy a (ID, distance) pair into an outgoing radio packet. */
    uint8_t copy_info_short(uint8_t ** buf, uint8_t size, uint16_t len, uint8_t * id,
            uint16_t dist)
    {
        if (size + 6 > len) {
            UARTOutput(OUT_WARNING, "Warning: Relay packet truncated\n");
            return size;
        }
        copy_id(*buf, id);
        *(int16_t *)(*buf+4) = dist | 0x8000;
        *buf += 6;
        return size + 6;
    }

    /* Fills a packet of data with distances measured locally suitable for
     * being relayed to neighboring nodes. */
    command uint8_t BeacManage.GetMeasurements(uint8_t * buf, uint16_t len)
    {
        uint8_t size = 0;
        uint8_t i, j;

        /* Fill the packet with (ID, distance) pairs for any neighbors we've
         * directly measured the distance to. */
        for (i=0; i<MAX_NEIGHBORS; i++) {
            if (neighbors[i].status != EMPTY) {
                if (!(neighbors[i].status & IS_MOVING)) {
                    /* Only the distance is copied for static nodes. */
                    size = copy_info_short(&buf, size, len, neighbors[i].id,
                            GET_DIST(SELF, i));
                }
                else {
                    /* If the node is moving, copy any recorded measurements
                     * to the node along with their timestamps. */
                    for (j=0; j<track_num; j++) {
                        uint8_t n = (track_start + j) % MAX_TRACK;
                        if (track[n].node != i || track[n].dist[SELF] == 0)
                            continue;
                        size = copy_info(&buf, size, len, neighbors[i].id,
                                track[n].dist[SELF], track[n].time);
                    }
                }
            }
        }
        return size;
    }

    /* Print the contents of the distance table to the serial port 
     * for all distances measured by a specific node. */
    void debug_print(int node)
    {
        uint8_t c, i;

        if (node == SELF) {
            UARTOutput(OUT_DEBUG, "  from %2u (self): ", node);
            i = 0;
        }
        else {
            UARTOutput(OUT_DEBUG, "  from %2u       : ", node);
            i = 0;
        }

        c=0;
        for (; i<=MAX_NEIGHBORS; i++) {
            if (GET_DIST(node,i) != 0 && neighbors[i].status != EMPTY) {
                if (c != 0 && ((c&0x3) == 0))
                    UARTOutput(OUT_DEBUG, "\n          ");
                UARTOutput(OUT_DEBUG, "%2u d=%5d; ", i, GET_DIST(node,i));
                c++;
            }
        }
        UARTOutput(OUT_DEBUG, "\n");
    }

    /* Print the entire contents of the distance table to the serial port
     * if id is NULL.  Otherwise, print only the distances measured by that
     * node. */
    command result_t BeacManage.DebugPrint(uint8_t * id)
    {
        uint8_t node;
        if (id) {
            node = node_find(id);
            if (node == EMPTY)
                return FAIL;
            if (neighbors[node].status & RELAY_DATA)
                debug_print(node);
            return SUCCESS;
        }
        
        for (node=0; node<=MAX_NEIGHBORS; node++) {
            if (neighbors[node].status != EMPTY &&
                    neighbors[node].status & RELAY_DATA) {
                debug_print(node);
                TOSH_flush_tasks();
            }
        }
        return SUCCESS;
    }
    
#define ABS(x)  ((((int16_t)(x))<0)?(-(x)):(x))

    /* This function is called by the localization code in order to
     * copy recorded distance measurements into a form that is
     * useful for localization.  Specifically, we discard nodes for
     * which we have no distance information or no relayed information.
     * We also average distances since we have two measurements for
     * each pair.
     *
     * Arguments:
     *      o[]         maps new indices to old indices (due to some
     *                  nodes being ignored)
     *      info[]      pointer to info about each node
     *      d_norm[][]  the "normalized" distances we compute
     */
    command int BeacManage.RefineDistances(uint8_t o[MAX_NEIGHBORS+1],
            struct NodeInfo ** info,
            uint16_t d_norm[MAX_NEIGHBORS+1][MAX_NEIGHBORS+1])
    {
        uint8_t i, j, c;
        uint16_t d1, d2;

        *info = neighbors;

        /* Look for any nodes that should be ignored due to lack
         * of data. */
        c = 0;
        for (i=0; i<MAX_NEIGHBORS; i++) {
            if (neighbors[i].status != EMPTY &&
                    neighbors[i].status & RELAY_DATA)
                /* Map the new index to the original index. */
                o[c++] = i;
        }
        o[c] = SELF;

        /* Consider each pair of nodes (with the new indices) */
        for (i=0; i<c; i++) {
            for (j=i+1; j<=c; j++) {
                uint8_t k = o[i];
                uint8_t l = o[j];
                /* If either we or our neighbor is moving, we use the
                 * filter state of that edge to predict the measurement
                 * at next_loc_time instead of when it was measured.
                 * 
                 * Note: this is a stupid idea.  Instead we should be
                 * using the consistent measurements for the moving
                 * node and localizing at _that_ time, not at
                 * next_loc_time. */
                if (l == SELF && ((neighbors[l].status & IS_MOVING) ||
                            (neighbors[k].status & IS_MOVING))) {
                    int32_t d;
                    int16_t age = (int16_t)(next_loc_time - filt[k].meas_time);
                    if (age < 1024 && age > -1024) {
                        d = (int32_t)GET_DIST(l,k) +
                            (int32_t)filt[k].vel*age/1024;
                        d2 = CLAMP(d, 0, MAX_DIST);
                        UARTOutput(OUT_DEBUG, "Pred:%02u,dist=%d,meas_time=%u,time=%u\n",
                                k, d2, filt[k].meas_time, next_loc_time);
                    }
                    else {
                        d2 = 0;
                    }
                    d1 = GET_DIST(k,l);
                }
                else {
                    d1 = GET_DIST(k,l);
                    d2 = GET_DIST(l,k);
                }
                
                /* If a node is moving, we only consider the distance
                 * measured by the static node (since the moving node
                 * probably has noise that disturbs its measurements). */
                if (neighbors[l].status & IS_MOVING) {
                    d_norm[i][j] = d_norm[j][i] = d1;
                }
                else if (neighbors[k].status & IS_MOVING) {
                    d_norm[i][j] = d_norm[j][i] = d2;
                }
                /* If one of the two distances is zero, chances are good
                 * that the other distance is bogus as well.  We ignore
                 * them both. */
                else if (d1 == 0 || d2 == 0) {
                    d_norm[i][j] = d_norm[j][i] = 0;
                }
//                else if (ABS(d1-d2) > ASYMMETRIC_THRESHOLD)
//                    d_norm[i][j] = d_norm[j][i] = 0;
                
                /* Otherwise, just average the two distances. */
                else {
                    if (has_cones)
                        d_norm[i][j] = d_norm[j][i] = ((d1+d2)>>1) - 180;
                    else
                        d_norm[i][j] = d_norm[j][i] = (d1+d2)>>1;
                }
            }
        }
        return c;
    }
}