diff --git a/msg/EstimatorInnovations.msg b/msg/EstimatorInnovations.msg index c8500d9e83..ecf637478d 100644 --- a/msg/EstimatorInnovations.msg +++ b/msg/EstimatorInnovations.msg @@ -19,7 +19,6 @@ float32 baro_vpos # barometer height innovation (m) and innovation variance (m** # Auxiliary velocity float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) -float32 aux_vvel # vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) # Optical flow float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 71150ff17b..fbe782f22e 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -83,19 +83,9 @@ public: static uint8_t getNumberOfStates() { return State::size; } -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; -#endif // CONFIG_EKF2_EXTERNAL_VISION - #if defined(CONFIG_EKF2_BAROMETER) const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } - - void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; } - void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } - void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; } #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_TERRAIN) @@ -115,28 +105,10 @@ public: # if defined(CONFIG_EKF2_RANGE_FINDER) const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; } - - void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; } - void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; } - void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; } # endif // CONFIG_EKF2_RANGE_FINDER # if defined(CONFIG_EKF2_OPTICAL_FLOW) const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } - - void getTerrainFlowInnov(float flow_innov[2]) const - { - flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; - } - - void getTerrainFlowInnovVar(float flow_innov_var[2]) const - { - flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; - } - - void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN @@ -146,32 +118,14 @@ public: const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } - void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } - void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } - - void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } - void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } - void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } + float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); } + float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); } + float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } - void getFlowInnov(float flow_innov[2]) const - { - flow_innov[0] = _aid_src_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_optical_flow.innovation[1]; - } - - void getFlowInnovVar(float flow_innov_var[2]) const - { - flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; - } - - void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); } - const Vector2f &getFlowVelBody() const { return _flow_vel_body; } const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } @@ -185,140 +139,93 @@ public: const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; } #endif // CONFIG_EKF2_OPTICAL_FLOW -#if defined(CONFIG_EKF2_AUXVEL) - void getAuxVelInnov(float aux_vel_innov[2]) const; - void getAuxVelInnovVar(float aux_vel_innov[2]) const; - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); } -#endif // CONFIG_EKF2_AUXVEL - - void getHeadingInnov(float &heading_innov) const + float getHeadingInnov() const { #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { - heading_innov = _aid_src_mag_heading.innovation; - return; + return _aid_src_mag_heading.innovation; + } - } else if (_control_status.flags.mag_3D) { - heading_innov = Vector3f(_aid_src_mag.innovation).max(); - return; + if (_control_status.flags.mag_3D) { + return Vector3f(_aid_src_mag.innovation).max(); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - heading_innov = _aid_src_gnss_yaw.innovation; - return; + return _aid_src_gnss_yaw.innovation; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - heading_innov = _aid_src_ev_yaw.innovation; - return; + return _aid_src_ev_yaw.innovation; } #endif // CONFIG_EKF2_EXTERNAL_VISION + + return 0.f; } - void getHeadingInnovVar(float &heading_innov_var) const + float getHeadingInnovVar() const { #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { - heading_innov_var = _aid_src_mag_heading.innovation_variance; - return; + return _aid_src_mag_heading.innovation_variance; + } - } else if (_control_status.flags.mag_3D) { - heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max(); - return; + if (_control_status.flags.mag_3D) { + return Vector3f(_aid_src_mag.innovation_variance).max(); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - heading_innov_var = _aid_src_gnss_yaw.innovation_variance; - return; + return _aid_src_gnss_yaw.innovation_variance; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - heading_innov_var = _aid_src_ev_yaw.innovation_variance; - return; + return _aid_src_ev_yaw.innovation_variance; } #endif // CONFIG_EKF2_EXTERNAL_VISION + + return 0.f; } - void getHeadingInnovRatio(float &heading_innov_ratio) const + float getHeadingInnovRatio() const { #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { - heading_innov_ratio = _aid_src_mag_heading.test_ratio; - return; + return _aid_src_mag_heading.test_ratio; + } - } else if (_control_status.flags.mag_3D) { - heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); - return; + if (_control_status.flags.mag_3D) { + return Vector3f(_aid_src_mag.test_ratio).max(); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - heading_innov_ratio = _aid_src_gnss_yaw.test_ratio; - return; + return _aid_src_gnss_yaw.test_ratio; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - heading_innov_ratio = _aid_src_ev_yaw.test_ratio; - return; + return _aid_src_ev_yaw.test_ratio; } #endif // CONFIG_EKF2_EXTERNAL_VISION - } -#if defined(CONFIG_EKF2_MAGNETOMETER) - void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); } - void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); } - void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); } -#endif // CONFIG_EKF2_MAGNETOMETER + return 0.f; + } #if defined(CONFIG_EKF2_DRAG_FUSION) const auto &aid_src_drag() const { return _aid_src_drag; } - - void getDragInnov(float drag_innov[2]) const - { - drag_innov[0] = _aid_src_drag.innovation[0]; - drag_innov[1] = _aid_src_drag.innovation[1]; - } - void getDragInnovVar(float drag_innov_var[2]) const - { - drag_innov_var[0] = _aid_src_drag.innovation_variance[0]; - drag_innov_var[1] = _aid_src_drag.innovation_variance[1]; - } - void getDragInnovRatio(float drag_innov_ratio[2]) const - { - drag_innov_ratio[0] = _aid_src_drag.test_ratio[0]; - drag_innov_ratio[1] = _aid_src_drag.test_ratio[1]; - } #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_AIRSPEED) - void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; } - void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; } - void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; } -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_SIDESLIP) - void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; } - void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; } - void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; } -#endif // CONFIG_EKF2_SIDESLIP - #if defined(CONFIG_EKF2_GRAVITY_FUSION) const auto &aid_src_gravity() const { return _aid_src_gravity; } - - void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); } - void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); } - void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); } #endif // CONFIG_EKF2_GRAVITY_FUSION // get the state vector at the delayed time horizon @@ -497,12 +404,6 @@ public: uint8_t getHeightSensorRef() const { return _height_sensor_ref; } -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } - - const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } -#endif // CONFIG_EKF2_EXTERNAL_VISION - #if defined(CONFIG_EKF2_AIRSPEED) const auto &aid_src_airspeed() const { return _aid_src_airspeed; } #endif // CONFIG_EKF2_AIRSPEED @@ -519,13 +420,12 @@ public: const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; } const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; } const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; } + + const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } + const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined bool collect_gps(const gpsMessage &gps) override; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e2eb52f3d3..e75d9c01cb 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -295,86 +295,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } -#if defined(CONFIG_EKF2_GNSS) -void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_gnss_vel.innovation[0]; - hvel[1] = _aid_src_gnss_vel.innovation[1]; - vvel = _aid_src_gnss_vel.innovation[2]; - - hpos[0] = _aid_src_gnss_pos.innovation[0]; - hpos[1] = _aid_src_gnss_pos.innovation[1]; - vpos = _aid_src_gnss_hgt.innovation; -} - -void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_gnss_vel.innovation_variance[0]; - hvel[1] = _aid_src_gnss_vel.innovation_variance[1]; - vvel = _aid_src_gnss_vel.innovation_variance[2]; - - hpos[0] = _aid_src_gnss_pos.innovation_variance[0]; - hpos[1] = _aid_src_gnss_pos.innovation_variance[1]; - vpos = _aid_src_gnss_hgt.innovation_variance; -} - -void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const -{ - hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]); - vvel = _aid_src_gnss_vel.test_ratio[2]; - - hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]); - vpos = _aid_src_gnss_hgt.test_ratio; -} -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) -void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_ev_vel.innovation[0]; - hvel[1] = _aid_src_ev_vel.innovation[1]; - vvel = _aid_src_ev_vel.innovation[2]; - - hpos[0] = _aid_src_ev_pos.innovation[0]; - hpos[1] = _aid_src_ev_pos.innovation[1]; - vpos = _aid_src_ev_hgt.innovation; -} - -void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_ev_vel.innovation_variance[0]; - hvel[1] = _aid_src_ev_vel.innovation_variance[1]; - vvel = _aid_src_ev_vel.innovation_variance[2]; - - hpos[0] = _aid_src_ev_pos.innovation_variance[0]; - hpos[1] = _aid_src_ev_pos.innovation_variance[1]; - vpos = _aid_src_ev_hgt.innovation_variance; -} - -void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const -{ - hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]); - vvel = _aid_src_ev_vel.test_ratio[2]; - - hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]); - vpos = _aid_src_ev_hgt.test_ratio; -} -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_AUXVEL) -void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const -{ - aux_vel_innov[0] = _aid_src_aux_vel.innovation[0]; - aux_vel_innov[1] = _aid_src_aux_vel.innovation[1]; -} - -void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const -{ - aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0]; - aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1]; -} -#endif // CONFIG_EKF2_AUXVEL - bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { origin_time = _pos_ref.getProjectionReferenceTimestamp(); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 80c15714b3..72d8244148 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1406,53 +1406,93 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) // publish estimator innovation data estimator_innovations_s innovations{}; innovations.timestamp_sample = _ekf.time_delayed_us(); + #if defined(CONFIG_EKF2_GNSS) - _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos); + // GPS + innovations.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation[0]; + innovations.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation[1]; + innovations.gps_vvel = _ekf.aid_src_gnss_vel().innovation[2]; + innovations.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation[0]; + innovations.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation[1]; + innovations.gps_vpos = _ekf.aid_src_gnss_hgt().innovation; #endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); + // External Vision + innovations.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation[0]; + innovations.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation[1]; + innovations.ev_vvel = _ekf.aid_src_ev_vel().innovation[2]; + innovations.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation[0]; + innovations.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation[1]; + innovations.ev_vpos = _ekf.aid_src_ev_hgt().innovation; #endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_BAROMETER) - _ekf.getBaroHgtInnov(innovations.baro_vpos); -#endif // CONFIG_EKF2_BAROMETER + + // Height sensors #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getRngHgtInnov(innovations.rng_vpos); + innovations.rng_vpos = _ekf.aid_src_rng_hgt().innovation; #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_BAROMETER) + innovations.baro_vpos = _ekf.aid_src_baro_hgt().innovation; +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_AUXVEL) - _ekf.getAuxVelInnov(innovations.aux_hvel); + // Auxiliary velocity + innovations.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation[0]; + innovations.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation[1]; #endif // CONFIG_EKF2_AUXVEL + #if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getFlowInnov(innovations.flow); + // Optical flow + innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0]; + innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1]; +# if defined(CONFIG_EKF2_TERRAIN) + innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0]; + innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1]; +# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW - _ekf.getHeadingInnov(innovations.heading); + + // heading + innovations.heading = _ekf.getHeadingInnov(); + #if defined(CONFIG_EKF2_MAGNETOMETER) - _ekf.getMagInnov(innovations.mag_field); + // mag_field + innovations.mag_field[0] = _ekf.aid_src_mag().innovation[0]; + innovations.mag_field[1] = _ekf.aid_src_mag().innovation[1]; + innovations.mag_field[2] = _ekf.aid_src_mag().innovation[2]; #endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_DRAG_FUSION) - _ekf.getDragInnov(innovations.drag); -#endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnov(innovations.airspeed); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnov(innovations.beta); -#endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_GRAVITY_FUSION) - _ekf.getGravityInnov(innovations.gravity); + // gravity + innovations.gravity[0] = _ekf.aid_src_gravity().innovation[0]; + innovations.gravity[1] = _ekf.aid_src_gravity().innovation[1]; + innovations.gravity[2] = _ekf.aid_src_gravity().innovation[2]; #endif // CONFIG_EKF2_GRAVITY_FUSION -#if defined(CONFIG_EKF2_TERRAIN) -# if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnov(innovations.hagl); -# endif //CONFIG_EKF2_RANGE_FINDER -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getTerrainFlowInnov(innovations.terr_flow); -# endif // CONFIG_EKF2_OPTICAL_FLOW -#endif // CONFIG_EKF2_TERRAIN +#if defined(CONFIG_EKF2_DRAG_FUSION) + // drag + innovations.drag[0] = _ekf.aid_src_drag().innovation[0]; + innovations.drag[1] = _ekf.aid_src_drag().innovation[1]; +#endif // CONFIG_EKF2_DRAG_FUSION - // Not yet supported - innovations.aux_vvel = NAN; +#if defined(CONFIG_EKF2_AIRSPEED) + // airspeed + innovations.airspeed = _ekf.aid_src_airspeed().innovation; +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) + // beta + innovations.beta = _ekf.aid_src_sideslip().innovation; +#endif // CONFIG_EKF2_SIDESLIP + +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) + // hagl + innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation; +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // hagl_rate + innovations.hagl_rate = _ekf.getHaglRateInnov(); +#endif // CONFIG_EKF2_RANGE_FINDER innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); @@ -1500,55 +1540,93 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) // publish estimator innovation test ratio data estimator_innovations_s test_ratios{}; test_ratios.timestamp_sample = _ekf.time_delayed_us(); + #if defined(CONFIG_EKF2_GNSS) - _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], - test_ratios.gps_vpos); + // GPS + test_ratios.gps_hvel[0] = _ekf.aid_src_gnss_vel().test_ratio[0]; + test_ratios.gps_hvel[1] = _ekf.aid_src_gnss_vel().test_ratio[1]; + test_ratios.gps_vvel = _ekf.aid_src_gnss_vel().test_ratio[2]; + test_ratios.gps_hpos[0] = _ekf.aid_src_gnss_pos().test_ratio[0]; + test_ratios.gps_hpos[1] = _ekf.aid_src_gnss_pos().test_ratio[1]; + test_ratios.gps_vpos = _ekf.aid_src_gnss_hgt().test_ratio; #endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); + // External Vision + test_ratios.ev_hvel[0] = _ekf.aid_src_ev_vel().test_ratio[0]; + test_ratios.ev_hvel[1] = _ekf.aid_src_ev_vel().test_ratio[1]; + test_ratios.ev_vvel = _ekf.aid_src_ev_vel().test_ratio[2]; + test_ratios.ev_hpos[0] = _ekf.aid_src_ev_pos().test_ratio[0]; + test_ratios.ev_hpos[1] = _ekf.aid_src_ev_pos().test_ratio[1]; + test_ratios.ev_vpos = _ekf.aid_src_ev_hgt().test_ratio; #endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_BAROMETER) - _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); -#endif // CONFIG_EKF2_BAROMETER + + // Height sensors #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); - _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); + test_ratios.rng_vpos = _ekf.aid_src_rng_hgt().test_ratio; #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_BAROMETER) + test_ratios.baro_vpos = _ekf.aid_src_baro_hgt().test_ratio; +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_AUXVEL) - _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); + // Auxiliary velocity + test_ratios.aux_hvel[0] = _ekf.aid_src_aux_vel().test_ratio[0]; + test_ratios.aux_hvel[1] = _ekf.aid_src_aux_vel().test_ratio[1]; #endif // CONFIG_EKF2_AUXVEL + #if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getFlowInnovRatio(test_ratios.flow[0]); + // Optical flow + test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0]; + test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1]; +# if defined(CONFIG_EKF2_TERRAIN) + test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0]; + test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1]; +# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW - _ekf.getHeadingInnovRatio(test_ratios.heading); + + // heading + test_ratios.heading = _ekf.getHeadingInnovRatio(); + #if defined(CONFIG_EKF2_MAGNETOMETER) - _ekf.getMagInnovRatio(test_ratios.mag_field[0]); + // mag_field + test_ratios.mag_field[0] = _ekf.aid_src_mag().test_ratio[0]; + test_ratios.mag_field[1] = _ekf.aid_src_mag().test_ratio[1]; + test_ratios.mag_field[2] = _ekf.aid_src_mag().test_ratio[2]; #endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_DRAG_FUSION) - _ekf.getDragInnovRatio(&test_ratios.drag[0]); -#endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnovRatio(test_ratios.airspeed); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnovRatio(test_ratios.beta); -#endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_GRAVITY_FUSION) - _ekf.getGravityInnovRatio(test_ratios.gravity[0]); + // gravity + test_ratios.gravity[0] = _ekf.aid_src_gravity().test_ratio[0]; + test_ratios.gravity[1] = _ekf.aid_src_gravity().test_ratio[1]; + test_ratios.gravity[2] = _ekf.aid_src_gravity().test_ratio[2]; #endif // CONFIG_EKF2_GRAVITY_FUSION -#if defined(CONFIG_EKF2_TERRAIN) -# if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnovRatio(test_ratios.hagl); -# endif -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); -# endif // CONFIG_EKF2_OPTICAL_FLOW -#endif // CONFIG_EKF2_TERRAIN +#if defined(CONFIG_EKF2_DRAG_FUSION) + // drag + test_ratios.drag[0] = _ekf.aid_src_drag().test_ratio[0]; + test_ratios.drag[1] = _ekf.aid_src_drag().test_ratio[1]; +#endif // CONFIG_EKF2_DRAG_FUSION - // Not yet supported - test_ratios.aux_vvel = NAN; +#if defined(CONFIG_EKF2_AIRSPEED) + // airspeed + test_ratios.airspeed = _ekf.aid_src_airspeed().test_ratio; +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) + // beta + test_ratios.beta = _ekf.aid_src_sideslip().test_ratio; +#endif // CONFIG_EKF2_SIDESLIP + +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) + // hagl + test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio; +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // hagl_rate + test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio(); +#endif // CONFIG_EKF2_RANGE_FINDER test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovation_test_ratios_pub.publish(test_ratios); @@ -1559,54 +1637,93 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) // publish estimator innovation variance data estimator_innovations_s variances{}; variances.timestamp_sample = _ekf.time_delayed_us(); -#if defined(CONFIG_EKF2_GNSS) - _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos); -#endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); -#endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_BAROMETER) - _ekf.getBaroHgtInnovVar(variances.baro_vpos); -#endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getRngHgtInnovVar(variances.rng_vpos); - _ekf.getHaglRateInnovVar(variances.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_AUXVEL) - _ekf.getAuxVelInnovVar(variances.aux_hvel); -#endif // CONFIG_EKF2_AUXVEL -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getFlowInnovVar(variances.flow); -#endif // CONFIG_EKF2_OPTICAL_FLOW - _ekf.getHeadingInnovVar(variances.heading); -#if defined(CONFIG_EKF2_MAGNETOMETER) - _ekf.getMagInnovVar(variances.mag_field); -#endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_DRAG_FUSION) - _ekf.getDragInnovVar(variances.drag); -#endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnovVar(variances.airspeed); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnovVar(variances.beta); -#endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_TERRAIN) -# if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnovVar(variances.hagl); -# endif // CONFIG_EKF2_RANGE_FINDER -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getTerrainFlowInnovVar(variances.terr_flow); -# endif // CONFIG_EKF2_OPTICAL_FLOW -#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) + // GPS + variances.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation_variance[0]; + variances.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation_variance[1]; + variances.gps_vvel = _ekf.aid_src_gnss_vel().innovation_variance[2]; + variances.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation_variance[0]; + variances.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation_variance[1]; + variances.gps_vpos = _ekf.aid_src_gnss_hgt().innovation_variance; +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + // External Vision + variances.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation_variance[0]; + variances.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation_variance[1]; + variances.ev_vvel = _ekf.aid_src_ev_vel().innovation_variance[2]; + variances.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation_variance[0]; + variances.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation_variance[1]; + variances.ev_vpos = _ekf.aid_src_ev_hgt().innovation_variance; +#endif // CONFIG_EKF2_EXTERNAL_VISION + + // Height sensors +#if defined(CONFIG_EKF2_RANGE_FINDER) + variances.rng_vpos = _ekf.aid_src_rng_hgt().innovation_variance; +#endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_BAROMETER) + variances.baro_vpos = _ekf.aid_src_baro_hgt().innovation_variance; +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_AUXVEL) + // Auxiliary velocity + variances.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation_variance[0]; + variances.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation_variance[1]; +#endif // CONFIG_EKF2_AUXVEL + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + // Optical flow + variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0]; + variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1]; +# if defined(CONFIG_EKF2_TERRAIN) + variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0]; + variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1]; +# endif // CONFIG_EKF2_TERRAIN +#endif // CONFIG_EKF2_OPTICAL_FLOW + + // heading + variances.heading = _ekf.getHeadingInnovVar(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + // mag_field + variances.mag_field[0] = _ekf.aid_src_mag().innovation_variance[0]; + variances.mag_field[1] = _ekf.aid_src_mag().innovation_variance[1]; + variances.mag_field[2] = _ekf.aid_src_mag().innovation_variance[2]; +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GRAVITY_FUSION) - _ekf.getGravityInnovVar(variances.gravity); + // gravity + variances.gravity[0] = _ekf.aid_src_gravity().innovation_variance[0]; + variances.gravity[1] = _ekf.aid_src_gravity().innovation_variance[1]; + variances.gravity[2] = _ekf.aid_src_gravity().innovation_variance[2]; #endif // CONFIG_EKF2_GRAVITY_FUSION - // Not yet supported - variances.aux_vvel = NAN; +#if defined(CONFIG_EKF2_DRAG_FUSION) + // drag + variances.drag[0] = _ekf.aid_src_drag().innovation_variance[0]; + variances.drag[1] = _ekf.aid_src_drag().innovation_variance[1]; +#endif // CONFIG_EKF2_DRAG_FUSION + +#if defined(CONFIG_EKF2_AIRSPEED) + // airspeed + variances.airspeed = _ekf.aid_src_airspeed().innovation_variance; +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) + // beta + variances.beta = _ekf.aid_src_sideslip().innovation_variance; +#endif // CONFIG_EKF2_SIDESLIP + +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) + // hagl + variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance; +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // hagl_rate + variances.hagl_rate = _ekf.getHaglRateInnovVar(); +#endif // CONFIG_EKF2_RANGE_FINDER variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovation_variances_pub.publish(variances); @@ -2043,12 +2160,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); #if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnov(wind.tas_innov); - _ekf.getAirspeedInnovVar(wind.tas_innov_var); + wind.tas_innov = _ekf.aid_src_airspeed().innovation; + wind.tas_innov_var = _ekf.aid_src_airspeed().innovation_variance; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnov(wind.beta_innov); - _ekf.getBetaInnovVar(wind.beta_innov_var); + wind.beta_innov = _ekf.aid_src_sideslip().innovation; + wind.beta_innov = _ekf.aid_src_sideslip().innovation_variance; #endif // CONFIG_EKF2_SIDESLIP wind.windspeed_north = wind_vel(0); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 99ce71674e..ba1c842ebe 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -245,22 +245,16 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) _sensor_simulator.runSeconds(1); - float hpos = 0.f; - float vpos = 0.f; - float hvel = 0.f; - float vvel = 0.f; - float baro_vpos = 0.f; - // After the change of origin, the pos and vel innovations should stay small - _ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos); - _ekf->getBaroHgtInnovRatio(baro_vpos); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f); - EXPECT_NEAR(hpos, 0.f, 0.05f); - EXPECT_NEAR(vpos, 0.f, 0.05f); - EXPECT_NEAR(baro_vpos, 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_baro_hgt().test_ratio, 0.f, 0.05f); - EXPECT_NEAR(hvel, 0.f, 0.02f); - EXPECT_NEAR(vvel, 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f); } TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) @@ -291,19 +285,14 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) _sensor_simulator.runSeconds(1); - float hpos = 0.f; - float vpos = 0.f; - float hvel = 0.f; - float vvel = 0.f; - // After the change of origin, the pos and vel innovations should stay small - _ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f); - EXPECT_NEAR(hpos, 0.f, 0.05f); - EXPECT_NEAR(vpos, 0.f, 0.05f); - - EXPECT_NEAR(hvel, 0.f, 0.02f); - EXPECT_NEAR(vvel, 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f); } TEST_F(EkfBasicsTest, global_position_from_local_ev) diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 1669b12180..a18c3fdf02 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -222,8 +222,7 @@ TEST_F(EkfGpsTest, altitudeDrift) _sensor_simulator.runSeconds(dt); } - float baro_innov; - _ekf->getBaroHgtInnov(baro_innov); + float baro_innov = _ekf->aid_src_baro_hgt().innovation; BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus(); printf("baro innov = %f\n", (double)baro_innov); diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index a9d70bb1d9..f83ad67cd4 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -198,13 +198,8 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f); // and the innovations are close to zero - float baro_innov = NAN; - _ekf->getBaroHgtInnov(baro_innov); - EXPECT_NEAR(baro_innov, 0.f, 0.2f); - - float rng_innov = NAN; - _ekf->getRngHgtInnov(rng_innov); - EXPECT_NEAR(rng_innov, 0.f, 0.2f); + EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f); + EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f); } TEST_F(EkfHeightFusionTest, baroRefFailOver)