From 21270bb06ad7297c1b0e18697576c5a9a87f8eac Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 Jan 2017 08:58:01 +1100 Subject: [PATCH 1/3] EKF: Use last known position when resuming OF aiding --- EKF/control.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 0b38b5a34f..28d8cc9465 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -279,15 +279,21 @@ void Ekf::controlOpticalFlowFusion() _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; - // reset the corresponding covariances - // we are by definition at the origin at commencement so variances are also zeroed - zeroRows(P,7,8); - zeroCols(P,7,8); - - // align the output observer to the EKF states - alignOutputFilter(); + } else { + // set to the last known position + _state.pos(0) = _last_known_posNE(0); + _state.pos(1) = _last_known_posNE(1); } + + // reset the corresponding covariances + // we are by definition at the origin at commencement so variances are also zeroed + zeroRows(P,7,8); + zeroCols(P,7,8); + + // align the output observer to the EKF states + alignOutputFilter(); + } } From 3fb7effb0c7a22a397d5774c7471e7c3f0be52cf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 Jan 2017 09:19:21 +1100 Subject: [PATCH 2/3] EKF: Allow optical flow use when on-ground with poor data quality --- EKF/estimator_interface.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 7b043e13e0..9575788d48 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -303,7 +303,7 @@ 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) { + if (delta_time_good && flow_magnitude_good && (flow_quality_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; @@ -311,8 +311,16 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl optflow_sample_new.quality = flow->quality; // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis. // copy the optical and gyro measured delta angles - optflow_sample_new.flowRadXY = - flow->flowdata; optflow_sample_new.gyroXYZ = - flow->gyrodata; + if (flow_quality_good) { + optflow_sample_new.flowRadXY = - flow->flowdata; + + } 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); + + } // compensate for body motion to give a LOS rate 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); From bcf7cac5d95e509ac5eccb26ccbe6c890df8c17c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 Jan 2017 20:14:18 +1100 Subject: [PATCH 3/3] EKF: remove hardcoded limit on optical flow time delta Allow for up to 50% lost data. --- EKF/estimator_interface.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 9575788d48..fa6081c1e6 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -287,9 +287,10 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // limit data rate to prevent data being lost if (time_usec - _time_last_optflow > _min_obs_interval_us) { - // check if enough integration time + // 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 >= 0.05f); + bool delta_time_good = (delta_time >= 5e-7f * (float)_min_obs_interval_us); // check magnitude is within sensor limits float flow_rate_magnitude;