fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 06:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: mag declination fusion always if there is no aiding

Browse Source
This commit is contained in:
Daniel Agar
2024-08-27 16:16:55 +02:00
committed by Mathieu Bresciani
parent 2a9e205442
commit ac48b8b51d
4 changed files with 21 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+14 -7
View File
@@ -205,14 +205,14 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
const bool no_ne_aiding_or_pre_takeoff = !using_ne_aiding || !_control_status.flags.in_air;
_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff;
const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest;
_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving;
if (_control_status.flags.mag) {
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
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 = imu_sample.time_us;
@@ -234,19 +234,26 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
if (_control_status.flags.mag_dec) {
// observation variance (rad**2)
const float R_DECL = sq(0.5f);
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_wmm_declination_rad)
) {
// using declination from the world magnetic model
fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states);
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {
fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f, update_all_states);
// using previously saved declination
fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states);
} else {
_control_status.flags.mag_dec = false;
// if there is no aiding coming from an inertial frame we need to fuse some declination
// even if we don't know the value, it's better to fuse 0 than nothing
float declination_rad = 0.f;
fuseDeclination(declination_rad, R_DECL);
}
}
}
@@ -254,7 +261,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
if (is_fusion_failing) {
if (no_ne_aiding_or_pre_takeoff) {
if (no_ne_aiding_or_not_moving) {
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
aid_src.time_last_fuse = imu_sample.time_us;
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+4 -7
View File
@@ -151,21 +151,18 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
return false;
}
bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states)
bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
{
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);
VectorState H;
float decl_pred;
float innovation_variance;
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON,
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
&decl_pred, &innovation_variance, &H);
const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
if (innovation_variance < R_DECL) {
if (innovation_variance < R) {
// variance calculation is badly conditioned
_fault_status.flags.bad_mag_decl = true;
return false;
@@ -187,7 +184,7 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma, bool upd
Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
}
const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
_fault_status.flags.bad_mag_decl = !is_fused;
src/modules/ekf2/EKF/ekf.h
+2 -2
View File
@@ -772,8 +772,8 @@ private:
bool update_all_states = false, bool update_tilt = false);
// fuse magnetometer declination measurement
// declination uncertainty in radians
bool fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states = false);
// R: declination observation variance (rad**2)
bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false);
#endif // CONFIG_EKF2_MAGNETOMETER
src/modules/ekf2/test/test_EKF_basics.cpp
+1
View File
@@ -108,6 +108,7 @@ TEST_F(EkfBasicsTest, initialControlMode)
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag);
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec);
EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air);
EXPECT_EQ(0, (int) _ekf->control_status_flags().wind);
EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);