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

ekf2_test: compare covariance prediction sympy vs symforce

Browse Source
This commit is contained in:
bresch
2022-10-14 11:42:23 -04:00
committed by Daniel Agar
parent a4e511b90e
commit df084d65e3
4 changed files with 848 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_helper/comparison_helper.cpp
+30
View File
@@ -83,3 +83,33 @@ DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &
return report;
}
DiffRatioReport computeDiffRatioSquareMatrix24f(const SquareMatrix24f &m1, const SquareMatrix24f &m2)
{
DiffRatioReport report = {};
for (int row = 0; row < 24; row++) {
for (int col = 0; col < 24; col++) {
float diff_fraction;
if (fabsf(m1(row, col)) > FLT_EPSILON) {
diff_fraction = fabsf(m2(row, col) - m1(row, col)) / fabsf(m1(row, col));
} else if (fabsf(m2(row, col)) > FLT_EPSILON) {
diff_fraction = fabsf(m2(row, col) - m1(row, col)) / fabsf(m2(row, col));
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > report.max_diff_fraction) {
report.max_diff_fraction = diff_fraction;
report.max_row = row;
report.max_v1 = m1(row, col);
report.max_v2 = m2(row, col);
}
}
}
return report;
}
src/modules/ekf2/test/test_helper/comparison_helper.h
+1
View File
@@ -55,4 +55,5 @@ SquareMatrix24f createRandomCovarianceMatrix24f();
// Find largest element-wise difference as a fraction of v1 or v2
DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2);
DiffRatioReport computeDiffRatioSquareMatrix24f(const SquareMatrix24f &m1, const SquareMatrix24f &m2);
#endif