Added vision delay compensation to LPE. (#5585)

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -77,6 +77,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_gps_epv_max(this, "EPV_MAX"),
 	_vision_xy_stddev(this, "VIS_XY"),
 	_vision_z_stddev(this, "VIS_Z"),
+	_vision_delay(this, "VIS_DELAY"),
 	_vision_on(this, "VIS_ON"),
 	_mocap_p_stddev(this, "VIC_P"),
 	_flow_gyro_comp(this, "FLW_GYRO_CMP"),
@@ -913,3 +914,26 @@ void BlockLocalPositionEstimator::predict()
 	_xLowPass.update(_x);
 	_aglLowPass.update(agl());
 }
+
+int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
+{
+	float t_delay = 0;
+	uint8_t i_hist = 0;
+
+	for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
+		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
+
+		if (t_delay > delay) {
+			break;
+		}
+	}
+
+	*periods = i_hist;
+
+	if (t_delay > DELAY_MAX) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] delayed data too old: %8.4f", double(t_delay));
+		return -1;
+	}
+
+	return OK;
+}

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -33,10 +33,10 @@
 using namespace matrix;
 using namespace control;
 
-static const float GPS_DELAY_MAX = 0.5f; // seconds
+static const float DELAY_MAX = 0.5f; // seconds
 static const float HIST_STEP = 0.05f; // 20 hz
 static const float BIAS_MAX = 1e-1f;
-static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP;
+static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
 static const size_t N_DIST_SUBS = 4;
 
 enum fault_t {
@@ -207,6 +207,7 @@ private:
 	void correctionLogic(Vector<float, n_x> &dx);
 	void covPropagationLogic(Matrix<float, n_x, n_x> &dP);
 	void detectDistanceSensors();
+	int getDelayPeriods(float delay, uint8_t *periods);
 
 	// publications
 	void publishLocalPos();
@@ -276,6 +277,7 @@ private:
 	// vision parameters
 	BlockParamFloat  _vision_xy_stddev;
 	BlockParamFloat  _vision_z_stddev;
+	BlockParamFloat  _vision_delay;
 	BlockParamInt   _vision_on;
 
 	// mocap parameters

src/modules/local_position_estimator/params.c

@@ -245,6 +245,17 @@ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
  */
 PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
 
+/**
+ * Vision delay compensaton
+ *
+ * @group Local Position Estimator
+ * @unit sec
+ * @min 0
+ * @max 0.1
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(LPE_VIS_DELAY, 0.1f);
+
 /**
  * Vision xy standard deviation.
  *

src/modules/local_position_estimator/sensors/gps.cpp

@@ -166,24 +166,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	R(4, 4) = var_vxy;
 	R(5, 5) = var_vz;
 
-	// get delayed x and P
-	float t_delay = 0;
-	int i_hist = 0;
+	// get delayed x
+	uint8_t i_hist = 0;
 
-	for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
-		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
-
-		if (t_delay > _gps_delay.get()) {
-			break;
-		}
-	}
-
-	// if you are 3 steps past the delay you wanted, this
-	// data is probably too old to use
-	if (t_delay > GPS_DELAY_MAX) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay));
-		return;
-	}
+	if (getDelayPeriods(_gps_delay.get(), &i_hist)  < 0) { return; }
 
 	Vector<float, n_x> x0 = _xDelay.get(i_hist);
 

src/modules/local_position_estimator/sensors/vision.cpp

@@ -76,9 +76,16 @@ void BlockLocalPositionEstimator::visionCorrect()
 	R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
 	R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
 
+	// vision delayed x
+	uint8_t i_hist = 0;
+
+	if (getDelayPeriods(_vision_delay.get(), &i_hist)  < 0) { return; }
+
+	Vector<float, n_x> x0 = _xDelay.get(i_hist);
+
 	// residual
 	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
-	Matrix<float, n_y_vision, 1> r = y - C * _x;
+	Matrix<float, n_y_vision, 1> r = y - C * x0;
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);