src/mcl/mcl.c

   1 /*
   2  * @(#)mcl.c       07/04/06
   3  *
   4  * Description  : A simple implementation of MCL.
   5  *                The Monte Carlo Localization algorithm for mobile
   6  *                robot navigation.
   7  *
   8  * Inspired by articles and codes from:
   9  *      http://homepages.inf.ed.ac.uk
  10  *      http://www.robotika.cz
  11  *
  12  * License      : GNU GPL v.2
  13  * Authors      : Tran Duy Khanh (www.tran.cz)
  14  *                Michal Sojka <sojkam1@fel.cvut.cz>
  15  */
  16
  17 #include <stdlib.h>
  18 #include <math.h>
  19 #include "mcl_math.h"
  20 #include <string.h>
  21 #include "mcl.h"
  22
  23 #ifdef MATLAB_MEX_FILE
  24 #define restrict
  25 #endif
  26
  27 /**
  28  * IMPLEMENTATION OF THE GENERIC PART OF MONTE CARLO ALGORITHM.
  29  */
  30
  31 /**
  32  * Initializes ::mcl_model and allocates neccessary memory. Should be
  33  * called from implementation init function.
  34  *
  35  * @param mcl MCL model structure
  36  * @param part_size Size of one particle
  37  * @param count The number of particles
  38  */
  39 void mcl_init(struct mcl_model *mcl, unsigned count, void *estimated)
  40 {
  41         mcl->count = count;
  42         mcl->weight = malloc(count*sizeof(*mcl->weight));
  43         memset(mcl->weight, 0, count*sizeof(*mcl->weight));
  44         mcl->estimated = estimated;
  45 }
  46
  47 /**
  48  * Deallocates memory allocated by mcl_init().
  49  *
  50  * @param mcl MCL model structure.
  51  */
  52 void mcl_done(struct mcl_model *mcl)
  53 {
  54         free(mcl->weight);
  55 }
  56
  57 /**
  58  * Normalize the particle weights.
  59  *
  60  * @param  mcl          the MCL model
  61  */
  62 void mcl_normalize(struct mcl_model *mcl)
  63 {
  64         double sum=0;
  65         int i;
  66
  67         for (i=0; i<mcl->count; i++)
  68                 sum += mcl->weight[i];
  69
  70         for (i=0; i<mcl->count; i++)
  71                 mcl->weight[i] /= sum;
  72 }
  73
  74 #if 0
  75 void mcl_resample_old(struct mcl_model *mcl)
  76 {
  77         void *parts = (struct mcl_particle *)mcl->parts;
  78         int i, j;
  79         int i_max=0, i_min=0;
  80         size_t num = 0;
  81         int count;
  82
  83         /* index of the particles with the lowest weight to be considered */
  84         for (i=0; i<mcl->count; i++)
  85                 if (parts[i].weight > mcl->w_min) {
  86 /*                      printf("MIN: i=%d weight=%f\n", i, parts[i].weight); */
  87                         break;
  88                 } else {
  89 /*                      printf("i=%d weight=%f\n", i, parts[i].weight); */
  90                 }
  91         i_min = (i>0) ? i-1: i;
  92
  93         /* index of the particles with the highest weight to be considered */
  94         for (i=0; i<mcl->count; i++)
  95                 if (parts[i].weight > mcl->w_max) {
  96 /*                      printf("MAX: i=%d weight=%f\n", i, parts[i].weight); */
  97                         break;
  98                 } else {
  99 /*                      printf("i=%d weight=%f\n", i, parts[i].weight); */
 100                 }
 101         i_max = (i>0) ? i-1: i;
 102
 103         for (i=i_max; i<mcl->count; i++) {
 104                 count = (int)parts[i].weight;
 105                 count = (count > 0) ? count : 1;
 106                 parts[i].weight /= count;
 107                 for (j=num; j<num+count-1; j++)
 108                         memcpy(&parts[j], &parts[i],
 109                                 sizeof(struct mcl_particle));
 110                 num += count-1;
 111         }
 112
 113         for (i=num; i<i_min; i++) {
 114                 i_max--;
 115                 parts[i_max].weight /= 2;
 116                 memcpy(&parts[i], &parts[i_max], sizeof(struct mcl_particle));
 117         }
 118
 119         /* Find the most probable position */
 120         struct mcl_particle best = mcl_get_pos(mcl);
 121         mcl->est_pos.x = best.x;
 122         mcl->est_pos.y = best.y;
 123         mcl->est_pos.angle = best.angle;
 124 }
 125 #endif
 126
 127 /**
 128  * End of the MCL stuffs.
 129  *****************************************************************************/