Spiri
/
PX4-Autopilot
3
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ekf2-mag_auto: always use mag 3D after takeoff

This commit is contained in:
bresch 2024-02-14 16:06:29 +01:00 committed by Daniel Agar
parent 086656dc7f
commit b80f15f7b5
10 changed files with 8 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -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

1
src/modules/ekf2/EKF/common.h
View File

@ -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};

8
src/modules/ekf2/EKF/ekf.cpp
View File

@ -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;

6
src/modules/ekf2/EKF/ekf.h
View File

@ -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<float> _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);

7
src/modules/ekf2/EKF/mag_3d_control.cpp
View File

@ -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;

24
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -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) {

1
src/modules/ekf2/EKF2.cpp
View File

@ -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),

1
src/modules/ekf2/EKF2.hpp
View File

@ -605,7 +605,6 @@ private:
(ParamExtInt<px4::params::EKF2_DECL_TYPE>) _param_ekf2_decl_type,
(ParamExtInt<px4::params::EKF2_MAG_TYPE>) _param_ekf2_mag_type,
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>) _param_ekf2_mag_acclim,
(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>) _param_ekf2_mag_yawlim,
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,

17
src/modules/ekf2/ekf2_params.c
View File

@ -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
*

6
src/modules/ekf2/test/test_EKF_mag.cpp
View File

@ -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;