From 81a64c04790375b422a52b4f7274261b45d0afe2 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 10 Oct 2017 14:39:44 +0200 Subject: [PATCH 1/7] ekf: fix flow direction when on ground --- EKF/estimator_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 4cf801a2f6..a0ec16e692 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -307,8 +307,8 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl } else { // when on the ground with poor flow quality, assume zero ground relative velocity - optflow_sample_new.flowRadXY(0) = + flow->gyrodata(0); - optflow_sample_new.flowRadXY(1) = + flow->gyrodata(1); + optflow_sample_new.flowRadXY(0) = - flow->gyrodata(0); + optflow_sample_new.flowRadXY(1) = - flow->gyrodata(1); } From b7e589b98aa02f43af0f56e9a117a2e16af2b0c4 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 10 Oct 2017 17:51:56 +0200 Subject: [PATCH 2/7] EKF: only fuse optical flow if terrain is valid --- EKF/control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index ac5ca71020..ed195e772a 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -383,8 +383,8 @@ void Ekf::controlOpticalFlowFusion() _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias; _delta_time_of += _imu_sample_delayed.delta_ang_dt; - // fuse the data - if (_control_status.flags.opt_flow) { + // fuse the data if the terrain/distance to bottom is valid + if (_control_status.flags.opt_flow && get_terrain_valid()) { // Update optical flow bias estimates calcOptFlowBias(); From ab9b8e19648b7b838bce87593c49609ee8e0f1bb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 11 Oct 2017 20:59:33 +1100 Subject: [PATCH 3/7] EKF: Prevent bad optical flow quality causing loss of aiding when on ground --- EKF/estimator_interface.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a0ec16e692..283b435630 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -282,7 +282,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check magnitude is within sensor limits float flow_rate_magnitude; - bool flow_magnitude_good = false; + bool flow_magnitude_good = true; if (delta_time_good) { flow_rate_magnitude = flow->flowdata.norm() / delta_time; @@ -292,7 +292,9 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check quality metric bool flow_quality_good = (flow->quality >= _params.flow_qual_min); - if (delta_time_good && flow_magnitude_good && (flow_quality_good || !_control_status.flags.in_air)) { + // Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling + // If flow quality fails checks on ground, assume zero flow rate after body rate compensation + if ((delta_time_good && flow_quality_good && flow_magnitude_good) || !_control_status.flags.in_air) { flowSample optflow_sample_new; // calculate the system time-stamp for the mid point of the integration period optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2; From d293c4231ded671d6c87a71d77641501e3172161 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 11 Oct 2017 21:54:47 +1100 Subject: [PATCH 4/7] EKF: Protect against divide by zero caused by invalid optical flow --- EKF/estimator_interface.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 283b435630..3b28b99297 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -278,7 +278,13 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check if enough integration time and fail if integration time is less than 50% // of min arrival interval because too much data is being lost float delta_time = 1e-6f * (float)flow->dt; - bool delta_time_good = (delta_time >= 5e-7f * (float)_min_obs_interval_us); + float delta_time_min = 5e-7f * (float)_min_obs_interval_us; + bool delta_time_good = delta_time >= delta_time_min; + if (!delta_time_good) { + // protect against overflow casued by division with very small delta_time + delta_time = delta_time_min; + } + // check magnitude is within sensor limits float flow_rate_magnitude; From cdf6e6cd36a40af1df97e9e84c845ec7f319c07f Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Wed, 11 Oct 2017 15:15:06 +0200 Subject: [PATCH 5/7] EKF: use delta_time to avoid division by zero --- EKF/estimator_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3b28b99297..3eedddf3a0 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -324,7 +324,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0); optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1); // convert integration interval to seconds - optflow_sample_new.dt = 1e-6f * (float)flow->dt; + optflow_sample_new.dt = delta_time; _time_last_optflow = time_usec; // push to buffer _flow_buffer.push(optflow_sample_new); From 1119a9b0ac1c276533dadf7e3aa8f4bf05ae5907 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Oct 2017 07:58:54 +1100 Subject: [PATCH 6/7] EKF: Enable optical flow reversion to work if flow data is lost Also improve reporting. --- EKF/control.cpp | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index ed195e772a..e8c1673d1f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -300,6 +300,7 @@ void Ekf::controlOpticalFlowFusion() // set the flag and reset the fusion timeout _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; + ECL_INFO("EKF Starting Optical Flow Use"); // if we are not using GPS then the velocity and position states and covariances need to be set if (!_control_status.flags.gps) { @@ -365,20 +366,6 @@ 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) { - // 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)) { - // 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(); - - } - } - // Accumulate autopilot gyro data across the same time interval as the flow sensor _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias; _delta_time_of += _imu_sample_delayed.delta_ang_dt; @@ -395,6 +382,21 @@ 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) { + // 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)) { + // 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"); + } + } + } void Ekf::controlGpsFusion() From 4db23b7b2e9ad4b903933cdba0c264b0a4c69672 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Oct 2017 08:26:04 +1100 Subject: [PATCH 7/7] EKF: Don't report terrain estimate invalid immediately when HAGL observations fail checks --- EKF/terrain_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index d6dfff8814..da6b56755a 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -159,7 +159,7 @@ void Ekf::fuseHagl() bool Ekf::get_terrain_valid() { if (_terrain_initialised && _range_data_continuous && !_rng_stuck && - !_innov_check_fail_status.flags.reject_hagl) { + (_time_last_imu - _time_last_hagl_fuse < 5E6)) { return true; } else {