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 c83debd64b
commit 85b116f14e
4 changed files with 8 additions and 14 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:
+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.ekf2_terr_grad) * (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)