mcl.c

/*
 * @(#)mcl.c       07/04/06
 * 
 * Description  : A simple implementation of MCL.
 *                The Monte Carlo Localization algorithm for mobile 
 *                robot navigation.
 *
 * Inspired by articles and codes from:
 *      http://homepages.inf.ed.ac.uk
 *      http://www.robotika.cz
 *
 * License      : GNU GPL v.2
 * Author       : Tran Duy Khanh (www.tran.cz)
 */

#include <stdlib.h>
#include <math.h>
#include <robomath.h>
#include <string.h>
#include "mcl.h"

void mcl_move(struct mcl_model *mcl, double dx, double dy, double dangle)
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        /*struct mcl_particle robot = *(struct mcl_particle *)mcl->system;*/
        int i=0;
        double xdiff, ydiff;

        /* move the particles with noises */
        for (i=0; i<mcl->count; i++) {
                xdiff = dx*cos(parts[i].angle) + dy*sin(parts[i].angle);
                ydiff = dy*cos(parts[i].angle) + dx*sin(parts[i].angle);

                parts[i].x += xdiff + mcl->mov_dnoise*xdiff*gaussrand();
                parts[i].y += ydiff + mcl->mov_dnoise*ydiff*gaussrand();
                parts[i].angle += dangle + mcl->mov_anoise*dangle*gaussrand();
                /*printf("x=%f y=%f w=%f r.x=%f r.y=%f\n", 
                        parts[i].x, parts[i].y, parts[i].weight,
                        robot.x, robot.y);*/
        }
}

void mcl_update(struct mcl_model *mcl, double x, double y, double angle)
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        double xdiff, ydiff, dist, avdist=0;
        int i;

        /* evaluate the weights of each particle */
        for (i=0; i<mcl->count; i++) {
                xdiff = parts[i].x - x;
                ydiff = parts[i].y - y;
                dist = fabs(sqrt(xdiff*xdiff + ydiff*ydiff));
                avdist += dist;

                /* evaluate weights with gaussian probability density */
                parts[i].weight *= evaluate_gaussian(dist, mcl->eval_sigma);
        }

        /* adjust the evaluation values */
        avdist /= mcl->count;
        if (avdist > mcl->maxavdist) {
                if (mcl->noisecycle > mcl->maxnoisecycle) {
                        mcl->flag = MCL_RESET;
                        return;
                } else
                        mcl->noisecycle++;
        }
}

void mcl_normalize(struct mcl_model *mcl)
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        double gain=0, w=0;
        int i;

        for (i=0; i<mcl->count; i++)
                w += parts[i].weight;
        gain = mcl->count / w;
        
        if (gain <= 0)
                return;

        for (i=0; i<mcl->count; i++)
                parts[i].weight *= gain;
}

/* Compare function. Used to compare the weight of particles. Sorting is
 * realised with the qsort() function. */
int compare(const void *a, const void *b)
{
        return ((double)(((struct mcl_particle *)a)->weight) > 
                (double)(((struct mcl_particle *)b)->weight));
}

void mcl_partsort(struct mcl_model *mcl)
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        qsort(parts, mcl->count, sizeof(struct mcl_particle), compare);
}

void mcl_resample(struct mcl_model *mcl)
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        int i, j;
        int i_max=0, i_min=0;
        size_t num = 0;
        int count;

        /* index of the particles with the lowest weight to be considered */
        for (i=0; i<mcl->count; i++)
                if (parts[i].weight > mcl->w_min)
                        break;
        i_min = (i>0) ? i-1: i;

        /* index of the particles with the highest weight to be considered */
        for (i=0; i<mcl->count; i++)
                if (parts[i].weight > mcl->w_max)
                        break;
        i_max = (i>0) ? i-1: i;
                
        for (i=i_max; i<mcl->count; i++) {
                count = (int)parts[i].weight;
                count = (count > 0) ? count : 1;
                parts[i].weight /= count;
                for (j=num; j<num+count-1; j++)
                        memcpy(&parts[j], &parts[i], 
                                sizeof(struct mcl_particle));
                num += count-1;
        }

        for (i=num; i<i_min; i++) {
                i_max--;
                parts[i_max].weight /= 2;
                memcpy(&parts[i], &parts[i_max], sizeof(struct mcl_particle));
        }
}

void mcl_init (struct mcl_model *mcl)
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        int i;
        /*int xgridsize, ygridsize;*/

        /*
        ygridsize = (int)sqrt((double)((double)mcl->count*(double)mcl->height/
                                (double)mcl->width));
        xgridsize = (int)((double)mcl->width/(double)mcl->height*ygridsize);
        printf("xgridsize=%d ygridsize=%d\n", xgridsize, ygridsize);*/

        /* particles initialization */
        for (i=0; i<mcl->count; i++) {
                /*xdiff = 5*cos(parts[i].angle) + 5*sin(parts[i].angle);
                ydiff = 5*cos(parts[i].angle) + 5*sin(parts[i].angle);

                parts[i].x = mcl->system->x + xdiff + 
                                POS_STDEV_FRAC*xdiff*gaussrand();
                parts[i].y = mcl->system->y + ydiff + 
                                POS_STDEV_FRAC*ydiff*gaussrand();
                parts[i].angle += dangle + POS_STDEV_FRAC*dangle*gaussrand();
                */

                /* particles with noises. Starting weight is 1, as we consider
                 * that any of the particles could be the right one.*/
                parts[i].x = (rand()%mcl->width) * mcl->gen_dnoise;
                parts[i].y = (rand()%mcl->height) * mcl->gen_dnoise;
                parts[i].angle = rand() % (int)mcl->gen_anoise;
                parts[i].weight = 1;

                /*
                parts[i].x = (i%xgridsize)*(mcl->width/xgridsize)+xgridsize/2;
                parts[i].y = (i/xgridsize)*(mcl->height/ygridsize);
                parts[i].angle = rand() % (int)mcl->gen_anoise;
                parts[i].weight = 1;*/

                /*printf("x=%f, y=%f, a=%f, w=%f\n", 
                        parts[i].x, parts[i].y, 
                        parts[i].angle, parts[i].weight);
                */
        }                                
}

inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl)
{
        return (((struct mcl_particle *)mcl->parts)[mcl->count-1]);
}

---