From 073013cf857df7709e43ecf14b9aee50515f7504 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 8 Aug 2025 19:34:19 +0200 Subject: [PATCH] reset terrain w flow based on current horizontal velocity --- .../optical_flow/optical_flow_control.cpp | 21 +++++++++++++++++-- src/modules/ekf2/EKF/ekf.h | 1 + 2 files changed, 20 insertions(+), 2 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 4913ab78b2..7774147610 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 @@ -125,11 +125,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (_flow_counter == 0) { _flow_vel_body_lpf.reset(_flow_vel_body); + _flow_rate_compensated_lpf.reset(_flow_rate_compensated); _flow_counter = 1; } else { _flow_vel_body_lpf.update(_flow_vel_body); + _flow_rate_compensated_lpf.update(_flow_rate_compensated); _flow_counter++; } @@ -240,8 +242,23 @@ void Ekf::resetTerrainToFlow() { ECL_INFO("reset hagl to flow"); - // TODO: use the flow data - const float new_terrain = -_gpos.altitude() + _params.ekf2_min_rng; + float new_terrain = -_gpos.altitude() + _params.ekf2_min_rng; + + if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) { + // ||vel_NE|| = ||( R * flow_body * range).xy()|| + // range = ||vel_NE|| / ||P * R * flow_body|| + constexpr float kProjXY[2][3] = {{1.f, 0.f, 0.f}, {0.f, 1.f, 0.f}}; + const matrix::Matrix proj(kProjXY); + + const Vector3f flow_body(-_flow_rate_compensated_lpf.getState()(1), _flow_rate_compensated_lpf.getState()(0), 0.f); + const float denom = Vector2f(proj * _R_to_earth * flow_body).norm(); + + if (denom > 1e-6f) { + const float range = _state.vel.xy().norm() / denom; + new_terrain = -_gpos.altitude() + max(range, _params.ekf2_min_rng); + } + } + const float delta_terrain = new_terrain - _state.terrain; _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c4aaed1cbf..b421bcb889 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -529,6 +529,7 @@ private: 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 + AlphaFilter _flow_rate_compensated_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED)