fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
View File

@ -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);