74 lines
3.0 KiB
C++
74 lines
3.0 KiB
C++
#include <doctest/doctest.h>
|
|
|
|
#include "fgc/Calibration.h"
|
|
|
|
#include <cmath>
|
|
|
|
using namespace fgc;
|
|
|
|
TEST_CASE("linearFit recovers slope/intercept exactly for collinear points") {
|
|
// counts = 983.33*deg + 500000 (a yaw-like calibration)
|
|
std::vector<std::pair<double, double>> xy;
|
|
for (int deg = -45; deg <= 45; deg += 10)
|
|
xy.emplace_back(deg, 983.33 * deg + 500000.0);
|
|
|
|
LinearFit f = linearFit(xy);
|
|
REQUIRE(f.ok);
|
|
CHECK(f.slope == doctest::Approx(983.33));
|
|
CHECK(f.intercept == doctest::Approx(500000.0));
|
|
CHECK(f.r2 == doctest::Approx(1.0));
|
|
}
|
|
|
|
TEST_CASE("linearFit is robust to noise (slope close, r2 high)") {
|
|
std::vector<std::pair<double, double>> xy = {
|
|
{-40, -39500}, {-20, -20100}, {0, 300}, {20, 19800}, {40, 40200}};
|
|
LinearFit f = linearFit(xy);
|
|
REQUIRE(f.ok);
|
|
CHECK(f.slope == doctest::Approx(1000.0).epsilon(0.05));
|
|
CHECK(f.r2 > 0.99);
|
|
}
|
|
|
|
TEST_CASE("linearFit rejects degenerate input") {
|
|
CHECK_FALSE(linearFit({}).ok);
|
|
CHECK_FALSE(linearFit({{1.0, 2.0}}).ok); // single point
|
|
CHECK_FALSE(linearFit({{5.0, 1.0}, {5.0, 9.0}}).ok); // no x spread
|
|
}
|
|
|
|
TEST_CASE("circularMeanDeg handles the +/-180 wrap") {
|
|
CHECK(circularMeanDeg({10, 20, 30}) == doctest::Approx(20.0));
|
|
// Mean of 170 and -170 is 180 (not 0) — must not average naively to 0.
|
|
CHECK(std::abs(circularMeanDeg({170, -170})) == doctest::Approx(180.0));
|
|
CHECK(circularMeanDeg({}) == doctest::Approx(0.0));
|
|
}
|
|
|
|
TEST_CASE("unwrapNear shifts an angle to within +/-180 of the previous sample") {
|
|
CHECK(unwrapNear(10.0, 20.0) == doctest::Approx(20.0)); // already near
|
|
CHECK(unwrapNear(350.0, 355.0) == doctest::Approx(355.0));
|
|
CHECK(unwrapNear(350.0, 5.0) == doctest::Approx(365.0)); // crossed 360 going up
|
|
CHECK(unwrapNear(10.0, 355.0) == doctest::Approx(-5.0)); // crossed 0 going down
|
|
CHECK(unwrapNear(720.0, 10.0) == doctest::Approx(730.0)); // multiple turns up
|
|
CHECK(unwrapNear(-360.0, 10.0) == doctest::Approx(-350.0)); // multiple turns down
|
|
}
|
|
|
|
TEST_CASE("unwrapping a 0..360 sweep across the wrap yields a clean linear fit") {
|
|
// A yaw sweep whose true (unwrapped) heading is 300..420 deg, but the IMU
|
|
// reports it wrapped into 0..360. With per-sample unwrapNear the fit recovers
|
|
// the straight line; without it the 360->0 jump would wreck R^2 (the bug from
|
|
// the field calibration log).
|
|
std::vector<std::pair<double, double>> pts; // (deg, counts), counts = 100*heading
|
|
double prev = 0.0;
|
|
bool have = false;
|
|
for (int i = 0; i <= 12; ++i) {
|
|
const double heading = 300.0 + i * 10.0; // 300..420 (true)
|
|
const double reported = std::fmod(heading, 360.0); // 0..360 (wrapped)
|
|
double d = have ? unwrapNear(prev, reported) : reported;
|
|
prev = d;
|
|
have = true;
|
|
pts.emplace_back(d, heading * 100.0);
|
|
}
|
|
LinearFit f = linearFit(pts);
|
|
CHECK(f.ok);
|
|
CHECK(f.slope == doctest::Approx(100.0));
|
|
CHECK(f.r2 > 0.9999);
|
|
}
|