From 613ec40d864e557fa76de658ee08b38401acc92c Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 8 Mar 2016 11:48:20 -0500
Subject: [PATCH] LPE: comment out gps delay handling, too much memory
 required.

---
 .../BlockLocalPositionEstimator.cpp           | 46 +++++++++----------
 .../BlockLocalPositionEstimator.hpp           | 10 ++--
 .../local_position_estimator_main.cpp         |  3 +-
 3 files changed, 29 insertions(+), 30 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index ad3bd2901d..5685e7fdd5 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -105,14 +105,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_gpsStats(this, ""),
 
 	// stats
-	_xDelay(this, ""),
-	_PDelay(this, ""),
-	_tDelay(this, ""),
+	//_xDelay(this, ""),
+	//_PDelay(this, ""),
+	//_tDelay(this, ""),
 
 	// misc
 	_polls(),
 	_timeStamp(hrt_absolute_time()),
-	_time_last_hist(0),
+	//_time_last_hist(0),
 	_time_last_xy(0),
 	_time_last_z(0),
 	_time_last_tz(0),
@@ -1034,11 +1034,11 @@ void BlockLocalPositionEstimator::predict()
 	       B * R * B.transpose() + Q) * getDt();
 
 	// propagate delayed state
-	if (_time_last_hist == 0 || _time_last_hist - _timeStamp > 1000) {
-		_xDelay.update(_x);
-		_PDelay.update(_P);
-		_tDelay.update(Scalar<uint64_t>(_timeStamp));
-	}
+	//if (_time_last_hist == 0 || _time_last_hist - _timeStamp > 1000) {
+	//_xDelay.update(_x);
+	//_PDelay.update(_P);
+	//_tDelay.update(Scalar<uint64_t>(_timeStamp));
+	//}
 }
 
 void BlockLocalPositionEstimator::correctFlow()
@@ -1488,23 +1488,23 @@ void BlockLocalPositionEstimator::correctGps()
 	R(5, 5) = var_vz;
 
 	// get delayed x and P
-	uint64_t t_delay = 0;
-	int i = 0;
+	//uint64_t t_delay = 0;
+	//int i = 0;
 
-	for (i = 0; i < HIST_LEN; i++) {
-		t_delay += _tDelay.get(i)(0, 0);
+	//for (i = 0; i < HIST_LEN; i++) {
+	//t_delay += _tDelay.get(i)(0, 0);
 
-		if (t_delay > 2000) {
-			break;
-		}
-	}
+	//if (t_delay > 2000) {
+	//break;
+	//}
+	//}
 
-	Vector<float, n_x> x0 = _xDelay.get(i);
-	Matrix<float, n_x, n_x> P0 = _PDelay.get(i);
+	//Vector<float, n_x> x0 = _xDelay.get(i);
+	//Matrix<float, n_x, n_x> P0 = _PDelay.get(i);
 
 	// residual
-	Vector<float, n_y_gps> r = y - C * x0;
-	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * P0 * C.transpose() + R);
+	Vector<float, n_y_gps> r = y - C * _x;
+	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);
@@ -1536,9 +1536,9 @@ void BlockLocalPositionEstimator::correctGps()
 
 	// kalman filter correction if no hard fault
 	if (_gpsFault < FAULT_SEVERE) {
-		Matrix<float, n_x, n_y_gps> K = P0 * C.transpose() * S_I;
+		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
 		_x += K * r;
-		_P -= K * C * P0;
+		_P -= K * C * _P;
 	}
 }
 
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 4241d57d39..dcbeb69dc0 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -41,7 +41,7 @@ using namespace Eigen;
 
 using namespace control;
 
-static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay
+//static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay
 
 enum fault_t {
 	FAULT_NONE = 0,
@@ -257,14 +257,14 @@ private:
 	BlockStats<double, 3> _gpsStats;
 
 	// delay blocks
-	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
-	BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay;
-	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
+	//BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
+	//BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay;
+	//BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
 
 	// misc
 	struct pollfd _polls[3];
 	uint64_t _timeStamp;
-	uint64_t _time_last_hist;
+	//uint64_t _time_last_hist;
 	uint64_t _time_last_xy;
 	uint64_t _time_last_z;
 	uint64_t _time_last_tz;
diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp
index b0cd25554e..03f7807d9a 100644
--- a/src/modules/local_position_estimator/local_position_estimator_main.cpp
+++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp
@@ -152,11 +152,10 @@ int local_position_estimator_thread_main(int argc, char *argv[])
 
 	using namespace control;
 
+	BlockLocalPositionEstimator est;
 
 	thread_running = true;
 
-	BlockLocalPositionEstimator est;
-
 	while (!thread_should_exit) {
 		est.update();
 	}