diff --git a/EKF/ekf.h b/EKF/ekf.h index 0f0c438e44..e2a70715a5 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -123,6 +123,9 @@ public: // 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); + // 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_vel_var(Vector3f &vel_var); void get_pos_var(Vector3f &pos_var); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e35a971a85..554796be26 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -773,6 +773,30 @@ void Ekf::get_ekf_lpos_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 velocity uncertainty +void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) +{ + float hvel_err; + float vvel_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) { + hvel_err = sqrtf(P[4][4] + P[5][5]); + vvel_err = sqrtf(P[6][6]); + + } else { + hvel_err = 0.0f; + vvel_err = 0.0f; + + } + + // report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift + bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6) && (_time_last_imu - _time_last_vel_fuse > 1e6) && (_time_last_imu - _time_last_of_fuse > 1e6); + + memcpy(ekf_evh, &hvel_err, sizeof(float)); + memcpy(ekf_evv, &vvel_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. diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 08c5b949b7..0ae88e9133 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -135,6 +135,9 @@ public: // 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; + // 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; + // 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; }