Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2: do not let mag heading and declination update xy gyro biases

Browse Source 
This can destabilize the tilt estimate when the mag field is disturbed
This commit is contained in:
bresch
2025-04-08 17:07:21 +02:00
committed by Mathieu Bresciani
parent a40377e544
commit e7250bc8d5
1 changed files with 8 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+8
View File
@@ -108,6 +108,10 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
// Also avoid corruption of the XY gyro biases
Kfusion(State::gyro_bias.idx + 0) = 0.f;
Kfusion(State::gyro_bias.idx + 1) = 0.f;
}
} else {
@@ -164,6 +168,10 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_s
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
// Also avoid corruption of the XY gyro biases
Kfusion(State::gyro_bias.idx + 0) = 0.f;
Kfusion(State::gyro_bias.idx + 1) = 0.f;
}
} else {