From d6abf3f2e4340abeca8ba86e6e1b1f9ff88d4b0a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 24 Feb 2017 09:25:39 +1100 Subject: [PATCH] EKF: add accessor function for local position accuracy --- EKF/ekf.h | 3 +++ EKF/ekf_helper.cpp | 25 +++++++++++++++++++++++++ EKF/estimator_interface.h | 3 +++ 3 files changed, 31 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 2426b9b4a3..0f0c438e44 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -120,6 +120,9 @@ public: // 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, bool *dead_reckoning); + // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position + void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning); + void get_vel_var(Vector3f &vel_var); void get_pos_var(Vector3f &pos_var); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index b454727ab9..e35a971a85 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -748,6 +748,31 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool)); } +// 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, bool *dead_reckoning) +{ + // TODO - allow for baro drift in vertical position error + float hpos_err; + float vpos_err; + bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + if (vel_pos_aiding && _NED_origin_initialised) { + hpos_err = sqrtf(P[7][7] + P[8][8]); + vpos_err = sqrtf(P[9][9]); + + } else { + hpos_err = 0.0f; + vpos_err = 0.0f; + + } + + // report dead reckoning if it is more than a second since we fused in position measurements + bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6); + + memcpy(ekf_eph, &hpos_err, sizeof(float)); + memcpy(ekf_epv, &vpos_err, sizeof(float)); + memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool)); +} + // get EKF innovation consistency check status information comprising of: // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 8c340e93e8..08c5b949b7 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -132,6 +132,9 @@ public: // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0; + // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position + virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0; + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }