fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-24 15:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fuseDeclination respect mag update_all_states

Browse Source 
- when both mag_hdg/mag_3d are inactive we should be able to continue
   updating mag without any possible impact on other states
This commit is contained in:
Daniel Agar
2024-08-23 15:30:44 -04:00
committed by Mathieu Bresciani
parent 9d57a3c02f
commit 2a9e205442
3 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+2 -2
View File
@@ -237,13 +237,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_wmm_declination_rad)
) {
fuseDeclination(_wmm_declination_rad, 0.5f);
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);
fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f, update_all_states);
} else {
_control_status.flags.mag_dec = false;
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+14 -1
View File
@@ -151,7 +151,7 @@ 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 Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states)
{
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);
@@ -174,6 +174,19 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma)
// Calculate the Kalman gains
VectorState Kfusion = P * H / innovation_variance;
if (!update_all_states) {
// zero non-mag Kalman gains if not updating all states
// copy mag_I and mag_B Kalman gains
const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
// zero all Kalman gains, then restore mag
Kfusion.setZero();
Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
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);
_fault_status.flags.bad_mag_decl = !is_fused;
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -773,7 +773,7 @@ private:
// fuse magnetometer declination measurement
// declination uncertainty in radians
bool fuseDeclination(float decl_measurement_rad, float decl_sigma);
bool fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states = false);
#endif // CONFIG_EKF2_MAGNETOMETER