diff --git a/EKF/ekf.h b/EKF/ekf.h index 062e97aabc..5c2ece43f6 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -67,44 +67,61 @@ public: void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos); - // gets the innovations of the earth magnetic field measurements - void get_mag_innov(float mag_innov[3]) override; + void getGpsVelPosInnovRatio(float& hvel, float &vvel, float& hpos, float &vpos); void getEvVelPosInnov(float hvel[2], float& vvel, float hpos[2], float& vpos); - void get_heading_innov(float *heading_innov); void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos); - // gets the innovation variances of the earth magnetic field measurements - void get_mag_innov_var(float mag_innov_var[3]) override; + void getEvVelPosInnovRatio(float& hvel, float &vvel, float& hpos, float &vpos); void getAuxVelInnov(float aux_vel_innov[2]); void getAuxVelInnovVar(float aux_vel_innov[2]); - // gets the innovations of synthetic sideslip measurement - void get_beta_innov(float *beta_innov) override; + void getAuxVelInnovRatio(float &aux_vel_innov_ratio); - // gets the innovation variance of the synthetic sideslip measurement - void get_beta_innov_var(float *beta_innov_var) override; + void getFlowInnov(float flow_innov[2]); - // gets the innovation variance of the heading measurement - void get_heading_innov_var(float *heading_innov_var) override; + void getFlowInnovVar(float flow_innov_var[2]); - // gets the innovation variance of the flow measurement - void get_flow_innov_var(float flow_innov_var[2]) override; + void getFlowInnovRatio(float &flow_innov_ratio); - // gets the innovation of the flow measurement - void get_flow_innov(float flow_innov[2]) override; + void getHeadingInnov(float &heading_innov); - // gets the innovation variance of the drag specific force measurement - void get_drag_innov_var(float drag_innov_var[2]) override; + void getHeadingInnovVar(float &heading_innov_var); - // gets the innovation of the drag specific force measurement - void get_drag_innov(float drag_innov[2]) override; + void getHeadingInnovRatio(float &heading_innov_ratio); - void getHaglInnovVar(float *hagl_innov_var) override; - void getHaglInnov(float *hagl_innov) override; + void getMagInnov(float mag_innov[3]); + + void getMagInnovVar(float mag_innov_var[3]); + + void getMagInnovRatio(float &mag_innov_ratio); + + void getDragInnov(float drag_innov[2]); + + void getDragInnovVar(float drag_innov_var[2]); + + void getDragInnovRatio(float drag_innov_ratio[2]); + + void getAirspeedInnov(float &airspeed_innov); + + void getAirspeedInnovVar(float &airspeed_innov_var); + + void getAirspeedInnovRatio(float &airspeed_innov_ratio); + + void getBetaInnov(float &beta_innov); + + void getBetaInnovVar(float &beta_innov_var); + + void getBetaInnovRatio(float &beta_innov_ratio); + + void getHaglInnov(float &hagl_innov); + + void getHaglInnovVar(float &hagl_innov_var); + + void getHaglInnovRatio(float &hagl_innov_ratio); // get the state vector at the delayed time horizon void get_state_delayed(float *state) override; @@ -244,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 *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) override; + 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); // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) override; @@ -367,20 +384,24 @@ private: float _aux_vel_innov[2] {}; ///< horizontal auxiliary velocity innovations: (m/sec) float _aux_vel_innov_var[2] {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) + + float _heading_innov{0.0f}; ///< heading measurement innovation (rad) + float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2) + float _mag_innov[3] {}; ///< earth magnetic field innovations (Gauss) float _mag_innov_var[3] {}; ///< earth magnetic field innovation variance (Gauss**2) + float _drag_innov[2] {}; ///< multirotor drag measurement innovation (m/sec**2) + float _drag_innov_var[2] {}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) + float _airspeed_innov{0.0f}; ///< airspeed measurement innovation (m/sec) float _airspeed_innov_var{0.0f}; ///< airspeed measurement innovation variance ((m/sec)**2) float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad) float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2) - float _drag_innov[2] {}; ///< multirotor drag measurement innovation (m/sec**2) - float _drag_innov_var[2] {}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) - - float _heading_innov{0.0f}; ///< heading measurement innovation (rad) - float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2) + float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) + float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) // optical flow processing float _flow_innov[2] {}; ///< flow measurement innovation (rad/sec) @@ -450,8 +471,6 @@ private: // Terrain height state estimation float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) - float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) - float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) uint64_t _time_last_hagl_fuse{0}; ///< last system time that the hagl measurement failed it's checks (uSec) bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized float _sin_tilt_rng{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e9e5fd311b..624b078645 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -840,10 +840,12 @@ void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float memcpy(&vpos, _gps_vel_pos_innov_var+5, sizeof(float) * 1); } -// writes the innovations of the earth magnetic field measurements -void Ekf::get_mag_innov(float mag_innov[3]) +void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) { - memcpy(mag_innov, _mag_innov, 3 * sizeof(float)); + memcpy(&hvel, _gps_vel_pos_test_ratio+HVEL, sizeof(float)); + memcpy(&vvel, _gps_vel_pos_test_ratio+VVEL, sizeof(float)); + memcpy(&hpos, _gps_vel_pos_test_ratio+HPOS, sizeof(float)); + memcpy(&vpos, _gps_vel_pos_test_ratio+VPOS, sizeof(float)); } void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) @@ -861,8 +863,13 @@ void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float & memcpy(hpos, _ev_vel_pos_innov_var+3, sizeof(float) * 2); memcpy(&vpos, _ev_vel_pos_innov_var+5, sizeof(float) * 1); } + +void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) { - memcpy(beta_innov, &_beta_innov, sizeof(float)); + memcpy(&hvel, _ev_vel_pos_test_ratio+HVEL, sizeof(float)); + memcpy(&vvel, _ev_vel_pos_test_ratio+VVEL, sizeof(float)); + memcpy(&hpos, _ev_vel_pos_test_ratio+HPOS, sizeof(float)); + memcpy(&vpos, _ev_vel_pos_test_ratio+VPOS, sizeof(float)); } void Ekf::getAuxVelInnov(float aux_vel_innov[2]) @@ -874,37 +881,115 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) { memcpy(aux_vel_innov_var, _aux_vel_innov_var, sizeof(_aux_vel_innov_var)); } + +void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) +{ + memcpy(&aux_vel_innov_ratio, &_aux_vel_test_ratio, sizeof(_aux_vel_test_ratio)); } -// gets the innovation variances of velocity and position measurements -// 0-2 vel, 3-5 pos -void Ekf::get_vel_pos_innov_var(float vel_pos_innov_var[6]) +void Ekf::getFlowInnov(float flow_innov[2]) { - memcpy(vel_pos_innov_var, _vel_pos_innov_var, sizeof(float) * 6); + memcpy(flow_innov, _flow_innov, sizeof(_flow_innov)); } -// gets the innovation variances of the earth magnetic field measurements -void Ekf::get_mag_innov_var(float mag_innov_var[3]) +void Ekf::getFlowInnovVar(float flow_innov_var[2]) { - memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3); + memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var)); } -// gets the innovation variance of the airspeed measurement -void Ekf::get_airspeed_innov_var(float *airspeed_innov_var) +void Ekf::getFlowInnovRatio(float &flow_innov_ratio) { - memcpy(airspeed_innov_var, &_airspeed_innov_var, sizeof(float)); + memcpy(&flow_innov_ratio, &_optflow_test_ratio, sizeof(_optflow_test_ratio)); } -// gets the innovation variance of the synthetic sideslip measurement -void Ekf::get_beta_innov_var(float *beta_innov_var) +void Ekf::getHeadingInnov(float &heading_innov) { - memcpy(beta_innov_var, &_beta_innov_var, sizeof(float)); + memcpy(&heading_innov, &_heading_innov, sizeof(_heading_innov)); } -// gets the innovation variance of the heading measurement -void Ekf::get_heading_innov_var(float *heading_innov_var) +void Ekf::getHeadingInnovVar(float &heading_innov_var) { - memcpy(heading_innov_var, &_heading_innov_var, sizeof(float)); + memcpy(&heading_innov_var, &_heading_innov_var, sizeof(_heading_innov_var)); +} + +void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) +{ + memcpy(&heading_innov_ratio, &_yaw_test_ratio, sizeof(_yaw_test_ratio)); +} + +void Ekf::getMagInnov(float mag_innov[3]) +{ + memcpy(mag_innov, _mag_innov, sizeof(_mag_innov)); +} + +void Ekf::getMagInnovVar(float mag_innov_var[3]) +{ + memcpy(mag_innov_var, _mag_innov_var, sizeof(_mag_innov_var)); +} + +void Ekf::getMagInnovRatio(float &mag_innov_ratio) +{ + mag_innov_ratio = math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2]); +} + +void Ekf::getDragInnov(float drag_innov[2]) +{ + memcpy(&drag_innov, _drag_innov, sizeof(_drag_innov)); +} + +void Ekf::getDragInnovVar(float drag_innov_var[2]) +{ + memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var)); +} + +void Ekf::getDragInnovRatio(float drag_innov_ratio[2]) +{ + memcpy(drag_innov_ratio, &_drag_test_ratio, sizeof(_drag_test_ratio)); +} + +void Ekf::getAirspeedInnov(float &airspeed_innov) +{ + memcpy(&airspeed_innov, &_airspeed_innov, sizeof(_airspeed_innov)); +} + +void Ekf::getAirspeedInnovVar(float &airspeed_innov_var) +{ + memcpy(&airspeed_innov_var, &_airspeed_innov_var, sizeof(_airspeed_innov_var)); +} + +void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio) +{ + memcpy(&airspeed_innov_ratio, &_tas_test_ratio, sizeof(_tas_test_ratio)); +} + +void Ekf::getBetaInnov(float &beta_innov) +{ + memcpy(&beta_innov, &_beta_innov, sizeof(_beta_innov)); +} + +void Ekf::getBetaInnovVar(float &beta_innov_var) +{ + memcpy(&beta_innov_var, &_beta_innov_var, sizeof(_beta_innov_var)); +} + +void Ekf::getBetaInnovRatio(float &beta_innov_ratio) +{ + memcpy(&beta_innov_ratio, &_beta_test_ratio, sizeof(_beta_test_ratio)); +} + +void Ekf::getHaglInnov(float &hagl_innov) +{ + memcpy(&hagl_innov, &_hagl_innov, sizeof(_hagl_innov)); +} + +void Ekf::getHaglInnovVar(float &hagl_innov_var) +{ + memcpy(&hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var)); +} + +void Ekf::getHaglInnovRatio(float &hagl_innov_ratio) +{ + memcpy(&hagl_innov_ratio, &_hagl_test_ratio, sizeof(_hagl_test_ratio)); } // get GPS check status @@ -1169,24 +1254,28 @@ 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 *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) +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) { // return the integer bitmask containing the consistency check pass/fail status - *status = _innov_check_fail_status.value; + 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 velocity innovation test ratio - *vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0], _vel_pos_test_ratio[1]), _vel_pos_test_ratio[2])); - // return the largest NE position innovation test ratio - *pos = sqrtf(math::max(_vel_pos_test_ratio[3], _vel_pos_test_ratio[4])); + 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_pos_test_ratio[HVEL], _gps_vel_pos_test_ratio[VVEL])); + // return the largest NE GPS position innovation test ratio + gps_pos = sqrtf(_gps_vel_pos_test_ratio[HPOS]); + // return the largest external vision velocity innovation test ratio + ev_vel = sqrtf(math::max(_ev_vel_pos_test_ratio[HVEL], _ev_vel_pos_test_ratio[VVEL])); + // return the largest horizontal external vision position innovation test ratio + ev_pos = sqrtf(_ev_vel_pos_test_ratio[HPOS]); // return the vertical position innovation test ratio - *hgt = sqrtf(_vel_pos_test_ratio[5]); + hgt = sqrtf(_gps_vel_pos_test_ratio[VPOS]); // return the airspeed fusion innovation test ratio - *tas = sqrtf(_tas_test_ratio); + tas = sqrtf(_tas_test_ratio); // return the terrain height innovation test ratio - *hagl = sqrtf(_terr_test_ratio); + hagl = sqrtf(_hagl_test_ratio); // return the synthetic sideslip innovation test ratio - *beta = sqrtf(_beta_test_ratio); + beta = sqrtf(_beta_test_ratio); } // return a bitmask integer that describes which state estimates are valid diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index af8fb0c45c..d50439b518 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -71,36 +71,80 @@ public: virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; // gets the GPS innovation variances of velocity and position measurements virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; + // gets the GPS innovation test ratios of velocity and position measurements + virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0; // gets the external vision innovations of velocity and position measurements virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; // gets the external vision innovation variances of velocity and position measurements virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; + // gets the external vision innovation test ratios of velocity and position measurements + virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0; - // gets the innovation of airspeed measurement - virtual void get_airspeed_innov(float *airspeed_innov) = 0; // gets the innovations for of the horizontal auxiliary velocity measurement virtual void getAuxVelInnov(float aux_vel_innov[2]) = 0; // gets the innovation variances for of the horizontal auxiliary velocity measurement virtual void getAuxVelInnovVar(float aux_vel_innov[2]) = 0; + // gets the innovation test ratios of the horizontal auxiliary velocity measurement + virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0; + + + // gets the innovation of the flow measurement + virtual void getFlowInnov(float flow_innov[2]) = 0; + // gets the innovation variance of the flow measurement + virtual void getFlowInnovVar(float flow_innov_var[2]) = 0; + // gets the innovation test ratios of the flow measurement + virtual void getFlowInnovRatio(float &flow_innov_ratio) = 0; + // gets the innovations of the heading measurement - virtual void get_heading_innov(float *heading_innov) = 0; - - - // gets the innovation variances of the earth magnetic field measurements - virtual void get_mag_innov_var(float mag_innov_var[3]) = 0; - - // gets the innovation variance of the airspeed measurement - virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0; - - // gets the innovation variance of the synthetic sideslip measurement - virtual void get_beta_innov_var(float *get_beta_innov_var) = 0; - + virtual void getHeadingInnov(float &heading_innov) = 0; // gets the innovation variance of the heading measurement - virtual void get_heading_innov_var(float *heading_innov_var) = 0; + virtual void getHeadingInnovVar(float &heading_innov_var) = 0; + // gets the innovation test ratios of the heading measurement + virtual void getHeadingInnovRatio(float &heading_innov_ratio) = 0; + + + // gets the innovations of the earth magnetic field measurements + virtual void getMagInnov(float mag_innov[3]) = 0; + // gets the innovation variances of the earth magnetic field measurements + virtual void getMagInnovVar(float mag_innov_var[3]) = 0; + // gets the innovation test ratios of the earth magnetic field measurements + virtual void getMagInnovRatio(float &mag_innov_ratio) = 0; + + + // gets the innovation of the drag specific force measurement + virtual void getDragInnov(float drag_innov[2]) = 0; + // gets the innovation variance of the drag specific force measurement + virtual void getDragInnovVar(float drag_innov_var[2]) = 0; + // gets the innovation test ratios of the drag specific force measurement + virtual void getDragInnovRatio(float drag_innov_ratio[2]) = 0; + + // gets the innovation of airspeed measurement + virtual void getAirspeedInnov(float &airspeed_innov) = 0; + // gets the innovation variance of the airspeed measurement + virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) = 0; + // gets the innovation test ratios of the airspeed measurement + virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) = 0; + + + // gets the innovation of the synthetic sideslip measurement + virtual void getBetaInnov(float &beta_innov) = 0; + // gets the innovation variance of the synthetic sideslip measurement + virtual void getBetaInnovVar(float &get_beta_innov_var) = 0; + // gets the innovation test ratios of the synthetic sideslip measurement + virtual void getBetaInnovRatio(float &beta_innov_ratio) = 0; + + + // gets the innovation of the HAGL measurement + virtual void getHaglInnov(float &hagl_innov) = 0; + // gets the innovation variance of the HAGL measurement + virtual void getHaglInnovVar(float &hagl_innov_var) = 0; + // gets the innovation test ratios of the HAGL measurement + virtual void getHaglInnovRatio(float &hagl_innov_ratio) = 0; + virtual void get_state_delayed(float *state) = 0; @@ -116,24 +160,6 @@ public: // gets the variances for the NED position states virtual void get_pos_var(Vector3f &pos_var) = 0; - // gets the innovation variance of the flow measurement - virtual void get_flow_innov_var(float flow_innov_var[2]) = 0; - - // gets the innovation of the flow measurement - virtual void get_flow_innov(float flow_innov[2]) = 0; - - // gets the innovation variance of the drag specific force measurement - virtual void get_drag_innov_var(float drag_innov_var[2]) = 0; - - // gets the innovation of the drag specific force measurement - virtual void get_drag_innov(float drag_innov[2]) = 0; - - virtual void getHaglInnovVar(float *hagl_innov_var) = 0; - virtual void getHaglInnov(float *hagl_innov) = 0; - //[[deprecated("Replaced by getHaglInnovVar")]] - void get_hagl_innov_var(float *hagl_innov_var) { getHaglInnovVar(hagl_innov_var); } - //[[deprecated("Replaced by getHaglInnov")]] - void get_hagl_innov(float *hagl_innov) { getHaglInnov(hagl_innov); } // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/s), (m) @@ -385,7 +411,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 *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) = 0; + 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; // 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; @@ -484,12 +510,15 @@ protected: float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). // innovation consistency check monitoring ratios - float _yaw_test_ratio{0.0f}; // yaw innovation consistency check ratio - float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios + float _yaw_test_ratio{}; // yaw innovation consistency check ratio + float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios float _gps_vel_pos_test_ratio[4] {}; // GPS velocity and position innovation consistency check ratios float _ev_vel_pos_test_ratio[4] {}; // EV velocity and position innovation consistency check ratios float _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio - float _beta_test_ratio{0.0f}; // sideslip innovation consistency check ratio + float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio + float _tas_test_ratio{}; // tas innovation consistency check ratio + float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio + float _beta_test_ratio{}; // sideslip innovation consistency check ratio float _drag_test_ratio[2] {}; // drag innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status{}; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 485d77b568..706d0622a4 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -48,7 +48,6 @@ void Ekf::fuseOptFlow() { float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f); - float optflow_test_ratio[2] = {0}; // get latest estimated orientation float q0 = _state.quat_nominal(0); @@ -262,9 +261,6 @@ void Ekf::fuseOptFlow() Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); - // run innovation consistency checks - optflow_test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]); - } else if (obs_index == 1) { // calculate Y axis observation Jacobian @@ -407,17 +403,18 @@ void Ekf::fuseOptFlow() Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); - // run innovation consistency check - optflow_test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]); - } } - // record the innovation test pass/fail + // run the innovation consistency check and record result bool flow_fail = false; + float test_ratio[2]; + test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]); + test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]); + _optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]); for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { - if (optflow_test_ratio[obs_index] > 1.0f) { + if (test_ratio[obs_index] > 1.0f) { flow_fail = true; _innov_check_fail_status.value |= (1 << (obs_index + 10)); @@ -515,28 +512,6 @@ void Ekf::fuseOptFlow() } } -void Ekf::get_flow_innov(float flow_innov[2]) -{ - memcpy(flow_innov, _flow_innov, sizeof(_flow_innov)); -} - - -void Ekf::get_flow_innov_var(float flow_innov_var[2]) -{ - memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var)); -} - -void Ekf::get_drag_innov(float drag_innov[2]) -{ - memcpy(drag_innov, _drag_innov, sizeof(_drag_innov)); -} - - -void Ekf::get_drag_innov_var(float drag_innov_var[2]) -{ - memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var)); -} - // calculate optical flow body angular rate compensation // returns false if bias corrected body rate data is unavailable bool Ekf::calcOptFlowBodyRateComp() diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 07aa51a77d..a3430ac70e 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -153,9 +153,9 @@ void Ekf::fuseHagl() // perform an innovation consistency check and only fuse data if it passes float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - _terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); - if (_terr_test_ratio <= 1.0f) { + if (_hagl_test_ratio <= 1.0f) { // calculate the Kalman gain float gain = _terrain_var / _hagl_innov_var; // correct the state @@ -308,13 +308,4 @@ void Ekf::getTerrainVertPos(float *ret) memcpy(ret, &_terrain_vpos, sizeof(float)); } -void Ekf::getHaglInnov(float *hagl_innov) -{ - memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov)); -} - - -void Ekf::getHaglInnovVar(float *hagl_innov_var) -{ - memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var)); }