robofsm: EKF initialiation hopefully correct

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 4a36e7ac103b147fd41004a1157077d8cfd300f1..68a1c09dd59d08f6a6a36b1ad3789cfb03d2ddd2 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -179,7 +179,6 @@ static void do_estimation()
                { 3.062,  2.162},
        };
        static ekf8_t ekf8;
-       static int virgo = 1; /*FIXME:deploy estimator_init() outside*/
        static uint32_t odo0[2];
        static real_t y[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
        static int missing_odo_count = 0;
@@ -217,13 +216,15 @@ static void do_estimation()
        DBG("UZV+ODO:  %f  %f  %f  %f  %f\n", y[0], y[1], y[2], y[3], y[4]);
        //DBG("ODO:  %f  %f    %u  %u\n", y[3], y[4], odo[0], odo[1]);
 
-       if (virgo) {
+       if (init_ekf_flag) {
                /*FIXME:reflect init pos & beacon coords accord.to our color */
-               real_t xy0[] = {2.63, 0.35};
+               ROBOT_LOCK(est_pos);
+               real_t xy0[] = {robot.est_pos.x, robot.est_pos.y};
                y[3] = y[4] = 0.0;
                ekf8_init(&ekf8, (real_t*)beacon_xy, D_MAX, 30.0, xy0, y);
-               ekf8.ekf.x[6] = -45.0*(M_PI/180.0);
-               virgo = 0;
+               ekf8.ekf.x[6] = robot.est_pos.phi;
+               init_ekf_flag = false;
+               ROBOT_UNLOCK(est_pos);
        }
 
        ekf8_step(&ekf8, x, P, err, y);

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index 029bb0cd510c6bc0fcf547be2c5095ae17f86cf4..42102082179bd2c7cc2cfc50117e3e9e25576314 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -73,6 +73,8 @@ void robot_trajectory_new_backward(struct TrajectoryConstraints *tc) {
        robot_trajectory_new_ex(tc, true);
 }
 
+bool init_ekf_flag = false;
+
 /** Sets actual position of the robot and with respoect to color of
  * the team. Should be used for setting initial position of the
  * robot. */
@@ -82,11 +84,11 @@ void robot_set_est_pos_notrans(double x, double y, double phi)
        if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
        if (y<0) y=0;
        if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
-       // TODO: initialize ekf here
        ROBOT_LOCK(est_pos);
        robot.est_pos.x = x;
        robot.est_pos.y = y;
        robot.est_pos.phi = phi;
+       init_ekf_flag = true;
        ROBOT_UNLOCK(est_pos);
 }
 

diff --git a/src/robofsm/movehelper.h b/src/robofsm/movehelper.h
index 469249b38d926eef7d52fffedf0c2fc64cbde529..9f955977b13038b4a39a12e70b3575b8f70ac834 100644 (file)
--- a/src/robofsm/movehelper.h
+++ b/src/robofsm/movehelper.h
@@ -127,6 +127,8 @@ bool get_arrive_from_point(double target_x_m, double target_y_m,
                           double *point_x_m, double *point_y_m);
 
 
+extern bool init_ekf_flag;
+
 void robot_set_est_pos_notrans(double x, double y, double phi);
 #define robot_set_est_pos_trans(x, y, phi) robot_set_est_pos_notrans(TRANS_X(x), TRANS_Y(y), TRANS_ANG(phi))