From 47a61978802a0a6b5af0067e9df0b31ba09a1ab4 Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 24 Apr 2009 11:21:37 +0200
Subject: [PATCH] robofsm: EKF initialiation hopefully correct

---
 src/robofsm/motion-control.cc | 11 ++++++-----
 src/robofsm/movehelper.cc     |  4 +++-
 src/robofsm/movehelper.h      |  2 ++
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 4a36e7ac..68a1c09d 100644
--- 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 029bb0cd..42102082 100644
--- 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 469249b3..9f955977 100644
--- 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))
 
-- 
2.39.2