From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 24 Apr 2009 09:15:19 +0000 (+0200)
Subject: Revert "robofsm: Added custom initialization of Kalman filter"
X-Git-Tag: eb2009~35^2~1
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/2fd65b6ee2aa96a0f1ee6d4981ccbb86d76220b7

Revert "robofsm: Added custom initialization of Kalman filter"

This reverts commit 76677b8b7df162bd184abbd3139af9ce8d696aa4.
---

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 65a47cce..4a36e7ac 100644
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -171,28 +171,18 @@ 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];
@@ -227,6 +217,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) {
+		/*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 65a1ebe2..029bb0cd 100644
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -82,10 +82,8 @@ 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 6bf9cc03..469249b3 100644
--- a/src/robofsm/movehelper.h
+++ b/src/robofsm/movehelper.h
@@ -169,7 +169,6 @@ static inline void robot_translate_coordinates(double *x, double *y, double *phi
 	}
 }
 
-void init_ekf(float x, float y, float ang);
 
 
 #ifdef __cplusplus