From dedecce39f38ebfb04939f14fd3a4c449232d7c8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Mar 2022 14:11:37 -0400 Subject: [PATCH] ekf2: add mag fusion timestamps --- src/modules/ekf2/EKF/ekf.h | 12 ++++-- src/modules/ekf2/EKF/mag_control.cpp | 12 ++++-- src/modules/ekf2/EKF/mag_fusion.cpp | 63 ++++++++++++++++++---------- 3 files changed, 58 insertions(+), 29 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 03e807f661..0343810a6a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -386,6 +386,8 @@ private: uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) + uint64_t _time_last_heading_fuse{0}; + uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) @@ -394,6 +396,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 @@ -581,10 +585,10 @@ private: void predictCovariance(); // ekf sequential fusion of magnetometer measurements - void fuseMag(const Vector3f &mag); + bool fuseMag(const Vector3f &mag, bool update_all_states = true); // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) - void fuseHeading(float measured_hdg = NAN, float obs_var = NAN); + bool fuseHeading(float measured_hdg = NAN, float obs_var = NAN); // 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) @@ -615,7 +619,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(); @@ -1025,7 +1029,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 6b631e3deb..6b74b940a9 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -356,12 +356,18 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) // the angle of the projection onto the horizontal gives the yaw angle float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - fuseHeading(measured_hdg, sq(_params.mag_heading_noise)); + if (fuseHeading(measured_hdg, sq(_params.mag_heading_noise))) { + _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 @@ -369,13 +375,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 5922d3211e..3c11b817cf 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,12 +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)); - if (_mag_test_ratio(index) > 1.0f) { - all_innovation_checks_passed = false; - _innov_check_fail_status.value |= (1 << (index + 3)); + bool rejected = (_mag_test_ratio(index) > 1.f); - } else { - _innov_check_fail_status.value &= ~(1 << (index + 3)); + 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 (rejected) { + all_innovation_checks_passed = false; } } @@ -188,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; @@ -276,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); @@ -356,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); @@ -419,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; } bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) @@ -715,13 +730,15 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f // apply the state corrections fuse(Kfusion, _heading_innov); + _time_last_heading_fuse = _time_last_imu; + return true; } return false; } -void Ekf::fuseHeading(float measured_hdg, float obs_var) +bool Ekf::fuseHeading(float measured_hdg, float obs_var) { // observation variance float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f; @@ -769,14 +786,14 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var) } if (shouldUse321RotationSequence(_R_to_earth)) { - fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); + return fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); } else { - fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov); + return fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov); } } -void Ekf::fuseDeclination(float decl_sigma) +bool Ekf::fuseDeclination(float decl_sigma) { // assign intermediate state variables const float &magN = _state.mag_I(0); @@ -791,7 +808,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); @@ -810,7 +827,7 @@ void Ekf::fuseDeclination(float decl_sigma) HK9 = HK4/innovation_variance; } else { // variance calculation is badly conditioned - return; + return false; } // Calculate the observation Jacobian @@ -843,6 +860,8 @@ void Ekf::fuseDeclination(float decl_sigma) if (is_fused) { limitDeclination(); } + + return is_fused; } void Ekf::limitDeclination()