From debb6152dd8151315875abc34da13030b9d2c691 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 13 Dec 2024 14:13:40 +0100 Subject: [PATCH] overhaul approach to work on unit tests and snow/blocking/normal flights --- .../range_finder_consistency_check.cpp | 110 +++++++++++------- .../range_finder_consistency_check.hpp | 44 +++---- .../range_finder/range_height_control.cpp | 14 ++- .../range_finder/sensor_range_finder.cpp | 5 +- .../EKF/python/ekf_derivation/derivation.py | 18 ++- .../generated/range_validation_filter.h | 4 +- .../ekf2/test/test_EKF_height_fusion.cpp | 17 ++- src/modules/ekf2/test/test_EKF_terrain.cpp | 39 +++++-- 8 files changed, 144 insertions(+), 107 deletions(-) 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 533005f56385..b1505190c30b 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 @@ -40,94 +40,114 @@ using namespace matrix; -void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z, float dist_bottom) +void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom, + const float &dist_bottom_var) { - _R.setZero(); - _A.setIdentity(); - float p[4] = {var_z, 0.f, 0.f, var_terrain}; + float p[4] = {z_var, 0.f, 0.f, z_var + dist_bottom_var}; _P = Matrix(p); sym::RangeValidationFilter(&_H); _x(RangeFilter::z.idx) = z; - _x(RangeFilter::terrain.idx) = z + dist_bottom; + _x(RangeFilter::terrain.idx) = z - dist_bottom; _initialized = true; - _sample_count = 0; - _state = KinematicState::UNKNOWN; + _state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT; + _test_ratio_lpf.reset(2.f); + _t_since_first_sample = 0.f; + _test_ratio_lpf.setAlpha(0.2f); } -void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float vz_var, float dist_bottom, - float dist_bottom_var, uint64_t time_us) +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 > 1.f) { _time_last_update_us = time_us; + init(z, z_var, dist_bottom, dist_bottom_var); return; } - if (_min_nr_of_samples == 0) { - _min_nr_of_samples = (int)(1.f / dt); - } - _time_last_update_us = time_us; - _R(RangeFilter::z.idx, RangeFilter::z.idx) = z_var; - _R(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var; + _x(RangeFilter::z.idx) -= dt * vz; + _P(0, 0) += dt * dt * vz_var + 0.001f; + _P(1, 1) += terrain_process_noise; - SquareMatrix Q; - Q(RangeFilter::z.idx, RangeFilter::z.idx) = dt * dt * vz_var + 0.001f; - Q(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = terrain_process_noise; + const Vector2f measurements(z, dist_bottom); - _x(RangeFilter::z.idx) += dt * vz; - _P = _A * _P * _A.transpose() + Q; + Vector2f Kv{1.f, 0.f}; + Vector2f test_ratio{0.f, 0.f}; + Vector2f R{z_var, dist_bottom_var}; + Vector2f y; - const Vector2f measurements(z, dist_bottom); - const Vector2f y = measurements - _H * _x; - const Matrix2f S = _H * _P * _H.transpose() + _R; - const float normalized_residual = (y.transpose() * S.I() * y)(0, 0); - float test_ratio = fminf(normalized_residual / sq(_gate), 2.f); + 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; + + 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); + } + } - Matrix2f K = _P * _H.transpose() * S.I(); + PH = _P.col(i); - K(RangeFilter::z.idx, RangeFilter::z.idx) = 1.f; - K(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f; + 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; + } - if (test_ratio > 1.f) { - K(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f; - K(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = 0.f; + _x = _x + Kv * y; } - _x = _x + K * y; - _P = _P - K * _H * _P; - _P = 0.5f * (_P + _P.transpose()); // Ensure symmetry _innov = y(RangeFilter::terrain.idx); - _innov_var = S(RangeFilter::terrain.idx, RangeFilter::terrain.idx); + _test_ratio_lpf.update(sign(_innov) * test_ratio(1)); - _test_ratio_lpf.update(sign(_innov) * test_ratio); - - if (_sample_count++ > _min_nr_of_samples) { + // 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) { - _state = KinematicState::CONSISTENT; + const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); + + if (!horizontal_motion && vertical_motion) { + _state = KinematicState::CONSISTENT; + + } else { + _state = KinematicState::UNKNOWN; + } } else { - _sample_count = 0; + _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) +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; - const float terrain_var = P(estimator::State::terrain.idx, estimator::State::terrain.idx); - init(z_var, terrain_var, z, dist_bottom); + 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 fe2a7bb7c410..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 @@ -56,26 +56,19 @@ class RangeFinderConsistencyCheck final UNKNOWN = 2 }; - float getTestRatioLpf() const { return _test_ratio_lpf.getState(); } - float getInnov() const { return _innov; } - float getInnovVar() const { return _innov_var; } - + 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 || _fixed_wing; } + 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() { - if (_initialized) { - if (_state == KinematicState::CONSISTENT) { - _state = KinematicState::UNKNOWN; - } - - _initialized = false; - } + _state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state; + _initialized = false; } - int getConsistencyState() const { return static_cast(_state); } - - 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); uint8_t current_posD_reset_count{0}; float terrain_process_noise{0.0f}; @@ -83,25 +76,20 @@ class RangeFinderConsistencyCheck final private: - void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, - uint64_t time_us); - void init(float var_z, float var_terrain, float z, float dist_bottom); - matrix::SquareMatrix _R{}; + 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 _A{}; matrix::SquareMatrix _H{}; matrix::Vector2f _x{}; bool _initialized{false}; - float _innov{}; - float _innov_var{}; + float _innov{0.f}; + float _innov_var{0.f}; uint64_t _time_last_update_us{0}; - float _dist_bottom_prev{}; - AlphaFilter _test_ratio_lpf{0.3}; + AlphaFilter _test_ratio_lpf{}; float _gate{1.f}; - int _sample_count{0}; KinematicState _state{KinematicState::UNKNOWN}; - int _min_nr_of_samples{0}; - bool _fixed_wing{false}; + float _t_since_first_sample{0.f}; uint8_t _last_posD_reset_count{0}; }; 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 8cc83f1f1275..799085de64bf 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); @@ -69,11 +70,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const float dist_var = getRngVar(); _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); - const bool updated_horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); + const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); - if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { + if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion + && _rng_consistency_check.isNotKinematicallyInconsistent()) { _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); } @@ -94,8 +97,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } - _control_status.flags.rng_kin_consistent = _rng_consistency_check.getConsistencyState() == 1; - _control_status.flags.rng_kin_unknown = _rng_consistency_check.getConsistencyState() == 2; + _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; @@ -228,7 +232,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); } 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/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 6035f706d090..0e682239194b 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -725,19 +725,17 @@ def compute_gravity_z_innov_var_and_h( return (innov_var, H.T) -def range_validation_filter() -> sf.matrix: +def range_validation_filter() -> sf.Matrix: - z = sf.Symbol("z") - dist_bottom = sf.Symbol("dist_bottom") - terrain = dist_bottom - z - state = sf.V2.symbolic("state") - state_indices = {"z": 0, "dist_bottom": 1} - state[state_indices["z"]] = z - state[state_indices["dist_bottom"]] = dist_bottom + state = Values( + z=sf.Symbol("z"), + terrain=sf.Symbol("terrain") + ) + dist_bottom = state["z"] - state["terrain"] H = sf.M22() - H[0, :] = sf.V1(z).jacobian(state, tangent_space=False) - H[1, :] = sf.V1(terrain).jacobian(state, tangent_space=False) + 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 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 index de8fed34ac77..9e8028e02f7b 100644 --- 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 @@ -35,8 +35,8 @@ void RangeValidationFilter(matrix::Matrix* const H = nullptr) { _h.setZero(); _h(0, 0) = 1; - _h(1, 0) = -1; - _h(1, 1) = 1; + _h(1, 0) = 1; + _h(1, 1) = -1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 0171ed1b26e5..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()); @@ -131,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); @@ -145,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; @@ -157,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) @@ -168,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()); @@ -221,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()); @@ -248,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); diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index b8d807085796..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); @@ -242,5 +267,5 @@ TEST_F(EkfTerrainTest, testRngStartInAir) const float corr_terrain_abias_z = P(State::accel_bias.idx + 2, State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain); - EXPECT_TRUE(corr_terrain_abias_z < -0.4f); + EXPECT_TRUE(corr_terrain_abias_z < -0.1f); }