From 84afc996b0caa53bf6e4c78fb3021837fc78d512 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 17 Jul 2024 10:59:33 -0400 Subject: [PATCH] ekf2: flow log calculated range from compensated flow and velocity estimate --- msg/VehicleOpticalFlowVel.msg | 1 + .../optical_flow/optical_flow_control.cpp | 7 +++++ .../optical_flow/optical_flow_fusion.cpp | 28 +++++++++++-------- src/modules/ekf2/EKF/ekf.h | 5 ++++ src/modules/ekf2/EKF2.cpp | 1 + 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/msg/VehicleOpticalFlowVel.msg b/msg/VehicleOpticalFlowVel.msg index 947131da4d..d6c7c68dae 100644 --- a/msg/VehicleOpticalFlowVel.msg +++ b/msg/VehicleOpticalFlowVel.msg @@ -3,6 +3,7 @@ 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 flow_range # range calculated from compensated flow and velocity estimate (m) 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 4d4023f088..e718c9f712 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 @@ -109,13 +109,20 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) const float range = predictFlowRange(); const Vector2f flow_vel_body{-flow_compensated(1) *range, flow_compensated(0) *range}; + // compute the range from the compensated optical flow and current velocity state + const Vector2f vel_body = flowSensorVelocity(flow_gyro_corrected).xy(); + const float flow_range = 0.5f * (math::max(0.f, vel_body(0) / -flow_compensated(1)) + + math::max(0.f, vel_body(1) / flow_compensated(0))); + if (_flow_counter == 0) { _flow_vel_body.reset(flow_vel_body); + _flow_range.reset(flow_range); _flow_counter = 1; } else { _flow_vel_body.update(flow_vel_body); + _flow_range.update(flow_range); _flow_counter++; } diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index e5c0e7bd35..a47d5b930e 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -109,6 +109,22 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) return false; } +Vector3f Ekf::flowSensorVelocity(const Vector3f &flow_gyro) const +{ + // calculate the sensor position relative to the IMU + const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; + + // calculate the velocity of the sensor relative to the imu in body frame + // Note: flow gyro is the negative of the body angular velocity, thus use minus sign + const Vector3f vel_rel_imu_body = -flow_gyro % pos_offset_body; + + // calculate the velocity of the sensor in the earth frame + const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; + + // rotate into body frame + return _state.quat_nominal.rotateVectorInverse(vel_rel_earth); +} + float Ekf::predictFlowRange() const { // calculate the sensor position relative to the IMU @@ -134,18 +150,8 @@ float Ekf::predictFlowRange() const Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const { - // calculate the sensor position relative to the IMU - const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; - - // calculate the velocity of the sensor relative to the imu in body frame - // Note: flow gyro is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = -flow_gyro % pos_offset_body; - - // calculate the velocity of the sensor in the earth frame - const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; - // rotate into body frame - const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy(); + const Vector2f vel_body = flowSensorVelocity(flow_gyro).xy(); // calculate range from focal point to centre of image const float range = predictFlowRange(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9243cc73ee..2af0ef08f1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -123,6 +123,7 @@ public: const Vector2f &getFlowVelBody() const { return _flow_vel_body.getState(); } Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); } + float getFlowRange() const { return _flow_range.getState(); } const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; } const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; } @@ -621,6 +622,7 @@ private: Vector3f _ref_body_rate{}; AlphaFilter _flow_vel_body{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) + AlphaFilter _flow_range{0.1f}; ///< range from corrected flow measurement (m) 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 @@ -868,6 +870,9 @@ private: // calculate optical flow body angular rate compensation void calcOptFlowBodyRateComp(const flowSample &flow_sample); + // velocity in the optical flow sensor body frame + Vector3f flowSensorVelocity(const Vector3f &flow_gyro) const; + float predictFlowRange() const; Vector2f predictFlow(const Vector3f &flow_gyro) const; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 005e303e74..086e994ec3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2030,6 +2030,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowVelBody().copyTo(flow_vel.vel_body); _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne); + flow_vel.flow_range = _ekf.getFlowRange(); _ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated); _ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated);