From 845b01a00d960e7b359ede77ddee95ef36e10cea Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 31 Aug 2023 12:46:01 -0400 Subject: [PATCH] ekf2: add kconfig for magnetometer support (enabled by default) --- src/modules/ekf2/CMakeLists.txt | 14 +- src/modules/ekf2/EKF/CMakeLists.txt | 14 +- src/modules/ekf2/EKF/control.cpp | 2 + src/modules/ekf2/EKF/covariance.cpp | 20 ++- src/modules/ekf2/EKF/ekf.cpp | 5 + src/modules/ekf2/EKF/ekf.h | 91 ++++++++---- src/modules/ekf2/EKF/ekf_helper.cpp | 27 +--- src/modules/ekf2/EKF/estimator_interface.cpp | 6 + src/modules/ekf2/EKF/estimator_interface.h | 10 +- src/modules/ekf2/EKF/ev_yaw_control.cpp | 1 - src/modules/ekf2/EKF/mag_3d_control.cpp | 19 +++ src/modules/ekf2/EKF/mag_fusion.cpp | 108 -------------- src/modules/ekf2/EKF/yaw_fusion.cpp | 147 +++++++++++++++++++ src/modules/ekf2/EKF2.cpp | 73 +++++++-- src/modules/ekf2/EKF2.hpp | 92 ++++++------ src/modules/ekf2/Kconfig | 7 + 16 files changed, 404 insertions(+), 232 deletions(-) create mode 100644 src/modules/ekf2/EKF/yaw_fusion.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 0f1c095890..64cd3c3bf6 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -84,12 +84,9 @@ list(APPEND EKF_SRCS EKF/gravity_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp - EKF/mag_3d_control.cpp - EKF/mag_control.cpp - EKF/mag_fusion.cpp - EKF/mag_heading_control.cpp EKF/output_predictor.cpp EKF/vel_pos_fusion.cpp + EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp EKF/zero_gyro_update.cpp EKF/zero_velocity_update.cpp @@ -121,6 +118,15 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_MAGNETOMETER) + list(APPEND EKF_SRCS + EKF/mag_3d_control.cpp + EKF/mag_control.cpp + EKF/mag_fusion.cpp + EKF/mag_heading_control.cpp + ) +endif() + if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS EKF/optical_flow_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 64230774af..99da3e6df4 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -49,12 +49,9 @@ list(APPEND EKF_SRCS gravity_fusion.cpp height_control.cpp imu_down_sampler.cpp - mag_3d_control.cpp - mag_control.cpp - mag_fusion.cpp - mag_heading_control.cpp output_predictor.cpp vel_pos_fusion.cpp + yaw_fusion.cpp zero_innovation_heading_update.cpp zero_gyro_update.cpp zero_velocity_update.cpp @@ -86,6 +83,15 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_MAGNETOMETER) + list(APPEND EKF_SRCS + mag_3d_control.cpp + mag_control.cpp + mag_fusion.cpp + mag_heading_control.cpp + ) +endif() + if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS optical_flow_control.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c35563135c..ad2e1ff2ba 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -102,8 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) } } +#if defined(CONFIG_EKF2_MAGNETOMETER) // control use of observations for aiding controlMagFusion(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_OPTICAL_FLOW) controlOpticalFlowFusion(imu_delayed); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 81fb7563f6..80a9b81f7b 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -178,23 +178,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned - float mag_I_sig; + float mag_I_sig = 0.0f; if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) { +#if defined(CONFIG_EKF2_MAGNETOMETER) mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); - - } else { - mag_I_sig = 0.0f; +#endif // CONFIG_EKF2_MAGNETOMETER } // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned - float mag_B_sig; + float mag_B_sig = 0.0f; if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) { +#if defined(CONFIG_EKF2_MAGNETOMETER) mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); - - } else { - mag_B_sig = 0.0f; +#endif // CONFIG_EKF2_MAGNETOMETER } float wind_vel_nsd_scaled; @@ -552,6 +550,7 @@ void Ekf::resetQuatCov(const float yaw_noise) void Ekf::resetMagCov() { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_mag_decl_cov_reset) { ECL_INFO("reset mag covariance"); _mag_decl_cov_reset = false; @@ -561,6 +560,11 @@ void Ekf::resetMagCov() P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); saveMagCovData(); +#else + P.uncorrelateCovarianceSetVariance<3>(16, 0.f); + P.uncorrelateCovarianceSetVariance<3>(19, 0.f); + +#endif } void Ekf::resetGyroBiasZCov() diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 7010626d89..0b6d5485ce 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -110,7 +110,10 @@ void Ekf::reset() _gps_alt_ref = NAN; _baro_counter = 0; + +#if defined(CONFIG_EKF2_MAGNETOMETER) _mag_counter = 0; +#endif // CONFIG_EKF2_MAGNETOMETER _time_bad_vert_accel = 0; _time_good_vert_accel = 0; @@ -142,8 +145,10 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_gnss_yaw); #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_MAGNETOMETER) resetEstimatorAidStatus(_aid_src_mag_heading); resetEstimatorAidStatus(_aid_src_mag); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AUXVEL) resetEstimatorAidStatus(_aid_src_aux_vel); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a1047b97e4..666abf9256 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -158,6 +158,7 @@ public: void getHeadingInnov(float &heading_innov) const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { heading_innov = _aid_src_mag_heading.innovation; return; @@ -166,6 +167,7 @@ public: heading_innov = Vector3f(_aid_src_mag.innovation).max(); return; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -184,6 +186,7 @@ public: void getHeadingInnovVar(float &heading_innov_var) const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { heading_innov_var = _aid_src_mag_heading.innovation_variance; return; @@ -192,6 +195,7 @@ public: heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max(); return; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -210,6 +214,7 @@ public: void getHeadingInnovRatio(float &heading_innov_ratio) const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { heading_innov_ratio = _aid_src_mag_heading.test_ratio; return; @@ -218,6 +223,7 @@ public: heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); return; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -234,9 +240,11 @@ public: #endif // CONFIG_EKF2_EXTERNAL_VISION } +#if defined(CONFIG_EKF2_MAGNETOMETER) void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); } void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); } void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } @@ -351,12 +359,16 @@ public: bool isYawFinalAlignComplete() const { +#if defined(CONFIG_EKF2_MAGNETOMETER) const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); const bool is_mag_alignment_in_flight_complete = is_using_mag && _control_status.flags.mag_aligned_in_flight && ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6); return _control_status.flags.yaw_align && (is_mag_alignment_in_flight_complete || !is_using_mag); +#else + return _control_status.flags.yaw_align; +#endif } // gyro bias (states 10, 11, 12) @@ -369,6 +381,7 @@ public: Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2 float getAccelBiasLimit() const { return _params.acc_bias_lim; } +#if defined(CONFIG_EKF2_MAGNETOMETER) const Vector3f &getMagEarthField() const { return _state.mag_I; } // mag bias (states 19, 20, 21) @@ -382,6 +395,7 @@ public: return _saved_mag_bf_covmat.diag(); } float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss +#endif // CONFIG_EKF2_MAGNETOMETER bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; } @@ -497,8 +511,10 @@ public: const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; } #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_MAGNETOMETER) const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } const auto &aid_src_mag() const { return _aid_src_mag; } +#endif // CONFIG_EKF2_MAGNETOMETER const auto &aid_src_gravity() const { return _aid_src_gravity; } @@ -545,9 +561,6 @@ private: bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised - float _mag_heading_prev{}; ///< previous value of mag heading (rad) - float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad) - // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused @@ -574,19 +587,9 @@ private: Vector3f _zgup_delta_ang{}; float _zgup_delta_ang_dt{0.f}; - // used by magnetometer fusion mode selection Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**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 about earth frame D axis (rad/sec) - bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable - bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable - uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected - AlphaFilter _mag_heading_innov_lpf{0.1f}; - float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad) - 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. - uint8_t _nb_mag_heading_reset_available{0}; - uint8_t _nb_mag_3d_reset_available{0}; - uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time SquareMatrix24f P{}; ///< state covariance matrix @@ -668,9 +671,6 @@ private: uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source #endif // CONFIG_EKF2_GNSS_YAW - estimator_aid_source1d_s _aid_src_mag_heading{}; - estimator_aid_source3d_s _aid_src_mag{}; - estimator_aid_source3d_s _aid_src_gravity{}; #if defined(CONFIG_EKF2_AUXVEL) @@ -694,20 +694,40 @@ private: // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation - uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) // Variables used to perform in flight resets and switch between height sources - AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) +#if defined(CONFIG_EKF2_MAGNETOMETER) + float _mag_heading_prev{}; ///< previous value of mag heading (rad) + float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad) + + // used by magnetometer fusion mode selection + bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable + bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable + uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected + AlphaFilter _mag_heading_innov_lpf{0.1f}; + float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad) + 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. + uint8_t _nb_mag_heading_reset_available{0}; + uint8_t _nb_mag_3d_reset_available{0}; + uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time + + estimator_aid_source1d_s _aid_src_mag_heading{}; + estimator_aid_source3d_s _aid_src_mag{}; + + AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) + uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation + // Variables used to control activation of post takeoff functionality uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) uint64_t _time_last_mag_check_failing{0}; Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2) +#endif // CONFIG_EKF2_MAGNETOMETER gps_check_fail_status_u _gps_check_fail_status{}; @@ -744,9 +764,6 @@ private: // predict ekf covariance void predictCovariance(const imuSample &imu_delayed); - // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); - // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status); bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW); @@ -767,12 +784,17 @@ private: #endif // CONFIG_EKF2_GNSS_YAW void stopGpsYawFusion(); +#if defined(CONFIG_EKF2_MAGNETOMETER) + // ekf sequential fusion of magnetometer measurements + bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); + // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians bool fuseDeclination(float decl_sigma); // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) // control fusion of air data observations @@ -899,8 +921,10 @@ private: void fuseFlowForTerrain(estimator_aid_source2d_s &flow); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_MAGNETOMETER) // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); +#endif // CONFIG_EKF2_MAGNETOMETER void clearInhibitedStateKalmanGains(Vector24f &K) const { @@ -1017,6 +1041,7 @@ private: bool shouldResetGpsFusion() const; bool isYawFailure() const; +#if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations void controlMagFusion(); void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src); @@ -1035,6 +1060,19 @@ private: bool checkMagField(const Vector3f &mag); static bool isMeasuredMatchingExpected(float measured, float expected, float gate); + void stopMagHdgFusion(); + void stopMagFusion(); + + // load and save mag field state covariance data for re-use + void loadMagCovData(); + void saveMagCovData(); + + // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter + // sensor measurement + float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); + +#endif // CONFIG_EKF2_MAGNETOMETER + // control fusion of fake position observations to constrain drift void controlFakePosFusion(); @@ -1064,9 +1102,6 @@ private: void controlBaroHeightFusion(); void controlGnssHeightFusion(const gpsSample &gps_sample); - void stopMagHdgFusion(); - void stopMagFusion(); - void stopBaroHgtFusion(); void stopGpsHgtFusion(); @@ -1087,19 +1122,11 @@ private: // Argument is additional yaw variance in rad**2 void increaseQuatYawErrVariance(float yaw_variance); - // load and save mag field state covariance data for re-use - void loadMagCovData(); - void saveMagCovData(); - void resetGyroBiasZCov(); // uncorrelate quaternion states from other states void uncorrelateQuatFromOtherStates(); - // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter - // sensor measurement - float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); - bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index f30686a8f7..d8465622d2 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -234,7 +234,10 @@ void Ekf::constrainStates() _state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit); _state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f); +#if defined(CONFIG_EKF2_MAGNETOMETER) _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); +#endif // CONFIG_EKF2_MAGNETOMETER + _state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f); } @@ -671,6 +674,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f // return the largest magnetometer innovation test ratio mag = 0.f; +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio)); } @@ -678,6 +682,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f if (_control_status.flags.mag_3D) { mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max())); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -791,6 +796,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const bool mag_innov_good = true; +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { if (_aid_src_mag_heading.test_ratio < 1.f) { mag_innov_good = false; @@ -801,6 +807,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const mag_innov_good = false; } } +#endif // CONFIG_EKF2_MAGNETOMETER const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; @@ -945,26 +952,6 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance) P.slice<4, 4>(0, 0) += q_cov; } -void Ekf::saveMagCovData() -{ - // save the NED axis covariance sub-matrix - _saved_mag_ef_covmat = P.slice<3, 3>(16, 16); - - // save the XYZ body covariance sub-matrix - _saved_mag_bf_covmat = P.slice<3, 3>(19, 19); -} - -void Ekf::loadMagCovData() -{ - // re-instate the NED axis covariance sub-matrix - P.uncorrelateCovarianceSetVariance<3>(16, 0.f); - P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat; - - // re-instate the XYZ body axis covariance sub-matrix - P.uncorrelateCovarianceSetVariance<3>(19, 0.f); - P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat; -} - void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) { // save a copy of the quaternion state for later use in calculating the amount of reset change diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index ab63ae5f7a..42345d8b14 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -47,7 +47,9 @@ EstimatorInterface::~EstimatorInterface() { delete _gps_buffer; +#if defined(CONFIG_EKF2_MAGNETOMETER) delete _mag_buffer; +#endif // CONFIG_EKF2_MAGNETOMETER delete _baro_buffer; #if defined(CONFIG_EKF2_RANGE_FINDER) delete _range_buffer; @@ -102,6 +104,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) #endif // CONFIG_EKF2_DRAG_FUSION } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EstimatorInterface::setMagData(const magSample &mag_sample) { if (!_initialised) { @@ -137,6 +140,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_MAGNETOMETER void EstimatorInterface::setGpsData(const gpsMessage &gps) { @@ -707,9 +711,11 @@ void EstimatorInterface::print_status() printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); } +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_mag_buffer) { printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); } +#endif // CONFIG_EKF2_MAGNETOMETER if (_baro_buffer) { printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index d784e442e0..6005843840 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -86,7 +86,9 @@ public: void setIMUData(const imuSample &imu_sample); +#if defined(CONFIG_EKF2_MAGNETOMETER) void setMagData(const magSample &mag_sample); +#endif // CONFIG_EKF2_MAGNETOMETER void setGpsData(const gpsMessage &gps); @@ -231,6 +233,7 @@ public: Vector3f getPosition() const { return _output_predictor.getPosition(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } +#if defined(CONFIG_EKF2_MAGNETOMETER) // Get the value of magnetic declination in degrees to be saved for use at the next startup // Returns true when the declination can be saved // At the next startup, set param.mag_declination_deg to the value saved @@ -263,6 +266,7 @@ public: strength_gs = _mag_strength; strength_ref_gs = _mag_strength_gps; } +#endif // CONFIG_EKF2_MAGNETOMETER // get EKF mode status const filter_control_status_u &control_status() const { return _control_status; } @@ -412,7 +416,12 @@ protected: RingBuffer _imu_buffer{kBufferLengthDefault}; RingBuffer *_gps_buffer{nullptr}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) RingBuffer *_mag_buffer{nullptr}; + uint64_t _time_last_mag_buffer_push{0}; +#endif // CONFIG_EKF2_MAGNETOMETER + RingBuffer *_baro_buffer{nullptr}; #if defined(CONFIG_EKF2_AIRSPEED) @@ -429,7 +438,6 @@ protected: RingBuffer *_system_flag_buffer{nullptr}; uint64_t _time_last_gps_buffer_push{0}; - uint64_t _time_last_mag_buffer_push{0}; uint64_t _time_last_baro_buffer_push{0}; uint64_t _time_last_gnd_effect_on{0}; diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 4d0472802f..d55d7f814e 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -158,7 +158,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { // turn on fusion of external vision yaw measurements and disable all other heading fusion - stopMagFusion(); stopGpsYawFusion(); stopGpsFusion(); diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 4d0e53983f..5b8f483555 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -217,3 +217,22 @@ void Ekf::stopMagFusion() } } +void Ekf::saveMagCovData() +{ + // save the NED axis covariance sub-matrix + _saved_mag_ef_covmat = P.slice<3, 3>(16, 16); + + // save the XYZ body covariance sub-matrix + _saved_mag_bf_covmat = P.slice<3, 3>(19, 19); +} + +void Ekf::loadMagCovData() +{ + // re-instate the NED axis covariance sub-matrix + P.uncorrelateCovarianceSetVariance<3>(16, 0.f); + P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat; + + // re-instate the XYZ body axis covariance sub-matrix + P.uncorrelateCovarianceSetVariance<3>(19, 0.f); + P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat; +} diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 6a1d36dea8..c494328372 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -254,114 +254,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo return false; } -// update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) -{ - Vector24f H_YAW; - computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); - - return fuseYaw(aid_src_status, H_YAW); -} - -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW) -{ - // define the innovation gate size - float gate_sigma = math::max(_params.heading_innov_gate, 1.f); - - // innovation test ratio - setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); - - if (aid_src_status.fusion_enabled) { - - // check if the innovation variance calculation is badly conditioned - if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.flags.bad_hdg = false; - - } else { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_hdg = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - ECL_ERR("yaw fusion numerical error - covariance reset"); - - return false; - } - - // calculate the Kalman gains - // only calculate gains for states we are using - Vector24f Kfusion; - const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; - - for (uint8_t row = 0; row < _k_num_states; row++) { - for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * H_YAW(col); - } - - Kfusion(row) *= heading_innov_var_inv; - } - - // set the magnetometer unhealthy if the test fails - if (aid_src_status.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; - - // if we are in air we don't want to fuse the measurement - // we allow to use it when on the ground because the large innovation could be caused - // by interference or a large initial gyro bias - if (!_control_status.flags.in_air - && isTimedOut(_time_last_in_air, (uint64_t)5e6) - && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) - ) { - // constrain the innovation to the maximum set by the gate - // we need to delay this forced fusion to avoid starting it - // immediately after touchdown, when the drone is still armed - float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); - aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); - - // also reset the yaw gyro variance to converge faster and avoid - // being stuck on a previous bad estimate - resetGyroBiasZCov(); - - } else { - return false; - } - - } else { - _innov_check_fail_status.flags.reject_yaw = false; - } - - if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { - - _time_last_heading_fuse = _time_delayed_us; - - aid_src_status.time_last_fuse = _time_delayed_us; - aid_src_status.fused = true; - - _fault_status.flags.bad_hdg = false; - - return true; - - } else { - _fault_status.flags.bad_hdg = true; - } - } - - // otherwise - aid_src_status.fused = false; - return false; -} - -void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const -{ - if (shouldUse321RotationSequence(_R_to_earth)) { - sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - - } else { - sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - } -} - bool Ekf::fuseDeclination(float decl_sigma) { if (!_control_status.flags.mag) { diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp new file mode 100644 index 0000000000..c50a35bf20 --- /dev/null +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ekf.h" + +#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" +#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" + +#include + +// update quaternion states and covariances using the yaw innovation and yaw observation variance +bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) +{ + Vector24f H_YAW; + computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); + + return fuseYaw(aid_src_status, H_YAW); +} + +bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW) +{ + // define the innovation gate size + float gate_sigma = math::max(_params.heading_innov_gate, 1.f); + + // innovation test ratio + setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); + + if (aid_src_status.fusion_enabled) { + + // check if the innovation variance calculation is badly conditioned + if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { + // the innovation variance contribution from the state covariances is not negative, no fault + _fault_status.flags.bad_hdg = false; + + } else { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.flags.bad_hdg = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + ECL_ERR("yaw fusion numerical error - covariance reset"); + + return false; + } + + // calculate the Kalman gains + // only calculate gains for states we are using + Vector24f Kfusion; + const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; + + for (uint8_t row = 0; row < _k_num_states; row++) { + for (uint8_t col = 0; col <= 3; col++) { + Kfusion(row) += P(row, col) * H_YAW(col); + } + + Kfusion(row) *= heading_innov_var_inv; + } + + // set the magnetometer unhealthy if the test fails + if (aid_src_status.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; + + // if we are in air we don't want to fuse the measurement + // we allow to use it when on the ground because the large innovation could be caused + // by interference or a large initial gyro bias + if (!_control_status.flags.in_air + && isTimedOut(_time_last_in_air, (uint64_t)5e6) + && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) + ) { + // constrain the innovation to the maximum set by the gate + // we need to delay this forced fusion to avoid starting it + // immediately after touchdown, when the drone is still armed + float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); + aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); + + // also reset the yaw gyro variance to converge faster and avoid + // being stuck on a previous bad estimate + resetGyroBiasZCov(); + + } else { + return false; + } + + } else { + _innov_check_fail_status.flags.reject_yaw = false; + } + + if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { + + _time_last_heading_fuse = _time_delayed_us; + + aid_src_status.time_last_fuse = _time_delayed_us; + aid_src_status.fused = true; + + _fault_status.flags.bad_hdg = false; + + return true; + + } else { + _fault_status.flags.bad_hdg = true; + } + } + + // otherwise + aid_src_status.fused = false; + return false; +} + +void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const +{ + if (shouldUse321RotationSequence(_R_to_earth)) { + sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + + } else { + sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + } +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d48a46a58d..9eb94dc9ec 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -60,7 +60,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), _param_ekf2_imu_ctrl(_params->imu_ctrl), - _param_ekf2_mag_delay(_params->mag_delay_ms), _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_gps_delay(_params->gps_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) @@ -70,8 +69,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_acc_noise(_params->accel_noise), _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise), _param_ekf2_acc_b_noise(_params->accel_bias_p_noise), - _param_ekf2_mag_e_noise(_params->mage_p_noise), - _param_ekf2_mag_b_noise(_params->magb_p_noise), _param_ekf2_wind_nsd(_params->wind_vel_nsd), _param_ekf2_gps_v_noise(_params->gps_vel_noise), _param_ekf2_gps_p_noise(_params->gps_pos_noise), @@ -93,6 +90,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_beta_noise(_params->beta_noise), _param_ekf2_fuse_beta(_params->beta_fusion_enabled), #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_MAGNETOMETER) + _param_ekf2_mag_delay(_params->mag_delay_ms), + _param_ekf2_mag_e_noise(_params->mage_p_noise), + _param_ekf2_mag_b_noise(_params->magb_p_noise), _param_ekf2_head_noise(_params->mag_heading_noise), _param_ekf2_mag_noise(_params->mag_noise), _param_ekf2_mag_decl(_params->mag_declination_deg), @@ -102,6 +103,11 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_type(_params->mag_fusion_type), _param_ekf2_mag_acclim(_params->mag_acc_gate), _param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate), + _param_ekf2_mag_check(_params->mag_check), + _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), + _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), + _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), +#endif // CONFIG_EKF2_MAGNETOMETER _param_ekf2_gps_check(_params->gps_check_mask), _param_ekf2_req_eph(_params->req_hacc), _param_ekf2_req_epv(_params->req_vacc), @@ -189,10 +195,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), _param_ekf2_pcoef_z(_params->static_pressure_coef_z), #endif // CONFIG_EKF2_BARO_COMPENSATION - _param_ekf2_mag_check(_params->mag_check), - _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), - _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), - _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) { // advertise expected minimal topic set immediately to ensure logging @@ -225,7 +227,9 @@ EKF2::~EKF2() #if defined(CONFIG_EKF2_AUXVEL) perf_free(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) perf_free(_msg_missed_magnetometer_perf); +#endif // CONFIG_EKF2_MAGNETOMETER perf_free(_msg_missed_odometry_perf); #if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_free(_msg_missed_optical_flow_perf); @@ -312,6 +316,7 @@ bool EKF2::multi_init(int imu, int mag) #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_MAGNETOMETER) // mag advertise if (_param_ekf2_mag_type.get() != MagFuseType::NONE) { @@ -319,13 +324,23 @@ bool EKF2::multi_init(int imu, int mag) _estimator_aid_src_mag_pub.advertise(); } +#endif // CONFIG_EKF2_MAGNETOMETER + _attitude_pub.advertise(); _local_position_pub.advertise(); _global_position_pub.advertise(); _odometry_pub.advertise(); _wind_pub.advertise(); - bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + + if (!_magnetometer_sub.ChangeInstance(mag)) { + changed_instance = false; + } + +#endif // CONFIG_EKF2_MAGNETOMETER const int status_instance = _estimator_states_pub.get_instance(); @@ -367,7 +382,9 @@ int EKF2::print_status() #if defined(CONFIG_EKF2_AUXVEL) perf_print_counter(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) perf_print_counter(_msg_missed_magnetometer_perf); +#endif // CONFIG_EKF2_MAGNETOMETER perf_print_counter(_msg_missed_odometry_perf); #if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_print_counter(_msg_missed_optical_flow_perf); @@ -435,6 +452,8 @@ void EKF2::Run() } } +#if defined(CONFIG_EKF2_MAGNETOMETER) + // if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output if (_params->mag_fusion_type != MagFuseType::NONE) { float sens_mag_rate = 0.f; @@ -450,6 +469,8 @@ void EKF2::Run() } } } + +#endif // CONFIG_EKF2_MAGNETOMETER } if (!_callback_registered) { @@ -678,7 +699,9 @@ void EKF2::Run() UpdateFlowSample(ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW UpdateGpsSample(ekf2_timestamps); +#if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagSample(ekf2_timestamps); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) UpdateRangeSample(ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER @@ -719,7 +742,9 @@ void EKF2::Run() UpdateAccelCalibration(now); UpdateGyroCalibration(now); +#if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagCalibration(now); +#endif // CONFIG_EKF2_MAGNETOMETER PublishSensorBias(now); PublishAidSourceStatus(now); @@ -918,6 +943,7 @@ void EKF2::VerifyParams() #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_MAGNETOMETER) // EKF2_MAG_TYPE obsolete options if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO) @@ -935,6 +961,8 @@ void EKF2::VerifyParams() _param_ekf2_mag_type.set(0); _param_ekf2_mag_type.commit(); } + +#endif // CONFIG_EKF2_MAGNETOMETER } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -975,11 +1003,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub); #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_MAGNETOMETER) // mag heading PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub); // mag 3d PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); +#endif // CONFIG_EKF2_MAGNETOMETER // gravity PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub); @@ -1291,7 +1321,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getTerrainFlowInnov(innovations.terr_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnov(innovations.heading); +#if defined(CONFIG_EKF2_MAGNETOMETER) _ekf.getMagInnov(innovations.mag_field); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnov(innovations.drag); #endif // CONFIG_EKF2_DRAG_FUSION @@ -1369,7 +1401,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovRatio(test_ratios.heading); +#if defined(CONFIG_EKF2_MAGNETOMETER) _ekf.getMagInnovRatio(test_ratios.mag_field[0]); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovRatio(&test_ratios.drag[0]); #endif // CONFIG_EKF2_DRAG_FUSION @@ -1412,7 +1446,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getTerrainFlowInnovVar(variances.terr_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovVar(variances.heading); +#if defined(CONFIG_EKF2_MAGNETOMETER) _ekf.getMagInnovVar(variances.mag_field); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovVar(variances.drag); #endif // CONFIG_EKF2_DRAG_FUSION @@ -1583,12 +1619,17 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) // estimator_sensor_bias const Vector3f gyro_bias{_ekf.getGyroBias()}; const Vector3f accel_bias{_ekf.getAccelBias()}; - const Vector3f mag_bias{_ekf.getMagBias()}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) + const Vector3f mag_bias {_ekf.getMagBias()}; +#endif // CONFIG_EKF2_MAGNETOMETER // publish at ~1 Hz, or sooner if there's a change if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f) || (accel_bias - _last_accel_bias_published).longerThan(0.001f) +#if defined(CONFIG_EKF2_MAGNETOMETER) || (mag_bias - _last_mag_bias_published).longerThan(0.001f) +#endif // CONFIG_EKF2_MAGNETOMETER || (timestamp >= _last_sensor_bias_published + 1_s)) { estimator_sensor_bias_s bias{}; @@ -1619,6 +1660,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) _last_accel_bias_published = accel_bias; } +#if defined(CONFIG_EKF2_MAGNETOMETER) + if (_device_id_mag != 0) { const Vector3f bias_var{_ekf.getMagBiasVariance()}; @@ -1631,6 +1674,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) _last_mag_bias_published = mag_bias; } +#endif // CONFIG_EKF2_MAGNETOMETER + bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); @@ -1695,10 +1740,13 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.accel_device_id = _device_id_accel; status.baro_device_id = _device_id_baro; status.gyro_device_id = _device_id_gyro; + +#if defined(CONFIG_EKF2_MAGNETOMETER) status.mag_device_id = _device_id_mag; _ekf.get_mag_checks(status.mag_inclination_deg, status.mag_inclination_ref_deg, status.mag_strength_gs, status.mag_strength_ref_gs); +#endif // CONFIG_EKF2_MAGNETOMETER status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_pub.publish(status); @@ -2313,6 +2361,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) } } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) { const unsigned last_generation = _magnetometer_sub.get_last_generation(); @@ -2356,6 +2405,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) (int64_t)ekf2_timestamps.timestamp / 100); } } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2556,6 +2606,7 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) bias_valid, learning_valid); } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { const Vector3f mag_bias = _ekf.getMagBias(); @@ -2585,6 +2636,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_MAGNETOMETER int EKF2::custom_command(int argc, char *argv[]) { @@ -2624,6 +2676,7 @@ int EKF2::task_spawn(int argc, char *argv[]) imu_instances = imu_instances_limited; } +#if defined(CONFIG_EKF2_MAGNETOMETER) int32_t sens_mag_mode = 1; const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE"); param_get(param_sens_mag_mode, &sens_mag_mode); @@ -2654,6 +2707,8 @@ int EKF2::task_spawn(int argc, char *argv[]) } else { mag_instances = 1; } + +#endif // CONFIG_EKF2_MAGNETOMETER } if (multi_mode && !replay_mode) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 63f7fff540..becc594471 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -85,7 +85,6 @@ #include #include #include -#include #include #include #include @@ -100,6 +99,10 @@ # include #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) +# include +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) # include # include @@ -212,7 +215,9 @@ private: bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); +#if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER @@ -230,7 +235,9 @@ private: const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid); void UpdateAccelCalibration(const hrt_abstime ×tamp); void UpdateGyroCalibration(const hrt_abstime ×tamp); +#if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagCalibration(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_MAGNETOMETER // publish helper for estimator_aid_source topics template @@ -271,15 +278,10 @@ private: perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; perf_counter_t _msg_missed_air_data_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; - perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; - // Used to control saving of mag declination to be used on next startup - bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved - InFlightCalibration _accel_cal{}; InFlightCalibration _gyro_cal{}; - InFlightCalibration _mag_cal{}; uint64_t _gps_time_usec{0}; int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid @@ -289,16 +291,13 @@ private: uint8_t _accel_calibration_count{0}; uint8_t _baro_calibration_count{0}; uint8_t _gyro_calibration_count{0}; - uint8_t _mag_calibration_count{0}; uint32_t _device_id_accel{0}; uint32_t _device_id_baro{0}; uint32_t _device_id_gyro{0}; - uint32_t _device_id_mag{0}; Vector3f _last_accel_bias_published{}; Vector3f _last_gyro_bias_published{}; - Vector3f _last_mag_bias_published{}; hrt_abstime _last_sensor_bias_published{0}; hrt_abstime _last_gps_status_published{0}; @@ -309,6 +308,27 @@ private: hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; +#if defined(CONFIG_EKF2_MAGNETOMETER) + uint32_t _device_id_mag {0}; + + perf_counter_t _msg_missed_magnetometer_perf {nullptr}; + + // Used to control saving of mag declination to be used on next startup + bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved + + InFlightCalibration _mag_cal{}; + uint8_t _mag_calibration_count{0}; + Vector3f _last_mag_bias_published{}; + + hrt_abstime _status_mag_pub_last{0}; + hrt_abstime _status_mag_heading_pub_last{0}; + + uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; + + uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; + uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_EXTERNAL_VISION) uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)}; uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; @@ -333,8 +353,6 @@ private: hrt_abstime _status_gnss_yaw_pub_last {0}; #endif // CONFIG_EKF2_GNSS_YAW - hrt_abstime _status_mag_pub_last{0}; - hrt_abstime _status_mag_heading_pub_last{0}; hrt_abstime _status_gravity_pub_last{0}; @@ -386,7 +404,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; @@ -448,9 +465,6 @@ private: uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; #endif // CONFIG_EKF2_GNSS_YAW - uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; - uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; - uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; // publications with topic dependent on multi-mode @@ -471,8 +485,6 @@ private: (ParamExtInt) _param_ekf2_predict_us, (ParamExtInt) _param_ekf2_imu_ctrl, - (ParamExtFloat) - _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) (ParamExtFloat) @@ -493,10 +505,6 @@ private: _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) (ParamExtFloat) _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) - (ParamExtFloat) - _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) (ParamExtFloat) _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) @@ -541,25 +549,24 @@ private: _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables #endif // CONFIG_EKF2_SIDESLIP - // control of magnetometer fusion - (ParamExtFloat) - _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) - (ParamExtFloat) - _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - - (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) - (ParamExtFloat) - _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) - (ParamExtInt) - _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data - (ParamExtInt) - _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) +#if defined(CONFIG_EKF2_MAGNETOMETER) + (ParamExtFloat) _param_ekf2_mag_delay, + (ParamExtFloat) _param_ekf2_mag_e_noise, + (ParamExtFloat) _param_ekf2_mag_b_noise, + (ParamExtFloat) _param_ekf2_head_noise, + (ParamExtFloat) _param_ekf2_mag_noise, + (ParamExtFloat) _param_ekf2_mag_decl, + (ParamExtFloat) _param_ekf2_hdg_gate, + (ParamExtFloat) _param_ekf2_mag_gate, + (ParamExtInt) _param_ekf2_decl_type, + (ParamExtInt) _param_ekf2_mag_type, + (ParamExtFloat) _param_ekf2_mag_acclim, + (ParamExtFloat) _param_ekf2_mag_yawlim, + (ParamExtInt) _param_ekf2_mag_check, + (ParamExtFloat) _param_ekf2_mag_chk_str, + (ParamExtFloat) _param_ekf2_mag_chk_inc, + (ParamExtInt) _param_ekf2_synthetic_mag_z, +#endif // CONFIG_EKF2_MAGNETOMETER (ParamExtInt) _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used @@ -736,11 +743,6 @@ private: #endif // CONFIG_EKF2_BARO_COMPENSATION (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time - (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check - (ParamExtFloat) _param_ekf2_mag_chk_str, - (ParamExtFloat) _param_ekf2_mag_chk_inc, - (ParamExtInt) - _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. // Used by EKF-GSF experimental yaw estimator (ParamExtFloat) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 6d27130363..38584adda6 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -56,6 +56,13 @@ depends on MODULES_EKF2 ---help--- EKF2 GNSS yaw fusion support. +menuconfig EKF2_MAGNETOMETER +depends on MODULES_EKF2 + bool "magnetometer support" + default y + ---help--- + EKF2 magnetometer support. + menuconfig EKF2_OPTICAL_FLOW depends on MODULES_EKF2 bool "optical flow fusion support"