From a30830a7a950a91594ae07135af2b327d3da6686 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 22 Feb 2016 20:15:44 +1100 Subject: [PATCH] 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];