fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 10:53:33 +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/ekf_helper.cpp
+10
View File
@@ -491,8 +491,12 @@ bool Ekf::realignYawGPS()
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
@@ -525,6 +529,9 @@ bool Ekf::realignYawGPS()
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -684,6 +691,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;