From 4d324da9f8e329f4853512738b58d6687ad98a88 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 26 Jun 2024 16:01:05 -0400 Subject: [PATCH] ekf2: update flow aid src status every sample --- .../optical_flow/optical_flow_control.cpp | 11 ++- .../optical_flow/optical_flow_fusion.cpp | 85 +++++++++---------- src/modules/ekf2/EKF/ekf.h | 12 +-- 3 files changed, 55 insertions(+), 53 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 04243f240d..5941616be7 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 @@ -64,19 +64,22 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (is_quality_good && is_magnitude_good && is_tilt_good) { - // compensate for body motion to give a LOS rate + calcOptFlowBodyRateComp(imu_delayed); - _flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy(); } else { // don't use this flow data and wait for the next data to arrive _flow_data_ready = false; - _flow_rate_compensated.setZero(); } + + updateOptFlow(_aid_src_optical_flow, _flow_sample_delayed); + + // logging + const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias; + _flow_rate_compensated = _flow_sample_delayed.flow_rate - flow_gyro_corrected.xy(); } if (_flow_data_ready) { - updateOptFlow(_aid_src_optical_flow); // Check if we are in-air and require optical flow to control position drift bool is_flow_required = _control_status.flags.in_air 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 2ba97942a2..74e203e859 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 @@ -42,28 +42,18 @@ #include #include -void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) +void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample) { - const Vector2f vel_body = predictFlowVelBody(); - const float range = predictFlowRange(); - // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed // correct for gyro bias errors in the data used to do the motion compensation // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - const Vector2f opt_flow_rate = _flow_rate_compensated; + const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias; + const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy(); - // compute the velocities in body and local frames from corrected optical flow measurement for logging only - _flow_vel_body(0) = -opt_flow_rate(1) * range; - _flow_vel_body(1) = opt_flow_rate(0) * range; - _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); - - Vector2f innovation{ - (vel_body(1) / range) - opt_flow_rate(0), - (-vel_body(0) / range) - opt_flow_rate(1) - }; + const Vector2f innovation = predictFlow(flow_gyro_corrected) - flow_compensated; // calculate the optical flow observation variance - const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); + const float R_LOS = calcOptFlowMeasVar(flow_sample); Vector2f innov_var; VectorState H; @@ -71,23 +61,26 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) // run the innovation consistency check and record result updateAidSourceStatus(aid_src, - _flow_sample_delayed.time_us, // sample timestamp - opt_flow_rate, // observation - Vector2f{R_LOS, R_LOS}, // observation variance - innovation, // innovation - innov_var, // innovation variance - math::max(_params.flow_innov_gate, 1.f)); // innovation gate + flow_sample.time_us, // sample timestamp + flow_compensated, // observation + Vector2f{R_LOS, R_LOS}, // observation variance + innovation, // innovation + innov_var, // innovation variance + math::max(_params.flow_innov_gate, 1.f)); // innovation gate + + + // 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)); + } bool Ekf::fuseOptFlow(const bool update_terrain) { - const float R_LOS = _aid_src_optical_flow.observation_variance[0]; - - // calculate the height above the ground of the optical flow camera. Since earth frame is NED - // a positive offset in earth frame leads to a smaller height above the ground. - float range = predictFlowRange(); - const auto state_vector = _state.vector(); + const float R_LOS = _aid_src_optical_flow.observation_variance[0]; Vector2f innov_var; VectorState H; @@ -113,6 +106,14 @@ bool Ekf::fuseOptFlow(const bool update_terrain) // fuse observation axes sequentially for (uint8_t index = 0; index <= 1; index++) { + + if (_aid_src_optical_flow.innovation_variance[index] < R_LOS) { + // we need to reinitialise the covariance matrix and abort this fusion step + ECL_ERR("Opt flow error - covariance reset"); + initialiseCovariance(); + return false; + } + if (index == 0) { // everything was already computed above @@ -121,16 +122,8 @@ bool Ekf::fuseOptFlow(const bool update_terrain) sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H); // recalculate the innovation using the updated state - const Vector2f vel_body = predictFlowVelBody(); - range = predictFlowRange(); - _aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1]; - - if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - initialiseCovariance(); - return false; - } + const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias; + _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1]; } VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index]; @@ -157,7 +150,7 @@ bool Ekf::fuseOptFlow(const bool update_terrain) return false; } -float Ekf::predictFlowRange() +float Ekf::predictFlowRange() const { // calculate the sensor position relative to the IMU const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; @@ -180,24 +173,28 @@ float Ekf::predictFlowRange() return flow_range; } -Vector2f Ekf::predictFlowVelBody() +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_sample_delayed.gyro_rate is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_rate - _flow_gyro_bias)) % pos_offset_body; + // 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).xy(); + const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy(); + + // calculate range from focal point to centre of image + const float range = predictFlowRange(); + + return Vector2f(vel_body(1) / range, -vel_body(0) / range); } -// calculate the measurement variance for the optical flow sensor (rad/sec)^2 -float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) +float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const { // calculate the observation noise variance - scaling noise linearly across flow quality range const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a8a5778d89..f4368c0f28 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -842,17 +842,19 @@ private: void updateOnGroundMotionForOpticalFlowChecks(); void resetOnGroundMotionForOpticalFlowChecks(); - // calculate the measurement variance for the optical flow sensor - float calcOptFlowMeasVar(const flowSample &flow_sample); + // calculate the measurement variance for the optical flow sensor (rad/sec)^2 + float calcOptFlowMeasVar(const flowSample &flow_sample) const; // calculate optical flow body angular rate compensation void calcOptFlowBodyRateComp(const imuSample &imu_delayed); + float predictFlowRange() const; + Vector2f predictFlow(const Vector3f &flow_gyro) const; + // fuse optical flow line of sight rate measurements - void updateOptFlow(estimator_aid_source2d_s &aid_src); + void updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample); bool fuseOptFlow(bool update_terrain); - float predictFlowRange(); - Vector2f predictFlowVelBody(); + #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_MAGNETOMETER)