Skip to content

Commit

Permalink
ekf2: split horizontal and vertical origin reset
Browse files Browse the repository at this point in the history 
Allow partial resets (only lat/lon or only altitude)
  • Loading branch information
@bresch
bresch committed Aug 28, 2024
1 parent f3d58cd commit 9169a7c
Show file tree
Hide file tree
Showing 2 changed files with 78 additions and 41 deletions.
5 changes: 5 additions & 0 deletions src/modules/ekf2/EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,8 @@ class Ekf final : public EstimatorInterface
// 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
Expand Down Expand Up @@ -765,6 +767,9 @@ class Ekf final : public EstimatorInterface
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;
Expand Down
114 changes: 73 additions & 41 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
if (!setLatLonOrigin(latitude, longitude, eph)) {
return false;
}

// altitude is optional
setAltOrigin(altitude, epv);

return true;
}

const float gps_alt_ref_prev = _gps_alt_ref;
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
{
if (!checkLatLonValidity(latitude, longitude, eph)) {
return false;
}

// reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
_gps_alt_ref = altitude;
bool current_pos_available = false;
double current_lat = static_cast<double>(NAN);
double current_lon = static_cast<double>(NAN);

_gpos_origin_eph = eph;
_gpos_origin_epv = epv;
// 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;
}

_NED_origin_initialised = true;
// reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
_gpos_origin_eph = eph;

_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(latitude)));
_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);
}
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;
}

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;
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();
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));
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);
// adjust existing GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
#endif // CONFIG_EKF2_GNSS
}

return true;
}

return false;
return true;
}

void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
Expand Down

0 comments on commit 9169a7c

Please sign in to comment.