diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 5622c1199b..3f768b4067 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/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"), diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 9fd0ecfcb1..cc7c0932bb 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/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; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 3e7833ac3e..56bda8579f 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -21,6 +21,17 @@ PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0); */ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f); +/** + * Optical flow scale + * + * @group Local Position Estimator + * @unit m + * @min 0.1 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f); + /** * Optical flow gyro compensation * @@ -32,39 +43,6 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f); */ PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1); -/** - * Optical flow xy velocity standard deviation. - * - * @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 - * @max 1 - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(LPE_FLW_VXY_R, 1.0f); - /** * 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 diff --git a/src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf b/src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf new file mode 100644 index 0000000000..fb5c03530c Binary files /dev/null and b/src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf differ diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 69c967ed6e..89f8ad23f3 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/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 &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 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);