fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix bug causing slow drift when high rate flow data is used

Browse Source 
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.
This commit is contained in:
Paul Riseborough
2018-06-08 15:42:40 +10:00
committed by Lorenz Meier
parent ee2dc7d790
commit d177e96508
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+2 -1
View File
@@ -43,6 +43,7 @@
#include "ekf.h" #include "ekf.h"
#include <ecl.h> #include <ecl.h>
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
#include <cfloat>
void Ekf::fuseOptFlow() void Ekf::fuseOptFlow()
{ {
@@ -538,7 +539,7 @@ void Ekf::calcOptFlowBias()
// if accumulation time differences are not excessive and accumulation time is adequate // 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 // 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 // calculate a reference angular rate
Vector3f reference_body_rate; Vector3f reference_body_rate;
reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);