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 ea78b14bf4..8e44a04828 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 @@ -38,6 +38,8 @@ #include "ekf.h" +#include + void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) { if (!_flow_buffer || (_params.flow_ctrl != 1)) { @@ -47,6 +49,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) bool flow_data_ready = false; + VectorState H; + // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) { @@ -88,11 +92,35 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) flow_data_ready = true; } - updateOptFlow(_aid_src_optical_flow, flow_sample); + // 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 Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias; + const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy(); + + // calculate the optical flow observation variance + const float R_LOS = calcOptFlowMeasVar(flow_sample); + + Vector2f innov_var; + sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H); + + // run the innovation consistency check and record result + updateAidSourceStatus(_aid_src_optical_flow, + flow_sample.time_us, // sample timestamp + flow_compensated, // observation + Vector2f{R_LOS, R_LOS}, // observation variance + predictFlow(flow_gyro_corrected) - flow_compensated, // innovation + innov_var, // innovation variance + math::max(_params.flow_innov_gate, 1.f)); // innovation gate // logging - const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias; - _flow_rate_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy(); + _flow_rate_compensated = flow_compensated; + + // 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)); } if (flow_data_ready) { @@ -116,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (_control_status.flags.opt_flow) { if (continuing_conditions_passing) { - fuseOptFlow(_hagl_sensor_status.flags.flow); + fuseOptFlow(H, _hagl_sensor_status.flags.flow); // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) { @@ -134,7 +162,34 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } else { if (starting_conditions_passing) { - startFlowFusion(); + // If the height is relative to the ground, terrain height cannot be observed. + _hagl_sensor_status.flags.flow = (_height_sensor_ref != HeightSensor::RANGE); + + if (isHorizontalAidingActive()) { + if (fuseOptFlow(H, _hagl_sensor_status.flags.flow)) { + ECL_INFO("starting optical flow"); + _control_status.flags.opt_flow = true; + + } else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) { + ECL_INFO("starting optical flow, resetting terrain"); + resetTerrainToFlow(); + _control_status.flags.opt_flow = true; + } + + } else { + if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) { + ECL_INFO("starting optical flow, resetting"); + resetFlowFusion(); + _control_status.flags.opt_flow = true; + + } else if (_hagl_sensor_status.flags.flow) { + ECL_INFO("starting optical flow, resetting terrain"); + resetTerrainToFlow(); + _control_status.flags.opt_flow = true; + } + } + + _hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); } } @@ -143,40 +198,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } } -void Ekf::startFlowFusion() -{ - if (_height_sensor_ref != HeightSensor::RANGE) { - // If the height is relative to the ground, terrain height cannot be observed. - _hagl_sensor_status.flags.flow = true; - } - - if (isHorizontalAidingActive()) { - if (fuseOptFlow(_hagl_sensor_status.flags.flow)) { - ECL_INFO("starting optical flow"); - _control_status.flags.opt_flow = true; - - } else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) { - ECL_INFO("starting optical flow, resetting terrain"); - resetTerrainToFlow(); - _control_status.flags.opt_flow = true; - } - - } else { - if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) { - ECL_INFO("starting optical flow, resetting"); - resetFlowFusion(); - _control_status.flags.opt_flow = true; - - } else if (_hagl_sensor_status.flags.flow) { - ECL_INFO("starting optical flow, resetting terrain"); - resetTerrainToFlow(); - _control_status.flags.opt_flow = true; - } - } - - _hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); -} - void Ekf::resetFlowFusion() { ECL_INFO("reset velocity to flow"); 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 74e203e859..91efdace2e 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,57 +42,9 @@ #include #include -void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample) -{ - // 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 Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias; - const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy(); - - const Vector2f innovation = predictFlow(flow_gyro_corrected) - flow_compensated; - - // calculate the optical flow observation variance - const float R_LOS = calcOptFlowMeasVar(flow_sample); - - Vector2f innov_var; - VectorState H; - sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H); - - // run the innovation consistency check and record result - updateAidSourceStatus(aid_src, - 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) +bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) { const auto state_vector = _state.vector(); - const float R_LOS = _aid_src_optical_flow.observation_variance[0]; - - Vector2f innov_var; - VectorState H; - sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(_aid_src_optical_flow.innovation_variance); - - if ((innov_var(0) < R_LOS) || (innov_var(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; - } _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); @@ -107,7 +59,7 @@ 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) { + if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) { // we need to reinitialise the covariance matrix and abort this fusion step ECL_ERR("Opt flow error - covariance reset"); initialiseCovariance(); @@ -119,6 +71,7 @@ bool Ekf::fuseOptFlow(const bool update_terrain) } else if (index == 1) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + const float R_LOS = _aid_src_optical_flow.observation_variance[1]; sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H); // recalculate the innovation using the updated state diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e5cc2cb24f..b3f52e18d3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -833,7 +833,6 @@ private: #if defined(CONFIG_EKF2_OPTICAL_FLOW) // control fusion of optical flow observations void controlOpticalFlowFusion(const imuSample &imu_delayed); - void startFlowFusion(); void resetFlowFusion(); void stopFlowFusion(); @@ -850,8 +849,7 @@ private: Vector2f predictFlow(const Vector3f &flow_gyro) const; // fuse optical flow line of sight rate measurements - void updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample); - bool fuseOptFlow(bool update_terrain); + bool fuseOptFlow(VectorState &H, bool update_terrain); #endif // CONFIG_EKF2_OPTICAL_FLOW