From a474c291b59afc98e65e7f60b9491ccdeb89990a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Jul 2024 14:19:33 -0400 Subject: [PATCH] ekf2: filter flow vel (used for flow velocity reset) - individual flow samples can be quite erratic --- .../optical_flow/optical_flow_control.cpp | 18 ++++++++++++++---- src/modules/ekf2/EKF/ekf.h | 9 +++++---- src/modules/ekf2/test/test_EKF_flow.cpp | 6 ++---- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 3c4c031f31..4d4023f088 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -107,10 +107,17 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // compute the velocities in body and local frames from corrected optical flow measurement for logging only const float range = predictFlowRange(); - _flow_vel_body(0) = -flow_compensated(1) * range; - _flow_vel_body(1) = flow_compensated(0) * range; - _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); + const Vector2f flow_vel_body{-flow_compensated(1) *range, flow_compensated(0) *range}; + if (_flow_counter == 0) { + _flow_vel_body.reset(flow_vel_body); + _flow_counter = 1; + + } else { + + _flow_vel_body.update(flow_vel_body); + _flow_counter++; + } // Check if we are in-air and require optical flow to control position drift bool is_flow_required = _control_status.flags.in_air @@ -131,6 +138,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) && is_quality_good && is_magnitude_good && is_tilt_good + && (_flow_counter > 10) && (isTerrainEstimateValid() || isHorizontalAidingActive()) && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching @@ -204,7 +212,7 @@ void Ekf::resetFlowFusion() { ECL_INFO("reset velocity to flow"); _information_events.flags.reset_vel_to_flow = true; - resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed)); + resetHorizontalVelocityTo(getFlowVelNE(), calcOptFlowMeasVar(_flow_sample_delayed)); // reset position, estimate is relative to initial position in this mode, so we start with zero error if (!_control_status.flags.in_air) { @@ -258,6 +266,8 @@ void Ekf::stopFlowFusion() _innov_check_fail_status.flags.reject_optflow_X = false; _innov_check_fail_status.flags.reject_optflow_Y = false; + + _flow_counter = 0; } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2a5b7ab317..9243cc73ee 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -121,8 +121,8 @@ public: #if defined(CONFIG_EKF2_OPTICAL_FLOW) const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } - const Vector2f &getFlowVelBody() const { return _flow_vel_body; } - const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } + const Vector2f &getFlowVelBody() const { return _flow_vel_body.getState(); } + Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); } const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; } const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; } @@ -618,10 +618,11 @@ private: // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) - Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) - Vector2f _flow_vel_ne{}; ///< velocity from corrected flow measurement (local frame) (m/s) Vector3f _ref_body_rate{}; + AlphaFilter _flow_vel_body{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) + uint32_t _flow_counter{0}; ///< number of flow samples read for initialization + Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive #endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 787560ce1b..650d168a98 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -203,10 +203,8 @@ TEST_F(EkfFlowTest, inAirConvergence) _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); - // Let it reset but not fuse more measurements. We actually need to send 2 - // samples to get a reset because the first one cannot be used as the gyro - // compensation needs to be accumulated between two samples. - _sensor_simulator.runTrajectorySeconds(0.14); + + _sensor_simulator.runTrajectorySeconds(1.0); // THEN: estimated velocity should match simulated velocity Vector3f estimated_velocity = _ekf->getVelocity();