From 8243b4f4743b11864a027aff8f4edf6c3e9b04a1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Feb 2024 21:17:01 -0500 Subject: [PATCH] ekf2: move vel/pos reset helpers --- src/modules/ekf2/EKF/ekf_helper.cpp | 177 ----------------------- src/modules/ekf2/EKF/position_fusion.cpp | 100 +++++++++++++ src/modules/ekf2/EKF/velocity_fusion.cpp | 79 ++++++++++ 3 files changed, 179 insertions(+), 177 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 419947f1c9..1f888cd73f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -45,121 +45,6 @@ #include #include -void Ekf::resetHorizontalVelocityToZero() -{ - _information_events.flags.reset_vel_to_zero = true; - ECL_INFO("reset velocity to zero"); - // Used when falling back to non-aiding mode of operation - resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f); -} - -void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var) -{ - resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1))); - resetVerticalVelocityTo(new_vel(2), new_vel_var(2)); -} - -void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var) -{ - const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel); - _state.vel.xy() = new_horz_vel; - - if (PX4_ISFINITE(new_horz_vel_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0))); - } - - if (PX4_ISFINITE(new_horz_vel_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); - } - - _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); - - // record the state change - if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { - _state_reset_status.velNE_change = delta_horz_vel; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.velNE_change += delta_horz_vel; - } - - _state_reset_status.reset_count.velNE++; - - // Reset the timout timer - _time_last_hor_vel_fuse = _time_delayed_us; -} - -void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) -{ - const float delta_vert_vel = new_vert_vel - _state.vel(2); - _state.vel(2) = new_vert_vel; - - if (PX4_ISFINITE(new_vert_vel_var)) { - P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var)); - } - - _output_predictor.resetVerticalVelocityTo(delta_vert_vel); - - // record the state change - if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { - _state_reset_status.velD_change = delta_vert_vel; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.velD_change += delta_vert_vel; - } - - _state_reset_status.reset_count.velD++; - - // Reset the timout timer - _time_last_ver_vel_fuse = _time_delayed_us; -} - -void Ekf::resetHorizontalPositionToLastKnown() -{ - ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); - - _information_events.flags.reset_pos_to_last_known = true; - - // Used when falling back to non-aiding mode of operation - resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); -} - -void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) -{ - const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; - _state.pos.xy() = new_horz_pos; - - if (PX4_ISFINITE(new_horz_pos_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); - } - - if (PX4_ISFINITE(new_horz_pos_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); - } - - _output_predictor.resetHorizontalPositionTo(delta_horz_pos); - - // record the state change - if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { - _state_reset_status.posNE_change = delta_horz_pos; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posNE_change += delta_horz_pos; - } - - _state_reset_status.reset_count.posNE++; - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); -#endif // CONFIG_EKF2_EXTERNAL_VISION - //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); - - // Reset the timout timer - _time_last_hor_pos_fuse = _time_delayed_us; -} - bool Ekf::isHeightResetRequired() const { // check if height is continuously failing because of accel errors @@ -171,68 +56,6 @@ bool Ekf::isHeightResetRequired() const return (continuous_bad_accel_hgt || hgt_fusion_timeout); } -void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) { - _information_events.flags.reset_pos_to_ext_obs = true; - ECL_INFO("reset position to external observation"); - resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); -} - -void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) -{ - const float old_vert_pos = _state.pos(2); - _state.pos(2) = new_vert_pos; - - if (PX4_ISFINITE(new_vert_pos_var)) { - // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); - } - - const float delta_z = new_vert_pos - old_vert_pos; - - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); - - // record the state change - if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { - _state_reset_status.posD_change = delta_z; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posD_change += delta_z; - } - - _state_reset_status.reset_count.posD++; - -#if defined(CONFIG_EKF2_BAROMETER) - _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); -#endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_GNSS) - _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_RANGE_FINDER) - _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_TERRAIN) - terrainHandleVerticalPositionReset(delta_z); -#endif - - // Reset the timout timer - _time_last_hgt_fuse = _time_delayed_us; -} - -void Ekf::resetVerticalVelocityToZero() -{ - // we don't know what the vertical velocity is, so set it to zero - // Set the variance to a value large enough to allow the state to converge quickly - // that does not destabilise the filter - resetVerticalVelocityTo(0.0f, 10.f); -} - #if defined(CONFIG_EKF2_BARO_COMPENSATION) float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const { diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 8649315e63..a645196633 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -108,3 +108,103 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) aid_src.fused = false; } } + +void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) +{ + const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; + _state.pos.xy() = new_horz_pos; + + if (PX4_ISFINITE(new_horz_pos_var(0))) { + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); + } + + if (PX4_ISFINITE(new_horz_pos_var(1))) { + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); + } + + _output_predictor.resetHorizontalPositionTo(delta_horz_pos); + + // record the state change + if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { + _state_reset_status.posNE_change = delta_horz_pos; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posNE_change += delta_horz_pos; + } + + _state_reset_status.reset_count.posNE++; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); +#endif // CONFIG_EKF2_EXTERNAL_VISION + //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); + + // Reset the timout timer + _time_last_hor_pos_fuse = _time_delayed_us; +} + +void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) +{ + const float old_vert_pos = _state.pos(2); + _state.pos(2) = new_vert_pos; + + if (PX4_ISFINITE(new_vert_pos_var)) { + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); + } + + const float delta_z = new_vert_pos - old_vert_pos; + + // apply the change in height / height rate to our newest height / height rate estimate + // which have already been taken out from the output buffer + _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); + + // record the state change + if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { + _state_reset_status.posD_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posD_change += delta_z; + } + + _state_reset_status.reset_count.posD++; + +#if defined(CONFIG_EKF2_BAROMETER) + _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); +#endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_RANGE_FINDER) + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_TERRAIN) + terrainHandleVerticalPositionReset(delta_z); +#endif + + // Reset the timout timer + _time_last_hgt_fuse = _time_delayed_us; +} + +void Ekf::resetHorizontalPositionToLastKnown() +{ + ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); + _information_events.flags.reset_pos_to_last_known = true; + + // Used when falling back to non-aiding mode of operation + resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); +} + +void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) +{ + ECL_INFO("reset position to external observation"); + _information_events.flags.reset_pos_to_ext_obs = true; + + resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); +} diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index 9e50946552..f44ecc874f 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -114,3 +114,82 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) aid_src.fused = false; } } + +void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var) +{ + const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel); + _state.vel.xy() = new_horz_vel; + + if (PX4_ISFINITE(new_horz_vel_var(0))) { + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0))); + } + + if (PX4_ISFINITE(new_horz_vel_var(1))) { + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); + } + + _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); + + // record the state change + if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { + _state_reset_status.velNE_change = delta_horz_vel; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.velNE_change += delta_horz_vel; + } + + _state_reset_status.reset_count.velNE++; + + // Reset the timout timer + _time_last_hor_vel_fuse = _time_delayed_us; +} + +void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) +{ + const float delta_vert_vel = new_vert_vel - _state.vel(2); + _state.vel(2) = new_vert_vel; + + if (PX4_ISFINITE(new_vert_vel_var)) { + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var)); + } + + _output_predictor.resetVerticalVelocityTo(delta_vert_vel); + + // record the state change + if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { + _state_reset_status.velD_change = delta_vert_vel; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.velD_change += delta_vert_vel; + } + + _state_reset_status.reset_count.velD++; + + // Reset the timout timer + _time_last_ver_vel_fuse = _time_delayed_us; +} + +void Ekf::resetHorizontalVelocityToZero() +{ + ECL_INFO("reset velocity to zero"); + _information_events.flags.reset_vel_to_zero = true; + + // Used when falling back to non-aiding mode of operation + resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f); +} + +void Ekf::resetVerticalVelocityToZero() +{ + // we don't know what the vertical velocity is, so set it to zero + // Set the variance to a value large enough to allow the state to converge quickly + // that does not destabilise the filter + resetVerticalVelocityTo(0.0f, 10.f); +} + +void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var) +{ + resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1))); + resetVerticalVelocityTo(new_vel(2), new_vel_var(2)); +}