From ffd25ff69e2b10b5e34a0ffb3dd6d9f3a7e1a33b 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 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 0e4b8b668ed6..717eec7b04e2 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 @@ -91,8 +91,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,