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

EKF: Calculate and publish horizontal and vertical position accuracy

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+15
View File
@@ -309,6 +309,21 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
}
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
{
// report absolute accuracy taking into account the uncertainty in location of the origin
// TODO we a need a way to allow for baro drift error
float temp1 = sqrtf(P[6][6] + P[7][7] + sq(_gps_origin_eph));
float temp2 = sqrtf(P[8][8] + sq(_gps_origin_epv));
memcpy(ekf_eph, &temp1, sizeof(float));
memcpy(ekf_epv, &temp2, sizeof(float));
// report dead reckoning if it is more than a second since we fused in GPS
bool temp3 = (_time_last_imu - _time_last_pos_fuse > 1e6);
memcpy(dead_reckoning, &temp3, sizeof(bool));
}
// fuse measurement
void Ekf::fuse(float *K, float innovation)
{