diff --git a/EKF/control.cpp b/EKF/control.cpp index afc699fe02..dad5b03d32 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -728,7 +728,7 @@ void Ekf::controlGpsFusion() // set innovation gate size gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f); - gps_pos_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f); + gps_vel_innov_gates(0) = gps_vel_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f); // fuse GPS measurement fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); diff --git a/EKF/ekf.h b/EKF/ekf.h index 557ebd13bb..6ff9ccf2aa 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -261,7 +261,7 @@ public: // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. - void get_innovation_test_status(uint16_t &status, float &mag, float &gps_vel, float &gps_pos, float &ev_vel, float &ev_pos, float &hgt, float &tas, float &hagl, float &beta) override; + void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) override; // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) override; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e7361e934c..9a05fee22b 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1277,20 +1277,18 @@ bool Ekf::reset_imu_bias() // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. -void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &gps_vel, float &gps_pos, float &ev_vel, float &ev_pos, float &hgt, float &tas, float &hagl, float &beta) +void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) { // return the integer bitmask containing the consistency check pass/fail status status = _innov_check_fail_status.value; // return the largest magnetometer innovation test ratio mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2]))); - // return the largest NED GPS velocity innovation test ratio - gps_vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1))); - // return the largest NE GPS position innovation test ratio - gps_pos = sqrtf(_gps_pos_test_ratio(0)); - // return the largest external vision velocity innovation test ratio - ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))); - // return the largest horizontal external vision position innovation test ratio - ev_pos = sqrtf(_ev_pos_test_ratio(0)); + // return the largest velocity innovation test ratio + vel = math::max(sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1))), + sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)))); + // return the largest position innovation test ratio + pos = math::max(sqrtf(_gps_pos_test_ratio(0)),sqrtf(_ev_pos_test_ratio(0))); + // return the vertical position innovation test ratio hgt = sqrtf(_gps_pos_test_ratio(0)); // return the airspeed fusion innovation test ratio diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 8d78b31d9e..6f236113c1 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -373,7 +373,7 @@ public: // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. - virtual void get_innovation_test_status(uint16_t &status, float &mag, float &gps_vel, float &gps_pos, float &ev_vel, float &ev_pos, float &hgt, float &tas, float &hagl, float &beta) = 0; + virtual void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) = 0; // return a bitmask integer that describes which state estimates can be used for flight control virtual void get_ekf_soln_status(uint16_t *status) = 0;