Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:26:50 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Daniel Agar
2024-07-12 13:29:23 -04:00
committed by Mathieu Bresciani
parent 13b62a74d6
commit 397ff4a102
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
+4 -3
View File
@@ -82,10 +82,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
{ {
float observation = 0.f; float observation = 0.f;
const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance
const float epsilon = 1e-3f;
float innov; float innov;
float innov_var; 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, updateAidSourceStatus(aid_src,
_time_delayed_us, // sample timestamp _time_delayed_us, // sample timestamp
@@ -130,10 +130,11 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
_fault_status.flags.bad_sideslip = false; _fault_status.flags.bad_sideslip = false;
const float epsilon = 1e-3f;
VectorState H; // Observation jacobian VectorState H; // Observation jacobian
VectorState K; // Kalman gain vector 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) { if (update_wind_only) {
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0); const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);