From 040639837eaa4f1fb9180039eca88ce04cd53ed5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 15 Mar 2017 08:41:11 +1100 Subject: [PATCH] EKF: Use common timeout test for reporting deadreckoning Use a parameter to set the time-out threshold Use a class variable for the time-out flag Clean up comments --- EKF/common.h | 8 ++++-- EKF/control.cpp | 7 ++++- EKF/ekf_helper.cpp | 57 +++++++++++-------------------------- EKF/estimator_interface.cpp | 1 + EKF/estimator_interface.h | 4 ++- 5 files changed, 33 insertions(+), 44 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 1346986fd2..2cef4a5644 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -276,7 +276,9 @@ struct parameters { float acc_bias_learn_tc; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being - // rejected + // rejected before attempting to reset the states to the GPS measurement (usec) + unsigned no_aid_timeout_max; // maximum lapsed time from last fusion of measurements that constrain drift before + // the EKF will report that it is dead-reckoning (usec) // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. parameters() @@ -385,7 +387,9 @@ struct parameters { acc_bias_learn_gyr_lim = 3.0f; acc_bias_learn_tc = 0.5f; - no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps + // dead reckoning timers + no_gps_timeout_max = 7e6; + no_aid_timeout_max = 1e6; } }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 22d6068302..4e69c7ab0c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -107,10 +107,15 @@ void Ekf::controlFusionModes() controlAirDataFusion(); controlBetaFusion(); - // for efficiency, fusion of direct state observations for position ad velocity is performed sequentially + // for efficiency, fusion of direct state observations for position and velocity is performed sequentially // in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc) controlVelPosFusion(); + // report dead reckoning if we are no longer fusing measurements that constrain velocity drift + _is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max) + && (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max) + && (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max); + } void Ekf::controlExternalVisionFusion() diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index be8726f74c..ffff704b7f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -741,27 +741,17 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko } - // report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift - uint64_t dead_reckoning_duration = math::min(math::min((_time_last_imu - _time_last_pos_fuse),(_time_last_imu - _time_last_vel_fuse)),(_time_last_imu - _time_last_of_fuse)); - bool is_dead_reckoning = dead_reckoning_duration > 1e6; - - // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error - // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors + // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error + // The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - float pos_err_alt = 0.0f; - if (is_dead_reckoning) { - // calculate a conservative estimate of the error in our position - if (_control_status.flags.gps || _control_status.flags.ev_pos) { - pos_err_alt = math::max(pos_err_alt, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4])); - - } - hpos_err = math::max(hpos_err, pos_err_alt); + if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) { + hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4])); } memcpy(ekf_eph, &hpos_err, sizeof(float)); memcpy(ekf_epv, &vpos_err, sizeof(float)); - memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool)); + memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool)); } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position @@ -781,27 +771,17 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko } - // report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift - uint64_t dead_reckoning_duration = math::min(math::min((_time_last_imu - _time_last_pos_fuse),(_time_last_imu - _time_last_vel_fuse)),(_time_last_imu - _time_last_of_fuse)); - bool is_dead_reckoning = dead_reckoning_duration > 1e6; - - // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error + // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - float pos_err_alt = 0.0f; - if (is_dead_reckoning) { - // calculate a conservative estimate of the error in our position - if (_control_status.flags.gps || _control_status.flags.ev_pos) { - pos_err_alt = math::max(pos_err_alt, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4])); - - } - hpos_err = math::max(hpos_err, pos_err_alt); + if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) { + hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4])); } memcpy(ekf_eph, &hpos_err, sizeof(float)); memcpy(ekf_epv, &vpos_err, sizeof(float)); - memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool)); + memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool)); } // get the 1-sigma horizontal and vertical velocity uncertainty @@ -820,27 +800,24 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon } - // report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift - bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6) && (_time_last_imu - _time_last_vel_fuse > 1e6) && (_time_last_imu - _time_last_of_fuse > 1e6); - // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error - // The reason is that complete rejection of measurements is often be casued by heading misalignment or inertial sensing errors - // and using state variances for accuracy reporting provides an overly optimistic assessment in these situations - float vel_err_alt = 0.0f; - if (is_dead_reckoning) { + // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors + // and using state variances for accuracy reporting is overly optimistic in these situations + float vel_err_conservative = 0.0f; + if (_is_dead_reckoning) { if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); - vel_err_alt = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(_flow_innov[0]*_flow_innov[0] + _flow_innov[1]*_flow_innov[1]); + vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(_flow_innov[0]*_flow_innov[0] + _flow_innov[1]*_flow_innov[1]); } if (_control_status.flags.gps || _control_status.flags.ev_pos) { - vel_err_alt = math::max(vel_err_alt, sqrtf(_vel_pos_innov[0]*_vel_pos_innov[0] + _vel_pos_innov[1]*_vel_pos_innov[1])); + vel_err_conservative = math::max(vel_err_conservative, sqrtf(_vel_pos_innov[0]*_vel_pos_innov[0] + _vel_pos_innov[1]*_vel_pos_innov[1])); } - hvel_err = math::max(hvel_err, vel_err_alt); + hvel_err = math::max(hvel_err, vel_err_conservative); } memcpy(ekf_evh, &hvel_err, sizeof(float)); memcpy(ekf_evv, &vvel_err, sizeof(float)); - memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool)); + memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool)); } // get EKF innovation consistency check status information comprising of: diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 9041a10e76..3e3e317f8e 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -73,6 +73,7 @@ EstimatorInterface::EstimatorInterface(): _tas_test_ratio(0.0f), _terr_test_ratio(0.0f), _beta_test_ratio(0.0f), + _is_dead_reckoning(false), _vibe_metrics{}, _time_last_imu(0), _time_last_gps(0), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 40e32f00bc..6dc4c275df 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -336,9 +336,11 @@ protected: float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios float _tas_test_ratio; // tas innovation consistency check ratio float _terr_test_ratio; // height above terrain measurement innovation consistency check ratio - float _beta_test_ratio; // sideslip innovation consistency check ratio + float _beta_test_ratio; // sideslip innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status{}; + bool _is_dead_reckoning; // true if we are no longer fusing measurements that constrain horizontal velocity drift + // IMU vibration monitoring Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement