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

EKF always fill position and velocity accuracy (#425)

Browse Source
This commit is contained in:
Daniel Agar
2018-04-23 21:15:29 +10:00
committed by Paul Riseborough
parent 6e3403ce28
commit a14434d6dc
3 changed files with 26 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+3 -3
View File
@@ -132,13 +132,13 @@ public:
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
// 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);
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv);
// 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_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv);
// get the 1-sigma horizontal and vertical velocity uncertainty
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning);
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv);
/*
Returns the following vehicle control limits required by the estimator.
EKF/ekf_helper.cpp
+20 -68
View File
@@ -981,114 +981,66 @@ void Ekf::get_imu_vibe_metrics(float vibe[3])
}
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
{
// report absolute accuracy taking into account the uncertainty in location of the origin
// If not aiding, return 0 for horizontal position estimate as no estimate is available
// TODO - allow for baro drift in vertical position error
float hpos_err;
float vpos_err;
if (global_position_is_valid()) {
hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
vpos_err = sqrtf(P[9][9] + sq(_gps_origin_epv));
} else {
hpos_err = 0.0f;
vpos_err = 0.0f;
}
float hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
}
memcpy(ekf_eph, &hpos_err, sizeof(float));
memcpy(ekf_epv, &vpos_err, sizeof(float));
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
*ekf_eph = hpos_err;
*ekf_epv = sqrtf(P[9][9] + sq(_gps_origin_epv));
}
// 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)
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
{
// 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 ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
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;
}
float hpos_err = sqrtf(P[7][7] + P[8][8]);
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
}
memcpy(ekf_eph, &hpos_err, sizeof(float));
memcpy(ekf_epv, &vpos_err, sizeof(float));
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
*ekf_eph = hpos_err;
*ekf_epv = sqrtf(P[9][9]);
}
// get the 1-sigma horizontal and vertical velocity uncertainty
void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning)
void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
{
float hvel_err;
float vvel_err;
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hvel_err = sqrtf(P[4][4] + P[5][5]);
vvel_err = sqrtf(P[6][6]);
} else {
hvel_err = 0.0f;
vvel_err = 0.0f;
}
float hvel_err = sqrtf(P[4][4] + P[5][5]);
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
float vel_err_conservative = 0.0f;
if (_is_dead_reckoning) {
float vel_err_conservative = 0.0f;
if (_control_status.flags.opt_flow) {
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)),
gndclearance) * sqrtf(_flow_innov[0] * _flow_innov[0] + _flow_innov[1] * _flow_innov[1]);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
}
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
vel_err_conservative = math::max(vel_err_conservative,
sqrtf(_vel_pos_innov[0] * _vel_pos_innov[0] + _vel_pos_innov[1] * _vel_pos_innov[1]));
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
}
hvel_err = math::max(hvel_err, vel_err_conservative);
}
memcpy(ekf_evh, &hvel_err, sizeof(float));
memcpy(ekf_evv, &vvel_err, sizeof(float));
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
*ekf_evh = hvel_err;
*ekf_evv = sqrtf(P[6][6]);
}
/*
EKF/estimator_interface.h
+3 -3
View File
@@ -144,13 +144,13 @@ public:
virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
// 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;
virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) = 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;
virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
// get the 1-sigma horizontal and vertical velocity uncertainty
virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) = 0;
virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0;
/*
Returns the following vehicle control limits required by the estimator.