From 6c24413888a27bc46c570f09210a13caa59f96aa 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 --- msg/VehicleOpticalFlowVel.msg | 3 +++ .../optical_flow/optical_flow_control.cpp | 15 +++++++++++++-- src/modules/ekf2/EKF/ekf.h | 11 ++++++++--- src/modules/ekf2/EKF2.cpp | 3 +++ src/modules/ekf2/test/test_EKF_flow.cpp | 6 ++---- 5 files changed, 29 insertions(+), 9 deletions(-) diff --git a/msg/VehicleOpticalFlowVel.msg b/msg/VehicleOpticalFlowVel.msg index 947131da4d..9d9b0ad876 100644 --- a/msg/VehicleOpticalFlowVel.msg +++ b/msg/VehicleOpticalFlowVel.msg @@ -4,6 +4,9 @@ uint64 timestamp_sample # the timestamp of the raw data (microsec float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) float32[2] vel_ne # same as vel_body but in local frame (m/s) +float32[2] vel_body_filtered # filtered velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne_filtered # filtered same as vel_body_filtered but in local frame (m/s) + float32[2] flow_rate_uncompensated # integrated optical flow measurement (rad/s) float32[2] flow_rate_compensated # integrated optical flow measurement compensated for angular motion (rad/s) 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 95db812fae..becece91c1 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 @@ -122,8 +122,16 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) 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)); + if (_flow_counter == 0) { + _flow_vel_body_lpf.reset(_flow_vel_body); + _flow_counter = 1; + + } else { + + _flow_vel_body_lpf.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 @@ -144,6 +152,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 @@ -219,7 +228,7 @@ void Ekf::resetFlowFusion() _information_events.flags.reset_vel_to_flow = true; const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed); - resetHorizontalVelocityTo(_flow_vel_ne, flow_vel_var); + resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var); // reset position, estimate is relative to initial position in this mode, so we start with zero error if (!_control_status.flags.in_air) { @@ -273,6 +282,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 7be3fdf92a..4449b2d233 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -122,7 +122,10 @@ public: 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; } + Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); } + + const Vector2f &getFilteredFlowVelBody() const { return _flow_vel_body_lpf.getState(); } + Vector2f getFilteredFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFilteredFlowVelBody()(0), getFilteredFlowVelBody()(1), 0.f)); } const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; } const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; } @@ -634,10 +637,12 @@ 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{}; + Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) + AlphaFilter _flow_vel_body_lpf{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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bb986208de..c339548b8c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2027,6 +2027,9 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowVelBody().copyTo(flow_vel.vel_body); _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne); + _ekf.getFilteredFlowVelBody().copyTo(flow_vel.vel_body_filtered); + _ekf.getFilteredFlowVelNE().copyTo(flow_vel.vel_ne_filtered); + _ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated); _ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated); 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();