fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 07:03:33 +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-19 17:34:31 +02:00
committed by Mathieu Bresciani
parent 10f9ac148f
commit 42f683fa64
3 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_covariance_prediction_generated.cpp
+1 -1
View File
@@ -811,6 +811,6 @@ TEST(CovariancePredictionGenerated, SympyVsSymforce)
} }
DiffRatioReport report = computeDiffRatioSquareMatrix24f(nextP_sympy, nextP_symforce); 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; " location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
} }
src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp
+2 -2
View File
@@ -70,7 +70,7 @@ TEST(YawFusionGenerated, singularityYawEquivalence)
// THEN: Even at the singularity point, the result is still correct, thanks to epsilon // THEN: Even at the singularity point, the result is still correct, thanks to epsilon
EXPECT_TRUE(isEqual(H_a, H_b)); EXPECT_TRUE(isEqual(H_a, H_b));
EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f); 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) TEST(YawFusionGenerated, gimbalLock321vs312)
@@ -98,7 +98,7 @@ TEST(YawFusionGenerated, gimbalLock321vs312)
// THEN: both computation are not equivalent, 321 is undefined but 312 is valid // THEN: both computation are not equivalent, 321 is undefined but 312 is valid
EXPECT_FALSE(isEqual(H_321, H_312)); EXPECT_FALSE(isEqual(H_321, H_312));
EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f); 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) TEST(YawFusionGenerated, positiveVarianceAllOrientations)
src/modules/ekf2/test/test_helper/comparison_helper.cpp
+4
View File
@@ -40,6 +40,7 @@ float randf()
SquareMatrix24f createRandomCovarianceMatrix24f() SquareMatrix24f createRandomCovarianceMatrix24f()
{ {
// Create a symmetric square matrix
SquareMatrix24f P; SquareMatrix24f P;
for (int col = 0; col <= 23; col++) { for (int col = 0; col <= 23; col++) {
@@ -53,6 +54,9 @@ SquareMatrix24f createRandomCovarianceMatrix24f()
} }
} }
// Make it positive definite
P = P.transpose() * P;
return P; return P;
} }