Improvements to lpe for flow and gps. (#5401)

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -79,12 +79,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocap_p_stddev(this, "VIC_P"),
 	_flow_z_offset(this, "FLW_OFF_Z"),
 	_flow_xy_stddev(this, "FLW_XY"),
+	_flow_xy_d_stddev(this, "FLW_XY_D"),
 	//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
 	//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
 	_flow_min_q(this, "FLW_QMIN"),
 	_pn_p_noise_density(this, "PN_P"),
 	_pn_v_noise_density(this, "PN_V"),
 	_pn_b_noise_density(this, "PN_B"),
+	_pn_t_noise_density(this, "PN_T"),
 	_t_max_grade(this, "T_MAX_GRADE"),
 
 	// init origin
@@ -150,7 +152,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// flow integration
 	_flowX(0),
 	_flowY(0),
-	_flowMeanQual(0),
 
 	// status
 	_validXY(false),
@@ -197,8 +198,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// initialize A, B,  P, x, u
 	_x.setZero();
 	_u.setZero();
-	_flowX = 0;
-	_flowY = 0;
 	initSS();
 
 	// perf counters
@@ -689,7 +688,11 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().dist_bottom = _aglLowPass.getState();
 		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
-		_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
+		// we estimate agl even when we don't have terrain info
+		// if you are in terrain following mode this is important
+		// so that if terrain estimation fails there isn't a
+		// sudden altitude jump
+		_pub_lpos.get().dist_bottom_valid = _validZ;
 		_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
 		_pub_lpos.update();
@@ -840,8 +843,11 @@ void BlockLocalPositionEstimator::updateSSParams()
 	_Q(X_bz, X_bz) = pn_b_sq;
 
 	// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
-	float pn_t_stddev = (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
-	_Q(X_tz, X_tz) = pn_t_stddev * pn_t_stddev;
+	float pn_t_noise_density =
+		_pn_t_noise_density.get() +
+		(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
+	_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
+
 }
 
 void BlockLocalPositionEstimator::predict()
@@ -853,6 +859,7 @@ void BlockLocalPositionEstimator::predict()
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
 		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
+		// note, bias is removed in dynamics function
 		_u = R_att * a;
 		_u(U_az) += 9.81f; // add g
 

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -98,10 +98,10 @@ class BlockLocalPositionEstimator : public control::SuperBlock
 // 	ax, ay, az (acceleration NED)
 //
 // states:
-// 	px, py, pz , ( position NED)
-// 	vx, vy, vz ( vel NED),
-// 	bx, by, bz ( accel bias)
-// 	tz (terrain altitude, ASL)
+// 	px, py, pz , ( position NED, m)
+// 	vx, vy, vz ( vel NED, m/s),
+// 	bx, by, bz ( accel bias, m/s^2)
+// 	tz (terrain altitude, ASL, m)
 //
 // measurements:
 //
@@ -184,6 +184,7 @@ private:
 	int  flowMeasure(Vector<float, n_y_flow> &y);
 	void flowCorrect();
 	void flowInit();
+	void flowDeinit();
 	void flowCheckTimeout();
 
 	// vision
@@ -282,6 +283,7 @@ private:
 	// flow parameters
 	BlockParamFloat  _flow_z_offset;
 	BlockParamFloat  _flow_xy_stddev;
+	BlockParamFloat  _flow_xy_d_stddev;
 	//BlockParamFloat  _flow_board_x_offs;
 	//BlockParamFloat  _flow_board_y_offs;
 	BlockParamInt    _flow_min_q;
@@ -290,6 +292,7 @@ private:
 	BlockParamFloat  _pn_p_noise_density;
 	BlockParamFloat  _pn_v_noise_density;
 	BlockParamFloat  _pn_b_noise_density;
+	BlockParamFloat  _pn_t_noise_density;
 	BlockParamFloat  _t_max_grade;
 
 	// init origin
@@ -354,7 +357,6 @@ private:
 	// flow integration
 	float _flowX;
 	float _flowY;
-	float _flowMeanQual;
 
 	// status
 	bool _validXY;

src/modules/local_position_estimator/params.c

@@ -25,6 +25,17 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
  */
 PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
 
+/**
+ * Optical flow xy standard deviation linear factor on distance
+ *
+ * @group Local Position Estimator
+ * @unit m / m
+ * @min 0.01
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
+
 /**
  * Optical flow minimum quality threshold
  *
@@ -285,8 +296,20 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
  */
 PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
 
+/**
+ * Terrain random walk noise density, hilly/outdoor (0.1), flat/Indoor (0.001)
+ *
+ * @group Local Position Estimator
+ * @unit (m/s)/(sqrt(hz))
+ * @min 0
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_PN_T, 0.001f);
+
 /**
  * Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg)
+ * Used to calculate increased terrain random walk nosie due to movement.
  *
  * @group Local Position Estimator
  * @unit %

src/modules/local_position_estimator/sensors/flow.cpp

@@ -29,11 +29,21 @@ void BlockLocalPositionEstimator::flowInit()
 					     "quality %d std %d",
 					     int(_flowQStats.getMean()(0)),
 					     int(_flowQStats.getStdDev()(0)));
+		// set flow x, y as estimate x, y at beginning of optical
+		// flow tracking
+		_flowX = _x(X_x);
+		_flowY = _x(X_y);
 		_flowInitialized = true;
 		_flowFault = FAULT_NONE;
 	}
 }
 
+void BlockLocalPositionEstimator::flowDeinit()
+{
+	_flowInitialized = false;
+	_flowQStats.reset();
+}
+
 int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 {
 	// check for agl
@@ -66,8 +76,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 
 		if (delta.norm() > 3) {
 			mavlink_and_console_log_info(&mavlink_log_pub,
-						     "[lpe] flow too far from GPS, disabled");
-			_flowInitialized = false;
+						     "[lpe] flow too far from GPS, resetting position");
+			_flowX = px;
+			_flowY = py;
 			return -1;
 		}
 	}
@@ -127,10 +138,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	SquareMatrix<float, n_y_flow> R;
 	R.setZero();
-	R(Y_flow_x, Y_flow_x) =
-		_flow_xy_stddev.get() * _flow_xy_stddev.get();
-	R(Y_flow_y, Y_flow_y) =
-		_flow_xy_stddev.get() * _flow_xy_stddev.get();
+	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
+	float flow_xy_stddev = _flow_xy_stddev.get() + _flow_xy_d_stddev.get() * d ;
+	R(Y_flow_x, Y_flow_x) = flow_xy_stddev * flow_xy_stddev;
+	R(Y_flow_y, Y_flow_y) = R(Y_flow_x, Y_flow_x);
 
 	// residual
 	Vector<float, 2> r = y - C * _x;
@@ -178,8 +189,7 @@ void BlockLocalPositionEstimator::flowCheckTimeout()
 {
 	if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
 		if (_flowInitialized) {
-			_flowInitialized = false;
-			_flowQStats.reset();
+			flowDeinit();
 			mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
 		}
 	}

src/modules/local_position_estimator/sensors/gps.cpp

@@ -148,6 +148,17 @@ void BlockLocalPositionEstimator::gpsCorrect()
 		var_z = _sub_gps.get().epv * _sub_gps.get().epv;
 	}
 
+	float gps_s_stddev =  _sub_gps.get().s_variance_m_s;
+
+	if (gps_s_stddev > _gps_vxy_stddev.get()) {
+		var_vxy = gps_s_stddev * gps_s_stddev;
+	}
+
+	if (gps_s_stddev > _gps_vz_stddev.get()) {
+		var_z = gps_s_stddev * gps_s_stddev;
+	}
+
+
 	R(0, 0) = var_xy;
 	R(1, 1) = var_xy;
 	R(2, 2) = var_z;

src/modules/mavlink/mavlink_messages.cpp

@@ -3158,12 +3158,33 @@ protected:
 			struct vehicle_local_position_s local_pos;
 			updated |= _local_pos_sub->update(&_local_pos_time, &local_pos);
 			msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
+
+			// publish this data if global isn't publishing
+			if (_global_pos_time == 0) {
+				if (local_pos.dist_bottom_valid) {
+					msg.bottom_clearance = local_pos.dist_bottom;
+					msg.altitude_terrain = msg.altitude_local - msg.bottom_clearance;
+
+				} else {
+					msg.bottom_clearance = NAN;
+					msg.altitude_terrain = NAN;
+				}
+			}
 		}
 
 		{
 			struct home_position_s home;
 			updated |= _home_sub->update(&_home_time, &home);
-			msg.altitude_relative = (_home_time > 0) ? (global_alt - home.alt) : NAN;
+
+			if (_global_pos_time > 0 and _home_time > 0) {
+				msg.altitude_relative = global_alt - home.alt;
+
+			} else if (_local_pos_time > 0 and _home_time > 0) {
+				msg.altitude_relative = msg.altitude_local;
+
+			} else {
+				msg.altitude_relative = NAN;
+			}
 		}
 
 		if (updated) {