fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 04:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: split horizontal and vertical origin reset

Browse Source 
Allow partial resets (only lat/lon or only altitude)
This commit is contained in:
bresch
2024-08-20 14:34:25 +02:00
committed by Mathieu Bresciani
parent f3d58cdf10
commit 9169a7c5fc
2 changed files with 89 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+5
View File
@@ -261,6 +261,8 @@ public:
// get the ekf WGS-84 origin position and height and the system time it was last set
// return true if the origin is valid
bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
bool checkLatLonValidity(double latitude, double longitude, float eph = 0.f);
bool checkAltitudeValidity(float altitude, float epv = 0.f);
bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -765,6 +767,9 @@ private:
P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
}
bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
bool setAltOrigin(float altitude, float epv = NAN);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
src/modules/ekf2/EKF/ekf_helper.cpp
+84 -52
View File
@@ -72,63 +72,95 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
return _NED_origin_initialised;
}
bool Ekf::checkLatLonValidity(const double latitude, const double longitude, const float eph)
{
const bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90));
const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180));
const bool eph_valid = (PX4_ISFINITE(eph) && (eph >= 0.f));
return (lat_valid && lon_valid && eph_valid);
}
bool Ekf::checkAltitudeValidity(const float altitude, const float epv)
{
// sanity check valid altitude anywhere between the Mariana Trench and edge of Space
const bool alt_valid = (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
const bool epv_valid = (PX4_ISFINITE(epv) && (epv >= 0.f));
return alt_valid && epv_valid;
}
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
const float epv)
{
// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
&& PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
&& PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
) {
bool current_pos_available = false;
double current_lat = static_cast<double>(NAN);
double current_lon = static_cast<double>(NAN);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
current_pos_available = true;
}
const float gps_alt_ref_prev = _gps_alt_ref;
// reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
_gps_alt_ref = altitude;
_gpos_origin_eph = eph;
_gpos_origin_epv = epv;
_NED_origin_initialised = true;
_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(latitude)));
if (current_pos_available) {
// reset horizontal position if we already have a global origin
Vector2f position = _pos_ref.project(current_lat, current_lon);
resetHorizontalPositionTo(position);
}
if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
// determine current z
const float z_prev = _state.pos(2);
const float current_alt = -z_prev + gps_alt_ref_prev;
#if defined(CONFIG_EKF2_GNSS)
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
#endif // CONFIG_EKF2_GNSS
resetVerticalPositionTo(_gps_alt_ref - current_alt);
ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
(double)_state.pos(2));
#if defined(CONFIG_EKF2_GNSS)
// adjust existing GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
#endif // CONFIG_EKF2_GNSS
}
return true;
if (!setLatLonOrigin(latitude, longitude, eph)) {
return false;
}
return false;
// altitude is optional
setAltOrigin(altitude, epv);
return true;
}
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
{
if (!checkLatLonValidity(latitude, longitude, eph)) {
return false;
}
bool current_pos_available = false;
double current_lat = static_cast<double>(NAN);
double current_lon = static_cast<double>(NAN);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
current_pos_available = true;
}
// reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
_gpos_origin_eph = eph;
_NED_origin_initialised = true;
if (current_pos_available) {
// reset horizontal position if we already have a global origin
Vector2f position = _pos_ref.project(current_lat, current_lon);
resetHorizontalPositionTo(position);
}
return true;
}
bool Ekf::setAltOrigin(const float altitude, const float epv)
{
if (!checkAltitudeValidity(altitude, epv)) {
return false;
}
const float gps_alt_ref_prev = _gps_alt_ref;
_gps_alt_ref = altitude;
_gpos_origin_epv = epv;
if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
// determine current z
const float z_prev = _state.pos(2);
const float current_alt = -z_prev + gps_alt_ref_prev;
#if defined(CONFIG_EKF2_GNSS)
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
#endif // CONFIG_EKF2_GNSS
resetVerticalPositionTo(_gps_alt_ref - current_alt);
ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
(double)_state.pos(2));
#if defined(CONFIG_EKF2_GNSS)
// adjust existing GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
#endif // CONFIG_EKF2_GNSS
}
return true;
}
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const