Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

ekf2: consolidate LPOS & GPOS accuracy methods

Browse Source
This commit is contained in:
Daniel Agar
2024-01-19 12:46:56 -05:00
parent 3ff1f213a4
commit bf7da6430d
1 changed files with 15 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+15 -21
View File
@@ -384,36 +384,30 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
return false; return false;
} }
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
{ {
// report absolute accuracy taking into account the uncertainty in location of the origin float eph = INFINITY;
// If not aiding, return 0 for horizontal position estimate as no estimate is available float epv = INFINITY;
// TODO - allow for baro drift in vertical position error
float hpos_err = sqrtf(P.trace<2>(State::pos.idx) + sq(_gpos_origin_eph));
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error if (global_origin_valid()) {
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // report absolute accuracy taking into account the uncertainty in location of the origin
// and using state variances for accuracy reporting is overly optimistic in these situations eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph));
if (_control_status.flags.inertial_dead_reckoning) { epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv));
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_horizontal_deadreckon_time_exceeded) {
if (_control_status.flags.ev_pos) { float lpos_eph = 0.f;
hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); float lpos_epv = 0.f;
get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv);
eph = math::max(eph, lpos_eph);
epv = math::max(epv, lpos_epv);
} }
#endif // CONFIG_EKF2_EXTERNAL_VISION
} }
*ekf_eph = hpos_err; *ekf_eph = eph;
*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv)); *ekf_epv = epv;
} }
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
{ {
// TODO - allow for baro drift in vertical position error // TODO - allow for baro drift in vertical position error