fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2_test: test yaw variance after reset

Browse Source
This commit is contained in:
bresch
2023-08-08 12:09:56 -04:00
committed by Daniel Agar
parent b6fb95247b
commit 39a83ab138
5 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_initialization.cpp
+8
View File
@@ -84,6 +84,12 @@ public:
EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3);
}
void yawVarianceBigEnoughAfterHeadingReset()
{
// The yaw variance is smaller than its reset value as we do not probe instantly after the reset
EXPECT_GT(sqrtf(_ekf->getYawVar()), _ekf_wrapper.getMagHeadingNoise() / 5.f);
}
void velocityAndPositionCloseToZero()
{
const Vector3f pos = _ekf->getPosition();
@@ -264,6 +270,7 @@ TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt)
initializedOrienationIsMatchingGroundTruth(quat_sim);
quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
yawVarianceBigEnoughAfterHeadingReset();
velocityAndPositionCloseToZero();
@@ -290,6 +297,7 @@ TEST_F(EkfInitializationTest, initializeWithTilt)
initializedOrienationIsMatchingGroundTruth(quat_sim);
quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
yawVarianceBigEnoughAfterHeadingReset();
velocityAndPositionCloseToZero();