dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix bug in accumulation of IMU data for flow sensor gyro bias calculation

This commit is contained in:
Paul Riseborough 2017-09-14 15:47:15 +10:00
parent 3b3326e250
commit 1d3e8edc46
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/control.cpp
View File

@ -379,6 +379,10 @@ void Ekf::controlOpticalFlowFusion()
} }
} }
// 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;
// fuse the data // fuse the data
if (_control_status.flags.opt_flow) { if (_control_status.flags.opt_flow) {
// Update optical flow bias estimates // Update optical flow bias estimates

8
EKF/optflow_fusion.cpp
View File

@ -524,20 +524,16 @@ void Ekf::get_drag_innov_var(float drag_innov_var[2])
// calculate optical flow gyro bias errors // calculate optical flow gyro bias errors
void Ekf::calcOptFlowBias() void Ekf::calcOptFlowBias()
{ {
// accumulate the bias corrected delta angles from the navigation sensor and lapsed time
_imu_del_ang_of += _imu_sample_delayed.delta_ang;
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
// reset the accumulators if the time interval is too large // reset the accumulators if the time interval is too large
if (_delta_time_of > 1.0f) { if (_delta_time_of > 1.0f) {
_imu_del_ang_of.setZero(); _imu_del_ang_of.setZero();
_delta_time_of = 0.0f; _delta_time_of = 0.0f;
return;
} }
// 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.05f) && (_delta_time_of > 0.01f) if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > 0.01f)) {
&& (_flow_sample_delayed.dt > 0.01f)) {
// 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);