LPE vision estimation fixes. (#5505)

launch/mavros.launch

@@ -6,11 +6,13 @@
     <arg name="gcs_url" default="udp://:14550@:14555" />
     <arg name="tgt_system" default="1" />
     <arg name="tgt_component" default="1" />
+    <arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
+    <arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
 
     <group ns="$(arg ns)">
         <include file="$(find mavros)/launch/node.launch">
-            <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
+            <arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
-            <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+            <arg name="config_yaml" value="$(arg config_yaml)" />
 
             <arg name="fcu_url" value="$(arg fcu_url)" />
             <arg name="gcs_url" value="$(arg gcs_url)" />
@@ -19,3 +21,5 @@
         </include>
     </group>
 </launch>
+
+<!-- vim: set ft=xml et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

launch/mavros_posix_sitl.launch

@@ -25,6 +25,9 @@
     <arg name="verbose" default="false"/>
     <arg name="paused" default="false"/>
 
+    <arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
+    <arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
+
     <include file="$(find px4)/launch/posix_sitl.launch">
         <arg name="x" value="$(arg x)"/>
         <arg name="y" value="$(arg y)"/>
@@ -48,6 +51,8 @@
         <arg name="ns" value="$(arg ns)"/>
         <arg name="gcs_url" value=""/> <!-- GCS link is provided by SITL -->
         <arg name="fcu_url" value="$(arg fcu_url)"/>
+        <arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
+        <arg name="config_yaml" value="$(arg config_yaml)" />
     </include>
 </launch>
 

launch/posix_sitl.launch

@@ -7,7 +7,6 @@
     <arg name="R" default="0"/>
     <arg name="P" default="0"/>
     <arg name="Y" default="0"/> 
-
     <arg name="est" default="lpe"/>
     <arg name="vehicle" default="iris"/>
     <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -57,7 +57,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_map_ref(),
 
 	// block parameters
-	_xy_pub_thresh(this, "XY_PUB"),
+	_pub_agl_z(this, "PUB_AGL_Z"),
+	_vxy_pub_thresh(this, "VXY_PUB"),
 	_z_pub_thresh(this, "Z_PUB"),
 	_sonar_z_stddev(this, "SNR_Z"),
 	_sonar_z_offset(this, "SNR_OFF_Z"),
@@ -78,9 +79,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_vision_z_stddev(this, "VIS_Z"),
 	_vision_on(this, "VIS_ON"),
 	_mocap_p_stddev(this, "VIC_P"),
+	_flow_gyro_comp(this, "FLW_GYRO_CMP"),
 	_flow_z_offset(this, "FLW_OFF_Z"),
-	_flow_xy_stddev(this, "FLW_XY"),
+	_flow_vxy_stddev(this, "FLW_VXY"),
-	_flow_xy_d_stddev(this, "FLW_XY_D"),
+	_flow_vxy_d_stddev(this, "FLW_VXY_D"),
+	_flow_vxy_r_stddev(this, "FLW_VXY_R"),
 	//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
 	//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
 	_flow_min_q(this, "FLW_QMIN"),
@@ -147,12 +150,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_altOriginInitialized(false),
 	_baroAltOrigin(0),
 	_gpsAltOrigin(0),
-	_visionOrigin(),
-	_mocapOrigin(),
-
-	// flow integration
-	_flowX(0),
-	_flowY(0),
 
 	// status
 	_validXY(false),
@@ -266,10 +263,6 @@ void BlockLocalPositionEstimator::update()
 	// 	_x(X_y) = 0;
 	// 	// reset Z or not? _x(X_z) = 0;
 
-	// 	// reset flow integral
-	// 	_flowX = 0;
-	// 	_flowY = 0;
-
 	// 	// we aren't moving, all velocities are zero
 	// 	_x(X_vx) = 0;
 	// 	_x(X_vy) = 0;
@@ -305,19 +298,25 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// is xy valid?
-	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
+	bool vxy_stddev_ok = false;
+
+	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
+		vxy_stddev_ok = true;
+	}
 
 	if (_validXY) {
 		// if valid and gps has timed out, set to not valid
-		if (!xy_stddev_ok && !_gpsInitialized) {
+		if (!vxy_stddev_ok && !_gpsInitialized) {
 			_validXY = false;
 		}
 
 	} else {
-		if (xy_stddev_ok) {
+		if (vxy_stddev_ok) {
+			if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
 				_validXY = true;
 			}
 		}
+	}
 
 	// is z valid?
 	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
@@ -380,6 +379,7 @@ void BlockLocalPositionEstimator::update()
 		// don't want it to take too long
 		if (!PX4_ISFINITE(_x(i))) {
 			reinit_x = true;
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
 			break;
 		}
 	}
@@ -684,7 +684,14 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().v_z_valid = _validZ;
 		_pub_lpos.get().x = xLP(X_x); 	// north
 		_pub_lpos.get().y = xLP(X_y);  	// east
+
+		if (_pub_agl_z.get()) {
+			_pub_lpos.get().z = -_aglLowPass.getState(); // agl
+
+		} else {
 			_pub_lpos.get().z = xLP(X_z); 	// down
+		}
+
 		_pub_lpos.get().vx = xLP(X_vx); // north
 		_pub_lpos.get().vy = xLP(X_vy); // east
 		_pub_lpos.get().vz = xLP(X_vz); // down

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -126,7 +126,7 @@ public:
 	enum {U_ax = 0, U_ay, U_az, n_u};
 	enum {Y_baro_z = 0, n_y_baro};
 	enum {Y_lidar_z = 0, n_y_lidar};
-	enum {Y_flow_x = 0, Y_flow_y, n_y_flow};
+	enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};
 	enum {Y_sonar_z = 0, n_y_sonar};
 	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
 	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
@@ -244,7 +244,8 @@ private:
 	struct map_projection_reference_s _map_ref;
 
 	// general parameters
-	BlockParamFloat  _xy_pub_thresh;
+	BlockParamInt  _pub_agl_z;
+	BlockParamFloat  _vxy_pub_thresh;
 	BlockParamFloat  _z_pub_thresh;
 
 	// sonar parameters
@@ -281,9 +282,11 @@ private:
 	BlockParamFloat  _mocap_p_stddev;
 
 	// flow parameters
+	BlockParamInt  _flow_gyro_comp;
 	BlockParamFloat  _flow_z_offset;
-	BlockParamFloat  _flow_xy_stddev;
+	BlockParamFloat  _flow_vxy_stddev;
-	BlockParamFloat  _flow_xy_d_stddev;
+	BlockParamFloat  _flow_vxy_d_stddev;
+	BlockParamFloat  _flow_vxy_r_stddev;
 	//BlockParamFloat  _flow_board_x_offs;
 	//BlockParamFloat  _flow_board_y_offs;
 	BlockParamInt    _flow_min_q;
@@ -351,12 +354,6 @@ private:
 	bool _altOriginInitialized;
 	float _baroAltOrigin;
 	float _gpsAltOrigin;
-	Vector3f _visionOrigin;
-	Vector3f _mocapOrigin;
-
-	// flow integration
-	float _flowX;
-	float _flowY;
 
 	// status
 	bool _validXY;

src/modules/local_position_estimator/params.c

@@ -2,6 +2,13 @@
 
 // 16 is max name length
 
+/**
+ * Publish AGL as Z
+ *
+ * @group Local Position Estimator
+ * @boolean
+ */
+PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
 
 /**
  * Optical flow z offset from center
@@ -15,7 +22,18 @@
 PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
 
 /**
- * Optical flow xy standard deviation.
+ * Optical flow gyro compensation
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min -1
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
+
+/**
+ * Optical flow xy velocity standard deviation.
  *
  * @group Local Position Estimator
  * @unit m
@@ -23,10 +41,10 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
  * @max 1
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
+PARAM_DEFINE_FLOAT(LPE_FLW_VXY, 0.04f);
 
 /**
- * Optical flow xy standard deviation linear factor on distance
+ * Optical flow xy velocity standard deviation linear factor on distance
  *
  * @group Local Position Estimator
  * @unit m / m
@@ -34,7 +52,18 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
  * @max 1
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
+PARAM_DEFINE_FLOAT(LPE_FLW_VXY_D, 0.04f);
+
+/**
+ * Optical flow xy velocity standard deviation linear factor on rotation rate
+ *
+ * @group Local Position Estimator
+ * @unit m / m
+ * @min 0.01
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_FLW_VXY_R, 1.0f);
 
 /**
  * Optical flow minimum quality threshold
@@ -44,7 +73,7 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
  * @max 255
  * @decimal 0
  */
-PARAM_DEFINE_INT32(LPE_FLW_QMIN, 75);
+PARAM_DEFINE_INT32(LPE_FLW_QMIN, 150);
 
 /**
  * Sonar z standard deviation.
@@ -225,7 +254,7 @@ PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
  * @max 1
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
+PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f);
 
 /**
  * Vision z standard deviation.
@@ -328,7 +357,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
  * @max 2
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
+PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f);
 
 /**
  * Local origin latitude for nav w/o GPS
@@ -364,15 +393,15 @@ PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
 PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
 
 /**
- * Required xy standard deviation to publish position
+ * Required velocity xy standard deviation to publish position
  *
  * @group Local Position Estimator
- * @unit m
+ * @unit m/s
- * @min 0.3
+ * @min 0.01
- * @max 5.0
+ * @max 1.0
- * @decimal 1
+ * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_XY_PUB, 1.0f);
+PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.1f);
 
 /**
  * Required z standard deviation to publish altitude/ terrain

src/modules/local_position_estimator/sensors/flow.cpp

@@ -29,10 +29,6 @@ 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;
 	}
@@ -46,6 +42,11 @@ void BlockLocalPositionEstimator::flowDeinit()
 
 int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 {
+	// check for sane pitch/roll
+	if (_sub_att.get().roll > 0.5f || _sub_att.get().pitch > 0.5f) {
+		return -1;
+	}
+
 	// check for agl
 	if (agl() < flow_min_agl) {
 		return -1;
@@ -65,33 +66,25 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 
 	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
 
-	// check for global accuracy
-	if (_gpsInitialized) {
-		double  lat = _sub_gps.get().lat * 1.0e-7;
-		double  lon = _sub_gps.get().lon * 1.0e-7;
-		float px = 0;
-		float py = 0;
-		map_projection_project(&_map_ref, lat, lon, &px, &py);
-		Vector2f delta(px - _flowX, py - _flowY);
-
-		if (delta.norm() > 3) {
-			mavlink_and_console_log_info(&mavlink_log_pub,
-						     "[lpe] flow too far from GPS, resetting position");
-			_flowX = px;
-			_flowY = py;
-			return -1;
-		}
-	}
-
 	// optical flow in x, y axis
 	float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
 	float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
+	float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
+
+	if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
+		return -1;
+	}
 
 	// angular rotation in x, y axis
-	float gyro_x_rad = _flow_gyro_x_high_pass.update(
+	float gyro_x_rad = 0;
+	float gyro_y_rad = 0;
+
+	if (_flow_gyro_comp.get()) {
+		gyro_x_rad = _flow_gyro_x_high_pass.update(
 				     _sub_flow.get().gyro_x_rate_integral);
-	float gyro_y_rad = _flow_gyro_y_high_pass.update(
+		gyro_y_rad = _flow_gyro_y_high_pass.update(
 				     _sub_flow.get().gyro_y_rate_integral);
+	}
 
 	//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
 	//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
@@ -107,19 +100,15 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	Matrix3f R_nb(_sub_att.get().R);
 	Vector3f delta_n = R_nb * delta_b;
 
-	// flow integration
-	_flowX += delta_n(0);
-	_flowY += delta_n(1);
-
-	// measurement
-	y(Y_flow_x) = _flowX;
-	y(Y_flow_y) = _flowY;
-
-	_flowQStats.update(Scalarf(_sub_flow.get().quality));
-
 	// imporant to timestamp flow even if distance is bad
 	_time_last_flow = _timeStamp;
 
+	// measurement
+	y(Y_flow_vx) = delta_n(0) / dt_flow;
+	y(Y_flow_vy) = delta_n(1) / dt_flow;
+
+	_flowQStats.update(Scalarf(_sub_flow.get().quality));
+
 	return OK;
 }
 
@@ -133,15 +122,20 @@ void BlockLocalPositionEstimator::flowCorrect()
 	// flow measurement matrix and noise matrix
 	Matrix<float, n_y_flow, n_x> C;
 	C.setZero();
-	C(Y_flow_x, X_x) = 1;
+	C(Y_flow_vx, X_vx) = 1;
-	C(Y_flow_y, X_y) = 1;
+	C(Y_flow_vy, X_vy) = 1;
 
 	SquareMatrix<float, n_y_flow> R;
 	R.setZero();
 	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 ;
+	float rot_rate_norm = sqrtf(_sub_att.get().rollspeed * _sub_att.get().rollspeed
-	R(Y_flow_x, Y_flow_x) = flow_xy_stddev * flow_xy_stddev;
+				    + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
-	R(Y_flow_y, Y_flow_y) = R(Y_flow_x, Y_flow_x);
+				    + _sub_att.get().yawspeed * _sub_att.get().yawspeed);
+	float flow_vxy_stddev = _flow_vxy_stddev.get()
+				+ _flow_vxy_d_stddev.get() * d
+				+ _flow_vxy_r_stddev.get() * rot_rate_norm;
+	R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
+	R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
 
 	// residual
 	Vector<float, 2> r = y - C * _x;
@@ -176,11 +170,6 @@ void BlockLocalPositionEstimator::flowCorrect()
 		_x += dx;
 		_P -= K * C * _P;
 
-	} else {
-		// reset flow integral to current estimate of position
-		// if a fault occurred
-		_flowX = _x(X_x);
-		_flowY = _x(X_y);
 	}
 
 }

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -49,10 +49,12 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
 	}
 
 	// update stats
-	_lidarStats.update(Scalarf(d + _lidar_z_offset.get()));
+	_lidarStats.update(Scalarf(d));
 	_time_last_lidar = _timeStamp;
 	y.setZero();
-	y(0) = d;
+	y(0) = (d + _lidar_z_offset.get()) *
+	       cosf(_sub_att.get().roll) *
+	       cosf(_sub_att.get().pitch);
 	return OK;
 }
 
@@ -63,11 +65,6 @@ void BlockLocalPositionEstimator::lidarCorrect()
 
 	if (lidarMeasure(y) != OK) { return; }
 
-	// account for leaning
-	y(0) = y(0) *
-	       cosf(_sub_att.get().roll) *
-	       cosf(_sub_att.get().pitch);
-
 	// measurement matrix
 	Matrix<float, n_y_lidar, n_x> C;
 	C.setZero();

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -21,7 +21,6 @@ void BlockLocalPositionEstimator::mocapInit()
 
 	// if finished
 	if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
-		_mocapOrigin = _mocapStats.getMean();
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
 					     "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
 					     double(_mocapStats.getMean()(0)),
@@ -35,7 +34,7 @@ void BlockLocalPositionEstimator::mocapInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
-			_altOrigin = _mocapOrigin(2);
+			_altOrigin = 0;
 		}
 	}
 }
@@ -58,9 +57,6 @@ void BlockLocalPositionEstimator::mocapCorrect()
 
 	if (mocapMeasure(y) != OK) { return; }
 
-	// make measurement relative to origin
-	y -= _mocapOrigin;
-
 	// mocap measurement matrix, measures position
 	Matrix<float, n_y_mocap, n_x> C;
 	C.setZero();

src/modules/local_position_estimator/sensors/vision.cpp

@@ -6,8 +6,13 @@ extern orb_advert_t mavlink_log_pub;
 
 // required number of samples for sensor
 // to initialize
-static const uint32_t 		REQ_VISION_INIT_COUNT = 20;
+
-static const uint32_t 		VISION_TIMEOUT =    500000;	// 0.5 s
+// this is a vision based position measurement so we assume as soon as we get one
+// measurement it is initialized, we also don't want to deinitialize it because
+// this will throw away a correction before it starts using the data so we
+// set the timeout to 10 seconds
+static const uint32_t 		REQ_VISION_INIT_COUNT = 1;
+static const uint32_t 		VISION_TIMEOUT =    10000000;	// 10 s
 
 void BlockLocalPositionEstimator::visionInit()
 {
@@ -21,7 +26,6 @@ void BlockLocalPositionEstimator::visionInit()
 
 	// increament sums for mean
 	if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
-		_visionOrigin = _visionStats.getMean();
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
 					     "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
 					     double(_visionStats.getMean()(0)),
@@ -35,7 +39,7 @@ void BlockLocalPositionEstimator::visionInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
-			_altOrigin = _visionOrigin(2);
+			_altOrigin = 0;
 		}
 	}
 }
@@ -58,9 +62,6 @@ void BlockLocalPositionEstimator::visionCorrect()
 
 	if (visionMeasure(y) != OK) { return; }
 
-	// make measurement relative to origin
-	y -= _visionOrigin;
-
 	// vision measurement matrix, measures position
 	Matrix<float, n_y_vision, n_x> C;
 	C.setZero();