diff --git a/EKF/ekf.h b/EKF/ekf.h index f657382916..f11682878d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -95,6 +95,9 @@ public: // get the ekf WGS-84 origin position and height and the system time it was last set void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt); + // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position + void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning); + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index af75b87b4a..8285763162 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -309,6 +309,21 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); } +// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position +void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) +{ + // report absolute accuracy taking into account the uncertainty in location of the origin + // TODO we a need a way to allow for baro drift error + float temp1 = sqrtf(P[6][6] + P[7][7] + sq(_gps_origin_eph)); + float temp2 = sqrtf(P[8][8] + sq(_gps_origin_epv)); + memcpy(ekf_eph, &temp1, sizeof(float)); + memcpy(ekf_epv, &temp2, sizeof(float)); + + // report dead reckoning if it is more than a second since we fused in GPS + bool temp3 = (_time_last_imu - _time_last_pos_fuse > 1e6); + memcpy(dead_reckoning, &temp3, sizeof(bool)); +} + // fuse measurement void Ekf::fuse(float *K, float innovation) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index e907fcdcab..b630d27b5f 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -83,6 +83,9 @@ public: // get the ekf WGS-84 origin position and height and the system time it was last set virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; + // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position + virtual void get_ekf_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; } @@ -198,6 +201,8 @@ protected: float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s) struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m) + float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin + float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin bool _mag_healthy; // computed by mag innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index fb00841941..77f94a1ee8 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -70,6 +70,9 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) _last_gps_origin_time_us = _time_last_imu; // set the magnetic declination returned by the geo library using the current GPS position _mag_declination_gps = math::radians(get_mag_declination(lat, lon)); + // save the horizontal and vertical position uncertainty of the origin + _gps_origin_eph = gps->eph; + _gps_origin_epv = gps->epv; } }