MCL: Working version

The particle weight represents probabilities of beacons in the past

diff --git a/src/mcl/matlab/reg_mcl.mdl b/src/mcl/matlab/reg_mcl.mdl
index efc30f975c8192ba72b5d7b01e0b1ccdd0d2dae7..55df82fd20fa213c0355fbfe38b833344bb99009 100644 (file)
--- a/src/mcl/matlab/reg_mcl.mdl
+++ b/src/mcl/matlab/reg_mcl.mdl
@@ -6,7 +6,7 @@ Model {
     NumRootInports         0
     NumRootOutports        0
     ParameterArgumentNames  ""
-    ComputedModelVersion    "1.329"
+    ComputedModelVersion    "1.332"
     NumModelReferences     0
     NumTestPointedSignals   0
   }
@@ -48,8 +48,8 @@ Model {
   ModifiedByFormat       "%<Auto>"
   LastModifiedBy         "wsh"
   ModifiedDateFormat     "%<Auto>"
-  LastModifiedDate       "Wed Apr 30 07:42:20 2008"
-  ModelVersionFormat     "1.%<AutoIncrement:329>"
+  LastModifiedDate       "Wed Apr 30 08:14:46 2008"
+  ModelVersionFormat     "1.%<AutoIncrement:332>"
   ConfigurationManager   "None"
   SimulationMode         "normal"
   LinearizationMsg       "none"
@@ -1177,7 +1177,7 @@ Model {
       MaskIconOpaque         on
       MaskIconRotate         "none"
       MaskIconUnits          "autoscale"
-      MaskValueString        "3000|0.01|5/180*pi|10/180*pi"
+      MaskValueString        "3000|0.01|5/180*pi|5/180*pi"
       MaskTabNameString              ",,,"
       Port {
        PortNumber              1
@@ -1287,7 +1287,7 @@ Model {
       System {
        Name                    "Robot kinematics"
        Location                [4, 55, 1176, 412]
-       Open                    off
+       Open                    on
        ModelBrowserVisibility  off
        ModelBrowserWidth       200
        ScreenColor             "white"

diff --git a/src/mcl/matlab/sf_mcl.c b/src/mcl/matlab/sf_mcl.c
index c8e27cb71904dacc50b52acebb3e7ef77c597e4d..a6829d3dbc786d9c6fd0a92abf1ab5369f3fc9d5 100644 (file)
--- a/src/mcl/matlab/sf_mcl.c
+++ b/src/mcl/matlab/sf_mcl.c
@@ -428,6 +428,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     }
 
     /* Update probability bitmap */
+    changed = false;           /* Never show bitmap before resampling */
     if (changed && nonzero)
            parts = l->resampled; /* Draw particles before resampling */
     else

diff --git a/src/mcl/mcl_laser.c b/src/mcl/mcl_laser.c
index 0989a32f506becc4f85858d6997f87590f2a8bed..bada53d15d50398e055a657312a30fe84558c432 100644 (file)
--- a/src/mcl/mcl_laser.c
+++ b/src/mcl/mcl_laser.c
@@ -89,14 +89,15 @@ static void mcl_laser_update(struct mcl_model *mcl, void *measurement)
 
        /* evaluate the weights of each particle */
        for (i=0; i<mcl->count; i++) {
-               if (parts[i].x < 0 || parts[i].x > l->width ||
-                   parts[i].y < 0 || parts[i].y > l->height) {
+               struct mcl_laser_state *part = &parts[i];
+               if (part->x < 0 || part->x > l->width ||
+                   part->y < 0 || part->y > l->height) {
                        /* We cannot be out of playground */
                        mcl->weight[i] = 0;
                        continue;
                }
 
-               mcl_pos2ang(&parts[i], theta, BEACON_CNT, l->beacon_color);
+               mcl_pos2ang(part, theta, BEACON_CNT, l->beacon_color);
                for (im=0; im<angles->count; im++) {
                        p=0;
                        for (it=0; it < BEACON_CNT; it++) {
@@ -104,19 +105,19 @@ static void mcl_laser_update(struct mcl_model *mcl, void *measurement)
                                if (diff > M_PI) diff = 2*M_PI-diff;
                                p += gaussian(diff, l->aeval_sigma);
                        }
-                       parts[i].w[parts[i].beacon] = p;
-                       mcl->weight[i] = 1;
-                       for (it=0; it < BEACON_CNT; it++) {
-                               mcl->weight[i] *= parts[i].w[it];
-                       }
-
+                       part->w[part->beacon] = p;
 
-                       parts[i].beacon++;
-                       parts[i].beacon %= BEACON_CNT;
+                       part->beacon++;
+                       part->beacon %= BEACON_CNT;
                }
+               mcl->weight[i] = 1;
+               for (it=0; it < BEACON_CNT; it++) {
+                       mcl->weight[i] *= part->w[it];
+               }
+
                if (mcl->weight[i] > wmax) {
                        wmax = mcl->weight[i];
-                       best = &parts[i];
+                       best = part;
                }
        }
        *(struct mcl_laser_state*)mcl->estimated = *best;

diff --git a/src/mcl/mcl_laser.h b/src/mcl/mcl_laser.h
index f8b2169b91e8fe13c7d2553ab6f25498776d3aba..70e02f7aa5a3bea6d87e05e5db54f22ee9f2aa62 100644 (file)
--- a/src/mcl/mcl_laser.h
+++ b/src/mcl/mcl_laser.h
@@ -11,6 +11,7 @@ struct mcl_laser_state {
        float x;
        float y;
        float angle;
+       float w[BEACON_CNT];
        int8_t beacon; /* from which beacon we expect next measure */
 };