dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: move vel/pos reset helpers

This commit is contained in:
Daniel Agar 2024-02-14 21:17:01 -05:00
parent 22b957696d
commit 8243b4f474
3 changed files with 179 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -45,121 +45,6 @@
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <cstdlib>
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
{

100
src/modules/ekf2/EKF/position_fusion.cpp
View File

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

79
src/modules/ekf2/EKF/velocity_fusion.cpp
View File

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