From 9857fb9eb6961c07d235dc6fa284d6d2b10fdbe4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 19 Sep 2017 08:38:19 +1000 Subject: [PATCH 01/11] EKF: publish control limits for optical flow navigation --- EKF/ekf.h | 7 +++++++ EKF/ekf_helper.cpp | 30 ++++++++++++++++++++++++++++++ EKF/estimator_interface.h | 7 +++++++ 3 files changed, 44 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index acbb43f7c8..080b296b73 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -138,6 +138,13 @@ public: // get the 1-sigma horizontal and vertical velocity uncertainty void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning); + /* + Returns the following vehicle control limits required by the estimator. + vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. + tilt_rate_max : maximum allowed tilt rate against the direction of travel (rad/sec). NaN when no limiting required. + */ + void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl); + 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 521e00e5c4..32a51a79df 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1000,6 +1000,36 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool)); } +/* +Returns the following vehicle control limits required by the estimator. +vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. +limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits. +*/ +void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) +{ + float flow_gnd_spd_max; + bool flow_limit_hagl; + + // If relying on optical flow for navigation we need to keep within flow and range sensor limits + bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); + if (relying_on_optical_flow) { + // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion + flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); + flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f); + + flow_limit_hagl = true; + + } else { + flow_gnd_spd_max = NAN; + flow_limit_hagl = false; + + } + + memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float)); + memcpy(limit_hagl, &flow_limit_hagl, 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 a386232720..07c0fec6c5 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -148,6 +148,13 @@ public: // get the 1-sigma horizontal and vertical velocity uncertainty virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) = 0; + /* + Returns the following vehicle control limits required by the estimator. + vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. + limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits. + */ + virtual void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) = 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; } From f5fd90533acc61c3de8fbf1763baa6ba3bd10316 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Wed, 18 Oct 2017 15:40:05 +0200 Subject: [PATCH 02/11] fix gps and flow flag handling gps flag was not turning false if there was flow only reset states if we were relying on that sensor only --- EKF/control.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index d27421b831..fa228705cc 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -384,16 +384,17 @@ void Ekf::controlOpticalFlowFusion() } // handle the case when we are relying on optical flow fusion and lose it - if (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) { + if ((_time_last_imu - _time_last_of_fuse > 5e6) && _control_status.flags.opt_flow) { + ECL_INFO("EKF Stopping Optical Flow Use"); + _control_status.flags.opt_flow = false; // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something - if ((_time_last_imu - _time_last_of_fuse > 5e6)) { + if (!_control_status.flags.gps && !_control_status.flags.ev_pos) { // Switch to the non-aiding mode, zero the velocity states // and set the synthetic position to the current estimate - _control_status.flags.opt_flow = false; _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); - ECL_WARN("EKF Stopping Optical Flow Use"); + ECL_WARN("EKF Resetting states"); } } @@ -492,16 +493,19 @@ void Ekf::controlGpsFusion() } else { // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it - if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6) && _control_status.flags.gps) { + if ((_time_last_imu - _time_last_gps > 10e6) && _control_status.flags.gps) { // if we don't have a source of aiding to constrain attitude drift, // then we need to switch to the non-aiding mode, zero the velocity states // and set the synthetic GPS position to the current estimate _control_status.flags.gps = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - ECL_WARN("EKF measurement timeout - stopping navigation"); + ECL_INFO("EKF Stopping GPS Use"); + if (!_control_status.flags.opt_flow && !_control_status.flags.ev_pos) { + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + ECL_WARN("EKF Resetting states"); + } } } } From 204a218ee6c31d4bf3297812aa3e62969150803b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 19 Oct 2017 08:26:16 +1100 Subject: [PATCH 03/11] EKF: Allow dead-reckoning using air data --- EKF/control.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index fa228705cc..3321d08ad5 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -394,7 +394,7 @@ void Ekf::controlOpticalFlowFusion() _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); - ECL_WARN("EKF Resetting states"); + ECL_WARN("EKF stopping navigation"); } } @@ -493,19 +493,20 @@ void Ekf::controlGpsFusion() } else { // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it - if ((_time_last_imu - _time_last_gps > 10e6) && _control_status.flags.gps) { - // if we don't have a source of aiding to constrain attitude drift, - // then we need to switch to the non-aiding mode, zero the velocity states - // and set the synthetic GPS position to the current estimate + bool lost_gps = _control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6); + bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos; + bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6); + // If we don't have a source of aiding to constrain attitude drift, then we need to switch + // to the non-aiding mode, zero the velocity states and set the position to the current estimate. + // Air data aiding prevents loss of attitude reference, constrains velocity drift and can assist + // a FW vehicle to recover to the launch area. + if (lost_gps && no_velpos_aiding && no_airdata_aiding) { _control_status.flags.gps = false; - ECL_INFO("EKF Stopping GPS Use"); + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + ECL_WARN("EKF stopping navigation"); - if (!_control_status.flags.opt_flow && !_control_status.flags.ev_pos) { - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - ECL_WARN("EKF Resetting states"); - } } } } From 929c5c2b37af8966aa6542a9af3dbe678c80a96a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 19 Oct 2017 18:21:47 +1100 Subject: [PATCH 04/11] EKF: enable gps fusion flag to be false while fusing air data --- EKF/common.h | 1 + EKF/control.cpp | 37 +++++++++++++++++++++++++------------ EKF/ekf_helper.cpp | 21 +++++++++++++++------ EKF/estimator_interface.h | 2 +- 4 files changed, 42 insertions(+), 19 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 70cb67cf49..a998987e1d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -416,6 +416,7 @@ union filter_control_status_u { uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used + uint32_t fuse_aspd : 1; ///< 19 - true when airspedd measurements are being fused } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 3321d08ad5..a962adacc1 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -494,14 +494,18 @@ void Ekf::controlGpsFusion() } else { // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it bool lost_gps = _control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6); - bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos; - bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6); // If we don't have a source of aiding to constrain attitude drift, then we need to switch // to the non-aiding mode, zero the velocity states and set the position to the current estimate. // Air data aiding prevents loss of attitude reference, constrains velocity drift and can assist // a FW vehicle to recover to the launch area. - if (lost_gps && no_velpos_aiding && no_airdata_aiding) { + if (lost_gps) { _control_status.flags.gps = false; + ECL_WARN("EKF GPS data stopped"); + + } + bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos; + bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6); + if (!_control_status.flags.gps && no_velpos_aiding && no_airdata_aiding) { _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); @@ -1010,9 +1014,18 @@ void Ekf::controlAirDataFusion() } - // Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode - bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; - if (_tas_data_ready && _control_status.flags.in_air && is_aiding) { + if (_control_status.flags.fuse_aspd && airspeed_timed_out) { + _control_status.flags.fuse_aspd = false; + + } + + // Always try to fuse airspeed data if available and we are in flight + if (_tas_data_ready && _control_status.flags.in_air) { + // always fuse airsped data if we are flying and data is present + if (!_control_status.flags.fuse_aspd) { + _control_status.flags.fuse_aspd = true; + } + // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1047,10 +1060,7 @@ void Ekf::controlBetaFusion() // Suffient time has lapsed sice the last fusion bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; - // The filter is operating in a mode where velocity states can be used - bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; - - if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) { + if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1267,8 +1277,11 @@ void Ekf::controlVelPosFusion() { // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + if (!_control_status.flags.gps && + !_control_status.flags.opt_flow && + !_control_status.flags.ev_pos && + !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) && + ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { _fuse_pos = true; _time_last_fake_gps = _time_last_imu; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 32a51a79df..466b48a8fb 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -902,7 +902,10 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko // 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); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph)); @@ -933,7 +936,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko // 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); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hpos_err = sqrtf(P[7][7] + P[8][8]); @@ -963,7 +969,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon { float hvel_err; float vvel_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hvel_err = sqrtf(P[4][4] + P[5][5]); @@ -1058,9 +1067,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) { ekf_solution_status soln_status{}; soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); - soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; soln_status.flags.pos_vert_agl = get_terrain_valid(); @@ -1146,7 +1155,7 @@ bool Ekf::inertial_dead_reckoning() bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6)); bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6); - bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6); + bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6) && (_time_last_imu - _time_last_beta_fuse <= 1E6); return !velPosAiding && !optFlowAiding && !airDataAiding; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 07c0fec6c5..91497618f0 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -199,7 +199,7 @@ public: void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;} // set flag if synthetic sideslip measurement should be fused - void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;} + void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);} // set flag if only only mag states should be updated by the magnetometer void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;} From a2dcd5b9b6164eb741b861ca593807968df7bf6c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 08:25:50 +1100 Subject: [PATCH 05/11] EKF: Consolidate no aiding reset logic --- EKF/control.cpp | 68 +++++++++++++++---------------------------------- 1 file changed, 21 insertions(+), 47 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a962adacc1..9cadcea392 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -134,7 +134,8 @@ void Ekf::controlFusionModes() // 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); + && (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max) + && ((_time_last_imu - _time_last_arsp_fuse > _params.no_aid_timeout_max) || (_time_last_imu - _time_last_beta_fuse > _params.no_aid_timeout_max)); } @@ -261,20 +262,11 @@ void Ekf::controlExternalVisionFusion() fuseHeading(); } - } + } else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > 5e6)) { + // Turn off EV fusion mode if no data has been received + _control_status.flags.ev_pos = false; + ECL_INFO("EKF External Vision Data Stopped"); - // handle the case when we are relying on ev data and lose it - if (_control_status.flags.ev_pos && !_control_status.flags.gps && !_control_status.flags.opt_flow) { - // We are relying on ev aiding to constrain drift so after 5s without aiding we need to do something - if ((_time_last_imu - _time_last_pos_fuse > 5e6)) { - // Switch to the non-aiding mode, zero the velocity states - // and set the synthetic position to the current estimate - _control_status.flags.ev_pos = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - - } } } @@ -381,21 +373,10 @@ void Ekf::controlOpticalFlowFusion() _last_known_posNE(1) = _state.pos(1); } - } - - // handle the case when we are relying on optical flow fusion and lose it - if ((_time_last_imu - _time_last_of_fuse > 5e6) && _control_status.flags.opt_flow) { - ECL_INFO("EKF Stopping Optical Flow Use"); + } else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > 5e6)) { + ECL_INFO("EKF Optical Flow Data Stopped"); _control_status.flags.opt_flow = false; - // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something - if (!_control_status.flags.gps && !_control_status.flags.ev_pos) { - // Switch to the non-aiding mode, zero the velocity states - // and set the synthetic position to the current estimate - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - ECL_WARN("EKF stopping navigation"); - } + } } @@ -491,27 +472,10 @@ void Ekf::controlGpsFusion() } - } else { - // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it - bool lost_gps = _control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6); - // If we don't have a source of aiding to constrain attitude drift, then we need to switch - // to the non-aiding mode, zero the velocity states and set the position to the current estimate. - // Air data aiding prevents loss of attitude reference, constrains velocity drift and can assist - // a FW vehicle to recover to the launch area. - if (lost_gps) { + } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) { _control_status.flags.gps = false; ECL_WARN("EKF GPS data stopped"); - } - bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos; - bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6); - if (!_control_status.flags.gps && no_velpos_aiding && no_airdata_aiding) { - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - ECL_WARN("EKF stopping navigation"); - - } } } @@ -1281,7 +1245,17 @@ void Ekf::controlVelPosFusion() !_control_status.flags.opt_flow && !_control_status.flags.ev_pos && !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) && - ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) + { + // Reset position and velocity states if we re-commence this aiding method + if ((_time_last_imu - _time_last_fake_gps) > 4E5) { + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + ECL_WARN("EKF stopping navigation"); + + } + _fuse_pos = true; _time_last_fake_gps = _time_last_imu; From 59f1c3e19e70e7fd0a9783971aa5063087f99227 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 10:50:34 +1100 Subject: [PATCH 06/11] EKF: Update dead-reckoning definition Use of air data to navigate should be classified as dead-reckoning because neither ground relative velocity or position is observed directly and errors grow faster. --- EKF/control.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9cadcea392..5daa3e92ca 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -131,11 +131,10 @@ void Ekf::controlFusionModes() // Additional data from an external vision sensor can also be fused. controlExternalVisionFusion(); - // report dead reckoning if we are no longer fusing measurements that constrain velocity drift + // report dead reckoning if we are no longer fusing measurements that directly 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) - && ((_time_last_imu - _time_last_arsp_fuse > _params.no_aid_timeout_max) || (_time_last_imu - _time_last_beta_fuse > _params.no_aid_timeout_max)); + && (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max); } From e4ffe199ed95b0c2cac6f7d16a2c4ce5a8fe775c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 10:51:17 +1100 Subject: [PATCH 07/11] EKF: fix bug in sideslip fusion activation --- EKF/sideslip_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index b3f102b0fc..ac5e83bf89 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -79,7 +79,7 @@ void Ekf::fuseSideslip() rel_wind = earth_to_body * rel_wind; // perform fusion of assumed sideslip = 0 - if (rel_wind(0) > 7.0f) { + if (rel_wind.norm() > 7.0f) { // Calculate the observation jacobians // intermediate variable from algebraic optimisation From 0d32128701e6326cf6c167b6033728442b399ed1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 10:55:00 +1100 Subject: [PATCH 08/11] EKF: Use dead-reckoning status to determine if air data should modify non-wind states --- EKF/airspeed_fusion.cpp | 5 ++++- EKF/sideslip_fusion.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index f52cb8e15d..89a3441471 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -72,6 +72,9 @@ void Ekf::fuseAirspeed() // Perform fusion of True Airspeed measurement if (v_tas_pred > 1.0f) { + // determine if we need the sideslip fusion to correct states other than wind + bool update_wind_only = !_is_dead_reckoning; + // Calculate the observation jacobian // intermediate variable from algebraic optimisation SH_TAS[0] = 1.0f/v_tas_pred; @@ -102,7 +105,7 @@ void Ekf::fuseAirspeed() SK_TAS[1] = SH_TAS[1]; - if (((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6)) { + if (update_wind_only) { // If we are getting aiding from other sources, then don't allow the airspeed measurements to affect the non-windspeed states for (unsigned row = 0; row <= 21; row++) { Kfusion[row] = 0.0f; diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index ac5e83bf89..a3eed52e35 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -117,7 +117,7 @@ void Ekf::fuseSideslip() } // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = ((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6); + bool update_wind_only = !_is_dead_reckoning; // intermediate variables - note SK_BETA[0] is 1/(innovation variance) _beta_innov_var = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3)))); From 55a2dc94dfcb7c0e65ba4d78a21d4aba07555860 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 10:56:24 +1100 Subject: [PATCH 09/11] EKF: handle air data fusion covariance reset consistently Both the sideslip and airspeed fusion should not be resetting covariances for states they do not modify. --- EKF/airspeed_fusion.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 89a3441471..6d8ae81132 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -98,8 +98,19 @@ void Ekf::fuseAirspeed() } else { // Reset the estimator covarinace matrix _fault_status.flags.bad_airspeed = true; - initialiseCovariance(); - ECL_ERR("EKF airspeed fusion numerical error - covariance reset"); + + // if we are getting aiding from other sources, warn and reset the wind states and covariances only + if (update_wind_only) { + resetWindStates(); + resetWindCovariance(); + ECL_ERR("EKF airspeed fusion badly conditioned - wind covariance reset"); + + } else { + initialiseCovariance(); + _state.wind_vel.setZero(); + ECL_ERR("EKF airspeed fusion badly conditioned - full covariance reset"); + } + return; } From 19074fdd9ef713aa6ca87d12599c0f4972547a9a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 11:11:50 +1100 Subject: [PATCH 10/11] EKF: Use consistent time limit for inertial dead reckoning test --- EKF/ekf_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 466b48a8fb..f9305acb61 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1153,9 +1153,9 @@ bool Ekf::global_position_is_valid() bool Ekf::inertial_dead_reckoning() { bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) - && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6)); - bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6); - bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6) && (_time_last_imu - _time_last_beta_fuse <= 1E6); + && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max)); + bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max); + bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max); return !velPosAiding && !optFlowAiding && !airDataAiding; } From e10ec590587ca0f590b0adc5f085b3289b051b37 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 11:16:48 +1100 Subject: [PATCH 11/11] EKF: Use consistent test for navigation validity reporting This will enable controller to take advantage of non-inertial dead reckoning. --- EKF/ekf_helper.cpp | 5 ++--- EKF/estimator_interface.cpp | 6 ++---- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f9305acb61..de27c0db40 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1144,9 +1144,8 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first bool Ekf::global_position_is_valid() { - // return true if the position estimate is valid - // TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status - return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps); + // return true if we are not doing unconstrained free inertial navigation and the origin is set + return (_NED_origin_initialised && !inertial_dead_reckoning()); } // return true if we are totally reliant on inertial dead-reckoning for position diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3eedddf3a0..ddee0c0e30 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -464,8 +464,6 @@ void EstimatorInterface::unallocate_buffers() bool EstimatorInterface::local_position_is_valid() { - // return true if the position estimate is valid - return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || - (((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) || - global_position_is_valid(); + // return true if we are not doing unconstrained free inertial navigation + return !inertial_dead_reckoning(); }