|
|
|
@ -752,10 +752,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
@@ -752,10 +752,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
|
|
|
|
// TODO - allow for baro drift in vertical position error
|
|
|
|
|
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
|
|
|
|
|
// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position 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
|
|
|
|
|
if (_is_dead_reckoning && _control_status.flags.gps) { |
|
|
|
|
if (_deadreckon_time_exceeded && _control_status.flags.gps) { |
|
|
|
|
hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -768,10 +768,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
@@ -768,10 +768,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
|
|
|
|
{ |
|
|
|
|
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
|
|
|
|
|
// If we are dead-reckoning for too long, 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
|
|
|
|
|
if (_is_dead_reckoning) { |
|
|
|
|
if (_deadreckon_time_exceeded) { |
|
|
|
|
float vel_err_conservative = 0.0f; |
|
|
|
|
|
|
|
|
|
if (_control_status.flags.opt_flow) { |
|
|
|
|