LPE flow improvements.

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -82,9 +82,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocap_p_stddev(this, "VIC_P"),
 	_flow_gyro_comp(this, "FLW_GYRO_CMP"),
 	_flow_z_offset(this, "FLW_OFF_Z"),
-	_flow_vxy_stddev(this, "FLW_VXY"),
-	_flow_vxy_d_stddev(this, "FLW_VXY_D"),
-	_flow_vxy_r_stddev(this, "FLW_VXY_R"),
+	_flow_scale(this, "FLW_SCALE"),
 	//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
 	//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
 	_flow_min_q(this, "FLW_QMIN"),

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -286,9 +286,7 @@ private:
 	// flow parameters
 	BlockParamInt  _flow_gyro_comp;
 	BlockParamFloat  _flow_z_offset;
-	BlockParamFloat  _flow_vxy_stddev;
-	BlockParamFloat  _flow_vxy_d_stddev;
-	BlockParamFloat  _flow_vxy_r_stddev;
+	BlockParamFloat  _flow_scale;
 	//BlockParamFloat  _flow_board_x_offs;
 	//BlockParamFloat  _flow_board_y_offs;
 	BlockParamInt    _flow_min_q;

src/modules/local_position_estimator/params.c

@@ -22,48 +22,26 @@ PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
 PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
 
 /**
- * Optical flow gyro compensation
+ * Optical flow scale
  *
  * @group Local Position Estimator
  * @unit m
- * @min -1
- * @max 1
+ * @min 0.1
+ * @max 10.0
  * @decimal 3
  */
-PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
+PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f);
 
 /**
- * Optical flow xy velocity standard deviation.
+ * Optical flow gyro compensation
  *
  * @group Local Position Estimator
  * @unit m
- * @min 0.01
- * @max 1
- * @decimal 3
- */
-PARAM_DEFINE_FLOAT(LPE_FLW_VXY, 0.04f);
-
-/**
- * Optical flow xy velocity 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_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
+ * @min -1
  * @max 1
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_FLW_VXY_R, 1.0f);
+PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
 
 /**
  * Optical flow minimum quality threshold
@@ -132,7 +110,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
  * @max 2
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
+PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.012f);
 
 /**
  * Accelerometer z noise density
@@ -145,7 +123,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
  * @max 2
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);
+PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.02f);
 
 /**
  * Barometric presssure altitude z standard deviation.
@@ -412,7 +390,7 @@ PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
  * @max 1.0
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.1f);
+PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.3f);
 
 /**
  * Required z standard deviation to publish altitude/ terrain

src/modules/local_position_estimator/sensors/flow.cpp

@@ -8,7 +8,7 @@ extern orb_advert_t mavlink_log_pub;
 // required number of samples for sensor
 // to initialize
 static const uint32_t 		REQ_FLOW_INIT_COUNT = 10;
-static const uint32_t 		FLOW_TIMEOUT =   	1000000;	// 1 s
+static const uint32_t 		FLOW_TIMEOUT = 1000000;	// 1 s
 
 // minimum flow altitude
 static const float flow_min_agl = 0.05;
@@ -67,8 +67,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
 
 	// 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;
+	// TODO consider making flow scale a states of the kalman filter
+	float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
+	float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
 	float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
 
 	if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
@@ -127,13 +128,38 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	SquareMatrix<float, n_y_flow> R;
 	R.setZero();
-	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
-	float rot_rate_norm = sqrtf(_sub_att.get().rollspeed * _sub_att.get().rollspeed
-				    + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
-				    + _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;
+
+	// polynomial noise model, found using least squares fit
+	// h, h**2, v, v*h, v*h**2
+	const float p[5] = {0.04005232f, -0.00656446f, -0.26265873f,  0.13686658f, -0.00397357f};
+
+	// prevent extrapolation past end of polynomial fit by bounding independent variables
+	float h = agl();
+	float v = y.norm();
+	const float h_min = 2.0f;
+	const float h_max = 8.0f;
+	const float v_min = 0.5f;
+	const float v_max = 1.0f;
+
+	if (h > h_max) {
+		h = h_max;
+	}
+
+	if (h < h_min) {
+		h = h_min;
+	}
+
+	if (v > v_max) {
+		v = v_max;
+	}
+
+	if (v < v_min) {
+		v = v_min;
+	}
+
+	// compute polynomial value
+	float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
+
 	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);