From 89b5c77d5d8a565fc0935d2fb041ae4592f83cc8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 30 Mar 2020 20:05:38 +1100 Subject: [PATCH] EKF: Enable GPS flight without magnetometer (#770) * EKF: Enable GPS flight without magnetometer Enables takeoff in a non-GPS flight mode with mag fusion type set to MAG_FUSE_TYPE_NONE. After sufficient movement the EKF will reset the yaw tot he EKF-GSF estimate. After that GPS fusion will commence. * EKF: Fix unconstrained yaw and yaw variance growth when on ground * EKF: Ensure first yaw alignment can't be blocked * EKF: Increase yaw variance limit allowed for alignment Flight test data indicates that an uncertainty value of 15 deg still provides a reliable yaw estimate and enables an earlier alignment/reset if required. * EKF: Remove unexecutable code * EKF: Restructure heading fusion * EKF: parameterise quarter variance check and retune default value * EKF: Pass by reference instead of pointer * EKF: Clarify reset logic * EKF: Remove incorrect setting of mag field alignment flag * EKF: Non-functional tidy up * EKF: Fix non-use of function argument The updateQuaternion function was using the _heading_innovation class variable instead of setting it using the innovation argument. * EKF: Fix undefined variable * EKF: Use single precision atan2 * EKF: remove unnecessary timer reset and unwanted execution of reset function * EKF: Don't declare a mag fault when non-use is user selected Doing so produces unnecessary user alerts. --- EKF/common.h | 5 +- EKF/control.cpp | 26 +- EKF/ekf.h | 33 ++- EKF/ekf_helper.cpp | 39 ++- EKF/gps_checks.cpp | 8 +- EKF/mag_control.cpp | 35 ++- EKF/mag_fusion.cpp | 586 +++++++++++++++++++++----------------- EKF/terrain_estimator.cpp | 5 + 8 files changed, 442 insertions(+), 295 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index a83b0f33a0..ceb538cdff 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -177,7 +177,7 @@ struct auxVelSample { #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 -#define MASK_USE_EVVEL (1<<8) ///< sset to true to use external vision velocity data +#define MASK_USE_EVVEL (1<<8) ///< set to true to use external vision velocity data enum TerrainFusionMask : int32_t { TerrainFuseRangeFinder = (1 << 0), @@ -265,6 +265,7 @@ struct parameters { int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) float mag_yaw_rate_gate{0.25f}; ///< yaw rate threshold used by mode select logic (rad/sec) + float quat_max_variance{0.0001f}; ///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this // airspeed fusion float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) @@ -365,7 +366,7 @@ struct parameters { // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter inpost takeoff phase before yaw is reset to EKF-GSF value - float EKFGSF_yaw_err_max{0.2f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) + float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) }; struct stateSample { diff --git a/EKF/control.cpp b/EKF/control.cpp index 134e17014e..a93f6af46d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -644,17 +644,27 @@ void Ekf::controlGpsFusion() ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use"); } + // handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before + // we can start using GPS + bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000); + if (align_yaw_using_gsf) { + if (resetYawToEKFGSF()) { + _ekfgsf_yaw_reset_time = _time_last_imu; + _do_ekfgsf_yaw_reset = false; + } + } + // handle the case when we now have GPS, but have not been using it for an extended period if (_control_status.flags.gps) { // We are relying on aiding to constrain drift so after a specified time // with no aiding we need to do something - bool do_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); // We haven't had an absolute position fix for a longer time so need to do something - do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); + do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); // A reset to the EKF-GSF estimate can be performed after a recent takeoff which will enable // recovery from a bad magnetometer or field estimate. @@ -667,14 +677,14 @@ void Ekf::controlGpsFusion() _time_last_on_ground_us = _time_last_imu; } const bool recent_takeoff = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000); - const bool reset_yaw_to_EKFGSF = (do_reset || _do_emergency_yaw_reset || stopped_following_gps_velocity) && + const bool do_yaw_vel_pos_reset = (do_vel_pos_reset || _do_ekfgsf_yaw_reset || stopped_following_gps_velocity) && recent_takeoff && - isTimedOut(_emergency_yaw_reset_time, 5000000); + isTimedOut(_ekfgsf_yaw_reset_time, 5000000); - if (reset_yaw_to_EKFGSF) { + if (do_yaw_vel_pos_reset) { if (resetYawToEKFGSF()) { - _emergency_yaw_reset_time = _time_last_imu; - _do_emergency_yaw_reset = false; + _ekfgsf_yaw_reset_time = _time_last_imu; + _do_ekfgsf_yaw_reset = false; // Reset the timeout counters _time_last_hor_pos_fuse = _time_last_imu; @@ -683,7 +693,7 @@ void Ekf::controlGpsFusion() _time_last_of_fuse = _time_last_imu; } - } else if (do_reset) { + } else if (do_vel_pos_reset) { // use GPS velocity data to check and correct yaw angle if a FW vehicle if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { // if flying a fixed wing aircraft, do a complete reset that includes yaw diff --git a/EKF/ekf.h b/EKF/ekf.h index f8eddd4be4..9b81e26c6e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -373,14 +373,14 @@ private: uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited - bool _mag_use_inhibit{false}; ///< true when magnetometer use is being inhibited - bool _mag_use_inhibit_prev{false}; ///< true when magnetometer use was being inhibited the previous frame bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve. float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements + bool _yaw_use_inhibit{false}; ///< true when yaw sensor use is being inhibited + matrix::SquareMatrix P; ///< state covariance matrix Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance @@ -540,6 +540,28 @@ 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 defined as the first rotation in a 321 Tait-Bryan rotation sequence + // yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad) + // yaw_variance : variance of the yaw angle observation (rad^2) + // zero_innovation : Fuse data with innovation set to zero + void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation); + + // fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence + // yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad) + // yaw_variance : variance of the yaw angle observation (rad^2) + // zero_innovation : Fuse data with innovation set to zero + void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation); + + // update quaternion states and covariances using an innovation, observation variance and Jacobian vector + // innovation : prediction - measurement + // variance : observaton variance + // gate_sigma : innovation consistency check gate size (Sigma) + // jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state + void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const float (&yaw_jacobian)[4]); + + // shrinks the yaw axis uncertainty of quaternion covariances by fusing a zero innovation yaw observation + void shrinkYawVariance(); + // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsAntYaw(); @@ -849,14 +871,15 @@ private: // Declarations used to control use of the EKF-GSF yaw estimator - int64_t _emergency_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) + int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec) - bool _do_emergency_yaw_reset{false}; // true when an emergency yaw reset has been requested + bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested // Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed void runYawEKFGSF(); - // Resets the main Nav EKf yaw to the esitmator from the EKF-GSF yaw estimator + // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator + // Resets the horizontal velocity and position to the default navigation sensor // Returns true if the reset was successful bool resetYawToEKFGSF(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 2a408a63e4..ec8c7b3162 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -544,8 +544,8 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); } - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { - // we are operating without knowing the earth frame yaw angle + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _yaw_use_inhibit) { + // we are operating temporarily without knowing the earth frame yaw angle return true; } else { @@ -1723,7 +1723,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) _state_reset_status.quat_counter++; } -// Reset main nav filter yaw to value from EKF-GSF and reset velocity and position to GPS +// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator +// Resets the horizontal velocity and position to the default navigation sensor +// Returns true if the reset was successful bool Ekf::resetYawToEKFGSF() { // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity @@ -1737,14 +1739,18 @@ bool Ekf::resetYawToEKFGSF() resetVelocity(); resetPosition(); - // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around - // and cause another navigation failure - _control_status.flags.mag_fault = true; - // record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _control_status.flags.yaw_align = true; - ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped"); + if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { + ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS"); + } else { + // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around + // and cause another navigation failure + _control_status.flags.mag_fault = true; + ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped"); + } return true; } @@ -1755,7 +1761,7 @@ bool Ekf::resetYawToEKFGSF() void Ekf::requestEmergencyNavReset() { - _do_emergency_yaw_reset = true; + _do_ekfgsf_yaw_reset = true; } bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) @@ -1803,3 +1809,18 @@ void Ekf::runYawEKFGSF() yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); } } + +void Ekf::shrinkYawVariance() +{ + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { + // rolled more than pitched so use 321 rotation order to define yaw angle + // and fuse a zero innovation yaw to shrink quaternion yaw variance + fuseYaw321(0.0f, 0.25f, true); + + } else { + // pitched more than rolled so use 312 rotation order to define yaw angle + // and fuse a zero innovation yaw to shrink quaternion yaw variance + fuseYaw312(0.0f, 0.25f, true); + + } +} diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index fe4db910f0..3bce478922 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -85,7 +85,13 @@ bool Ekf::collect_gps(const gps_message &gps) _mag_strength_gps = 0.01f * get_mag_strength(lat, lon); // request a reset of the yaw using the new declination - _mag_yaw_reset_req = true; + if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { + // try to reset the yaw using the EKF-GSF yaw esitimator + _do_ekfgsf_yaw_reset = true; + _ekfgsf_yaw_reset_time = 0; + } else { + _mag_yaw_reset_req = true; + } // save the horizontal and vertical position uncertainty of the origin _gps_origin_eph = gps.eph; _gps_origin_epv = gps.epv; diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index 5963fcd15f..b74c9eb8d2 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -41,20 +41,31 @@ void Ekf::controlMagFusion() { + // handle undefined behaviour + if (_params.mag_fusion_type > MAG_FUSE_TYPE_NONE) { + return; + } + + // When operating without a magnetometer, yaw fusion is run selectively to prevent + // enable yaw gyro bias learning hen stationary on ground and to prevent uncontrolled + // yaw variance growth + if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { + _yaw_use_inhibit = true; + fuseHeading(); + return; + } + updateMagFilter(); checkMagFieldStrength(); - // 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 we are on ground, 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) { - _last_on_ground_posD = _state.pos(2); _control_status.flags.mag_aligned_in_flight = false; _num_bad_flight_yaw_events = 0; } - if ((_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) - || _control_status.flags.mag_fault - || !_control_status.flags.yaw_align) { + if (_control_status.flags.mag_fault || !_control_status.flags.yaw_align) { stopMagFusion(); return; } @@ -143,12 +154,12 @@ void Ekf::runOnGroundYawReset() bool Ekf::isYawResetAuthorized() const { - return !_mag_use_inhibit; + return !_yaw_use_inhibit; } bool Ekf::canResetMagHeading() const { - return !isStrongMagneticDisturbance(); + return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE); } void Ekf::runInAirYawReset() @@ -255,8 +266,8 @@ void Ekf::checkMagDeclRequired() void Ekf::checkMagInhibition() { - _mag_use_inhibit = shouldInhibitMag(); - if (!_mag_use_inhibit) { + _yaw_use_inhibit = shouldInhibitMag(); + if (!_yaw_use_inhibit) { _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; } @@ -271,7 +282,8 @@ bool Ekf::shouldInhibitMag() const // If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer // if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding // is available, assume that we are operating indoors and the magnetometer should not be used. - // Also inhibit mag fusion when a strong magnetic field interference is detected + // Also inhibit mag fusion when a strong magnetic field interference is detected or the user + // has explicitly stopped magnetometer use. const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR); const bool heading_not_required_for_navigation = !_control_status.flags.gps @@ -324,7 +336,6 @@ void Ekf::runMagAndMagDeclFusions() { if (_control_status.flags.mag_3D) { run3DMagAndDeclFusions(); - } else if (_control_status.flags.mag_hdg) { fuseHeading(); } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 6206278232..861fe04039 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -435,7 +435,7 @@ void Ekf::fuseMag() } } -void Ekf::fuseHeading() +void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) { // assign intermediate state variables const float q0 = _state.quat_nominal(0); @@ -443,296 +443,206 @@ void Ekf::fuseHeading() const float q2 = _state.quat_nominal(2); const float q3 = _state.quat_nominal(3); - float R_YAW = 1.0f; - float predicted_hdg; + float R_YAW = fmaxf(yaw_variance, 1.0e-4f); + float measurement = wrap_pi(yaw); float H_YAW[4]; - 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))) { - // calculate observation jacobian when we are observing the first rotation in a 321 sequence - float t9 = q0*q3; - float t10 = q1*q2; - float t2 = t9+t10; - float t3 = q0*q0; - float t4 = q1*q1; - float t5 = q2*q2; - float t6 = q3*q3; - float t7 = t3+t4-t5-t6; + // calculate observation jacobian when we are observing the first rotation in a 321 sequence + float t9 = q0*q3; + float t10 = q1*q2; + float t2 = t9+t10; + float t3 = q0*q0; + float t4 = q1*q1; + float t5 = q2*q2; + float t6 = q3*q3; + float t7 = t3+t4-t5-t6; - float t16 = q3*t3; - float t17 = q3*t5; - float t18 = q0*q1*q2*2.0f; - float t19 = t16+t17+t18-q3*t4+q3*t6; + float t16 = q3*t3; + float t17 = q3*t5; + float t18 = q0*q1*q2*2.0f; + float t19 = t16+t17+t18-q3*t4+q3*t6; - float t24 = q2*t4; - float t25 = q2*t6; - float t26 = q0*q1*q3*2.0f; - float t27 = t24+t25+t26-q2*t3+q2*t5; - float t28 = q1*t3; - float t29 = q1*t5; - float t30 = q0*q2*q3*2.0f; - float t31 = t28+t29+t30+q1*t4-q1*t6; - float t32 = q0*t4; - float t33 = q0*t6; - float t34 = q1*q2*q3*2.0f; - float t35 = t32+t33+t34+q0*t3-q0*t5; + float t24 = q2*t4; + float t25 = q2*t6; + float t26 = q0*q1*q3*2.0f; + float t27 = t24+t25+t26-q2*t3+q2*t5; + float t28 = q1*t3; + float t29 = q1*t5; + float t30 = q0*q2*q3*2.0f; + float t31 = t28+t29+t30+q1*t4-q1*t6; + float t32 = q0*t4; + float t33 = q0*t6; + float t34 = q1*q2*q3*2.0f; + float t35 = t32+t33+t34+q0*t3-q0*t5; - // two computational paths are provided to work around singularities in calculation of the Jacobians - float t8 = t7*t7; - float t15 = t2*t2; - if (t8 > t15 && t8 > 1E-6f) { - // this path has a singularities at yaw = +-90 degrees - t8 = 1.0f/t8; - float t11 = t2*t2; - float t12 = t8*t11*4.0f; - float t13 = t12+1.0f; - float t14 = 1.0f/t13; + // two computational paths are provided to work around singularities in calculation of the Jacobians + float t8 = t7*t7; + float t15 = t2*t2; + if (t8 > t15 && t8 > 1E-6f) { + // this path has a singularities at yaw = +-90 degrees + t8 = 1.0f/t8; + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; + float t14 = 1.0f/t13; - H_YAW[0] = t8*t14*t19*(-2.0f); - H_YAW[1] = t8*t14*t27*(-2.0f); - H_YAW[2] = t8*t14*t31*2.0f; - H_YAW[3] = t8*t14*t35*2.0f; + H_YAW[0] = t8*t14*t19*(-2.0f); + H_YAW[1] = t8*t14*t27*(-2.0f); + H_YAW[2] = t8*t14*t31*2.0f; + H_YAW[3] = t8*t14*t35*2.0f; - } else if (t15 > 1E-6f) { - // this path has singularities at yaw = 0 and +-180 deg - t15 = 1.0f/t15; - float t20 = t7*t7; - float t21 = t15*t20*0.25f; - float t22 = t21+1.0f; + } else if (t15 > 1E-6f) { + // this path has singularities at yaw = 0 and +-180 deg + t15 = 1.0f/t15; + float t20 = t7*t7; + float t21 = t15*t20*0.25f; + float t22 = t21+1.0f; - if (fabsf(t22) > 1E-6f) { - float t23 = 1.0f/t22; + if (fabsf(t22) > 1E-6f) { + float t23 = 1.0f/t22; - H_YAW[0] = t15*t19*t23*(-0.5f); - H_YAW[1] = t15*t23*t27*(-0.5f); - H_YAW[2] = t15*t23*t31*0.5f; - H_YAW[3] = t15*t23*t35*0.5f; + H_YAW[0] = t15*t19*t23*(-0.5f); + H_YAW[1] = t15*t23*t27*(-0.5f); + H_YAW[2] = t15*t23*t31*0.5f; + H_YAW[3] = t15*t23*t35*0.5f; - } else { - return; - - } } else { return; } + } else { + return; - // rotate the magnetometer measurement into earth frame + } + + // calculate the yaw innovation and wrap to the interval between +-pi + float innovation; + if (zero_innovation) { + innovation = 0.0f; + } else { Eulerf euler321(_state.quat_nominal); - predicted_hdg = euler321(2); // we will need the predicted heading to calculate the innovation - - // calculate the observed yaw angle - if (_control_status.flags.mag_hdg) { - // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle - euler321(2) = 0.0f; - const Dcmf R_to_earth(euler321); - - // rotate the magnetometer measurements into earth frame using a zero yaw angle - if (_control_status.flags.mag_3D) { - // don't apply bias corrections if we are learning them - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - - } else { - mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); - } - - // the angle of the projection onto the horizontal gives the yaw angle - measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - - } else if (_control_status.flags.ev_yaw) { - // calculate the yaw angle for a 321 sequence - // Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m - const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); - const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); - measured_hdg = atan2f(Tbn_1_0,Tbn_0_0); - - } else if (_mag_use_inhibit) { - // Special case where we either use the current or last known stationary value - // so set to current value as a safe default - measured_hdg = predicted_hdg; - - } else { - // Should not be doing yaw fusion - return; - - } - - } else { - // calculate observation jacobian when we are observing a rotation in a 312 sequence - float t9 = q0*q3; - float t10 = q1*q2; - float t2 = t9-t10; - float t3 = q0*q0; - float t4 = q1*q1; - float t5 = q2*q2; - float t6 = q3*q3; - float t7 = t3-t4+t5-t6; - - float t16 = q3*t3; - float t17 = q3*t4; - float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f; - float t23 = q2*t3; - float t24 = q2*t4; - float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f; - float t26 = q1*t5; - float t27 = q1*t6; - float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f; - float t29 = q0*t5; - float t30 = q0*t6; - float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f; - - // two computational paths are provided to work around singularities in calculation of the Jacobians - float t8 = t7*t7; - float t15 = t2*t2; - if (t8 > t15 && t8 > 1E-6f) { - // this path has a singularities at yaw = +-90 degrees - t8 = 1.0f/t8; - float t11 = t2*t2; - float t12 = t8*t11*4.0f; - float t13 = t12+1.0f; - float t14 = 1.0f/t13; - - H_YAW[0] = t8*t14*t18*(-2.0f); - H_YAW[1] = t8*t14*t25*(-2.0f); - H_YAW[2] = t8*t14*t28*2.0f; - H_YAW[3] = t8*t14*t31*2.0f; - - } else if (t15 > 1E-6f) { - // this path has singularities at yaw = 0 and +-180 deg - t15 = 1.0f/t15; - float t19 = t7*t7; - float t20 = t15*t19*0.25f; - float t21 = t20+1.0f; - - if (fabsf(t21) > 1E-6f) { - float t22 = 1.0f/t21; - - H_YAW[0] = t15*t18*t22*(-0.5f); - H_YAW[1] = t15*t22*t25*(-0.5f); - H_YAW[2] = t15*t22*t28*0.5f; - H_YAW[3] = t15*t22*t31*0.5f; - - } else { - return; - - } - - } else { - return; - - } - - // Use a Tait-Bryan 312 rotation sequence - Vector3f rotVec312; - predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); - rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame - rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll) - rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch) - - // calculate the observed yaw angle - if (_control_status.flags.mag_hdg) { - - // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence - // with yaw angle set to to zero - - const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312); - // rotate the magnetometer measurements into earth frame using a zero yaw angle - if (_control_status.flags.mag_3D) { - // don't apply bias corrections if we are learning them - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - } else { - mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); - } - - // the angle of the projection onto the horizontal gives the yaw angle - measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - - } else if (_control_status.flags.ev_yaw) { - // calculate the yaw angle for a 312 sequence - // Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m - float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); - float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); - measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1); - - } else if (_mag_use_inhibit) { - // Special case where we either use the current or last known stationary value - // so set to current value as a safe default - measured_hdg = predicted_hdg; - - } else { - // Should not be doing yaw fusion - return; - - } + innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement); } - // 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 = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); - - } else { - // default value - R_YAW = 0.01f; - } - - // wrap the heading to the interval between +-pi - measured_hdg = wrap_pi(measured_hdg); - - // calculate the innovation and define the innovation gate + // define the innovation gate size float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - if (_mag_use_inhibit) { - // The magnetometer cannot be trusted but we need to fuse a heading to prevent a badly - // conditioned covariance matrix developing over time. - if (!_control_status.flags.vehicle_at_rest) { - // Vehicle is not at rest so fuse a zero innovation and record the - // predicted heading to use as an observation when movement ceases. - _heading_innov = 0.0f; - _last_static_yaw = predicted_hdg; + + // Update the quaternion states and covariance matrix + updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); +} + +void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) +{ + // assign intermediate state variables + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); + + float R_YAW = fmaxf(yaw_variance, 1.0e-4f); + float measurement = wrap_pi(yaw); + float H_YAW[4]; + + // calculate observation jacobian when we are observing a rotation in a 312 sequence + float t9 = q0*q3; + float t10 = q1*q2; + float t2 = t9-t10; + float t3 = q0*q0; + float t4 = q1*q1; + float t5 = q2*q2; + float t6 = q3*q3; + float t7 = t3-t4+t5-t6; + + float t16 = q3*t3; + float t17 = q3*t4; + float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f; + float t23 = q2*t3; + float t24 = q2*t4; + float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f; + float t26 = q1*t5; + float t27 = q1*t6; + float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f; + float t29 = q0*t5; + float t30 = q0*t6; + float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f; + + // two computational paths are provided to work around singularities in calculation of the Jacobians + float t8 = t7*t7; + float t15 = t2*t2; + if (t8 > t15 && t8 > 1E-6f) { + // this path has a singularities at yaw = +-90 degrees + t8 = 1.0f/t8; + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; + float t14 = 1.0f/t13; + + H_YAW[0] = t8*t14*t18*(-2.0f); + H_YAW[1] = t8*t14*t25*(-2.0f); + H_YAW[2] = t8*t14*t28*2.0f; + H_YAW[3] = t8*t14*t31*2.0f; + + } else if (t15 > 1E-6f) { + // this path has singularities at yaw = 0 and +-180 deg + t15 = 1.0f/t15; + float t19 = t7*t7; + float t20 = t15*t19*0.25f; + float t21 = t20+1.0f; + + if (fabsf(t21) > 1E-6f) { + float t22 = 1.0f/t21; + + H_YAW[0] = t15*t18*t22*(-0.5f); + H_YAW[1] = t15*t22*t25*(-0.5f); + H_YAW[2] = t15*t22*t28*0.5f; + H_YAW[3] = t15*t22*t31*0.5f; } else { - // Vehicle is at rest so use the last moving prediction as an observation - // to prevent the heading from drifting and to enable yaw gyro bias learning - // before takeoff. - _heading_innov = predicted_hdg - _last_static_yaw; - innov_gate = 5.0f; + return; } + } else { - _heading_innov = predicted_hdg - measured_hdg; - _last_static_yaw = predicted_hdg; + return; } - _mag_use_inhibit_prev = _mag_use_inhibit; - // wrap the innovation to the interval between +-pi - _heading_innov = wrap_pi(_heading_innov); + float innovation; + if (zero_innovation) { + innovation = 0.0f; + } else { + // calculate the the innovation and wrap to the interval between +-pi + innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement); + } + // define the innovation gate size + float innov_gate = math::max(_params.heading_innov_gate, 1.0f); + + // Update the quaternion states and covariance matrix + updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); +} + +// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian +void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const float (&yaw_jacobian)[4]) +{ // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero // calculate the innovation variance float PH[4]; - _heading_innov_var = R_YAW; - + _heading_innov_var = variance; 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]; + PH[row] += P(row,col) * yaw_jacobian[col]; } - _heading_innov_var += H_YAW[row] * PH[row]; + _heading_innov_var += yaw_jacobian[row] * PH[row]; } float heading_innov_var_inv; // check if the innovation variance calculation is badly conditioned - if (_heading_innov_var >= R_YAW) { + if (_heading_innov_var >= variance) { // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.flags.bad_hdg = false; heading_innov_var_inv = 1.0f / _heading_innov_var; @@ -755,7 +665,7 @@ void Ekf::fuseHeading() Kfusion[row] = 0.0f; for (uint8_t col = 0; col <= 3; col++) { - Kfusion[row] += P(row,col) * H_YAW[col]; + Kfusion[row] += P(row,col) * yaw_jacobian[col]; } Kfusion[row] *= heading_innov_var_inv; @@ -766,7 +676,7 @@ void Ekf::fuseHeading() Kfusion[row] = 0.0f; for (uint8_t col = 0; col <= 3; col++) { - Kfusion[row] += P(row,col) * H_YAW[col]; + Kfusion[row] += P(row,col) * yaw_jacobian[col]; } Kfusion[row] *= heading_innov_var_inv; @@ -774,7 +684,7 @@ void Ekf::fuseHeading() } // innovation test ratio - _yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var); + _yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _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)); @@ -791,12 +701,13 @@ void Ekf::fuseHeading() } 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); + float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var)); + _heading_innov = math::constrain(innovation, -gate_limit, gate_limit); } } else { _innov_check_fail_status.flags.reject_yaw = false; + _heading_innov = innovation; } // apply covariance correction via P_new = (I -K*H)*P @@ -807,10 +718,10 @@ void Ekf::fuseHeading() 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]; + KH[0] = Kfusion[row] * yaw_jacobian[0]; + KH[1] = Kfusion[row] * yaw_jacobian[1]; + KH[2] = Kfusion[row] * yaw_jacobian[2]; + KH[3] = Kfusion[row] * yaw_jacobian[3]; for (unsigned column = 0; column < _k_num_states; column++) { float tmp = KH[0] * P(0,column); @@ -858,6 +769,165 @@ void Ekf::fuseHeading() } } +void Ekf::fuseHeading() +{ + + Vector3f mag_earth_pred; + float measured_hdg; + float predicted_hdg; + + // Calculate the observation variance + float R_YAW; + if (_control_status.flags.mag_hdg) { + // using magnetic heading tuning parameter + R_YAW = sq(_params.mag_heading_noise); + + } else if (_control_status.flags.ev_yaw) { + // using error estimate from external vision data + R_YAW = _ev_sample_delayed.angVar; + + } else { + // default value + R_YAW = 0.01f; + } + + // update transformation matrix from body to world frame using the current state estimate + _R_to_earth = Dcmf(_state.quat_nominal); + + // determine if a 321 or 312 Euler sequence is best + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { + // rolled more than pitched so use 321 rotation order to calculate the observed yaw angle + Eulerf euler321(_state.quat_nominal); + predicted_hdg = euler321(2); + if (_control_status.flags.mag_hdg) { + // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle + euler321(2) = 0.0f; + const Dcmf R_to_earth(euler321); + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } + + // the angle of the projection onto the horizontal gives the yaw angle + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + + } else if (_control_status.flags.ev_yaw) { + // calculate the yaw angle for a 321 sequence + // Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m + const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); + const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); + measured_hdg = atan2f(Tbn_1_0,Tbn_0_0); + + } else { + measured_hdg = predicted_hdg; + } + + // handle special case where yaw measurement is unavailable + bool fuse_zero_innov = false; + if (_yaw_use_inhibit) { + // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly + // conditioned covariance matrix developing over time. + if (!_control_status.flags.vehicle_at_rest) { + // Vehicle is not at rest so fuse a zero innovation if necessary to prevent + // unconstrained quaterniion variance growth and record the predicted heading + // to use as an observation when movement ceases. + // TODO a better way of determining when this is necessary + float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3); + if (sumQuatVar > _params.quat_max_variance) { + fuse_zero_innov = true; + R_YAW = 0.25f; + + } + _last_static_yaw = predicted_hdg; + + } else { + // Vehicle is at rest so use the last moving prediction as an observation + // to prevent the heading from drifting and to enable yaw gyro bias learning + // before takeoff. + measured_hdg = _last_static_yaw; + + } + } else { + _last_static_yaw = predicted_hdg; + + } + + fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); + + } else { + + // pitched more than rolled so use 312 rotation order to calculate the observed yaw angle + predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); + if (_control_status.flags.mag_hdg) { + + // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence + // with yaw angle set to to zero + Vector3f rotVec312; + rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame + rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll) + rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch) + const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312); + + // rotate the magnetometer measurements into earth frame using a zero yaw angle + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } + + // the angle of the projection onto the horizontal gives the yaw angle + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + + } else if (_control_status.flags.ev_yaw) { + // calculate the yaw angle for a 312 sequence + // Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m + float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); + float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); + measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1); + + } else { + measured_hdg = predicted_hdg; + } + + // handle special case where yaw measurement is unavailable + bool fuse_zero_innov = false; + if (_yaw_use_inhibit) { + // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly + // conditioned covariance matrix developing over time. + if (!_control_status.flags.vehicle_at_rest) { + // Vehicle is not at rest so fuse a zero innovation if necessary to prevent + // unconstrained quaterniion variance growth and record the predicted heading + // to use as an observation when movement ceases. + // TODO a better way of determining when this is necessary + float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3); + if (sumQuatVar > _params.quat_max_variance) { + fuse_zero_innov = true; + R_YAW = 0.25f; + + } + _last_static_yaw = predicted_hdg; + + } else { + // Vehicle is at rest so use the last moving prediction as an observation + // to prevent the heading from drifting and to enable yaw gyro bias learning + // before takeoff. + measured_hdg = _last_static_yaw; + + } + } else { + _last_static_yaw = predicted_hdg; + + } + + fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov); + + } +} + void Ekf::fuseDeclination(float decl_sigma) { // assign intermediate state variables diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 976543f431..d0c3494be3 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -88,6 +88,11 @@ bool Ekf::initHagl() void Ekf::runTerrainEstimator() { + // If we are on ground, store the local position and time to use as a reference + if (!_control_status.flags.in_air) { + _last_on_ground_posD = _state.pos(2); + } + // Perform initialisation check and // on ground, continuously reset the terrain estimator if (!_terrain_initialised || !_control_status.flags.in_air) {