DCE-Eurobot: laser-nav/mcl/mcl.h Source File

00001 /*
00002  * @(#)mcl.h       07/04/06
00003  * 
00004  * Description  : A simple implementation of MCL.
00005  *                The Monte Carlo Localization algorithm for mobile 
00006  *                robot navigation.
00007  *
00008  * Inspired by articles and codes from:
00009  *      http://homepages.inf.ed.ac.uk
00010  *      http://www.robotika.cz
00011  *
00012  * License      : GNU GPL v.2
00013  * Author       : Tran Duy Khanh (www.tran.cz)
00014  */
00015 
00016 #ifndef __mcl__
00017 #define __mcl__
00018 
00019 #define POS_STDEV_FRAC          2
00020 
00021 /* MCL sample */
00022 struct mcl_particle {
00023         double x;
00024         double y;
00025         double angle;
00026         double weight; 
00027 };
00028 
00029 struct mcl_model {
00030         /* operations with particles */
00031         void (*init) (struct mcl_model *mcl);
00032         void (*move) (struct mcl_model *mcl, double dx, double dy, double dangle);
00033         void (*update) (struct mcl_model *mcl, double x, double y, double angle);
00034         void (*normalize) (struct mcl_model *mcl);
00035         void (*sort) (struct mcl_model *mcl);
00036         void (*resample) (struct mcl_model *mcl);
00037         /* particles */
00038         void *parts;
00039         int count;
00040         /* the system (robot, ...) */
00041         void *system;
00042         /* noises: movement noise,  */
00043         double gen_dnoise;
00044         double gen_anoise;
00045         double mov_dnoise;
00046         double mov_anoise;
00047         /* used to evaluate the particles */
00048         double w_min;
00049         double w_max;
00050         double eval_sigma;
00051         /* size of the playground */
00052         int width;
00053         int height;
00054         /* counters stuff */
00055         int cycle;
00056         int noisecycle;
00057         /* used to reinitialisation after too many bad measures */
00058         int maxnoisecycle;
00059         double maxavdist;
00060         unsigned char flag;
00061 };
00062 
00063 enum {
00064         MCL_STOP = 0,
00065         MCL_RUN = 1,
00066         MCL_RESET = 255
00067 };
00068 
00072 void mcl_move(struct mcl_model *mcl, double dx, double dy, double dangle);
00073 void mcl_update(struct mcl_model *mcl, double x, double y, double angle);
00074 void mcl_normalize(struct mcl_model *mcl);
00075 void mcl_partsort(struct mcl_model *mcl);
00076 void mcl_resample(struct mcl_model *mcl);
00077 void mcl_init (struct mcl_model *mcl);
00078 inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl);
00079 
00080 #endif  /* __mcl__ */