src/mcl/mcl_laser.c

   1 /*
   2  * This file is part of MCL library.
   3  *
   4  * MCL library is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published
   6  * by the Free Software Foundation, either version 3 of the License,
   7  * or (at your option) any later version.
   8  *
   9  * MCL library is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with MCL library. If not, see <http://www.gnu.org/licenses/>.
  16  *
  17  * Copyright (C) 2007 - 2009 Czech Technical University in Prague
  18  *
  19  * Authors: Michal Sojka <sojkam1@fel.cvut.cz>
  20  *          Marek Peca
  21  *          Tran Duy Khanh
  22  */
  23
  24 #include "mcl_laser.h"
  25 #include <stdlib.h>
  26 #include <math.h>
  27 #include "mcl_math.h"
  28
  29 #ifdef MATLAB_MEX_FILE
  30 #define S_FUNCTION_LEVEL 2
  31 #define S_FUNCTION_NAME  sf_mcl
  32 #include "simstruc.h"
  33 #endif
  34
  35
  36 static void mcl_laser_reset(struct mcl_model *mcl)
  37 {
  38         struct mcl_laser *l = mcl_to_laser(mcl);
  39         struct mcl_laser_state *parts = l->parts;
  40         int i;
  41
  42         /* particles initialization */
  43         for (i=0; i<mcl->count; i++) {
  44                 /* generate particles with noises. They have the same
  45                  * starting weight, as we consider that any of the
  46                  * particles could be the right one.
  47                  */
  48                 parts[i].x = (rand()&0xffff)/65535.0*l->width;
  49                 parts[i].y = (rand()&0xffff)/65535.0*l->height;
  50                 parts[i].angle = (rand() % 360)/360.0*(2.0*M_PI);
  51                 parts[i].beacon = 0 /* rand() % BEACON_CNT */;
  52         }
  53
  54 }
  55
  56 static void mcl_laser_set_estimated(struct mcl_model *mcl, void *new_estimated)
  57 {
  58         struct mcl_laser *l = mcl_to_laser(mcl);
  59         struct mcl_laser_state *parts = l->parts;
  60 /*      struct mcl_robot_pos *estimated = mcl->estimated; */
  61         struct mcl_robot_pos *new_est = new_estimated;
  62         int i;
  63         struct mcl_laser_state p;
  64         p.x = new_est->x;
  65         p.y = new_est->y;
  66         p.angle = new_est->angle;
  67         p.beacon = 0;
  68
  69         l->estimated = *new_est;
  70         for (i=0; i<BEACON_CNT; i++) {
  71                 p.w[i] = 1;
  72         }
  73
  74
  75         for (i=0; i<mcl->count; i++) {
  76                 parts[i] = p;   /* TODO: Add some noise here */
  77         }
  78 }
  79
  80
  81 static void mcl_laser_predict(struct mcl_model *mcl, void *delta)
  82 {
  83         struct mcl_laser *l = mcl_to_laser(mcl);
  84         struct mcl_laser_state *parts = l->parts;
  85         struct mcl_robot_odo *d = delta;
  86         struct mcl_robot_pos *estimated = mcl->estimated;
  87
  88         int i=0;
  89         double xdiff, ydiff, a;
  90
  91         /* move the particles with noises */
  92         for (i=0; i<mcl->count; i++) {
  93                 double a = parts[i].angle;
  94                 xdiff = d->dx*cos(a) - d->dy*sin(a);
  95                 ydiff = d->dy*cos(a) + d->dx*sin(a);
  96
  97                 parts[i].x += xdiff + l->pred_dnoise*gaussian_random();
  98                 parts[i].y += ydiff + l->pred_dnoise*gaussian_random();
  99                 parts[i].angle += d->dangle + l->pred_anoise*gaussian_random();
 100 /*              if (drand48() < l->beacon_miss)  parts[i].beacon++; */
 101 /*              if (drand48() < l->false_beacon) parts[i].beacon--; */
 102 /*              if (parts[i].beacon >= BEACON_CNT) */
 103 /*                      parts[i].beacon = 0; */
 104 /*              if (parts[i].beacon < 0) */
 105 /*                      parts[i].beacon = BEACON_CNT-1; */
 106
 107 /*              printf("%3d: x=%g  y=%g  a=%g b=%d\n",  */
 108 /*                     i, parts[i].x, parts[i].y, parts[i].angle, parts[i].beacon); */
 109         }
 110
 111         /* Update the last estimated position without noise */
 112         a = estimated->angle;
 113         estimated->x += d->dx*cos(a) - d->dy*sin(a);
 114         estimated->y += d->dx*sin(a) + d->dy*cos(a);
 115         estimated->angle += d->dangle;
 116 }
 117
 118 static inline double gaussian(double val, double sigma)
 119 {
 120         return exp(-0.5 * (val*val / (sigma*sigma)));
 121 }
 122
 123
 124 static void mcl_laser_update(struct mcl_model *mcl, void *measurement)
 125 {
 126         struct mcl_laser *l = mcl_to_laser(mcl);
 127         struct mcl_laser_state * __restrict parts = l->parts;
 128         const struct mcl_laser_state * __restrict best = &parts[0];
 129         const struct mcl_laser_measurement *angles = measurement;
 130         /* double theta; */
 131         mcl_thetas theta;
 132         double p;
 133         double diff;
 134         int i, im, it;
 135         mcl_weight_t wmax = 0;
 136         /* evaluate the weights of each particle */
 137         for (i=0; i<mcl->count; i++) {
 138                 struct mcl_laser_state *part = &parts[i];
 139                 if (part->x < 0 || part->x > l->width ||
 140                     part->y < 0 || part->y > l->height) {
 141                         /* We cannot be out of the playground */
 142                         mcl->weight[i] = 0;
 143                         continue;
 144                 }
 145
 146                 mcl_pos2ang(part, theta, BEACON_CNT, l->beacon_color);
 147                 for (im=0; im<angles->count; im++) {
 148                         p=0;
 149                         for (it=0; it < BEACON_CNT; it++) {
 150                                 diff = fabs(theta[it] - angles->val[im]);
 151                                 if (diff > M_PI) diff = 2*M_PI-diff;
 152                                 p += gaussian(diff, l->aeval_sigma);
 153                         }
 154                         part->w[part->beacon] = p;
 155
 156                         part->beacon++;
 157                         part->beacon %= BEACON_CNT;
 158                 }
 159                 mcl->weight[i] = 1;
 160                 for (it=0; it < BEACON_CNT; it++) {
 161                         mcl->weight[i] *= part->w[it];
 162                 }
 163
 164                 if (mcl->weight[i] > wmax) {
 165                         wmax = mcl->weight[i];
 166                         best = part;
 167                 }
 168         }
 169
 170         l->estimated.x = best->x;
 171         l->estimated.y = best->y;
 172         l->estimated.angle = best->angle;
 173         return;
 174 }
 175
 176
 177 static inline double rnd()
 178 {
 179         return 1 - drand48();   /* exclude zero */
 180 }
 181
 182 /**
 183  * Resample the particles. Mystic alg. 1 from "Improved particle
 184  * filter for nonlinear problems".
 185  *
 186  * @note It already contains normalization so it is not neccessary to
 187  * call mcl_normalize().
 188  *
 189  * @param  mcl          the MCL model
 190  */
 191 static void mcl_laser_resample(struct mcl_model *mcl)
 192 {
 193         struct mcl_laser *l = mcl_to_laser(mcl);
 194         struct mcl_laser_state *parts = l->parts;
 195         struct mcl_laser_state *resampled = l->resampled;
 196         mcl_weight_t *Q = l->Q, *T=l->T, *weight = mcl->weight;
 197
 198         double sum;
 199         int i, j;
 200
 201         /* Q is shifted one element to the left sice the initial zero
 202          * is never accessed. */
 203         Q[0] = weight[0];
 204         for (i = 1; i < mcl->count; i++)
 205                 Q[i] = Q[i-1] + weight[i];
 206
 207         /* We normalize here since Q should be in cache now and we
 208          * also save one pass as compared to standalone
 209          * mcl_normalize(). */
 210         sum = Q[mcl->count-1];
 211         if (sum <= 0)
 212                 return;
 213         for (i = 0; i < mcl->count; i++)
 214                 Q[i] /= sum;
 215
 216         /* We do not store the last element of T since it is always
 217          * one and during resample loop this element is never
 218          * reached. */
 219         T[0] = -log(rnd());
 220         for (i = 1; i < mcl->count; i++)
 221                 T[i] = T[i-1] - log(rnd());
 222         sum = T[mcl->count-1] - log(rnd());
 223         for (i = 0; i < mcl->count; i++)
 224                 T[i] /= sum;
 225
 226         for (i = 0, j = 0; i < mcl->count; ) {
 227                 if (Q[j] > T[i]) {
 228                         resampled[i] = parts[j];
 229                         ++i;
 230                 }
 231                 else
 232                         ++j;
 233         }
 234
 235         /* Swap resampled and non-resampled particles */
 236         l->parts = resampled;
 237         l->resampled = parts;
 238 }
 239
 240 void mcl_laser_done(struct mcl_model *mcl)
 241 {
 242         struct mcl_laser *l = mcl_to_laser(mcl);
 243
 244         mcl_done(mcl);
 245         free(l->parts);
 246         free(l->resampled);
 247         free(l->Q);
 248         free(l->T);
 249 }
 250
 251
 252 struct mcl_model *mcl_laser_init(struct mcl_laser *l, unsigned count)
 253 {
 254         struct mcl_model *mcl = &l->mcl;
 255
 256         mcl_init(mcl, count, &l->estimated);
 257         mcl->priv = l;
 258
 259         l->parts = malloc(count*sizeof(struct mcl_laser_state));
 260         l->resampled = malloc(count*sizeof(struct mcl_laser_state));
 261         l->Q = malloc(count*sizeof(*l->Q));
 262         l->T = malloc(count*sizeof(*l->T));
 263
 264
 265         /* Initialize method pointers */
 266         mcl->reset = mcl_laser_reset;
 267         mcl->set_estimated = mcl_laser_set_estimated;
 268         mcl->predict = mcl_laser_predict;
 269         mcl->update = mcl_laser_update;
 270         mcl->resample = mcl_laser_resample;
 271         mcl->done = mcl_laser_done;
 272
 273         mcl_laser_reset(mcl);
 274         return mcl;
 275 }
 276
 277 double mcl_beacon_angle(const struct mcl_laser_state *part,
 278                         unsigned char color)
 279 {
 280         double head, theta;
 281         int i;
 282         const struct beacon_pos *beacon;
 283
 284         if (color == BEACON_RED) {
 285                 head = part->angle + M_PI;
 286                 beacon = beacon_red;
 287         } else {
 288                 head = part->angle;
 289                 beacon = beacon_green;
 290         }
 291
 292         head = (head < 0) ? 2*M_PI + head : head;
 293
 294
 295         i = part->beacon;
 296         theta = atan2(beacon[i].y-part->y, beacon[i].x-part->x);
 297         /* normalize angles */
 298         if (theta < 0) theta = 2*M_PI + theta;
 299         theta = (theta-head < 0) ?
 300                 2*M_PI+theta-head : theta-head;
 301         return theta;
 302 }
 303
 304 /**
 305  * Convert the position to angles between robot's head and reflectors
 306  * (angles sense is counter-clockwise)
 307  *
 308  * @param  part         MCL particle
 309  * @param  theta        Returned angles to reflectors
 310  * @param  color        beacon color
 311  * @param  cnt          beacon count
 312  */
 313 void mcl_pos2ang(const struct mcl_laser_state *part,
 314                  mcl_thetas theta,
 315                  unsigned char cnt, unsigned char color)
 316 {
 317         int i;
 318         struct mcl_laser_state p = *part;
 319         for (i=0; i<BEACON_CNT; i++) {
 320                 p.beacon = i;
 321                 theta[i] = mcl_beacon_angle(&p, color);
 322         }
 323 }