Browse Source

lpos accuracy: bump variance if deadreckon time exceeded

master
bresch 4 years ago committed by Daniel Agar
parent
commit
5aaa6c6dde
  1. 8
      EKF/ekf_helper.cpp

8
EKF/ekf_helper.cpp

@ -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) {

Loading…
Cancel
Save