From 920d83d68ce6b146f51967953c8a7cb366aeb8bc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 May 2016 16:10:34 +0930 Subject: [PATCH] EKF: Fix bugs preventing use of external vision yaw data --- EKF/common.h | 1 - EKF/control.cpp | 13 +++++++++++-- EKF/mag_fusion.cpp | 17 +++++++++++++---- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 35d2966704..f7ac319286 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -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 diff --git a/EKF/control.cpp b/EKF/control.cpp index 2d7dbf6dec..006e933564 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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) { diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 950516b449..d93e416159 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -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;