fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-15 13:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add gps altitude drift test

Browse Source
This commit is contained in:
bresch
2022-08-31 15:01:02 +02:00
committed by Daniel Agar
parent a2a5093881
commit 92fbd86b46
2 changed files with 25 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_accelerometer.cpp
+1 -1
View File
@@ -101,7 +101,7 @@ TEST_F(EkfAccelerometerTest, biasEstimatePositive)
TEST_F(EkfAccelerometerTest, biasEstimateNegative)
{
const float biases[4] = {-0.14f, -0.24f, -0.31, -0.4f};
const float biases[4] = {-0.12f, -0.22f, -0.31, -0.4f};
for (int i = 0; i < 4; i ++) {
testBias(biases[i], 10, 0.03f);
src/modules/ekf2/test/test_EKF_gps.cpp
+24
View File
@@ -172,3 +172,27 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
}
TEST_F(EkfGpsTest, altitudeDrift)
{
// GIVEN: a drifting GNSS altitude
const float dt = 0.2f;
const float height_rate = 0.15f;
const float duration = 80.f;
// WHEN: running on ground
for (int i = 0; i < (duration / dt); i++) {
_sensor_simulator._gps.stepHeightByMeters(height_rate * dt);
_sensor_simulator.runSeconds(dt);
}
float baro_innov;
_ekf->getBaroHgtInnov(baro_innov);
BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
printf("baro innov = %f\n", (double)baro_innov);
printf("bias: %f, innov bias = %f\n", (double)status.bias, (double)status.innov);
// THEN: the baro and local position should follow it
EXPECT_LT(fabsf(baro_innov), 0.1f);
}