From e917d6c7f2ca66f6ad68e98afbe914130ebc5dc1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Mar 2016 23:22:53 +1100 Subject: [PATCH] EKF: Add fusion of external yaw data --- EKF/common.h | 1 + EKF/control.cpp | 27 ++++++++++++++++++++++++--- EKF/ekf.cpp | 4 ++++ EKF/mag_fusion.cpp | 38 +++++++++++++++++++++++++++++++------- 4 files changed, 60 insertions(+), 10 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 9ea14ad859..53c89f3c0f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -163,6 +163,7 @@ 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 557afd0633..f16ab8da63 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -70,9 +70,30 @@ void Ekf::controlFusionModes() } // external vision yaw aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align - && (_time_last_imu - _time_last_ext_vision) < 5e5) { - //TODO + if ((_params.mag_fusion_type == USE_EV_YAW) && !_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 + // get the roll, pitch, yaw estimates from the quaternion states + matrix::Quaternion q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), + _state.quat_nominal(3)); + matrix::Euler euler_init(q_init); + + // get initial yaw from the observation quaternion + extVisionSample ev_newest = _ext_vision_buffer.get_newest(); + matrix::Quaternion q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); + matrix::Euler euler_obs(q_obs); + euler_init(2) = euler_obs(2); + + // calculate initial quaternion states for the ekf + _state.quat_nominal = Quaternion(euler_init); + + // flag the yaw as aligned + _control_status.flags.yaw_align = true; + + // turn on fusion of external vision yaw measurements + _control_status.flags.ev_yaw = true; + } } // optical flow fusion mode selection logic diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4bac4bdd63..49df81540f 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -310,6 +310,10 @@ bool Ekf::update() _fuse_pos = true; _fuse_height = true; } + // use external vision yaw observation + if (_control_status.flags.ev_yaw) { + fuseHeading(); + } } // If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 88c133deae..e55062dbd9 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -385,6 +385,7 @@ void Ekf::fuseHeading() float predicted_hdg; float H_YAW[4]; matrix::Vector3f mag_earth_pred; + float measured_hdg; // determine if a 321 or 312 Euler sequence is best if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { @@ -426,8 +427,21 @@ void Ekf::fuseHeading() euler321(2) = 0.0f; matrix::Dcm R_to_earth(euler321); - // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // calculate the observed yaw angle + if (_control_status.flags.mag_hdg) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // the angle of the projection onto the horizontal gives the yaw angle + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else if (_control_status.flags.ev_yaw) { + // convert the observed quaternion to a rotation matrix + matrix::Dcm R_to_earth(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + measured_hdg = atan2f(R_to_earth(1, 0) , R_to_earth(0, 0)); + } else { + // there is no yaw observation + return; + } } else { // calculate observaton jacobian when we are observing a rotation in a 312 sequence @@ -491,8 +505,21 @@ void Ekf::fuseHeading() R_to_earth(2, 0) = -s2 * c1; R_to_earth(2, 1) = s1; - // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // calculate the observed yaw angle + if (_control_status.flags.mag_hdg) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // the angle of the projection onto the horizontal gives the yaw angle + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else if (_control_status.flags.ev_yaw) { + // convert the observed quaternion to a rotation matrix + matrix::Dcm R_to_earth(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + measured_hdg = atan2f(-R_to_earth(0, 1) , R_to_earth(1, 1)); + } 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 @@ -553,9 +580,6 @@ void Ekf::fuseHeading() } } - // Use the difference between the horizontal projection of the mag field and declination to give the measured heading - float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; - // wrap the heading to the interval between +-pi measured_hdg = matrix::wrap_pi(measured_hdg);