From 91f886cb5e65aac6505c27bc768a92bf0a25e91d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 28 Aug 2018 07:22:48 +1000 Subject: [PATCH] EKF: Add support for use of GPS heading data. Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned. --- EKF/CMakeLists.txt | 1 + EKF/common.h | 8 +- EKF/control.cpp | 38 ++++ EKF/ekf.h | 7 + EKF/ekf_helper.cpp | 9 + EKF/estimator_interface.cpp | 3 + EKF/estimator_interface.h | 1 + EKF/gps_yaw_fusion.cpp | 433 ++++++++++++++++++++++++++++++++++++ 8 files changed, 499 insertions(+), 1 deletion(-) create mode 100644 EKF/gps_yaw_fusion.cpp diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index e56a158c7e..da1b71096e 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -45,6 +45,7 @@ add_library(ecl_EKF sideslip_fusion.cpp terrain_estimator.cpp vel_pos_fusion.cpp + gps_yaw_fusion.cpp ) add_dependencies(ecl_EKF prebuild_targets) diff --git a/EKF/common.h b/EKF/common.h index 54badc3f10..973975854c 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -59,7 +59,9 @@ struct gps_message { int32_t lat; ///< Latitude in 1E-7 degrees int32_t lon; ///< Longitude in 1E-7 degrees int32_t alt; ///< Altitude in 1E-3 meters (millimeters) above MSL - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time + float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI]) + float yaw_offset; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic float eph; ///< GPS horizontal position accuracy in m float epv; ///< GPS vertical position accuracy in m float sacc; ///< GPS speed accuracy in m/s @@ -111,6 +113,7 @@ struct gpsSample { Vector2f pos; ///< NE earth frame gps horizontal position measurement (m) float hgt; ///< gps height measurement (m) Vector3f vel; ///< NED earth frame gps velocity measurement (m/sec) + float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI]) float hacc; ///< 1-std horizontal position error (m) float vacc; ///< 1-std vertical position error (m) float sacc; ///< 1-std speed error (m/sec) @@ -185,6 +188,7 @@ struct auxVelSample { #define MASK_USE_EVYAW (1<<4) ///< set to true to use exernal vision quaternion data for yaw #define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind #define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used +#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic @@ -192,6 +196,7 @@ struct auxVelSample { #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 MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. #define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetomer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates. +#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetomer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter. // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) @@ -442,6 +447,7 @@ union filter_control_status_u { uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough + uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 700e118a28..e18713a647 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -511,6 +511,35 @@ void Ekf::controlGpsFusion() // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { + // GPS yaw aiding selection logic + if ((_params.fusion_mode & MASK_USE_GPSYAW) + && isfinite(_gps_sample_delayed.yaw) + && _control_status.flags.tilt_align + && (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align) + && (_time_last_imu - _time_last_gps < 2 * GPS_MAX_INTERVAL)) { + + if (resetGpsAntYaw()) { + // flag the yaw as aligned + _control_status.flags.yaw_align = true; + + // turn on fusion of external vision yaw measurements and disable all other yaw fusion + _control_status.flags.gps_yaw = true; + _control_status.flags.ev_yaw = false; + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = false; + _control_status.flags.mag_dec = false; + + ECL_INFO("EKF commencing GPS yaw fusion"); + // flag the yaw as aligned + _control_status.flags.yaw_align = true; + } + } + + // fuse the yaw observation + if (_control_status.flags.gps_yaw) { + fuseGpsAntYaw(); + } + // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6); @@ -1310,6 +1339,15 @@ void Ekf::controlDragFusion() void Ekf::controlMagFusion() { + if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) { + // do not use the magnetomer and deactivate magnetic field states + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = false; + return; + } + // If we are on ground, store the local position and time to use as a reference // Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude if (!_control_status.flags.in_air) { diff --git a/EKF/ekf.h b/EKF/ekf.h index b23ca6c732..e9c9fdf835 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -481,6 +481,13 @@ private: // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) void fuseHeading(); + // fuse the yaw angle obtained from a dual antenna GPS unit + void fuseGpsAntYaw(); + + // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit + // return true if the reset was successful + bool resetGpsAntYaw(); + // fuse magnetometer declination measurement void fuseDeclination(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f0f62bcbc9..98636196c6 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -545,6 +545,15 @@ bool Ekf::realignYawGPS() // Reset heading and magnetic field states bool Ekf::resetMagHeading(Vector3f &mag_init) { + if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) { + // do not use the magnetomer and deactivate magnetic field states + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = false; + return false; + } + // save a copy of the quaternion state for later use in calculating the amount of reset change Quatf quat_before_reset = _state.quat_nominal; Quatf quat_after_reset = _state.quat_nominal; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 46e8d4fa83..8b1b0d2800 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -220,6 +220,9 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) gps_sample_new.hgt = (float)gps->alt * 1e-3f; + gps_sample_new.yaw = gps->yaw; + _gps_yaw_offset = gps->yaw_offset; + // Only calculate the relative position if the WGS-84 location of the origin is set if (collect_gps(time_usec, gps)) { float lpos_x = 0.0f; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 9f7e60874e..81b08a0153 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -470,6 +470,7 @@ protected: struct map_projection_reference_s _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin struct map_projection_reference_s _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) + float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). // innovation consistency check monitoring ratios float _yaw_test_ratio{0.0f}; // yaw innovation consistency check ratio diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp new file mode 100644 index 0000000000..61f72db433 --- /dev/null +++ b/EKF/gps_yaw_fusion.cpp @@ -0,0 +1,433 @@ +/**************************************************************************** + * + * Copyright (c) 2018 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps_yaw_fusion.cpp + * Definition of functions required to use yaw obtained from GPS dual antenna measurements. + * + * @author Paul Riseborough + * + */ + +#include "ekf.h" + +#include +#include +#include + +void Ekf::fuseGpsAntYaw() +{ + // assign intermediate state variables + float q0 = _state.quat_nominal(0); + float q1 = _state.quat_nominal(1); + float q2 = _state.quat_nominal(2); + float q3 = _state.quat_nominal(3); + + float R_YAW = 1.0f; + float predicted_hdg; + float H_YAW[4]; + float measured_hdg; + + // check if data has been set to NAN indicating no measurement + if (isfinite(_gps_sample_delayed.yaw)) { + // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement + measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset; + + // define the predicted antenna array vector and rotate into earth frame + Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; + Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + + // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + return; + } + + // calculate predicted antenna yaw angle + predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0)); + + // calculate observation jacobian + float t2 = sin(_gps_yaw_offset); + float t3 = cos(_gps_yaw_offset); + float t4 = q0*q3*2.0f; + float t5 = q0*q0; + float t6 = q1*q1; + float t7 = q2*q2; + float t8 = q3*q3; + float t9 = q1*q2*2.0f; + float t10 = t5+t6-t7-t8; + float t11 = t3*t10; + float t12 = t4+t9; + float t13 = t3*t12; + float t14 = t5-t6+t7-t8; + float t15 = t2*t14; + float t16 = t13+t15; + float t17 = t4-t9; + float t19 = t2*t17; + float t20 = t11-t19; + float t18 = (t20*t20); + if (t18 < 1e-6f) { + return; + } + t18 = 1.0f / t18; + float t21 = t16*t16; + float t22 = sq(t11-t19); + if (t22 < 1e-6f) { + return; + } + t22 = 1.0f/t22; + float t23 = q1*t3*2.0f; + float t24 = q2*t2*2.0f; + float t25 = t23+t24; + float t26 = 1.0f/t20; + float t27 = q1*t2*2.0f; + float t28 = t21*t22; + float t29 = t28+1.0f; + if (fabsf(t29) < 1e-6f) { + return; + } + float t30 = 1.0f/t29; + float t31 = q0*t3*2.0f; + float t32 = t31-q3*t2*2.0f; + float t33 = q3*t3*2.0f; + float t34 = q0*t2*2.0f; + float t35 = t33+t34; + + H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f); + H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25); + H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f)); + H_YAW[3] = t30*(t26*t32+t16*t22*t35); + + // using magnetic heading tuning parameter + R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + + } else { + // there is nothing to fuse + return; + } + + // wrap the heading to the interval between +-pi + measured_hdg = wrap_pi(measured_hdg); + + // calculate the innovation and define the innovaton gate + float innov_gate = math::max(_params.heading_innov_gate, 1.0f); + _heading_innov = predicted_hdg - measured_hdg; + + // wrap the innovation to the interval between +-pi + _heading_innov = wrap_pi(_heading_innov); + + // 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; + + for (uint8_t col = 0; col <= 3; col++) { + PH[row] += P[row][col] * H_YAW[col]; + } + + _heading_innov_var += H_YAW[row] * PH[row]; + } + + float heading_innov_var_inv; + + // check if the innovation variance calculation is badly conditioned + if (_heading_innov_var >= R_YAW) { + // the innovation variance contribution from the state covariances is not negative, no fault + _fault_status.flags.bad_mag_hdg = false; + heading_innov_var_inv = 1.0f / _heading_innov_var; + + } else { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.flags.bad_mag_hdg = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + ECL_ERR("EKF GPS yaw fusion numerical error - covariance reset"); + return; + } + + // calculate the Kalman gains + // only calculate gains for states we are using + float Kfusion[_k_num_states] = {}; + + for (uint8_t row = 0; row <= 15; row++) { + Kfusion[row] = 0.0f; + + for (uint8_t col = 0; col <= 3; col++) { + Kfusion[row] += P[row][col] * H_YAW[col]; + } + + Kfusion[row] *= heading_innov_var_inv; + } + + if (_control_status.flags.wind) { + for (uint8_t row = 22; row <= 23; row++) { + Kfusion[row] = 0.0f; + + for (uint8_t col = 0; col <= 3; col++) { + Kfusion[row] += P[row][col] * H_YAW[col]; + } + + Kfusion[row] *= heading_innov_var_inv; + } + } + + // innovation test ratio + _yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var); + + // we are no longer using 3-axis fusion so set the reported test levels to zero + memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); + + // set the magnetometer unhealthy if the test fails + if (_yaw_test_ratio > 1.0f) { + _innov_check_fail_status.flags.reject_yaw = true; + + // if we are in air we don't want to fuse the measurement + // we allow to use it when on the ground because the large innovation could be caused + // by interference or a large initial gyro bias + if (_control_status.flags.in_air) { + return; + + } else { + // constrain the innovation to the maximum set by the gate + float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var)); + _heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit); + } + + } else { + _innov_check_fail_status.flags.reject_yaw = false; + } + + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + float KHP[_k_num_states][_k_num_states]; + float KH[4]; + + for (unsigned row = 0; row < _k_num_states; row++) { + + KH[0] = Kfusion[row] * H_YAW[0]; + KH[1] = Kfusion[row] * H_YAW[1]; + KH[2] = Kfusion[row] * H_YAW[2]; + KH[3] = Kfusion[row] * H_YAW[3]; + + for (unsigned column = 0; column < _k_num_states; column++) { + float tmp = KH[0] * P[0][column]; + tmp += KH[1] * P[1][column]; + tmp += KH[2] * P[2][column]; + tmp += KH[3] * P[3][column]; + KHP[row][column] = tmp; + } + } + + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_mag_hdg = false; + + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P, i, i); + zeroCols(P, i, i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + _fault_status.flags.bad_mag_hdg = true; + + } + } + + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, _heading_innov); + + } +} + +bool Ekf::resetGpsAntYaw() +{ + // check if data has been set to NAN indicating no measurement + if (isfinite(_gps_sample_delayed.yaw)) { + + // define the predicted antenna array vector and rotate into earth frame + Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; + Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + + // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + return false; + } + + float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0)); + + // get measurement and correct for antenna array yaw offset + float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset; + + // caclulate the amount the yaw needs to be rotated by + float yaw_delta = wrap_pi(measured_yaw - predicted_yaw); + + // save a copy of the quaternion state for later use in calculating the amount of reset change + Quatf quat_before_reset = _state.quat_nominal; + Quatf quat_after_reset = _state.quat_nominal; + + // obtain the yaw angle using the best conditioned from either a Tait-Bryan 321 or 312 sequence + // to avoid gimbal lock + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { + // get the roll, pitch, yaw estimates from the quaternion states using a 321 Tait-Bryan rotation sequence + Quatf q_init(_state.quat_nominal); + Eulerf euler_init(q_init); + + // correct the yaw angle + euler_init(2) += yaw_delta; + euler_init(2) = wrap_pi(euler_init(2)); + + // update the quaternions + quat_after_reset = Quatf(euler_init); + + } else { + // Calculate the 312 Tait-Bryan sequence euler angles that rotate from earth to body frame + // PX4 math library does not support this so are using equations from + // http://www.atacolorado.com/eulersequences.doc + Vector3f euler312; + euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw) + euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll) + euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch) + + // correct the yaw angle + euler312(0) += yaw_delta; + euler312(0) = wrap_pi(euler312(0)); + + // Calculate the body to earth frame rotation matrix from the corrected euler angles + float c2 = cosf(euler312(2)); + float s2 = sinf(euler312(2)); + float s1 = sinf(euler312(1)); + float c1 = cosf(euler312(1)); + float s0 = sinf(euler312(0)); + float c0 = cosf(euler312(0)); + + Dcmf R_to_earth; + R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; + R_to_earth(1, 1) = c0 * c1; + R_to_earth(2, 2) = c2 * c1; + R_to_earth(0, 1) = -c1 * s0; + R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0; + R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0; + R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2; + R_to_earth(2, 0) = -s2 * c1; + R_to_earth(2, 1) = s1; + + // update the quaternions + quat_after_reset = Quatf(R_to_earth); + } + + // calculate the amount that the quaternion has changed by + Quatf q_error = quat_before_reset.inversed() * _state.quat_nominal; + q_error.normalize(); + + // convert the quaternion delta to a delta angle + Vector3f delta_ang_error; + float scalar; + + if (q_error(0) >= 0.0f) { + scalar = -2.0f; + + } else { + scalar = 2.0f; + } + + delta_ang_error(0) = scalar * q_error(1); + delta_ang_error(1) = scalar * q_error(2); + delta_ang_error(2) = scalar * q_error(3); + + // calculate the variance for the rotation estimate expressed as a rotation vector + // this will be used later to reset the quaternion state covariances + Vector3f angle_err_var_vec = calcRotVecVariances(); + + // update the quaternion state estimates and corresponding covariances only if the change in angle has been large or the yaw is not yet aligned + if (delta_ang_error.norm() > math::radians(15.0f) || !_control_status.flags.yaw_align) { + // update quaternion states + _state.quat_nominal = quat_after_reset; + + // record the state change + _state_reset_status.quat_change = q_error; + + // update transformation matrix from body to world frame using the current estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + // reset the rotation from the EV to EKF frame of reference if it is being used + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)) { + resetExtVisRotMat(); + } + + // update the yaw angle variance using the variance of the measurement + angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + + // reset the quaternion covariances using the rotation vector variances + initialiseQuatCovariances(angle_err_var_vec); + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + // Note q1 *= q2 is equivalent to q1 = q2 * q1 + _output_buffer[i].quat_nominal *= _state_reset_status.quat_change; + } + + // apply the change in attitude quaternion to our newest quaternion estimate + // which was already taken out from the output buffer + _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + + // capture the reset event + _state_reset_status.quat_counter++; + + } + + return true; + } + + return false; +}