dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: add gps altitude drift test

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/test/test_EKF_accelerometer.cpp
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);

24
src/modules/ekf2/test/test_EKF_gps.cpp
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);
}