[RELEASE/1.14] Opt flow fixes

src/modules/ekf2/EKF/common.h

@@ -416,6 +416,7 @@ struct parameters {
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
 	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
 	int32_t flow_qual_min{1};               ///< minimum acceptable quality integer from  the flow sensor
+	int32_t flow_qual_min_gnd{0};           ///< minimum acceptable quality integer from  the flow sensor when on ground
 	float flow_innov_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)
 
 	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -760,9 +760,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
-#if defined(CONFIG_EKF2_RANGE_FINDER)
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
-#endif // CONFIG_EKF2_RANGE_FINDER
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
@@ -1026,15 +1024,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
-#if defined(CONFIG_EKF2_RANGE_FINDER)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
 			float height = _terrain_vpos - _state.pos(2);
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
-		} else
-#endif // CONFIG_EKF2_RANGE_FINDER
-		if (_control_status.flags.gnd_effect) {
+		} else if (_control_status.flags.gnd_effect) {
 			// Turn off ground effect compensation if it times out
 			if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 				_control_status.flags.gnd_effect = false;

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -77,6 +77,16 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
 	aid_src.observation_variance[0] = R_LOS;
 	aid_src.observation_variance[1] = R_LOS;
+
+	const Vector24f state_vector = getStateAtFusionHorizonAsVector();
+
+	Vector2f innov_var;
+	Vector24f H;
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	innov_var.copyTo(aid_src.innovation_variance);
+
+	// run the innovation consistency check and record result
+	setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f));
 }
 
 void Ekf::fuseOptFlow()

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -59,7 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 
 	// Accumulate autopilot gyro data across the same time interval as the flow sensor
 	const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt));
-	if (_delta_time_of < 0.1f) {
+	if (_delta_time_of < 0.2f) {
 		_imu_del_ang_of += delta_angle;
 		_delta_time_of += imu_delayed.delta_ang_dt;
 
@@ -70,7 +70,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	}
 
 	if (_flow_data_ready) {
-		const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
+		int32_t min_quality = _params.flow_qual_min;
+		if (!_control_status.flags.in_air) {
+			min_quality = _params.flow_qual_min_gnd;
+		}
+
+		const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
 		const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
 		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
 

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -415,10 +415,12 @@ void Ekf::controlHaglFakeFusion()
 
 bool Ekf::isTerrainEstimateValid() const
 {
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// we have been fusing range finder measurements in the last 5 seconds
 	if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
 		return true;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds

src/modules/ekf2/EKF2.cpp

@@ -154,6 +154,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_of_n_min(_params->flow_noise),
 	_param_ekf2_of_n_max(_params->flow_noise_qual_min),
 	_param_ekf2_of_qmin(_params->flow_qual_min),
+	_param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd),
 	_param_ekf2_of_gate(_params->flow_innov_gate),
 	_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
 	_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
@@ -1295,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
+
+		// set dist bottom to scale flow innovation
+		const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
+		_preflt_checker.setDistBottom(dist_bottom);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -1461,13 +1466,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	// Distance to bottom surface (ground) in meters
-	// constrain the distance to ground to _rng_gnd_clearance
-	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
+	// Distance to bottom surface (ground) in meters, must be positive
+	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
-#endif // CONFIG_EKF2_RANGE_FINDER
 
 	_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
 	_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);

src/modules/ekf2/EKF2.hpp

@@ -662,7 +662,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
 		_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
 		(ParamExtInt<px4::params::EKF2_OF_QMIN>)
-		_param_ekf2_of_qmin, ///< minimum acceptable quality integer from  the flow sensor
+		_param_ekf2_of_qmin, ///< minimum acceptable quality integer from  the flow sensor when in air
+		(ParamExtInt<px4::params::EKF2_OF_QMIN_GND>)
+		_param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from  the flow sensor when on ground
 		(ParamExtFloat<px4::params::EKF2_OF_GATE>)
 		_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
 		(ParamExtFloat<px4::params::EKF2_OF_POS_X>)

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -85,7 +85,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
 	if (_is_using_flow_aiding) {
-		const Vector2f flow_innov = Vector2f(innov.flow);
+		const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom;
 		Vector2f flow_innov_lpf;
 		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
 		flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -77,6 +77,7 @@ class PreFlightChecker
 	void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
+	void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
 	void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
@@ -179,6 +180,7 @@ class PreFlightChecker
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	bool _is_using_flow_aiding {};
+	float _flow_dist_bottom {};
 	InnovationLpf _filter_flow_x_innov;	///< Preflight low pass filter optical flow innovation (rad)
 	InnovationLpf _filter_flow_y_innov;	///< Preflight low pass filter optical flow innovation (rad)
 

src/modules/ekf2/ekf2_params.c

@@ -901,14 +901,23 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f);
 PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
 
 /**
- * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN.
+ * Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN.
  *
  * @group EKF2
  * @min 0
  * @max 255
  */
 PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1);
 
+/**
+ * Optical Flow data will only be used on the ground if the sensor reports a quality metric >= EKF2_OF_QMIN_GND.
+ *
+ * @group EKF2
+ * @min 0
+ * @max 255
+ */
+PARAM_DEFINE_INT32(EKF2_OF_QMIN_GND, 0);
+
 /**
  * Gate size for optical flow fusion
  *