diff --git a/EKF/ekf.h b/EKF/ekf.h index 7625bd01aa..c7bd4ff6ce 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -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. diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 1e48c7e8a1..5062c770bd 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -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]); } /* diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 85382e3f98..8f8ddec0f2 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -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.