From 22b957696d0cea108909469128ae6d01ea6d3f9f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Feb 2024 21:01:32 -0500 Subject: [PATCH] ekf2: velocity/position fusion helper minor consistency cleanup --- src/modules/ekf2/EKF/auxvel_fusion.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 4 +- src/modules/ekf2/EKF/position_fusion.cpp | 72 ++++++++++++------------ src/modules/ekf2/EKF/velocity_fusion.cpp | 52 ++++++++--------- 4 files changed, 66 insertions(+), 66 deletions(-) diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/auxvel_fusion.cpp index dbe88ccb87..4ef94e31ee 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/auxvel_fusion.cpp @@ -42,10 +42,10 @@ void Ekf::controlAuxVelFusion() resetEstimatorAidStatus(_aid_src_aux_vel); - updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); + updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); if (isHorizontalAidingActive()) { - fuseVelocity(_aid_src_aux_vel); + fuseHorizontalVelocity(_aid_src_aux_vel); } } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 3393c7fd0d..dc08d5375f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -828,7 +828,7 @@ private: void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; // 2d & 3d velocity aid source - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; // horizontal and vertical position fusion @@ -836,7 +836,7 @@ private: void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); // 2d & 3d velocity fusion - void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); + void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); #if defined(CONFIG_EKF2_TERRAIN) diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 76376ecd1c..8649315e63 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -33,6 +33,25 @@ #include "ekf.h" +void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, + const float innov_gate, estimator_aid_source2d_s &aid_src) const +{ + resetEstimatorAidStatus(aid_src); + + for (int i = 0; i < 2; i++) { + aid_src.observation[i] = obs(i); + aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; + + aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); + const int state_index = State::pos.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; + } + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + aid_src.timestamp_sample = time_us; +} + void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const { @@ -57,52 +76,35 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa aid_src.timestamp_sample = time_us; } -void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::pos.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y - if (!aid_src.innovation_rejected) { - if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) - ) { - aid_src.fused = true; - aid_src.time_last_fuse = _time_delayed_us; + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; - _time_last_hor_pos_fuse = _time_delayed_us; + _time_last_hor_pos_fuse = _time_delayed_us; - } else { - aid_src.fused = false; - } + } else { + aid_src.fused = false; } } void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z - if (!aid_src.innovation_rejected) { - if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) { - aid_src.fused = true; - aid_src.time_last_fuse = _time_delayed_us; + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; - _time_last_hgt_fuse = _time_delayed_us; - } + _time_last_hgt_fuse = _time_delayed_us; + + } else { + aid_src.fused = false; } } diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index 964752509c..9e50946552 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -33,8 +33,8 @@ #include "ekf.h" -void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const +void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, + const float innov_gate, estimator_aid_source2d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -79,40 +79,38 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob aid_src.timestamp_sample = time_us; } -void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) +void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) { - if (!aid_src.innovation_rejected) { - // vx, vy - if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) - ) { - aid_src.fused = true; - aid_src.time_last_fuse = _time_delayed_us; + // vx, vy + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; - _time_last_hor_vel_fuse = _time_delayed_us; + _time_last_hor_vel_fuse = _time_delayed_us; - } else { - aid_src.fused = false; - } + } else { + aid_src.fused = false; } } void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { - if (!aid_src.innovation_rejected) { - // vx, vy, vz - if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) - && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) - ) { - aid_src.fused = true; - aid_src.time_last_fuse = _time_delayed_us; + // vx, vy, vz + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; - _time_last_hor_vel_fuse = _time_delayed_us; - _time_last_ver_vel_fuse = _time_delayed_us; + _time_last_hor_vel_fuse = _time_delayed_us; + _time_last_ver_vel_fuse = _time_delayed_us; - } else { - aid_src.fused = false; - } + } else { + aid_src.fused = false; } }