dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: add accessor function for local position accuracy

This commit is contained in:
Paul Riseborough 2017-02-24 09:25:39 +11:00 committed by Lorenz Meier
parent b7d0b3c4d0
commit d6abf3f2e4
3 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.h
View File

@ -120,6 +120,9 @@ public:
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
void get_vel_var(Vector3f &vel_var);
void get_pos_var(Vector3f &pos_var);

25
EKF/ekf_helper.cpp
View File

@ -748,6 +748,31 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
}
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
{
// TODO - allow for baro drift in vertical position error
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8]);
vpos_err = sqrtf(P[9][9]);
} else {
hpos_err = 0.0f;
vpos_err = 0.0f;
}
// report dead reckoning if it is more than a second since we fused in position measurements
bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6);
memcpy(ekf_eph, &hpos_err, sizeof(float));
memcpy(ekf_epv, &vpos_err, sizeof(float));
memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
}
// get EKF innovation consistency check status information comprising of:
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.

3
EKF/estimator_interface.h
View File

@ -132,6 +132,9 @@ public:
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0;
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0;
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }