fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 09:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: optical flow adjust jacobian epsilon to avoid numerical issues

Browse Source 
- in the generated code there's a 1 / eps^2 term if the height and
   terrain estimates are the same
This commit is contained in:
Daniel Agar
2024-07-16 11:10:58 +02:00
committed by Mathieu Bresciani
parent 76cf54c948
commit 13b62a74d6
2 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+2 -1
View File
@@ -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,
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+2 -1
View File
@@ -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;