robofsm: Added custom initialization of Kalman filter

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 4a36e7ac103b147fd41004a1157077d8cfd300f1..65a47cce87dde7551503dc2a0a703c21b3b81839 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -171,18 +171,28 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
        }
 }
 
+static real_t beacon_xy[3][2] = {
+       { 3.062, -0.05},
+       {-0.062,  1.05},
+       { 3.062,  2.162},
+};
+static real_t ekf_y[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
+static ekf8_t ekf8;
+
+void init_ekf(float x, float y, float ang)
+{
+               /*FIXME:reflect init pos & beacon coords accord.to our color */
+       real_t xy0[] = {x, y}; // 2.63, 0.35
+       ekf_y[3] = ekf_y[4] = 0.0;
+       ekf8_init(&ekf8, (real_t*)beacon_xy, D_MAX, 30.0, xy0, ekf_y);
+       ekf8.ekf.x[6] = ang;
+}
+
 static void do_estimation()
 {
-       static real_t beacon_xy[3][2] = {
-               { 3.062, -0.05},
-               {-0.062,  1.05},
-               { 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;
+       real_t *y = ekf_y;
        uint32_t t[3], odo[2];
        real_t x[8], P[8*8];
        int i, odo_received, err[5];
@@ -217,15 +227,6 @@ 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) {
-               /*FIXME:reflect init pos & beacon coords accord.to our color */
-               real_t xy0[] = {2.63, 0.35};
-               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_step(&ekf8, x, P, err, y);
 
        DBG("EKF: x=%f   y=%f   phi=%8.4f\n", x[0], x[1], x[6]*(180.0/M_PI));

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index 029bb0cd510c6bc0fcf547be2c5095ae17f86cf4..65a1ebe2099160bcba4aa26c68e9c9f5c245271f 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -82,8 +82,10 @@ 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);
+       init_ekf(x, y, phi);
        robot.est_pos.x = x;
        robot.est_pos.y = y;
        robot.est_pos.phi = phi;

diff --git a/src/robofsm/movehelper.h b/src/robofsm/movehelper.h
index 469249b38d926eef7d52fffedf0c2fc64cbde529..6bf9cc03c8a736f18fcd894bd75deac7aefa512d 100644 (file)
--- a/src/robofsm/movehelper.h
+++ b/src/robofsm/movehelper.h
@@ -169,6 +169,7 @@ static inline void robot_translate_coordinates(double *x, double *y, double *phi
        }
 }
 
+void init_ekf(float x, float y, float ang);
 
 
 #ifdef __cplusplus