From b80f15f7b54d6f8ed82f52184c118df1d66fa3df Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 14 Feb 2024 16:06:29 +0100 Subject: [PATCH] ekf2-mag_auto: always use mag 3D after takeoff --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 - src/modules/ekf2/EKF/common.h | 1 - src/modules/ekf2/EKF/ekf.cpp | 8 -------- src/modules/ekf2/EKF/ekf.h | 6 ------ src/modules/ekf2/EKF/mag_3d_control.cpp | 7 ++----- src/modules/ekf2/EKF/mag_control.cpp | 24 ----------------------- src/modules/ekf2/EKF2.cpp | 1 - src/modules/ekf2/EKF2.hpp | 1 - src/modules/ekf2/ekf2_params.c | 17 ++-------------- src/modules/ekf2/test/test_EKF_mag.cpp | 6 ++++-- 10 files changed, 8 insertions(+), 64 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index a8de66cd87..32991e59b9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -28,7 +28,6 @@ param set-default EKF2_ARSP_THR 8 param set-default EKF2_FUSE_BETA 1 param set-default EKF2_GPS_CHECK 21 param set-default EKF2_MAG_ACCLIM 0 -param set-default EKF2_MAG_YAWLIM 0 param set-default EKF2_REQ_EPH 10 param set-default EKF2_REQ_EPV 10 param set-default EKF2_REQ_HDRIFT 0.5 diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 20b6fc3dbe..880df1d4fe 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -364,7 +364,6 @@ struct parameters { int32_t mag_declination_source{7}; ///< bitmask used to control the handling of declination data 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.20f}; ///< yaw rate threshold used by mode select logic (rad/sec) // compute synthetic magnetomter Z value if possible int32_t synthesize_mag_z{0}; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index b6af4e7c41..b6de89f544 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -326,14 +326,6 @@ void Ekf::predictState(const imuSample &imu_delayed) const float alpha = 1.0f - imu_delayed.delta_vel_dt; _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - // calculate a yaw change about the earth frame vertical - const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2))); - _yaw_delta_ef += spin_del_ang_D; - - // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic - // Note fixed coefficients are used to save operations. The exact time constant is not important. - _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt; - // Calculate low pass filtered height rate float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2a8c402887..11b98f4173 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -576,8 +576,6 @@ private: Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _height_rate_lpf{0.0f}; - 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) SquareMatrixState P{}; ///< state covariance matrix @@ -706,9 +704,7 @@ private: 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. @@ -724,7 +720,6 @@ private: // 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}; #endif // CONFIG_EKF2_MAGNETOMETER @@ -1048,7 +1043,6 @@ private: bool haglYawResetReq(); void checkYawAngleObservability(); - void checkMagBiasObservability(); void checkMagHeadingConsistency(); bool checkMagField(const Vector3f &mag); diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 2e42e1c39f..89f502ac93 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -45,13 +45,12 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star resetEstimatorAidStatus(aid_src); - const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse); + const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion // determine if we should use mag fusion bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) && _control_status.flags.tilt_align && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6)) && mag_sample.mag.longerThan(0.f) && mag_sample.mag.isAllFinite(); @@ -65,8 +64,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star && _control_status.flags.mag_aligned_in_flight && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps) && !_control_status.flags.mag_fault - && isRecent(aid_src.time_last_fuse, 500'000) - && getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f)) && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw; @@ -92,7 +89,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star if (continuing_conditions_passing && _control_status.flags.yaw_align) { - if (mag_sample.reset || checkHaglYawResetReq()) { + if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) { ECL_INFO("reset to %s", AID_SRC_NAME); resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index cd714e91ec..b491228c39 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -47,15 +47,9 @@ void Ekf::controlMagFusion() _control_status.flags.mag_aligned_in_flight = false; } - // check mag state observability checkYawAngleObservability(); - checkMagBiasObservability(); checkMagHeadingConsistency(); - if (_mag_bias_observable || _yaw_angle_observable) { - _time_last_mov_3d_mag_suitable = _time_delayed_us; - } - if (_params.mag_fusion_type == MagFuseType::NONE) { stopMagFusion(); stopMagHdgFusion(); @@ -245,24 +239,6 @@ void Ekf::checkYawAngleObservability() } } -void Ekf::checkMagBiasObservability() -{ - // 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; - - } else if (_mag_bias_observable) { - // require sustained yaw motion of 50% the initial yaw rate threshold - const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started); - const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt; - _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; - } - - _yaw_delta_ef = 0.0f; - _time_yaw_started = _time_delayed_us; -} - void Ekf::checkMagHeadingConsistency() { if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f0a4ac7955..c75093711e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -135,7 +135,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_decl_type(_params->mag_declination_source), _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), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 2107af227b..1d54315b8c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -605,7 +605,6 @@ private: (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, diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 73b26edd86..396ba96515 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -503,9 +503,9 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); /** - * Horizontal acceleration threshold used by automatic selection of magnetometer fusion method. + * Horizontal acceleration threshold used for heading observability check * - * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered horizontal acceleration is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion. + * The heading is assumed to be observable when the body acceleration is greater than this parameter when a global position/velocity aiding source is active. * * @group EKF2 * @min 0.0 @@ -515,19 +515,6 @@ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); */ PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f); -/** - * Yaw rate threshold used by automatic selection of magnetometer fusion method. - * - * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered yaw rate is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion. - * - * @group EKF2 - * @min 0.0 - * @max 1.0 - * @unit rad/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.20f); - /** * Gate size for barometric and GPS height fusion * diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 32903e6e6c..6641b4fe6d 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -70,7 +70,9 @@ TEST_F(EkfMagTest, fusionStartWithReset) { // GIVEN: some meaningful mag data const float mag_heading = M_PI_F / 3.f; - const Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + const float incl = 63.1f; + const Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), + 0.4f * sinf(incl) * sqrtf(0.2f * 0.2f + 0.4f * 0.4f)); _sensor_simulator._mag.setData(mag_data); const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); @@ -95,7 +97,7 @@ TEST_F(EkfMagTest, fusionStartWithReset) float mag_decl = atan2f(mag_earth(1), mag_earth(0)); float mag_decl_wmm_deg = 0.f; _ekf->get_mag_decl_deg(mag_decl_wmm_deg); - EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-6f); + EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-5f); float mag_incl = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); float mag_incl_wmm_deg = 0.f;