src/mcl/mcl_laser_pos.c

   1 /**
   2  * First method:
   3  * Evaluation of the position.
   4  * The update phase:
   5  * check the distance of predicted and measured states and evaluate
   6  * particle`s weight.
   7  *
   8  * @param  mcl          the MCL model
   9  * @param  data         measured data (x, y, turning)
  10  */
  11 void mcl_laser_pos(struct mcl_model *mcl, void *data)
  12 {
  13         struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
  14         double xdiff, ydiff, dist, avdist=0;
  15         int i;
  16         double x = ((double *)data)[0];
  17         double y = ((double *)data)[1];
  18         /*double angle = ((double *)data)[2];*/
  19
  20         /* evaluate the weights of each particle */
  21         for (i=0; i<mcl->count; i++) {
  22                 xdiff = parts[i].x - x;
  23                 ydiff = parts[i].y - y;
  24                 dist = fabs(sqrt(xdiff*xdiff + ydiff*ydiff));
  25                 avdist += dist;
  26
  27                 /* evaluate weights with gaussian probability density */
  28                 parts[i].weight *= evaluate_gaussian(dist, mcl->eval_sigma);
  29         }
  30
  31         /* adjust the evaluation values */
  32         avdist /= mcl->count;
  33         if (avdist > mcl->maxavdist) {
  34                 if (mcl->noisecycle > mcl->maxnoisecycle) {
  35                         mcl->flag = MCL_RESET;
  36                         return;
  37                 } else
  38                         mcl->noisecycle++;
  39         }
  40 }
  41