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

EKF: protect declination fusion from badly conditioned earth field estimates

Browse Source 
Fusion with large initial magnetometer biases errors can result in the the NE earth field states reducing in magnitude and effectively flipping sign.

EKF: Move declination state limiting into a separate function

EKF: Limit NE mag states after each 3-axis mag fusion

EKF: Fix bug in mag field strength look-up scale factor
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent 7bddbd1cc5
commit 3accab1ac5
6 changed files with 71 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_checks.cpp
+5 -1
View File
@@ -77,8 +77,12 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
_NED_origin_initialised = true;
_last_gps_origin_time_us = _time_last_imu;
// set the magnetic declination returned by the geo library using the current GPS position
// set the magnetic field data returned by the geo library using the current GPS position
_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
_mag_inclination_gps = math::radians(get_mag_inclination(lat, lon));
_mag_strength_gps = 0.01f * get_mag_strength(lat, lon);
// request a reset of the yaw using the new declination
_mag_yaw_reset_req = true;
// save the horizontal and vertical position uncertainty of the origin