ekf2: use dedicated aid_src message for flow for terrain aiding

msg/EstimatorAidSource2d.msg

@@ -19,4 +19,4 @@ bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow

msg/EstimatorInnovations.msg

@@ -23,6 +23,7 @@ float32    aux_vvel	# vertical auxiliary velocity innovation from landing target
 
 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
+float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2)
 
 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)

src/modules/ekf2/EKF/ekf.h

@@ -115,6 +115,10 @@ public:
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
 
+	void getTerrainFlowInnov(float flow_innov[2]) const;
+	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]); }
+
 	void getHeadingInnov(float &heading_innov) const
 	{
 		if (_control_status.flags.mag_hdg) {
@@ -431,6 +435,7 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 
 	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:
 
@@ -570,6 +575,7 @@ private:
 	estimator_aid_source2d_s _aid_src_aux_vel{};
 
 	estimator_aid_source2d_s _aid_src_optical_flow{};
+	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
 
 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
@@ -745,7 +751,7 @@ private:
 	void startHaglFlowFusion();
 	void resetHaglFlow();
 	void stopHaglFlowFusion();
-	void fuseFlowForTerrain();
+	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
 
 	void controlHaglFakeFusion();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -427,6 +427,18 @@ void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
 	flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
 }
 
+void Ekf::getTerrainFlowInnov(float flow_innov[2]) const
+{
+	flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
+	flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
+}
+
+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];
+}
+
 // get the state vector at the delayed time horizon
 matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 {

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -254,7 +254,7 @@ void Ekf::controlHaglFlowFusion()
 	}
 
 	if (_flow_data_ready) {
-		updateOptFlow(_aid_src_optical_flow);
+		updateOptFlow(_aid_src_terrain_optical_flow);
 
 		const bool continuing_conditions_passing = _control_status.flags.in_air
 		                                           && !_control_status.flags.opt_flow
@@ -267,7 +267,7 @@ void Ekf::controlHaglFlowFusion()
 			if (continuing_conditions_passing) {
 
 				// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-				fuseFlowForTerrain();
+				fuseFlowForTerrain(_aid_src_terrain_optical_flow);
 				_flow_data_ready = false;
 
 				// TODO: do something when failing continuously the innovation check
@@ -298,15 +298,15 @@ void Ekf::startHaglFlowFusion()
 {
 	_hagl_sensor_status.flags.flow = true;
 	// TODO: do a reset instead of trying to fuse the data?
-	fuseFlowForTerrain();
+	fuseFlowForTerrain(_aid_src_terrain_optical_flow);
 	_flow_data_ready = false;
 }
 
 void Ekf::stopHaglFlowFusion()
 {
 	if (_hagl_sensor_status.flags.flow) {
-
 		_hagl_sensor_status.flags.flow = false;
+		resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
 	}
 }
 
@@ -318,11 +318,11 @@ void Ekf::resetHaglFlow()
 	_terrain_vpos_reset_counter++;
 }
 
-void Ekf::fuseFlowForTerrain()
+void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
 {
-	_aid_src_optical_flow.fusion_enabled = true;
+	flow.fusion_enabled = true;
 
-	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
+	const float R_LOS = flow.observation_variance[0];
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
@@ -332,10 +332,10 @@ void Ekf::fuseFlowForTerrain()
 	Vector2f innov_var;
 	float H;
 	sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
-	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
+	innov_var.copyTo(flow.innovation_variance);
 
-	if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
+	if ((flow.innovation_variance[0] < R_LOS)
-	    || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
+	    || (flow.innovation_variance[1] < R_LOS)) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
 		ECL_ERR("Opt flow error - covariance reset");
 		_terrain_var = 100.0f;
@@ -343,13 +343,13 @@ void Ekf::fuseFlowForTerrain()
 	}
 
 	// run the innovation consistency check and record result
-	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
+	setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
 
-	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
+	_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
-	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
+	_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
 
 	// if either axis fails we abort the fusion
-	if (_aid_src_optical_flow.innovation_rejected) {
+	if (flow.innovation_rejected) {
 		return;
 	}
 
@@ -360,14 +360,14 @@ void Ekf::fuseFlowForTerrain()
 
 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
+			sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H);
 
 			// recalculate the innovation using the updated state
 			const Vector2f vel_body = predictFlowVelBody();
 			range = predictFlowRange();
-			_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
+			flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1];
 
-			if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
+			if (flow.innovation_variance[1] < R_LOS) {
 				// we need to reinitialise the covariance matrix and abort this fusion step
 				ECL_ERR("Opt flow error - covariance reset");
 				_terrain_var = 100.0f;
@@ -375,9 +375,9 @@ void Ekf::fuseFlowForTerrain()
 			}
 		}
 
-		float Kfusion = _terrain_var * H / _aid_src_optical_flow.innovation_variance[index];
+		float Kfusion = _terrain_var * H / flow.innovation_variance[index];
 
-		_terrain_vpos += Kfusion * _aid_src_optical_flow.innovation[0];
+		_terrain_vpos += Kfusion * flow.innovation[0];
 		// constrain terrain to minimum allowed value and predict height above ground
 		_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
 

src/modules/ekf2/EKF2.cpp

@@ -787,6 +787,8 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 
 	// optical flow
 	PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
+	PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
+			       _estimator_aid_src_terrain_optical_flow_pub);
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -1073,6 +1075,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getBetaInnov(innovations.beta);
 	_ekf.getHaglInnov(innovations.hagl);
 	_ekf.getHaglRateInnov(innovations.hagl_rate);
+	_ekf.getTerrainFlowInnov(innovations.terr_flow);
 	// Not yet supported
 	innovations.aux_vvel = NAN;
 
@@ -1122,6 +1125,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getBetaInnovRatio(test_ratios.beta);
 	_ekf.getHaglInnovRatio(test_ratios.hagl);
 	_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
+	_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
 	// Not yet supported
 	test_ratios.aux_vvel = NAN;
 
@@ -1147,6 +1151,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getBetaInnovVar(variances.beta);
 	_ekf.getHaglInnovVar(variances.hagl);
 	_ekf.getHaglRateInnovVar(variances.hagl_rate);
+	_ekf.getTerrainFlowInnovVar(variances.terr_flow);
 	// Not yet supported
 	variances.aux_vvel = NAN;
 

src/modules/ekf2/EKF2.hpp

@@ -288,6 +288,7 @@ private:
 	hrt_abstime _status_aux_vel_pub_last{0};
 
 	hrt_abstime _status_optical_flow_pub_last{0};
+	hrt_abstime _status_terrain_optical_flow_pub_last{0};
 
 	float _last_baro_bias_published{};
 	float _last_gnss_hgt_bias_published{};
@@ -378,6 +379,7 @@ private:
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
 
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
+	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)};
 
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;

src/modules/logger/logged_topics.cpp

@@ -196,6 +196,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
 
 	// log all raw sensors at minimal rate (at least 1 Hz)
@@ -283,6 +284,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);
 
 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */