From a30830a7a950a91594ae07135af2b327d3da6686 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 22 Feb 2016 20:15:44 +1100 Subject: [PATCH 1/2] EKF: Scale position observation noise with GPS quality This allows the filter to adapt to variations in GPs quality. The range of adjustment in observation noise is limited to the range between a lower limit set by the GPS observation noise parameter and an upper limit set by the no-aiding observation noise. --- EKF/estimator_interface.cpp | 1 + EKF/estimator_interface.h | 1 + EKF/vel_pos_fusion.cpp | 6 +++++- 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index d73869d00e..85d39927c1 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -160,6 +160,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) _gps_speed_valid = gps->vel_ned_valid; _gps_speed_accuracy = gps->sacc; + _gps_hpos_accuracy = gps->eph; float lpos_x = 0.0f; float lpos_y = 0.0f; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 07920aa7ea..e907fcdcab 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -197,6 +197,7 @@ protected: bool _gps_speed_valid; 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) bool _mag_healthy; // computed by mag innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index fd54177083..6f2214442d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -40,6 +40,7 @@ */ #include "ekf.h" +#include "mathlib.h" void Ekf::fuseVelPosHeight() { @@ -90,7 +91,10 @@ void Ekf::fuseVelPosHeight() R[3] = fmaxf(_params.pos_noaid_noise, 0.5f); } else { - R[3] = fmaxf(_params.gps_pos_noise, 0.01f); + float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); + R[3] = math::constrain(_gps_hpos_accuracy, lower_limit, upper_limit); + } R[3] = R[3] * R[3]; From cd0cac066aea1ec2fd265a94aedb1a2d9af4f2cf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 23 Feb 2016 08:35:38 +1100 Subject: [PATCH 2/2] EKF: Calculate and publish horizontal and vertical position accuracy This calculation takes into account the uncertainty of the origin. Dead reckoning status is also published --- EKF/ekf.h | 3 +++ EKF/ekf_helper.cpp | 15 +++++++++++++++ EKF/estimator_interface.h | 5 +++++ EKF/gps_checks.cpp | 3 +++ 4 files changed, 26 insertions(+) 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; } }