ekf2: add kconfig option to enable/disable optical flow fusion

src/modules/ekf2/EKF/CMakeLists.txt

@@ -51,8 +51,6 @@ list(APPEND EKF_SRCS
 	imu_down_sampler.cpp
 	mag_control.cpp
 	mag_fusion.cpp
-	optical_flow_control.cpp
-	optflow_fusion.cpp
 	output_predictor.cpp
 	range_finder_consistency_check.cpp
 	range_height_control.cpp
@@ -85,11 +83,17 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
 	)
 endif()
 
-
 if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_OPTICAL_FLOW)
+	list(APPEND EKF_SRCS
+		optical_flow_control.cpp
+		optflow_fusion.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS sideslip_fusion.cpp)
 endif()

src/modules/ekf2/EKF/common.h

@@ -310,7 +310,6 @@ struct parameters {
 	float mag_delay_ms{0.0f};               ///< magnetometer measurement delay relative to the IMU (mSec)
 	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
 	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
-	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 
 	// input noise
@@ -412,11 +411,15 @@ struct parameters {
 	// gravity fusion
 	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
+
 	// optical flow fusion
 	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
 	float flow_innov_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// these parameters control the strictness of GPS quality checks used to determine if the GPS is
 	// good enough to set a local origin and commence aiding

src/modules/ekf2/EKF/control.cpp

@@ -104,7 +104,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 	// control use of observations for aiding
 	controlMagFusion();
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	controlOpticalFlowFusion(imu_delayed);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	controlGpsFusion(imu_delayed);
 
 #if defined(CONFIG_EKF2_AIRSPEED)

src/modules/ekf2/EKF/ekf.cpp

@@ -148,8 +148,10 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_aux_vel);
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	resetEstimatorAidStatus(_aid_src_optical_flow);
 	resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 }
 
 bool Ekf::update()

src/modules/ekf2/EKF/ekf.h

@@ -105,6 +105,7 @@ public:
 	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	void getFlowInnov(float flow_innov[2]) const;
 	void getFlowInnovVar(float flow_innov_var[2]) const;
 	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
@@ -122,6 +123,10 @@ public:
 	void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
 	void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
 
+	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
+	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	void getHeadingInnov(float &heading_innov) const
 	{
 		if (_control_status.flags.mag_hdg) {
@@ -494,9 +499,6 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
-	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
-	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
-
 private:
 
 	// set the internal states and status to their default value
@@ -645,8 +647,10 @@ private:
 	estimator_aid_source2d_s _aid_src_aux_vel{};
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	estimator_aid_source2d_s _aid_src_optical_flow{};
 	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
@@ -809,12 +813,6 @@ private:
 
 	void resetVerticalVelocityToZero();
 
-	// fuse optical flow line of sight rate measurements
-	void updateOptFlow(estimator_aid_source2d_s &aid_src);
-	void fuseOptFlow();
-	float predictFlowRange();
-	Vector2f predictFlowVelBody();
-
 	// horizontal and vertical position aid source
 	void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
 	void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
@@ -850,6 +848,7 @@ private:
 	void stopHaglRngFusion();
 	float getRngVar();
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// update the terrain vertical position estimate using an optical flow measurement
 	void controlHaglFlowFusion();
 	void startHaglFlowFusion();
@@ -857,6 +856,13 @@ private:
 	void stopHaglFlowFusion();
 	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
 
+	// fuse optical flow line of sight rate measurements
+	void updateOptFlow(estimator_aid_source2d_s &aid_src);
+	void fuseOptFlow();
+	float predictFlowRange();
+	Vector2f predictFlowVelBody();
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	void controlHaglFakeFusion();
 
 	// reset the heading and magnetic field states using the declination and magnetometer measurements

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -355,6 +355,7 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 }
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void Ekf::getFlowInnov(float flow_innov[2]) const
 {
 	flow_innov[0] = _aid_src_optical_flow.innovation[0];
@@ -378,6 +379,7 @@ void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const
 	flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
 	flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
 }
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 // get the state vector at the delayed time horizon
 matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
@@ -519,10 +521,12 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	if (_horizontal_deadreckon_time_exceeded) {
 		float vel_err_conservative = 0.0f;
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
 			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 		if (_control_status.flags.gps) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
@@ -568,7 +572,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 
 	// TODO : calculate visual odometry limits
 	const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);
-	const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
 
 	// Keep within range sensor limit when using rangefinder as primary height source
 	if (relying_on_rangefinder) {
@@ -576,7 +579,10 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_max = rangefinder_hagl_max;
 	}
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// Keep within flow AND range sensor limits when exclusively using optical flow
+	const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
+
 	if (relying_on_optical_flow) {
 		// Calculate optical flow limits
 		const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
@@ -591,6 +597,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_min = flow_hagl_min;
 		*hagl_max = flow_hagl_max;
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 }
 
 void Ekf::resetImuBias()
@@ -680,10 +687,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
 		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
 		vel = math::max(of_vel, FLT_MIN);
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// return the combined vertical position innovation test ratio
 	float hgt_sum = 0.f;
@@ -802,7 +811,10 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
 				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
 
-	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+	bool optFlowAiding = false;
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	bool airDataAiding = false;
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -53,7 +53,9 @@ EstimatorInterface::~EstimatorInterface()
 #if defined(CONFIG_EKF2_AIRSPEED)
 	delete _airspeed_buffer;
 #endif // CONFIG_EKF2_AIRSPEED
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	delete _flow_buffer;
+#endif // _flow_buffer
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	delete _ext_vision_buffer;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -322,6 +324,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 	}
 }
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 {
 	if (!_initialised) {
@@ -356,6 +359,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 		ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
@@ -560,9 +564,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
 	}
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_params.ev_ctrl > 0) {
@@ -704,9 +710,11 @@ void EstimatorInterface::print_status()
 	}
 #endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (_flow_buffer) {
 		printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_ext_vision_buffer) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -96,9 +96,19 @@ public:
 
 	void setRangeData(const rangeSample &range_sample);
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
 	void setOpticalFlowData(const flowSample &flow);
 
+	// set sensor limitations reported by the optical flow sensor
+	void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
+	{
+		_flow_max_rate = max_flow_rate;
+		_flow_min_distance = min_distance;
+		_flow_max_distance = max_distance;
+	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// set external vision position and attitude data
 	void setExtVisionData(const extVisionSample &evdata);
@@ -159,14 +169,6 @@ public:
 		_range_sensor.setLimits(min_distance, max_distance);
 	}
 
-	// set sensor limitations reported by the optical flow sensor
-	void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
-	{
-		_flow_max_rate = max_flow_rate;
-		_flow_min_distance = min_distance;
-		_flow_max_distance = max_distance;
-	}
-
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
 
@@ -302,8 +304,6 @@ protected:
 	airspeedSample _airspeed_sample_delayed{};
 #endif // CONFIG_EKF2_AIRSPEED
 
-	flowSample _flow_sample_delayed{};
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	extVisionSample _ev_sample_prev{};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -312,10 +312,16 @@ protected:
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
+
+	flowSample _flow_sample_delayed{};
+
 	// Sensor limitations
 	float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
 	float _flow_min_distance{0.0f};	///< minimum distance that the optical flow sensor can operate at (m)
 	float _flow_max_distance{10.f};	///< maximum distance that the optical flow sensor can operate at (m)
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
@@ -365,7 +371,6 @@ protected:
 #if defined(CONFIG_EKF2_AIRSPEED)
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
 #endif // CONFIG_EKF2_AIRSPEED
-	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};

src/modules/ekf2/EKF/gps_control.cpp

@@ -274,9 +274,19 @@ bool Ekf::shouldResetGpsFusion() const
 	/* We are relying on aiding to constrain drift so after a specified time
 	 * with no aiding we need to do something
 	 */
-	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-			&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-			&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
+	bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
+			&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+
+	if (has_horizontal_aiding_timed_out) {
+		// horizontal aiding hasn't timed out if optical flow still active
+		if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
+			has_horizontal_aiding_timed_out = false;
+		}
+	}
+
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -45,7 +45,10 @@
 
 void Ekf::initHagl()
 {
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	stopHaglFlowFusion();
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	stopHaglRngFusion();
 
 	// assume a ground clearance
@@ -66,7 +69,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
 	predictHagl(imu_delayed);
 
 	controlHaglRngFusion();
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	controlHaglFlowFusion();
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 	controlHaglFakeFusion();
 
 	// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
@@ -246,6 +251,7 @@ void Ekf::fuseHaglRng()
 	}
 }
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void Ekf::controlHaglFlowFusion()
 {
 	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
@@ -392,6 +398,7 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
 	//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?
 	_aid_src_optical_flow.fused = true;
 }
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 void Ekf::controlHaglFakeFusion()
 {