diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 705a5f9a71fd..18efd055b332 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -47,6 +47,7 @@ bool cs_rng_terrain # 39 - true if we are fusing range finder data f bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused bool cs_constant_pos # 42 - true if the vehicle is at a constant position +bool cs_rng_kin_unknown # 43 - true when the range finder kinematic consistency check is not running # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index e6b520b60024..a57d8aeed61e 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -36,56 +36,119 @@ */ #include +#include "ekf_derivation/generated/range_validation_filter.h" -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, - bool horizontal_motion, uint64_t time_us) +using namespace matrix; + +void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom, + const float &dist_bottom_var) { - if (horizontal_motion) { - _time_last_horizontal_motion = time_us; - } + float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var}; + _P = SquareMatrix(p); + _H = sym::RangeValidationFilter(); + _x(RangeFilter::z.idx) = z; + _x(RangeFilter::terrain.idx) = z - dist_bottom; + _initialized = true; + _state = KinematicState::UNKNOWN; + _test_ratio_lpf.reset(2.f); + _t_since_first_sample = 0.f; + _test_ratio_lpf.setAlpha(0.2f); +} +void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, const float &vz, const float &vz_var, + const float &dist_bottom, const float &dist_bottom_var, const uint64_t &time_us) +{ const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; - if ((_time_last_update_us == 0) - || (dt < 0.001f) || (dt > 0.5f)) { + if (_time_last_update_us == 0 || dt > 1.f) { _time_last_update_us = time_us; - _dist_bottom_prev = dist_bottom; + init(z, z_var, dist_bottom, dist_bottom_var); return; } - const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt; - _innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down + _time_last_update_us = time_us; - // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 - const float var = 2.f * dist_bottom_var / (dt * dt); - _innov_var = var + vz_var; + _x(RangeFilter::z.idx) -= dt * vz; + _P(0, 0) += dt * dt * vz_var + 0.001f; + _P(1, 1) += terrain_process_noise; - const float normalized_innov_sq = (_innov * _innov) / _innov_var; - _test_ratio = normalized_innov_sq / (_gate * _gate); - _signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau); - const float signed_test_ratio = matrix::sign(_innov) * _test_ratio; - _signed_test_ratio_lpf.update(signed_test_ratio); + const Vector2f measurements(z, dist_bottom); - updateConsistency(vz, time_us); + Vector2f Kv{1.f, 0.f}; + Vector2f test_ratio{0.f, 0.f}; + Vector2f R{z_var, dist_bottom_var}; + Vector2f y; - _time_last_update_us = time_us; - _dist_bottom_prev = dist_bottom; -} + for (int i = 0; i < 2 ; i++) { + y = measurements - _H * _x; + Vector2f H = _H.row(i); + _innov_var = (H.transpose() * _P * H + R(i))(0, 0); + Kv(RangeFilter::terrain.idx) = _P(RangeFilter::terrain.idx, i) / _innov_var; -void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) -{ - if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { - if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { - _is_kinematically_consistent = false; - _time_last_inconsistent_us = time_us; + Vector2f PH = _P.row(i); + + for (int u = 0; u < RangeFilter::size; u++) { + for (int v = 0; v < RangeFilter::size; v++) { + _P(u, v) -= Kv(u) * PH(v); + } } - } else { - if ((fabsf(vz) > _min_vz_for_valid_consistency) - && (_test_ratio < 1.f) - && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us) - ) { - _is_kinematically_consistent = true; + PH = _P.col(i); + + for (int u = 0; u < RangeFilter::size; u++) { + for (int v = 0; v <= u; v++) { + _P(u, v) = _P(u, v) - PH(u) * Kv(v) + Kv(u) * R(i) * Kv(v); + _P(v, u) = _P(u, v); + } + } + + test_ratio(i) = fminf(sq(y(i)) / sq(_gate), 4.f); + + if (i == (int)RangeFilter::z.idx && test_ratio(1) > 1.f) { + Kv(1) = 0.f; } + + _x = _x + Kv * y; } + + _innov = y(RangeFilter::terrain.idx); + _test_ratio_lpf.update(sign(_innov) * test_ratio(1)); + + // start the consistency check after 1s + if (_t_since_first_sample + dt > 1.0f) { + _t_since_first_sample = 2.0f; + + if (abs(_test_ratio_lpf.getState()) < 1.f) { + const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); + + if (!horizontal_motion && vertical_motion) { + _state = KinematicState::CONSISTENT; + + } else { + _state = KinematicState::UNKNOWN; + } + + } else { + _t_since_first_sample = 0.f; + _state = KinematicState::INCONSISTENT; + } + + } else { + _t_since_first_sample += dt; + } +} + +void RangeFinderConsistencyCheck::run(const float &z, const float &vz, + const matrix::SquareMatrix &P, + const float &dist_bottom, const float &dist_bottom_var, uint64_t time_us) +{ + const float z_var = P(estimator::State::pos.idx + 2, estimator::State::pos.idx + 2); + const float vz_var = P(estimator::State::vel.idx + 2, estimator::State::vel.idx + 2); + + if (!_initialized || current_posD_reset_count != _last_posD_reset_count) { + _last_posD_reset_count = current_posD_reset_count; + init(z, z_var, dist_bottom, dist_bottom_var); + } + + update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us); } diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index d031e12d975b..1efcc1067ad9 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -41,6 +41,8 @@ #define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP #include +#include + class RangeFinderConsistencyCheck final { @@ -48,36 +50,54 @@ class RangeFinderConsistencyCheck final RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; - void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us); + enum class KinematicState : int { + INCONSISTENT = 0, + CONSISTENT = 1, + UNKNOWN = 2 + }; - void setGate(float gate) { _gate = gate; } + float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; } + float getInnov() const { return _initialized ? _innov : 0.f; } + float getInnovVar() const { return _initialized ? _innov_var : 0.f; } + bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } + bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; } + void setGate(const float gate) { _gate = gate; } + void run(const float &z, const float &vz, const matrix::SquareMatrix &P, + const float &dist_bottom, const float &dist_bottom_var, uint64_t time_us); + void reset() + { + _state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state; + _initialized = false; + } - float getTestRatio() const { return _test_ratio; } - float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); } - float getInnov() const { return _innov; } - float getInnovVar() const { return _innov_var; } - bool isKinematicallyConsistent() const { return _is_kinematically_consistent; } + uint8_t current_posD_reset_count{0}; + float terrain_process_noise{0.0f}; + bool horizontal_motion{false}; private: - void updateConsistency(float vz, uint64_t time_us); - - uint64_t _time_last_update_us{}; - float _dist_bottom_prev{}; - - float _test_ratio{}; - AlphaFilter _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data - float _gate{.2f}; - float _innov{}; - float _innov_var{}; - bool _is_kinematically_consistent{true}; - uint64_t _time_last_inconsistent_us{}; - uint64_t _time_last_horizontal_motion{}; - - static constexpr float _signed_test_ratio_tau = 2.f; + void update(const float &z, const float &z_var, const float &vz, const float &vz_var, const float &dist_bottom, + const float &dist_bottom_var, const uint64_t &time_us); + void init(const float &z, const float &z_var, const float &dist_bottom, const float &dist_bottom_var); + matrix::SquareMatrix _P{}; + matrix::SquareMatrix _H{}; + matrix::Vector2f _x{}; + bool _initialized{false}; + float _innov{0.f}; + float _innov_var{0.f}; + uint64_t _time_last_update_us{0}; + AlphaFilter _test_ratio_lpf{}; + float _gate{1.f}; + KinematicState _state{KinematicState::UNKNOWN}; + float _t_since_first_sample{0.f}; + uint8_t _last_posD_reset_count{0}; +}; - static constexpr float _min_vz_for_valid_consistency = .5f; - static constexpr uint64_t _consistency_hyst_time_us = 1e6; +namespace RangeFilter +{ +static constexpr estimator::IdxDof z{0, 1}; +static constexpr estimator::IdxDof terrain{1, 1}; +static constexpr uint8_t size{2}; }; #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index e6cd566a3fef..11610763db85 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -56,6 +56,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); _range_sensor.setMaxFogDistance(_params.rng_fog); + _rng_consistency_check.setGate(_params.range_kin_consistency_gate); _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); @@ -66,30 +67,38 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); if (_control_status.flags.in_air) { - const bool horizontal_motion = _control_status.flags.fixed_wing - || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); + const float dist_var = getRngVar(); + _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); - const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - const float var = sq(_params.range_noise) + dist_dependant_var; + const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); - _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), - P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us); + if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { + _rng_consistency_check.reset(); + } + + _rng_consistency_check.horizontal_motion = updated_horizontal_motion; + _rng_consistency_check.run(_gpos.altitude(), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us); } } else { // If we are supposed to be using range finder data but have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_control_status.flags.in_air - && _range_sensor.isRegularlySendingData() + if (_range_sensor.isRegularlySendingData() && _range_sensor.isDataReady()) { + if (!_control_status.flags.in_air) { + + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setValidity(true); // bypass the checks - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setValidity(true); // bypass the checks + } else { + _rng_consistency_check.reset(); + } } } _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() + && _rng_consistency_check.isNotKinematicallyInconsistent(); } else { return; @@ -107,13 +116,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && _control_status.flags.tilt_align && measurement_valid && _range_sensor.isDataHealthy() - && _rng_consistency_check.isKinematicallyConsistent(); + && _rng_consistency_check.isNotKinematicallyInconsistent(); const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) && _range_sensor.isRegularlySendingData(); - const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); @@ -137,7 +145,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _control_status.flags.rng_hgt = true; stopRngTerrFusion(); - if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected + && _rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } @@ -163,7 +172,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) ECL_INFO("starting %s height fusion", HGT_SRC_NAME); _control_status.flags.rng_hgt = true; - if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected + && _rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } @@ -200,14 +210,14 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) stopRngHgtFusion(); stopRngTerrFusion(); - } else { + } else if (_rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } } } else { - ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); } @@ -221,7 +231,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - if (aid_src.innovation_rejected) { + if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } @@ -268,11 +278,9 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) float Ekf::getRngVar() const { - return fmaxf( - P(State::pos.idx + 2, State::pos.idx + 2) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()), - 0.f); + const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); + const float dist_var = sq(_params.range_noise) + dist_dependant_var; + return dist_var; } void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index c0ae2a71ebf6..e0fcd319ab21 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -81,10 +81,10 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) } else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) { // We did not receive bad quality data for some time + updateFogCheck(_sample.rng, _sample.time_us); if (isTiltOk() && isDataInRange()) { updateStuckCheck(); - updateFogCheck(getDistBottom(), _sample.time_us); if (!_is_stuck && !_is_blocked) { _is_sample_valid = true; @@ -162,6 +162,9 @@ void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t t } _prev_median_dist = median_dist; + + } else { + _prev_median_dist = dist_bottom; } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 85d075e0f5cf..612c6d93ac3c 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -620,6 +620,7 @@ uint64_t mag_heading_consistent : uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position + uint64_t rng_kin_unknown : 1; ///< 43 - true when the range finder kinematic consistency check is not running } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 0c9a5df6cba2..55e959777aaa 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -226,6 +226,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // process noise due to terrain gradient terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( 1))); + _rng_consistency_check.terrain_process_noise = terrain_process_noise; P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8df41e6ba823..c25e02b3d1ea 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -116,7 +116,7 @@ class Ekf final : public EstimatorInterface float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); } float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); } - float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); } + float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 0ad3fef4ed0e..040cab0811be 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -725,6 +725,20 @@ def compute_gravity_z_innov_var_and_h( return (innov_var, H.T) +def range_validation_filter() -> sf.Matrix: + + state = Values( + z=sf.Symbol("z"), + terrain=sf.Symbol("terrain") + ) + dist_bottom = state["z"] - state["terrain"] + + H = sf.M22() + H[0, :] = sf.V1(state["z"]).jacobian(state.to_storage(), tangent_space=False) + H[1, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False) + + return H + print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=None) @@ -756,5 +770,6 @@ def compute_gravity_z_innov_var_and_h( generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"]) generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"]) generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"]) +generate_px4_function(range_validation_filter, output_names=None) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h new file mode 100644 index 000000000000..3eed6284437d --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h @@ -0,0 +1,44 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: range_validation_filter + * + * Args: + * + * Outputs: + * res: Matrix22 + */ +template +matrix::Matrix RangeValidationFilter() { + // Total ops: 0 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(0, 0) = 1; + _res(1, 0) = 1; + _res(1, 1) = -1; + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c85527b9121a..84d380d188d5 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1928,6 +1928,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; + status_flags.cs_rng_kin_unknown = _ekf.control_status_flags().rng_kin_unknown; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 3b9cd2363b7b..2fb59f0949ec 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -103,7 +103,7 @@ TEST_F(EkfHeightFusionTest, baroRef) _ekf_wrapper.enableGpsHeightFusion(); _ekf_wrapper.enableRangeHeightFusion(); /* _ekf_wrapper.enableExternalVisionHeightFusion(); */ //TODO: this currently sets the reference to EV - _sensor_simulator.runSeconds(1); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); @@ -113,8 +113,14 @@ TEST_F(EkfHeightFusionTest, baroRef) // AND WHEN: the baro data increases const float baro_increment = 4.f; - _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_increment); - _sensor_simulator.runSeconds(60); + const float baro_initial = _sensor_simulator._baro.getData(); + const float baro_inc = 0.2f; + + // increase slowly, height jump leads to invalid terrain estimate + while (_sensor_simulator._baro.getData() + baro_inc < baro_initial + baro_increment + 0.01f) { + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_inc); + _sensor_simulator.runSeconds(3); + } // THEN: the height estimate converges to the baro value // and the other height sources are getting their bias estimated @@ -125,8 +131,8 @@ TEST_F(EkfHeightFusionTest, baroRef) const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus(); EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f); - const float terrain = _ekf->getTerrainVertPos(); - EXPECT_NEAR(terrain, -baro_increment, 1.2f); + const float hagl = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain; + EXPECT_NEAR(hagl, baro_increment, 1.2f); const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus(); EXPECT_EQ(ev_status.bias, 0.f); @@ -139,7 +145,6 @@ TEST_F(EkfHeightFusionTest, baroRef) EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); - EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); // AND WHEN: the gps height increases const float gps_increment = 1.f; @@ -151,8 +156,8 @@ TEST_F(EkfHeightFusionTest, baroRef) // the estimated height follows the GPS height EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f); // and the range finder bias is adjusted to follow the new reference - const float terrain2 = _ekf->getTerrainVertPos(); - EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f); + const float hagl2 = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain; + EXPECT_NEAR(hagl2, (baro_increment + gps_increment), 1.3f); } TEST_F(EkfHeightFusionTest, gpsRef) @@ -162,7 +167,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) _ekf_wrapper.enableBaroHeightFusion(); _ekf_wrapper.enableGpsHeightFusion(); _ekf_wrapper.enableRangeHeightFusion(); - _sensor_simulator.runSeconds(1); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); @@ -174,7 +179,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus(); const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt; - EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f); + EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.8f); // AND WHEN: the baro data increases const float baro_increment = 5.f; @@ -215,7 +220,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion) _ekf_wrapper.enableBaroHeightFusion(); _ekf_wrapper.disableGpsHeightFusion(); _ekf_wrapper.enableRangeHeightFusion(); - _sensor_simulator.runSeconds(1); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); @@ -242,7 +247,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver) _ekf_wrapper.enableGpsHeightFusion(); _ekf_wrapper.enableRangeHeightFusion(); - _sensor_simulator.runSeconds(0.1); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); @@ -408,7 +413,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) reset_logging_checker.capturePostResetState(); // An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS) - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f); + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.4f); EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f); diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index 7fbf7fcd3b64..327acd2c32a4 100644 --- a/src/modules/ekf2/test/test_EKF_terrain.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset) const float new_baro_height = _sensor_simulator._baro.getData() + 50.f; _sensor_simulator._baro.setData(new_baro_height); _sensor_simulator.stopGps(); // prevent from switching to GNSS height - _sensor_simulator.runSeconds(10); + _sensor_simulator.runSeconds(100); // THEN: a height reset occurred and the estimated distance to the ground remains constant reset_logging_checker.capturePostResetState(); @@ -219,11 +219,36 @@ TEST_F(EkfTerrainTest, testRngStartInAir) { // GIVEN: rng for terrain but not flow _ekf_wrapper.disableFlowFusion(); - _ekf_wrapper.enableRangeHeightFusion(); - - const float rng_height = 18; - const float flow_height = 1.f; - runFlowAndRngScenario(rng_height, flow_height); + const float rng_height = 16.f; + + _sensor_simulator.startGps(); + _ekf->set_min_required_gps_health_time(1e6); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + const Vector3f simulated_velocity(0.5f, -1.0f, 0.0f); + + // Configure GPS simulator data + _sensor_simulator._gps.setVelocity(simulated_velocity); + _sensor_simulator._gps.setPositionRateNED(simulated_velocity); + + // Simulate flight above max range finder distance + // run for a while to let terrain uncertainty increase + _sensor_simulator._rng.setData(30.f, 100); + _sensor_simulator._rng.setLimits(0.1f, 20.f); + _sensor_simulator.startRangeFinder(); + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _sensor_simulator.runSeconds(40); + + // quick range finder change to bypass stuck-check + _sensor_simulator._rng.setData(rng_height - 1.f, 100); + _sensor_simulator.runSeconds(1); + _sensor_simulator._rng.setData(rng_height, 100); + _sensor_simulator.runSeconds(10); // THEN: the terrain should reset using rng EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f); @@ -234,13 +259,13 @@ TEST_F(EkfTerrainTest, testRngStartInAir) const float corr_terrain_vz = P(State::vel.idx + 2, State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain); - EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f); + EXPECT_TRUE(corr_terrain_vz > 0.6f); const float corr_terrain_z = P(State::pos.idx + 2, State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain); - EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f); + EXPECT_TRUE(corr_terrain_z > 0.8f); const float corr_terrain_abias_z = P(State::accel_bias.idx + 2, State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain); - EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f); + EXPECT_TRUE(corr_terrain_abias_z < -0.1f); }