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

EKF: Improve resistance to bad initial mag offset 

When magnetic field states have been reset in-flight using a single sample, the magnetic field states are not used to constrain heading drift for a period after the reset. This period has been shortened from 10 to 5 seconds which is enough time to average out the effects of measurement noise (the original concern). The shorter time has enabled the previous practice for RW vehicles of using magnetic heading in that time period to constrain yaw drift to be discontinued. This is necessary becasue while magnetic heading is being used, it fights the yaw corrections obtained from GPs observations and lengthens the time required to recover from a bad mag calibration.
This commit is contained in:
Paul Riseborough 2017-11-10 10:32:49 +11:00
parent df9f48d2d3
commit 72a7ab2c25
1 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/control.cpp
View File

@ -1244,8 +1244,9 @@ void Ekf::controlMagFusion()
_control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW)
&& _control_status.flags.fixed_wing;
// For the first 10 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise
_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)1e7);
// For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise
// before they are used to constrain heading drift
_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6);
if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) {
// When re-commencing use of magnetometer to correct vehicle states
@ -1296,12 +1297,6 @@ void Ekf::controlMagFusion()
fuseDeclination();
}
if (_flt_mag_align_converging && !_control_status.flags.fixed_wing) {
_control_status.flags.mag_hdg = true;
fuseHeading();
_control_status.flags.mag_hdg = false;
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();