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

EKF: Prevent rapid changes in declination estimate after a reset

Browse Source 
Ensures that each time the earth field covariance and variance data is reset, that the off-diagonal elements containing earth field declination angle certainty is restored.
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent 6e7c119b31
commit 25148e1b45
5 changed files with 21 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+3
View File
@@ -889,6 +889,9 @@ void Ekf::resetMagCovariance()
for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
P[rc_index][rc_index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
}
void Ekf::resetWindCovariance()