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

[WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

This commit is contained in:
Daniel Agar 2024-02-15 12:42:13 -05:00
parent 84a7d42566
commit e678ad72f8
2 changed files with 32 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -171,10 +171,11 @@ bool Ekf::isHeightResetRequired() const
return (continuous_bad_accel_hgt || hgt_fusion_timeout); return (continuous_bad_accel_hgt || hgt_fusion_timeout);
} }
void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) { 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"); _information_events.flags.reset_pos_to_ext_obs = true;
resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); 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) void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
@ -290,9 +291,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
{ {
// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
&& PX4_ISFINITE(longitude) && (abs(longitude) <= 180) && PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
&& PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
) { ) {
bool current_pos_available = false; bool current_pos_available = false;
double current_lat = static_cast<double>(NAN); double current_lat = static_cast<double>(NAN);
double current_lon = static_cast<double>(NAN); double current_lon = static_cast<double>(NAN);
@ -303,7 +304,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
current_pos_available = true; current_pos_available = true;
} }
const float gps_alt_ref_prev = getEkfGlobalOriginAltitude(); const float gps_alt_ref_prev = _gps_alt_ref;
// reinitialize map projection to latitude, longitude, altitude, and reset position // reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us); _pos_ref.initReference(latitude, longitude, _time_delayed_us);
@ -321,6 +322,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
_wmm_gps_time_last_set = _time_delayed_us; _wmm_gps_time_last_set = _time_delayed_us;
} }
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
_gpos_origin_eph = eph; _gpos_origin_eph = eph;
@ -328,32 +330,35 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
_NED_origin_initialised = true; _NED_origin_initialised = true;
// minimum change in position or height that triggers a reset
static constexpr float MIN_RESET_DIST_M = 0.01f;
if (current_pos_available) { if (current_pos_available) {
// reset horizontal position // reset horizontal position if we already have a global origin
Vector2f position = _pos_ref.project(current_lat, current_lon); Vector2f position = _pos_ref.project(current_lat, current_lon);
resetHorizontalPositionTo(position);
if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) {
resetHorizontalPositionTo(position);
}
} }
// reset vertical position (if there's any change) if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) { // reset vertical position if we already had a GPS altitude reference
// determine current z
float current_alt = -_state.pos(2) + gps_alt_ref_prev;
#if defined(CONFIG_EKF2_GNSS) ECL_DEBUG("EKF global origin updated, GPS altitude ref %.1fm -> %.1fm", (double)gps_alt_ref_prev, (double)_gps_alt_ref);
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
#endif // CONFIG_EKF2_GNSS
resetVerticalPositionTo(_gps_alt_ref - current_alt);
if (_height_sensor_ref == HeightSensor::GNSS) {
// determine current z
float z_prev = _state.pos(2);
float current_alt = -z_prev + gps_alt_ref_prev;
resetVerticalPositionTo(_gps_alt_ref - current_alt);
ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, (double)_state.pos(2));
} else {
#if defined(CONFIG_EKF2_GNSS) #if defined(CONFIG_EKF2_GNSS)
// preserve GPS height bias // adjust existing GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias); const float gps_hgt_bias = _gps_hgt_b_est.getBias();
float delta_z = _gps_alt_ref - gps_alt_ref_prev;
_gps_hgt_b_est.setBias(gps_hgt_bias + delta_z);
ECL_DEBUG("EKF global origin updated, GPS height bias %.1fm -> %.1fm", (double)gps_hgt_bias, (double)_gps_hgt_b_est.getBias());
#endif // CONFIG_EKF2_GNSS #endif // CONFIG_EKF2_GNSS
}
} }
return true; return true;

2
src/modules/ekf2/test/test_EKF_height_fusion.cpp
View File

@ -292,5 +292,5 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
EXPECT_NEAR(_ekf->getRngHgtBiasEstimatorStatus().bias, alt_increment, 1.f); EXPECT_NEAR(_ekf->getRngHgtBiasEstimatorStatus().bias, alt_increment, 1.f);
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
} }