From 13b62a74d6010e3dab43b0b15b37bdb96844f3a1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 12 Jul 2024 13:25:24 -0400 Subject: [PATCH] ekf2: optical flow adjust jacobian epsilon to avoid numerical issues - in the generated code there's a 1 / eps^2 term if the height and terrain estimates are the same --- .../ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp | 3 ++- .../ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp | 3 ++- 2 files changed, 4 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 390a957992..3c4c031f31 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 @@ -89,8 +89,9 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // calculate the optical flow observation variance const float R_LOS = calcOptFlowMeasVar(flow_sample); + const float epsilon = 1e-3f; Vector2f innov_var; - sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H); + sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, epsilon, &innov_var, &H); // run the innovation consistency check and record result updateAidSourceStatus(_aid_src_optical_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 531a7b56d0..860c6eca5e 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 @@ -72,7 +72,8 @@ bool Ekf::fuseOptFlow(VectorState &H, 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); + const float epsilon = 1e-3f; + sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, epsilon, &_aid_src_optical_flow.innovation_variance[1], &H); // recalculate the innovation using the updated state const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;