ekf2 - preflt checks: scale flow innovation checks

Opt flow raw innovations can be really large on ground due to the small distance to the ground (vel = flow / dist). To make the pre-flight check more meaningful, scale it with the current distance.
PX4 · Jul 17, 2023 · eb23779 · eb23779
1 parent a07b8c0
commit eb23779
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 1 deletion.
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -1296,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)

diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/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);

diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/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)