EKF: Calculate and publish horizontal and vertical position accuracy

This calculation takes into account the uncertainty of the origin.
Dead reckoning status is also published
This commit is contained in:
Paul Riseborough
2016-02-25 10:38:07 +11:00
parent a30830a7a9
commit cd0cac066a
4 changed files with 26 additions and 0 deletions
+3
View File
@@ -70,6 +70,9 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
_last_gps_origin_time_us = _time_last_imu;
// set the magnetic declination returned by the geo library using the current GPS position
_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
// save the horizontal and vertical position uncertainty of the origin
_gps_origin_eph = gps->eph;
_gps_origin_epv = gps->epv;
}
}