MCL: Added MCL type for odometry data

diff --git a/src/mcl/Makefile.omk b/src/mcl/Makefile.omk
index 7885fa0b988c308a3e9065f6a3675878c343e08a..d9426ed94def8b10fe63fb688158f39f72554a56 100644 (file)
--- a/src/mcl/Makefile.omk
+++ b/src/mcl/Makefile.omk
@@ -6,7 +6,7 @@ lib_LIBRARIES = mcl
 mcl_SOURCES = mcl.c mcl_laser.c
 mcl_LIBS = robomath m 
 
-include_HEADERS = mcl.h mcl_laser.h
+include_HEADERS = mcl.h mcl_laser.h mcl_robot.h
 
 # -ffast-math is the most important optimization here
 CFLAGS := $(filter-out -fno-strict-aliasing,$(CFLAGS)) --std=c99 -D_GNU_SOURCE -ffast-math -O3 -funroll-loops -ftree-vectorize

diff --git a/src/mcl/mcl_laser.c b/src/mcl/mcl_laser.c
index bada53d15d50398e055a657312a30fe84558c432..a62ff7f5b9df341368a8c1f0bf5447281ff94a84 100644 (file)
--- a/src/mcl/mcl_laser.c
+++ b/src/mcl/mcl_laser.c
@@ -35,7 +35,7 @@ static void mcl_laser_predict(struct mcl_model *mcl, void *delta)
 {
        struct mcl_laser *l = mcl_to_laser(mcl);
        struct mcl_laser_state *parts = l->parts;
-       struct mcl_laser_state *d = delta;
+       struct mcl_robot_odo *d = delta;
        struct mcl_laser_state *estimated = mcl->estimated;
        
        int i=0;
@@ -44,12 +44,12 @@ static void mcl_laser_predict(struct mcl_model *mcl, void *delta)
        /* move the particles with noises */
        for (i=0; i<mcl->count; i++) {
                double a = parts[i].angle;
-               xdiff = d->x*cos(a) - d->y*sin(a);
-               ydiff = d->y*cos(a) + d->x*sin(a);
+               xdiff = d->dx*cos(a) - d->dy*sin(a);
+               ydiff = d->dy*cos(a) + d->dx*sin(a);
 
                parts[i].x += xdiff + l->pred_dnoise*gaussian_random();
                parts[i].y += ydiff + l->pred_dnoise*gaussian_random();
-               parts[i].angle += d->angle + l->pred_anoise*gaussian_random();
+               parts[i].angle += d->dangle + l->pred_anoise*gaussian_random();
 /*             if (drand48() < l->beacon_miss)  parts[i].beacon++; */
 /*             if (drand48() < l->false_beacon) parts[i].beacon--; */
 /*             if (parts[i].beacon >= BEACON_CNT) */
@@ -63,9 +63,9 @@ static void mcl_laser_predict(struct mcl_model *mcl, void *delta)
 
        /* Update the last estimated position without noise */
        a = estimated->angle;
-       estimated->x += d->x*cos(a) - d->y*sin(a);
-       estimated->y += d->x*sin(a) + d->y*cos(a);
-       estimated->angle += d->angle;
+       estimated->x += d->dx*cos(a) - d->dy*sin(a);
+       estimated->y += d->dx*sin(a) + d->dy*cos(a);
+       estimated->angle += d->dangle;
 }
 
 static inline double gaussian(double val, double sigma)

diff --git a/src/mcl/mcl_laser.h b/src/mcl/mcl_laser.h
index 70e02f7aa5a3bea6d87e05e5db54f22ee9f2aa62..d5dd473658254ccdda75737b27d6764e694d8be1 100644 (file)
--- a/src/mcl/mcl_laser.h
+++ b/src/mcl/mcl_laser.h
@@ -2,6 +2,7 @@
 #define MCL_LASER_H
 
 #include <mcl.h>
+#include <mcl_robot.h>
 #include <robodim_eb2008.h>
 #include <stdint.h>
 

diff --git a/src/mcl/mcl_robot.h b/src/mcl/mcl_robot.h
new file mode 100644 (file)
index 0000000..ed75659
--- /dev/null
+++ b/src/mcl/mcl_robot.h
@@ -0,0 +1,12 @@
+#ifndef MCL_ROBOT_H
+#define MCL_ROBOT_H
+
+/**
+ * Data type for MCL prediction. This whould be probably same for all
+ * MCL implementations.
+ */
+struct mcl_robot_odo {
+       double dx, dy, dangle;
+};
+
+#endif