diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 83b1271ea4..f5e2ae7034 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1296,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _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 index bf7b62f6ac..1ac4b2dd26 100644 --- 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 index 5e9af2c4a9..a4fe6ab482 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -77,6 +77,7 @@ public: 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 @@ private: #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)