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

EKF: Fix on ground yaw drift when using EKF2_MAG_TYPE = 4

This commit is contained in:
Paul Riseborough 2019-08-06 18:22:00 +10:00 committed by Paul Riseborough
parent 4e946d5bcb
commit eae6e8f19c
3 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -1582,7 +1582,7 @@ void Ekf::controlMagFusion()
save_mag_cov_data();
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING || _params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) {
// always use heading fusion
_control_status.flags.mag_hdg = true;

1
EKF/ekf.h
View File

@ -359,7 +359,6 @@ private:
bool _mag_use_inhibit_prev{false}; ///< true when magnetometer use was being inhibited the previous frame
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements

29
EKF/mag_fusion.cpp
View File

@ -511,9 +511,15 @@ void Ekf::fuseHeading()
float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
} else if (_mag_use_inhibit) {
// Special case where we either use the current or last known stationary value
// so set to current value as a safe default
measured_hdg = predicted_hdg;
} else {
// there is no yaw observation
// Should not be doing yaw fusion
return;
}
} else {
@ -604,8 +610,13 @@ void Ekf::fuseHeading()
float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1);
} else if (_mag_use_inhibit) {
// Special case where we either use the current or last known stationary value
// so set to current value as a safe default
measured_hdg = predicted_hdg;
} else {
// there is no yaw observation
// Should not be doing yaw fusion
return;
}
@ -621,8 +632,8 @@ void Ekf::fuseHeading()
R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else {
// there is no yaw observation
return;
// default value
R_YAW = 0.01f;
}
// wrap the heading to the interval between +-pi
@ -637,22 +648,20 @@ void Ekf::fuseHeading()
// Vehicle is not at rest so fuse a zero innovation and record the
// predicted heading to use as an observation when movement ceases.
_heading_innov = 0.0f;
_vehicle_at_rest_prev = false;
_last_static_yaw = predicted_hdg;
} else {
// Vehicle is at rest so use the last moving prediction as an observation
// to prevent the heading from drifting and to enable yaw gyro bias learning
// before takeoff.
if (!_vehicle_at_rest_prev || !_mag_use_inhibit_prev) {
_last_static_yaw = predicted_hdg;
_vehicle_at_rest_prev = true;
}
_heading_innov = predicted_hdg - _last_static_yaw;
R_YAW = 0.01f;
innov_gate = 5.0f;
}
} else {
_heading_innov = predicted_hdg - measured_hdg;
_last_static_yaw = predicted_hdg;
}
_mag_use_inhibit_prev = _mag_use_inhibit;