From d177e96508d2572f6fa8eb7ff41852749c882548 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 8 Jun 2018 15:42:40 +1000 Subject: [PATCH] EKF: Fix bug causing slow drift when high rate flow data is used High rate optical flow data could make flow fusion to run every major update cycle, resulting in the calculation of bias errors in the body rates used to compensate flow data failing time validity checks and not running. This resulted in a slow drift of the nav solution if bias errors were present in the in the gyro data used for flow sensor motion compensation. --- EKF/optflow_fusion.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index a79a5b0a65..45342bc4f9 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -43,6 +43,7 @@ #include "ekf.h" #include #include +#include void Ekf::fuseOptFlow() { @@ -538,7 +539,7 @@ void Ekf::calcOptFlowBias() // if accumulation time differences are not excessive and accumulation time is adequate // compare the optical flow and and navigation rate data and calculate a bias error - if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > 0.01f)) { + if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) { // calculate a reference angular rate Vector3f reference_body_rate; reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);