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

ekf2-decl: do not always update tilt

Browse Source
This commit is contained in:
bresch
2025-05-02 10:23:43 +02:00
committed by Mathieu Bresciani
parent ef0f0884f0
commit 6ec6f1b297
3 changed files with 12 additions and 5 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
@@ -239,13 +239,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
&& PX4_ISFINITE(_wmm_declination_rad)
) {
// using declination from the world magnetic model
fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states);
fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states, update_tilt);
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {
// using previously saved declination
fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states);
fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states, update_tilt);
} else {
// if there is no aiding coming from an inertial frame we need to fuse some declination
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+8 -2
View File
@@ -140,7 +140,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
return true;
}
bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states, bool update_tilt)
{
VectorState H;
float decl_pred;
@@ -160,7 +160,13 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_s
// Calculate the Kalman gains
VectorState Kfusion = P * H / innovation_variance;
if (!update_all_states) {
if (update_all_states) {
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
}
} else {
// zero non-mag Kalman gains if not updating all states
// copy mag_I and mag_B Kalman gains
src/modules/ekf2/EKF/ekf.h
+2 -1
View File
@@ -668,7 +668,8 @@ private:
// fuse magnetometer declination measurement
// R: declination observation variance (rad**2)
bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false);
bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false,
bool update_tilt = false);
#endif // CONFIG_EKF2_MAGNETOMETER