diff --git a/EKF/common.h b/EKF/common.h index 6e86ad8d4d..37269de871 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -230,6 +230,8 @@ struct parameters { float mag_innov_gate{3.0f}; // magnetometer fusion innovation consistency gate size (STD) int mag_declination_source{7}; // bitmask used to control the handling of declination data int 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/s**2) + float mag_yaw_rate_gate{0.25f}; // yaw rate threshold used by mode select logic (rad/sec) // airspeed fusion float tas_innov_gate{5.0f}; // True Airspeed Innovation consistency gate size in standard deciation diff --git a/EKF/control.cpp b/EKF/control.cpp index db1dfcc5c9..a4399c9486 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -888,9 +888,10 @@ void Ekf::controlMagFusion() } // If we are on ground, store the local position and time to use as a reference + // Also reset the flight alignment falg 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); - + _flt_mag_align_complete = false; } // checs for new magnetometer data tath has fallen beind the fusion time horizon @@ -899,16 +900,66 @@ void Ekf::controlMagFusion() // 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) { - // start 3D fusion if in-flight and height has increased sufficiently - // to be away from ground magnetic anomalies - // don't switch back to heading fusion until we are back on the ground + // Check if height has increased sufficiently to be away from ground magnetic anomalies bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; - bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved); - if (use_3D_fusion && _control_status.flags.tilt_align) { - // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states + // Check if there has been enough change in horizontal velocity to make yaw observable + // Apply hysteresis to check to avoid rapid toggling + if (_yaw_angle_observable) { + _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate; + } else { + _yaw_angle_observable = _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate; + } + _yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos); + + // check if there is enough yaw rotation to make the mag bias states observable + if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) { + // initial yaw motion is detected + _mag_bias_observable = true; + _yaw_delta_ef = 0.0f; + _time_yaw_started = _imu_sample_delayed.time_us; + } else if (_mag_bias_observable) { + // monitor yaw rotation in 45 deg sections. + // a rotation of 45 deg is sufficient to make the mag bias observable + if (fabsf(_yaw_delta_ef) > 0.7854f) { + _time_yaw_started = _imu_sample_delayed.time_us; + _yaw_delta_ef = 0.0f; + } + // require sustained yaw motion of 50% the initial yaw rate threshold + float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * (1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started)); + _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; + } else { + _mag_bias_observable = false; + } + + // record the last time that movement was suitable for use of 3-axis magnetometer fusion + if (_mag_bias_observable || _yaw_angle_observable) { + _time_last_movement = _imu_sample_delayed.time_us; + } + + // decide whether 3-axis magnetomer fusion can be used + bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates + _control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies + (_flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height + ((_imu_sample_delayed.time_us - _time_last_movement) < 2000000); // Using 3-axis fusion for a minimum period after to allow for false negatives + + // perform switch-over + if (use_3D_fusion) { if (!_control_status.flags.mag_3D) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + 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; + } else { + // reset the mag field covariances + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + + // re-instate the last used variances + for (uint8_t index = 0; index <= 5; index ++) { + P[index+16][index+16] = _saved_mag_variance[index]; + } + } } // use 3D mag fusion when airborne @@ -916,9 +967,14 @@ void Ekf::controlMagFusion() _control_status.flags.mag_3D = true; } else { - // use heading fusion when on the ground + // save magnetic field state variances for next time + if (_control_status.flags.mag_3D) { + for (uint8_t index = 0; index <= 5; index ++) { + _saved_mag_variance[index] = P[index+16][index+16]; + } + _control_status.flags.mag_3D = false; + } _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; } } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { @@ -943,7 +999,7 @@ void Ekf::controlMagFusion() } // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift - // fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time + // fusing declination when gps aiding is available is optional, but recommended to prevent problem if the vehicle is static for extended periods of time if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) { _control_status.flags.mag_dec = true; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 42fe9265e2..d6c8f0b9fb 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -467,13 +467,6 @@ void Ekf::predictCovariance() // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion if (_control_status.flags.mag_3D) { - // Check if we have just transitioned into 3-axis fusion and set the state variances - if (!_control_status_prev.flags.mag_3D) { - for (uint8_t index = 16; index <= 21; index++) { - P[index][index] = sq(fmaxf(_params.mag_noise, 0.001f)); - } - } - // calculate variances and upper diagonal covariances for earth and body magnetic field states nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)/2; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b87eb1b9b8..65a6fb1b30 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -379,8 +379,17 @@ void Ekf::predictState() // update transformation matrix from body to world frame _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + // Calculate an earth frame delta velocity + Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; + + // calculate a filtered horizontal acceleration with a 1 sec time constant + // this are used for manoeuvre detection elsewhere + float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; + _accel_lpf_NE(0) = _accel_lpf_NE(0) * alpha + corrected_delta_vel_ef(0); + _accel_lpf_NE(1) = _accel_lpf_NE(1) * alpha + corrected_delta_vel_ef(1); + // calculate the increment in velocity using the current orientation - _state.vel += _R_to_earth * corrected_delta_vel; + _state.vel += corrected_delta_vel_ef; // compensate for acceleration due to gravity _state.vel(2) += _gravity_mss * _imu_sample_delayed.delta_vel_dt; @@ -479,6 +488,15 @@ void Ekf::calculateOutputStates() delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction; delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction; + // calculate a yaw change about the earth frame vertical + float spin_del_ang_D = _R_to_earth_now(2,0) * delta_angle(0) + + _R_to_earth_now(2,1) * delta_angle(1) + + _R_to_earth_now(2,2) * delta_angle(2); + _yaw_delta_ef += spin_del_ang_D; + + // calculate filtered yaw rate to be used by the magnetomer fusion type selection logic + _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / _imu_sample_new.delta_ang_dt; + // correct delta velocity for bias offsets Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction; diff --git a/EKF/ekf.h b/EKF/ekf.h index 432f12e193..e98ad52fe2 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -266,6 +266,13 @@ private: matrix::Dcm _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition + // used by magnetomer fusion mode selection + Vector2f _accel_lpf_NE; // Low pass filtered horizontal earth frame acceleration (m/s**2) + float _yaw_delta_ef{0.0f}; // Recent change in yaw angle measured about the earth frame D axis (rad) + float _yaw_rate_lpf_ef{0.0f}; // Filtered angular rate abut earth frame D axis (rad/sec) + bool _mag_bias_observable{false}; // true when there is enough rotation to make magnetomer bias errors observable + bool _yaw_angle_observable{false}; // true when there is enough horizontal acceleration to make yaw observable + float P[_k_num_states][_k_num_states] {}; // state covariance matrix float _vel_pos_innov[6] {}; // innovations: 0-2 vel, 3-5 pos @@ -328,7 +335,11 @@ private: float _baro_hgt_offset{0.0f}; // baro height reading at the local NED origin (m) // Variables used to control activation of post takeoff functionality - float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m) + float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m) + bool _flt_mag_align_complete{true}; // true when the in-flight mag field alignment has been completed + uint64_t _time_last_movement{0}; // last system time in usec that sufficient movement to use 3-axis magnetomer fusion was detected + float _saved_mag_variance[6] {}; // magnetic field state variances that have been saved for use at the next initialisation (Ga**2) + uint64_t _time_yaw_started{0}; // last system time in usec that a yaw rotation moaneouvre was detected gps_check_fail_status_u _gps_check_fail_status{}; @@ -343,7 +354,7 @@ private: float _terrain_var{1e4f}; // variance of terrain position estimate (m^2) float _hagl_innov{0.0f}; // innovation of the last height above terrain measurement (m) float _hagl_innov_var{0.0f}; // innovation variance for the last height above terrain measurement (m^2) - uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks + uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks bool _terrain_initialised{false}; // true when the terrain estimator has been intialised float _sin_tilt_rng{0.0f}; // sine of the range finder tilt rotation about the Y body axis float _cos_tilt_rng{0.0f}; // cosine of the range finder tilt rotation about the Y body axis diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 27c432a471..47122d2e81 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -501,7 +501,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) void Ekf::calcMagDeclination() { // set source of magnetic declination for internal use - if (_params.mag_declination_source & MASK_USE_GEO_DECL) { + if (_flt_mag_align_complete) { + // Use value consistent with earth field state + _mag_declination = atan2f(_state.mag_I(1),_state.mag_I(0)); + } else if (_params.mag_declination_source & MASK_USE_GEO_DECL) { // use parameter value until GPS is available, then use value returned by geo library if (_NED_origin_initialised) { _mag_declination = _mag_declination_gps;