fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: extract setting origin from current lat/lon/alt

Browse Source 
This is not only needed when GNSS is available but also for other global
sources of position (e.g.: aux global pos and manual pos updates)
This commit is contained in:
bresch 2024-08-21 16:21:17 +02:00 committed by Mathieu Bresciani
parent 9169a7c5fc
commit af752536b9
3 changed files with 77 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View File

@ -59,34 +59,15 @@ void Ekf::collect_gps(const gnssSample &gps)
{
if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
const double lat = gps.lat;
const double lon = gps.lon;
if (!_pos_ref.isInitialized()) {
_pos_ref.initReference(lat, lon, gps.time_us);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (isHorizontalAidingActive()) {
double est_lat;
double est_lon;
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, gps.time_us);
}
setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
}
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
if (!PX4_ISFINITE(_gps_alt_ref)) {
_gps_alt_ref = gps.alt + _state.pos(2);
setAltOriginFromCurrentPos(gps.alt, gps.vacc);
}
_NED_origin_initialised = true;
// save the horizontal and vertical position uncertainty of the origin
_gpos_origin_eph = gps.hacc;
_gpos_origin_epv = gps.vacc;
_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(gps.lat)));
_information_events.flags.gps_checks_passed = true;
ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",

11
src/modules/ekf2/EKF/ekf.h
View File

@ -261,9 +261,11 @@ 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);
bool checkLatLonValidity(double latitude, double longitude);
bool checkAltitudeValidity(float altitude);
bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN,
float epv = NAN);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@ -770,6 +772,9 @@ private:
bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
bool setAltOrigin(float altitude, float epv = NAN);
bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
bool setAltOriginFromCurrentPos(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;

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

@ -72,22 +72,18 @@ 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)
bool Ekf::checkLatLonValidity(const double latitude, const double longitude)
{
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);
return (lat_valid && lon_valid);
}
bool Ekf::checkAltitudeValidity(const float altitude, const float epv)
bool Ekf::checkAltitudeValidity(const float altitude)
{
// 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;
return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
}
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
@ -105,7 +101,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
{
if (!checkLatLonValidity(latitude, longitude, eph)) {
if (!checkLatLonValidity(latitude, longitude)) {
return false;
}
@ -121,7 +117,10 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
// reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
_gpos_origin_eph = eph;
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
_gpos_origin_eph = eph;
}
_NED_origin_initialised = true;
@ -136,13 +135,16 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
bool Ekf::setAltOrigin(const float altitude, const float epv)
{
if (!checkAltitudeValidity(altitude, epv)) {
if (!checkAltitudeValidity(altitude)) {
return false;
}
const float gps_alt_ref_prev = _gps_alt_ref;
_gps_alt_ref = altitude;
_gpos_origin_epv = epv;
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
_gpos_origin_epv = epv;
}
if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
// determine current z
@ -163,6 +165,59 @@ bool Ekf::setAltOrigin(const float altitude, const float epv)
return true;
}
bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
const float eph, const float epv)
{
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
return false;
}
// altitude is optional
setAltOriginFromCurrentPos(altitude, epv);
return true;
}
bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
{
if (!checkLatLonValidity(latitude, longitude)) {
return false;
}
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (isHorizontalAidingActive()) {
double est_lat;
double est_lon;
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
}
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
_gpos_origin_eph = eph;
}
_NED_origin_initialised = true;
return true;
}
bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
{
if (!checkAltitudeValidity(altitude)) {
return false;
}
_gps_alt_ref = altitude + _state.pos(2);
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
_gpos_origin_epv = epv;
}
return true;
}
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
{
float eph = INFINITY;