beacmanagem.nc

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

    /* In general, we need to keep track of two kinds of nodes,
     * moving nodes and static nodes.  This also translates into
     * distances -- we need to keep track of moving distances
     * and static distances.  The static distances are easy since we
     * just need the most recent one to perform localization --
     * they are stored in dists[][].  The moving distances are
     * trickier since we need them captured at a single moment in
     * time in order to be consistent.  That's what the track[]
     * array is for.  When we receive a new measurement to a moving
     * node, we record it in the track[] array, and hope to
     * receive other nodes measurements of the same underlying
     * pulse later at a later time.
     * These are recorded in the same array so that localization
     * is performed on a consistent set of measurements.
     *
     * This function takes such a measurement to a moving node and
     * records it for later use.  The measurement can either
     * be measured directly by us, or it can be a measurement
     * relayed from another node.
     * 
     * Arguments:
     *      timestamp       The timestamp of the beacon that produced
     *                      the pulse used for measurement.
     *      tracked_node    The moving node (which we are tracking)
     *      meas_node       The node which made the measurement.
     *      dist            The distance measured.
     */
    void new_tracked_dist(uint16_t timestamp, uint8_t tracked_node,
            uint8_t meas_node, uint16_t dist)
    {
        uint8_t n, i;
        int16_t dt;

        /* Check if the measurement is older than we are interested */
        dt = timestamp - tracked_latest;
        if (dt < 10) {
            UARTOutput(OUT_DEBUG, "Pulse %u too old\n", timestamp);
            return;
        }
        
        /* Check each slot in the list of existing beacons to see
         * if we already have a slot for the pulse. */
        for (i=0; i<track_num; i++) {
            n = (track_start + i) % MAX_TRACK;
            dt = timestamp - track[n].time;
            if (dt <= -10) {
                /* The pulse is older than the slot, so we try and
                 * insert a new slot into the array.  Note: by
                 * construction, the array is always sorted from
                 * oldest to newest beacon. */
                int8_t j;
                if (track_num == MAX_TRACK) {
                    /* No slots available */
                    UARTOutput(OUT_DEBUG, "Pulse dropped, full\n");
                    return;
                }
                /* Shift over the array contents by one to make room
                 * for the new slot. */
                for (j=track_num-1; j>=i; j--) {
                    uint8_t n2 = (track_start + j + 1) % MAX_TRACK;
                    n = (track_start + j) % MAX_TRACK;
                    memcpy(track+n2, track+n, sizeof(struct MovingTracker));
                }
                /* Exit the loop to initialize the new slot. */
                break;
            }
            if (dt < 10 && dt > -10) {
                /* The pulse matches the slot, so we incorporate the
                 * new data. */
                if (track[n].node == tracked_node) {
                    /* Add the new distance to the list. */
                    track[n].dist[meas_node] = dist;
                    UARTOutput(OUT_DEBUG, "Pulse %u (%d) node %d meas %d dist %d\n",
                            timestamp, n, tracked_node, meas_node, dist);
                }
                else
                    UARTOutput(OUT_DEBUG, "Pulse node mismatch\n");
                return;
            }
        }

        if (i == MAX_TRACK) {
            UARTOutput(OUT_DEBUG, "Pulse dropped, full\n");
            return;
        }

        /* Initialize the new slot */
        n = (track_start + i) % MAX_TRACK;
        track[n].time = timestamp;
        track[n].node = tracked_node;
        for (i=0; i<=MAX_NEIGHBORS; i++)
            track[n].dist[i] = 0;
        /* Record the distance we know */
        track[n].dist[meas_node] = dist;
        track_num++;
        UARTOutput(OUT_DEBUG, "Pulse %u (%d) node %d meas %d dist %d new\n",
                timestamp, n, tracked_node, meas_node, dist);
    }

    /* Looks through the list of beacons from mobile nodes and
     * localizes any nodes whose beacons occured long enough ago
     * to allow time for measurements to propagate from neighboring
     * nodes. */
    command result_t BeacManage.LocalizeMovingNodes()
    {
        /* Loop through the list of beacons. */
        while (track_num > 0) {
            /* Get the current clock time. */
            uint16_t curtime = call GetClockLow();
            /* The age of the beacon in milliseconds. */
            int16_t dt = curtime - track[track_start].time;
            uint8_t i;

            /* If the beacon is young enough, we're done.  (The
             * array is always sorted from oldest to youngest) */
            if (dt < MOBILE_LOCALIZATION_DELAY)
                break;

            /* Otherwise, localize the mobile node. */
            UARTOutput(OUT_INFO, "Tracking %2d time=%u,clock=%u\n",
                    track[track_start].node, track[track_start].time,
                    curtime);
            /* Print the list of distances to the mobile node for
             * debugging purposes. */
            UARTOutput(OUT_DEBUG, "  dist ");
            for (i=0; i<=MAX_NEIGHBORS; i++) {
                if (track[track_start].dist[i] != 0)
                    UARTOutput(OUT_DEBUG, "%u d=%d; ", i,
                            track[track_start].dist[i]);
                /* Remove any marked outliers for the final calculation */
                if (track[track_start].dist[i] == OUTLIER_DIST)
                    track[track_start].dist[i] = 0;
            }
            UARTOutput(OUT_DEBUG, "\n");

            /* Update state */
            tracked_latest = track[track_start].time;
            /* Do the localization */
            call Localize.TrackMoving(track[track_start].node,
                    track[track_start].dist);
            /* Remove the slot from the array. */
            track_num--;
            track_start = (track_start+1) % MAX_TRACK;
        }
        return SUCCESS;
    }

    /* Stores a new distance as measured by us to a neighbor
     * specified by its ID.  The distance should be in timer ticks and
     * is converted to microseconds internally. */
    command result_t BeacManage.NewMeasurement(uint8_t * id, uint16_t x,
            uint16_t timestamp, uint8_t flags)
    {
        uint8_t node;
        float * P;
        float newP[4];
        int16_t v;
        float d, a, dt;
        float K[2];
        uint8_t moving = 0;
        float mdist;

        /* If ticks do not equal microseconds, do the conversion to
         * microseconds. */
        if (US_PER_TICK != 1.0) {
            x = (uint16_t)((float)x * US_PER_TICK);
        }

        /* Obvious outliers are rejected. */
        if (x > MAX_DIST)
            return SUCCESS;

        /* Find the ID in our list, or create a new entry if we haven't
         * seen this node before. */
        node = node_find(id);
        if (node == EMPTY) {
            uint16_t * sd;
            node = node_add(id);
            if (node == EMPTY)
                return FAIL;

            /* The node is new to us, so we initialize the Kalman
             * filter.  We do this by taking the median of the first
             * NUM_INIT_MEAS measurements as the initial state for
             * the filter.  Since we only have one measurement so far,
             * we add it to the list. */

            /* Official distance and velocity are not known yet
             * (so set them to zero). */
            SET_DIST(SELF, node, 0);
            filt[node].vel = 0;
            /* Number of recorded distances is 1 */
            filt[node].n = 1;
            sd = filt[node].d;
            sd[1] = sd[2] = sd[3] = sd[4] = sd[5] = sd[6] = sd[7] = 0;
            /* Known distance */
            sd[0] = x;

            neighbors[node].status |= IS_NEW;
            filt[node].bad_count = 0;
            return SUCCESS;
        }

        /* Clear the aging flag */
        neighbors[node].status &= ~OLD_MEAS;

        if (flags & CR_MOVING)
            neighbors[node].status |= IS_MOVING;
        else
            neighbors[node].status &= ~IS_MOVING;

        /* A convenience pointer */
        P = filt[node].P;

        /* If the filter is still being initialized */
        if (neighbors[node].status & IS_NEW) {
            uint8_t n = filt[node].n;
            uint16_t * sd = filt[node].d;
            /* Incorporate the new distance */
            sd[n] = x;
            n++;

            if (n == NUM_INIT_MEAS) {
                /* Enough measurements are recorded to initialize the
                 * Kalman filter */
                uint8_t i, j;
                uint16_t t;
                /* Bubble sort the initial measurements */
                for (i=0; i<n; i++) {
                    for (j=i+1; j<n; j++) {
                        if (sd[j] < sd[i]) {
                            t = sd[i];
                            sd[i] = sd[j];
                            sd[j] = t;
                        }
                    }
                }
                /* Pick the median as the initial filter state */
                SET_DIST(SELF, node, sd[(n-1)>>1]);

                /* Initialize the covariance */
                P[0] = P[1] = P[2] = P[3] = 0;
                neighbors[node].status &= ~IS_NEW;

                /* Record the timestamp */
                filt[node].meas_time = timestamp;
                UARTOutput(OUT_DEBUG, "filt:%02u,meas=%u,fdist=%d,fvel=%d,clk=%u init\n",
                        node, x, GET_DIST(SELF, node), filt[node].vel, timestamp);
            }
            else {
                filt[node].n = n;
            }
            return SUCCESS;
        }

        /* If there have been three outliers in a row, reset the filter. */
        if (filt[node].bad_count == 3) {
            P[0] = P[1] = P[2] = P[3] = 0;
            filt[node].meas_time = timestamp;
            SET_DIST(SELF, node, x);
            filt[node].vel = 0;
            filt[node].bad_count = 0;
            UARTOutput(OUT_DEBUG, "filt:%02u,meas=%u,fdist=%d,fvel=%d,clk=%u reset\n",
                    node, x, GET_DIST(SELF, node), filt[node].vel, timestamp);
            return SUCCESS;
        }

        /* Time since last measurement in seconds */
        dt = (float)(timestamp - filt[node].meas_time)/1024.0;
        v = filt[node].vel;

        /* Kalman filtering begins here */

        /* Prediction step for the filter state */
        d = (float)GET_DIST(SELF, node) + v*dt;

        /* Moving distances get a much larger process noise than
         * static distances. */
        if ((neighbors[node].status & IS_MOVING) ||
                (neighbors[SELF].status & IS_MOVING)) {
            moving = 1;
            a = VEL_VARIANCE_MOTION;
        }
        else
            a = VEL_VARIANCE;
#if 0
        /* This code is for a Kalman filter with acceleration as the
         * random process. */
        newP[0] = P[0] + P[2]*dt + P[1]*dt + P[3]*dt*dt + a*dt*dt/4.0;
        newP[1] = P[1] + P[3]*dt                        + a*dt/2.0;
        newP[2] = P[2] + P[3]*dt                        + a*dt/2.0;
        newP[3] = P[3]                                  + a;
#endif
        /* Prediction step for covariance.
         * This code is for a Kalman filter with velocity as the
         * random process. */
        newP[0] = P[0] + P[2]*dt + P[1]*dt + P[3]*dt*dt + a*dt*dt;
        newP[1] = P[1] + P[3]*dt                        + a*dt;
        newP[2] = P[2] + P[3]*dt                        + a*dt;
        newP[3] = P[3]                                  + a;

        /* Gain step */
        a = newP[0] + MEAS_VARIANCE;
        K[0] = newP[0] / a;
        K[1] = newP[2] / a;

        /* Compute the Mahalanobis distance of the measurement */
        mdist = ((float)x - d)/sqrt(a);

        if (fabs(mdist) <= 3.0) {
            /* Incorporate the measurement into the filter if it is within
             * a Mahalanobis distance of 3.0 */

            /* Update step for the filter state (distance) */
            a = CLAMP(d + K[0]*((float)x - d), 0, MAX_DIST);
            SET_DIST(SELF, node, a);
            
            if (moving) {
                /* Update step for the filter state (velocity) */
                v = CLAMP(v + K[1]*((float)x - d), -MAX_DIST, MAX_DIST);