From 12489654eaf76557e2342522003e57df38e10a5a Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 8 Mar 2016 08:08:20 -0500
Subject: [PATCH] GPS delay compensation for LPE.

---
 .../BlockLocalPositionEstimator.cpp           | 50 +++++++++++++------
 .../BlockLocalPositionEstimator.hpp           |  5 ++
 src/modules/local_position_estimator/params.c |  4 +-
 3 files changed, 41 insertions(+), 18 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 352362bc52..a5746a0c5b 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -95,7 +95,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_flow_gyro_x_high_pass(this, "FGYRO_HP"),
 	_flow_gyro_y_high_pass(this, "FGYRO_HP"),
 
-	// sats
+	// stats
 	_baroStats(this, ""),
 	_sonarStats(this, ""),
 	_lidarStats(this, ""),
@@ -104,6 +104,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocapStats(this, ""),
 	_gpsStats(this, ""),
 
+	// stats
+	_gpsDelay(this, ""),
+
 	// misc
 	_polls(),
 	_timeStamp(hrt_absolute_time()),
@@ -586,11 +589,22 @@ void BlockLocalPositionEstimator::initGps()
 		return;
 	}
 
+	// gps measurement
+	Vector<double, 6> h;
+	h.setZero();
+	h(0) = _sub_gps.get().lat;
+	h(1) = _sub_gps.get().lon;
+	h(2) = _sub_gps.get().alt;
+	h(3) = _sub_gps.get().vel_n_m_s;
+	h(4) = _sub_gps.get().vel_e_m_s;
+	h(5) = _sub_gps.get().vel_d_m_s;
+	_gpsDelay.update(h);
+
 	// collect gps data
 	Vector3<double> p(
 		_sub_gps.get().lat * 1e-7,
 		_sub_gps.get().lon * 1e-7,
-		_sub_gps.get().alt * 1e-3f);
+		_sub_gps.get().alt * 1e-3);
 
 	// increament sums for mean
 	_gpsStats.update(p);
@@ -1420,30 +1434,34 @@ void BlockLocalPositionEstimator::correctGps()
 		return;
 	}
 
-	// gps measurement in local frame
-	double  lat = _sub_gps.get().lat * 1.0e-7;
-	double  lon = _sub_gps.get().lon * 1.0e-7;
-	float  alt = _sub_gps.get().alt * 1.0e-3f;
-	_time_last_gps = _timeStamp;
+	// gps measurement
+	Vector<double, 6> h0;
+	h0.setZero();
+	h0(0) = _sub_gps.get().lat;
+	h0(1) = _sub_gps.get().lon;
+	h0(2) = _sub_gps.get().alt;
+	h0(3) = _sub_gps.get().vel_n_m_s;
+	h0(4) = _sub_gps.get().vel_e_m_s;
+	h0(5) = _sub_gps.get().vel_d_m_s;
+	Vector<double, 6> h = _gpsDelay.update(h0);
 
-	// local coordinates
+	// gps measurement in local frame
+	_time_last_gps = _timeStamp;
+	double  lat = h(0) * 1.0e-7;
+	double  lon = h(1) * 1.0e-7;
+	float  alt = h(2) * 1.0e-3;
 	float px = 0;
 	float py = 0;
 	float pz = -(alt - _gpsAltHome);
 	map_projection_project(&_map_ref, lat, lon, &px, &py);
-
-	//printf("gps: lat %10g, lon, %10g alt %10g\n", lat, lon, double(alt));
-	//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
-	//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
-
 	Vector<float, 6> y;
 	y.setZero();
 	y(0) = px;
 	y(1) = py;
 	y(2) = pz;
-	y(3) = _sub_gps.get().vel_n_m_s;
-	y(4) = _sub_gps.get().vel_e_m_s;
-	y(5) = _sub_gps.get().vel_d_m_s;
+	y(3) = h(3);
+	y(4) = h(4);
+	y(5) = h(5);
 
 	// gps measurement matrix, measures position and velocity
 	Matrix<float, n_y_gps, n_x> C;
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 0eec7eea4c..eb819b03a2 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -41,6 +41,8 @@ using namespace Eigen;
 
 using namespace control;
 
+static const size_t GPS_HIST_LEN = 2; // delay, at 10 hz, (200 ms)
+
 enum fault_t {
 	FAULT_NONE = 0,
 	FAULT_MINOR,
@@ -254,6 +256,9 @@ private:
 	BlockStats<float, 3> _mocapStats;
 	BlockStats<double, 3> _gpsStats;
 
+	// delay blocks
+	BlockDelay<double, n_y_gps, GPS_HIST_LEN> _gpsDelay;
+
 	// misc
 	struct pollfd _polls[3];
 	uint64_t _timeStamp;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index a68648fb82..fc1589bde5 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -171,7 +171,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
  * @max 2
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
+PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 1.0f);
 
 /**
  * GPS z velocity standard deviation.
@@ -182,7 +182,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
  * @max 2
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
+PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 1.0f);
 
 /**
  * GPS max eph