From f064915889db3dd1e72043dd6f110b4907a2e4ba Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 2 Nov 2016 17:48:52 +1100 Subject: [PATCH] EKF: Enable planes to recover from bad mag data at start of flight Adjusts yaw by the amount of the error between GPS and EKF course if innovations are large. --- EKF/control.cpp | 17 +++-- EKF/ekf.h | 4 ++ EKF/ekf_helper.cpp | 152 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 168 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index d465962dfb..b340439886 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -946,7 +946,7 @@ void Ekf::controlBetaFusion() if (!_control_status.flags.wind) { // activate the wind states _control_status.flags.wind = true; - // reset the timout timers to prevent repeated resets + // reset the timeout timers to prevent repeated resets _time_last_beta_fuse = _time_last_imu; _time_last_arsp_fuse = _time_last_imu; // reset the wind speed states and corresponding covariances @@ -996,7 +996,7 @@ void Ekf::controlMagFusion() _flt_mag_align_complete = false; } - // checs for new magnetometer data tath has fallen beind the fusion time horizon + // check for new magnetometer data that has fallen behind the fusion time horizon if (_mag_data_ready) { // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances @@ -1049,9 +1049,16 @@ void Ekf::controlMagFusion() if (use_3D_fusion) { if (!_control_status.flags.mag_3D) { if (!_flt_mag_align_complete) { - // if transitioning into 3-axis fusion mode for the first time, we need to initialise the yaw angle and field states - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - _flt_mag_align_complete = true; + // If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading + // if we are doing wind estimation and using the zero sideslip assumotion, then it is reasonable to assume the plane is flying forward + bool zeroSideslipValid = _control_status.flags.fuse_beta; + if (zeroSideslipValid) { + _flt_mag_align_complete = _control_status.flags.yaw_align = realignYawGPS(); + + } else { + _flt_mag_align_complete = _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + + } } else { // reset the mag field covariances zeroRows(P, 16, 21); diff --git a/EKF/ekf.h b/EKF/ekf.h index 3ef6295597..adb44eee9e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -438,6 +438,10 @@ private: // return true if successful bool resetMagHeading(Vector3f &mag_init); + // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. + // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle. + bool realignYawGPS(); + // calculate the magnetic declination to be used by the alignment and fusion processing void calcMagDeclination(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 194dc0592d..8653cde4a6 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -39,6 +39,7 @@ * */ +#include "../ecl.h" #include "ekf.h" #include "mathlib.h" #include @@ -336,6 +337,138 @@ void Ekf::alignOutputFilter() } } +// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. +// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle. +bool Ekf::realignYawGPS() +{ + // Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15 for a reliable alignment + float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1))); + if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { + // calculate course yaw angle + float velYaw = atan2f(_state.vel(1),_state.vel(0)); + + // calculate course yaw angle from GPS velocity + float gpsYaw = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0)); + + // Check the EKF and GPS course for consistency + float courseYawError = gpsYaw - velYaw; + courseYawError = wrap_pi(courseYawError); + + // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad + bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps; + bool badYawErr = fabsf(courseYawError) > 0.5f; + bool badMagYaw = (badYawErr && badVelInnov) || !_control_status.flags.yaw_align; + + // correct yaw angle using GPS ground course if compass yaw bad + if (badMagYaw) { + // save a copy of the quaternion state for later use in calculating the amount of reset change + Quatf quat_before_reset = _state.quat_nominal; + + // 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 transformation matrix from body to world frame using the current state estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + // Use the best conditioned of a 321 or 312 Euler sequence to avoid gimbal lock + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { + // get quaternion from existing filter states and calculate roll, pitch and yaw angles + Eulerf euler321(_state.quat_nominal); + + // calculate new filter quaternion states using previous Roll/Pitch and yaw angle corrected + // for ground course discrepancy + euler321(2) += courseYawError; + _state.quat_nominal = Quatf(euler321); + + // update transformation matrix from body to world frame using the current state estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + } else { + // Calculate the 312 sequence euler angles that rotate from earth to body frame + // See 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 yaw angle for ground course discrepancy + euler312(0) += courseYawError; + + // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence + 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)); + + _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; + + _state.quat_nominal = Quatf(_R_to_earth); + } + + // reset the velocity and posiiton states as they will be inaccurate due to bad yaw + resetVelocity(); + resetPosition(); + + // Use the last magnetometer measurements to reset the field states + _state.mag_B.zero(); + _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; + + // use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment + float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P[4][4] + P[5][5]; + float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f); + angle_err_var_vec(2) = sq(asinf(sineYawError)); + + // reset the quaternion covariances using the rotation vector variances + initialiseQuatCovariances(angle_err_var_vec); + + // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + + for (uint8_t index = 16; index <= 21; index ++) { + P[index][index] = sq(_params.mag_noise); + } + + // calculate the amount that the quaternion has changed by + _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); + + // add the reset amount to the output observer buffered data + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal *= _state_reset_status.quat_change; + _output_buffer.push_to_index(i,output_states); + } + + // 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; + + // capture the reset event + _state_reset_status.quat_counter++; + + } + + return true; + + } else { + return false; + + } +} + // Reset heading and magnetic field states bool Ekf::resetMagHeading(Vector3f &mag_init) { @@ -383,6 +516,25 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // we don't change the output attitude to avoid jumps _state.quat_nominal = Quatf(euler321); + // calculate the amount that the quaternion has changed by + _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); + + // add the reset amount to the output observer buffered data + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal *= _state_reset_status.quat_change; + _output_buffer.push_to_index(i,output_states); + } + + // 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; + + // capture the reset event + _state_reset_status.quat_counter++; + } else { // use a 312 sequence