ekf2: sideslip symforce increase epsilon to avoid 1/e^2 numerical issues

PX4 · Jul 16, 2024 · 397ff4a · 397ff4a
1 parent 13b62a7
commit 397ff4a
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
@@ -82,10 +82,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 {
 	float observation = 0.f;
 	const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance
-
+	const float epsilon = 1e-3f;
 	float innov;
 	float innov_var;
-	sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var);
+	sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, epsilon, &innov, &innov_var);
 
 	updateAidSourceStatus(aid_src,
 			      _time_delayed_us,                         // sample timestamp
@@ -130,10 +130,11 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 
 	_fault_status.flags.bad_sideslip = false;
 
+	const float epsilon = 1e-3f;
 	VectorState H; // Observation jacobian
 	VectorState K; // Kalman gain vector
 
-	sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);
+	sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K);
 
 	if (update_wind_only) {
 		const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);