remove terrain checks form baroRef unit-tests, vehicle vertical motion is not defined in test

This commit is contained in:
Marco Hauswirth
2025-04-04 19:48:50 +02:00
parent bd8e457859
commit 88cb8875f0
5 changed files with 8 additions and 15 deletions
@@ -80,18 +80,16 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
float R;
Vector2f H;
// dist_bottom
Vector2f H = _Ht;
float R = dist_bottom_var;
// z, direct state measurement
if (measurement_idx == 0) {
// direct state measurement
H(RangeFilter::z.idx) = 1.f;
H(RangeFilter::terrain.idx) = 0.f;
R = z_var;
} else if (measurement_idx == 1) {
H = _Ht;
R = dist_bottom_var;
}
// residual
@@ -42,7 +42,6 @@
#include <mathlib/math/filter/AlphaFilter.hpp>
class RangeFinderConsistencyCheck final
{
public:
-1
View File
@@ -623,7 +623,6 @@ uint64_t mag_heading_consistent :
uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
uint64_t rng_kin_unknown : 1; ///< 45 - true when the range finder kinematic consistency check is not running
} flags;
uint64_t value;
};
+4 -1
View File
@@ -226,8 +226,11 @@ 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.set_terrain_process_noise(terrain_process_noise);
P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
#if defined(CONFIG_EKF2_RANGE_FINDER)
_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
#endif // CONFIG_EKF2_RANGE_FINDER
}
#endif // CONFIG_EKF2_TERRAIN
@@ -125,9 +125,6 @@ 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 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);
@@ -149,9 +146,6 @@ TEST_F(EkfHeightFusionTest, baroRef)
EXPECT_EQ(gps_status.bias, _ekf->getGpsHgtBiasEstimatorStatus().bias);
// 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 hagl2 = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain;
EXPECT_NEAR(hagl2, (baro_increment + gps_increment), 1.3f);
}
TEST_F(EkfHeightFusionTest, gpsRef)