fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2_test: test using positive definite covariance matrix

Browse Source 
A covariance matrix needs to be positive definite
This commit is contained in:
bresch 2022-10-18 12:23:44 +02:00 committed by Mathieu Bresciani
parent 10f9ac148f
commit 42f683fa64
3 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/test/test_EKF_covariance_prediction_generated.cpp
View File

@ -811,6 +811,6 @@ TEST(CovariancePredictionGenerated, SympyVsSymforce)
}
DiffRatioReport report = computeDiffRatioSquareMatrix24f(nextP_sympy, nextP_symforce);
EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Max diff fraction = " << report.max_diff_fraction <<
EXPECT_LT(report.max_diff_fraction, 2e-5f) << "Max diff fraction = " << report.max_diff_fraction <<
" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
}

4
src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp
View File

@ -70,7 +70,7 @@ TEST(YawFusionGenerated, singularityYawEquivalence)
// THEN: Even at the singularity point, the result is still correct, thanks to epsilon
EXPECT_TRUE(isEqual(H_a, H_b));
EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f);
EXPECT_TRUE(innov_var_a < 5.f && innov_var_a > R) << "innov_var = " << innov_var_a;
EXPECT_TRUE(innov_var_a < 50.f && innov_var_a > R) << "innov_var = " << innov_var_a;
}
TEST(YawFusionGenerated, gimbalLock321vs312)
@ -98,7 +98,7 @@ TEST(YawFusionGenerated, gimbalLock321vs312)
// THEN: both computation are not equivalent, 321 is undefined but 312 is valid
EXPECT_FALSE(isEqual(H_321, H_312));
EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f);
EXPECT_TRUE(innov_var_312 < 5.f && innov_var_312 > R) << "innov_var = " << innov_var_312;
EXPECT_TRUE(innov_var_312 < 50.f && innov_var_312 > R) << "innov_var = " << innov_var_312;
}
TEST(YawFusionGenerated, positiveVarianceAllOrientations)

4
src/modules/ekf2/test/test_helper/comparison_helper.cpp
View File

@ -40,6 +40,7 @@ float randf()
SquareMatrix24f createRandomCovarianceMatrix24f()
{
// Create a symmetric square matrix
SquareMatrix24f P;
for (int col = 0; col <= 23; col++) {
@ -53,6 +54,9 @@ SquareMatrix24f createRandomCovarianceMatrix24f()
}
}
// Make it positive definite
P = P.transpose() * P;
return P;
}