diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1c92bd8d24..65e39b42fc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -420,6 +420,8 @@ private: uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) + uint64_t _time_last_mag_heading_fuse{0}; + uint64_t _time_last_mag_3d_fuse{0}; uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source @@ -605,9 +607,9 @@ private: void predictCovariance(); // ekf sequential fusion of magnetometer measurements - void fuseMag(const Vector3f &mag); + bool fuseMag(const Vector3f &mag, bool update_all_states = true); - // update quaternion states and covariances using a yaw innovation and yaw observation variance + // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance bool fuseYaw(const float innovation, const float variance); @@ -621,7 +623,7 @@ private: // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians - void fuseDeclination(float decl_sigma); + bool fuseDeclination(float decl_sigma); // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); @@ -1035,7 +1037,7 @@ private: // yaw : Euler yaw angle (rad) // yaw_variance : yaw error variance (rad^2) // update_buffer : true if the state change should be also applied to the output observer buffer - void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer); + void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true); // Declarations used to control use of the EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 87facb0fe8..c829506d62 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -345,12 +345,19 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f); - fuseYaw(innovation, obs_var); + + if (fuseYaw(innovation, obs_var)) { + _time_last_mag_heading_fuse = _time_last_imu; + } } } void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) { + // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise + // before they are used to constrain heading drift + const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); + if (!_mag_decl_cov_reset) { // After any magnetic field covariance reset event the earth field state // covariances need to be corrected to incorporate knowledge of the declination @@ -358,13 +365,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) // states for the first few observations. fuseDeclination(0.02f); _mag_decl_cov_reset = true; - fuseMag(mag); + fuseMag(mag, update_all_states); } else { // The normal sequence is to fuse the magnetometer data first before fusing // declination angle at a higher uncertainty to allow some learning of // declination angle over time. - fuseMag(mag); + fuseMag(mag, update_all_states); if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 433f108e81..4dd1f39411 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -45,7 +45,7 @@ #include -void Ekf::fuseMag(const Vector3f &mag) +bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) { // assign intermediate variables const float q0 = _state.quat_nominal(0); @@ -96,7 +96,7 @@ void Ekf::fuseMag(const Vector3f &mag) // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); ECL_ERR("magX %s", numerical_error_covariance_reset_string); - return; + return false; } _fault_status.flags.bad_mag_x = false; @@ -138,7 +138,7 @@ void Ekf::fuseMag(const Vector3f &mag) // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); ECL_ERR("magY %s", numerical_error_covariance_reset_string); - return; + return false; } _fault_status.flags.bad_mag_y = false; @@ -150,7 +150,7 @@ void Ekf::fuseMag(const Vector3f &mag) // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); ECL_ERR("magZ %s", numerical_error_covariance_reset_string); - return; + return false; } _fault_status.flags.bad_mag_z = false; @@ -174,20 +174,24 @@ void Ekf::fuseMag(const Vector3f &mag) for (uint8_t index = 0; index <= 2; index++) { _mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index)); - const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f); + bool rejected = (_mag_test_ratio(index) > 1.f); - if (innov_check_fail) { - all_innovation_checks_passed = false; + switch (index) { + case 0: + _innov_check_fail_status.flags.reject_mag_x = rejected; + break; + + case 1: + _innov_check_fail_status.flags.reject_mag_y = rejected; + break; + + case 2: + _innov_check_fail_status.flags.reject_mag_z = rejected; + break; } - if (index == 0) { - _innov_check_fail_status.flags.reject_mag_x = innov_check_fail; - - } else if (index == 1) { - _innov_check_fail_status.flags.reject_mag_y = innov_check_fail; - - } else { - _innov_check_fail_status.flags.reject_mag_z = innov_check_fail; + if (rejected) { + all_innovation_checks_passed = false; } } @@ -196,13 +200,9 @@ void Ekf::fuseMag(const Vector3f &mag) // if any axis fails, abort the mag fusion if (!all_innovation_checks_passed) { - return; + return false; } - // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise - // before they are used to constrain heading drift - const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); - // Observation jacobian and Kalman gain vectors SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; Vector24f Kfusion; @@ -284,7 +284,7 @@ void Ekf::fuseMag(const Vector3f &mag) // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); ECL_ERR("magY %s", numerical_error_covariance_reset_string); - return; + return false; } const float HKY24 = 1.0F/_mag_innov_var(1); @@ -364,7 +364,7 @@ void Ekf::fuseMag(const Vector3f &mag) // we need to re-initialise covariances and abort this fusion step resetMagRelatedCovariances(); ECL_ERR("magZ %s", numerical_error_covariance_reset_string); - return; + return false; } const float HKZ24 = 1.0F/_mag_innov_var(2); @@ -427,6 +427,13 @@ void Ekf::fuseMag(const Vector3f &mag) limitDeclination(); } } + + if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) { + _time_last_mag_3d_fuse = _time_last_imu; + return true; + } + + return false; } // update quaternion states and covariances using the yaw innovation and yaw observation variance @@ -678,7 +685,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance) return false; } -void Ekf::fuseDeclination(float decl_sigma) +bool Ekf::fuseDeclination(float decl_sigma) { // assign intermediate state variables const float magN = _state.mag_I(0); @@ -693,7 +700,7 @@ void Ekf::fuseDeclination(float decl_sigma) // Calculate intermediate variables if (fabsf(magN) < sq(N_field_min)) { // calculation is badly conditioned close to +-90 deg declination - return; + return false; } const float HK0 = ecl::powf(magN, -2); @@ -712,7 +719,7 @@ void Ekf::fuseDeclination(float decl_sigma) HK9 = HK4/innovation_variance; } else { // variance calculation is badly conditioned - return; + return false; } // Calculate the observation Jacobian @@ -745,6 +752,8 @@ void Ekf::fuseDeclination(float decl_sigma) if (is_fused) { limitDeclination(); } + + return is_fused; } void Ekf::limitDeclination()