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

EKF: Add fusion method to constrain declination when unobservable

Browse Source 
When fusing 3-axis magnetometer data without absolute external aiding leg GPS), the declination is ultimately unobservable and the declination of the  field states and the vehicle heading will slowly drift over time. To prevent this we need to fuse in a declination to constraint the NE earth field estimates.
This commit is contained in:
Paul Riseborough
2016-02-08 14:18:43 +11:00
parent 9347afe250
commit dbb8e12948
3 changed files with 143 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+3 -5
View File
@@ -146,11 +146,9 @@ bool Ekf::update()
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.angle_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
// TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding
// otherwise heading will slowly drift
}
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
fuseHeading();
}