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

EKF: Fix bugs preventing use of external vision yaw data

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-14 16:10:34 +09:30
parent 13c3a95bc1
commit 920d83d68c
3 changed files with 24 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -163,7 +163,6 @@ struct extVisionSample {
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define USE_EV_YAW 3 // Fuse yaw angle from external vision system
// Maximum sensor intervals in usec
#define GPS_MAX_INTERVAL 5e5

13
EKF/control.cpp
View File

@ -86,7 +86,7 @@ void Ekf::controlExternalVisionAiding()
}
// external vision yaw aiding selection logic
if ((_params.mag_fusion_type == USE_EV_YAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
@ -107,8 +107,12 @@ void Ekf::controlExternalVisionAiding()
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements
// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
printf("EKF switching to external vision yaw fusion\n");
}
}
@ -462,6 +466,11 @@ void Ekf::controlHeightAiding()
void Ekf::controlMagAiding()
{
// If we are using external vision data for heading then no magnetometer fusion is used
if (_control_status.flags.ev_yaw) {
return;
}
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
// or the more accurate 3-axis fusion
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {

17
EKF/mag_fusion.cpp
View File

@ -379,9 +379,7 @@ void Ekf::fuseHeading()
float q2 = _state.quat_nominal(2);
float q3 = _state.quat_nominal(3);
float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
R_YAW = R_YAW * R_YAW;
float R_YAW = 1.0f;
float predicted_hdg;
float H_YAW[4];
matrix::Vector3f mag_earth_pred;
@ -522,11 +520,22 @@ void Ekf::fuseHeading()
}
}
// Calculate the observation variance
if (_control_status.flags.mag_hdg) {
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
} else if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else {
// there is no yaw observation
return;
}
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
// calculate the innovaton variance
float PH[4];
_heading_innov_var = R_YAW;
for (unsigned row = 0; row <= 3; row++) {
PH[row] = 0.0f;