Skip to content

Commit

Permalink
ekf2: optical flow adjust jacobian epsilon to avoid numerical issues
Browse files Browse the repository at this point in the history
 - in the generated code there's a 1 / eps^2 term if the height and
   terrain estimates are the same
  • Loading branch information
dagar committed Jul 12, 2024
1 parent 33be5d8 commit ffd25ff
Showing 1 changed file with 2 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down

0 comments on commit ffd25ff

Please sign in to comment.