robofsm: Fixed race condition in setting of estimated position (UZV)

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index f968da5698c838c65857a011e0043386e992f58e..fbf7e4eb69a34a0ca0f057897c55be033aa2304e 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -222,7 +222,6 @@ static void do_estimation()
                        beacon_xy[i][1] = bp[i].y;
                }
 
-               /*FIXME: is initial pos set correctly??*/
                ROBOT_LOCK(est_pos_uzv);
                real_t xy0[] = {robot.est_pos_uzv.x, robot.est_pos_uzv.y};
                y[3] = y[4] = 0.0;
@@ -240,9 +239,11 @@ static void do_estimation()
        DBG("EKF: x=%f   y=%f   phi=%8.4f\n", x[0], x[1], x[6]*(180.0/M_PI));
 
        ROBOT_LOCK(est_pos_uzv);
-       robot.est_pos_uzv.x = x[0] - ODO_D*cos(x[6]);
-       robot.est_pos_uzv.y = x[1] - ODO_D*sin(x[6]);
-       robot.est_pos_uzv.phi = x[6];
+       if (!init_ekf_flag) {
+               robot.est_pos_uzv.x = x[0] - ODO_D*cos(x[6]);
+               robot.est_pos_uzv.y = x[1] - ODO_D*sin(x[6]);
+               robot.est_pos_uzv.phi = x[6];
+       }
        ROBOT_UNLOCK(est_pos_uzv);
 }
 

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index 20658a55e08d4775c52f0f0cf39bc0b9555b007c..55c071a8e70dea6de405a2e6841bdc0211309600 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -88,7 +88,7 @@ void robot_set_est_pos_notrans(double x, double y, double phi)
        robot.est_pos_uzv.x = x;
        robot.est_pos_uzv.y = y;
        robot.est_pos_uzv.phi = phi;
-       init_ekf_flag = true;
+       init_ekf_flag = true;   // Tell kalman filter to reinitialize itself
 
        ROBOT_LOCK(est_pos_odo);
        robot.est_pos_odo.x = x;