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

mag test: add ellipsoid test

Browse Source
This commit is contained in:
bresch 2021-01-20 16:02:02 +01:00 committed by Lorenz Meier
parent 1da2cd5b4d
commit c79bd033c0
1 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/modules/commander/mag_calibration_test.cpp
View File

@ -235,7 +235,7 @@ TEST_F(MagCalTest, replayTestData)
const float mag_str_true = 0.4f;
const Vector3f offset_true = {-0.18f, 0.05f, -0.58f};
const Vector3f scale_true = {1.f, 1.f, 1.f};
const Vector3f scale_true = {1.f, 1.06f, 0.94f};
float fitness = 1.0e30f;
float sphere_lambda = 1.f;
@ -274,14 +274,37 @@ TEST_F(MagCalTest, replayTestData)
}
}
printf("Ellipsoid fit\n");
bool ellipsoid_fit_success = false;
for (int i = 0; i < 100; i++) {
const bool ret = run_lm_ellipsoid_fit(mag_data1_x, mag_data1_y, mag_data1_z,
fitness, sphere_lambda, N_SAMPLES,
&offset(0), &offset(1), &offset(2),
&sphere_radius,
&diag(0), &diag(1), &diag(2),
&offdiag(0), &offdiag(1), &offdiag(2));
printf("fitness: %.6f\t sphere_lambda: %.3f\t radius: %.3f\n",
(double)fitness, (double)sphere_lambda, (double)sphere_radius);
if (ret == 0) {
ellipsoid_fit_success = true;
} else if (ellipsoid_fit_success) {
break;
}
}
// THEN: the algorithm should converge and find the correct parameters
EXPECT_TRUE(sphere_fit_success);
EXPECT_TRUE(ellipsoid_fit_success);
EXPECT_LT(fitness, 1e-3f);
EXPECT_NEAR(sphere_radius, mag_str_true, 0.1f) << "radius: " << sphere_radius;
EXPECT_NEAR(offset(0), offset_true(0), 0.01f) << "offset X: " << offset(0);
EXPECT_NEAR(offset(1), offset_true(1), 0.01f) << "offset Y: " << offset(1);
EXPECT_NEAR(offset(2), offset_true(2), 0.01f) << "offset Z: " << offset(2);
EXPECT_NEAR(diag(0), scale_true(0), 0.01f) << "scale X: " << scale_true(0);
EXPECT_NEAR(diag(1), scale_true(1), 0.01f) << "scale Y: " << scale_true(1);
EXPECT_NEAR(diag(2), scale_true(2), 0.01f) << "scale Z: " << scale_true(2);
EXPECT_NEAR(diag(0), scale_true(0), 0.01f) << "scale X: " << diag(0);
EXPECT_NEAR(diag(1), scale_true(1), 0.01f) << "scale Y: " << diag(1);
EXPECT_NEAR(diag(2), scale_true(2), 0.01f) << "scale Z: " << diag(2);
}