mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 14:17:35 +08:00
remove terrain checks form baroRef unit-tests, vehicle vertical motion is not defined in test
This commit is contained in:
@@ -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:
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user