From 84e3a22baa32480b48e26ebc8657ee5e4280b3ca Mon Sep 17 00:00:00 2001 From: HarryXChen <51322624+HarryXChen3@users.noreply.github.com> Date: Fri, 12 Jan 2024 20:05:50 -0500 Subject: [PATCH] [sysid] Fix peak acceleration filtering behavior in dynamic velocity test (#6207) --- .../native/cpp/analysis/FilteringUtils.cpp | 7 +- .../test/native/cpp/analysis/FilterTest.cpp | 101 +++++++++++++++--- 2 files changed, 92 insertions(+), 16 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp index 838968fc49..103dc8a458 100644 --- a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp +++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include using namespace sysid; @@ -138,7 +139,11 @@ sysid::TrimStepVoltageData(std::vector* data, auto maxAccel = std::max_element( data->begin(), data->end(), [](const auto& a, const auto& b) { - return std::abs(a.acceleration) < std::abs(b.acceleration); + // Since we don't know if its a forward or backwards test here, we use + // the sign of each point's velocity to determine how to compare their + // accelerations. + return wpi::sgn(a.velocity) * a.acceleration < + wpi::sgn(b.velocity) * b.acceleration; }); units::second_t velocityDelay; diff --git a/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/sysid/src/test/native/cpp/analysis/FilterTest.cpp index 8cfebe4f2a..099bebc98a 100644 --- a/sysid/src/test/native/cpp/analysis/FilterTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FilterTest.cpp @@ -43,25 +43,96 @@ TEST(FilterTest, NoiseFloor) { EXPECT_NEAR(0.953, noiseFloor, 0.001); } +void FillStepVoltageData(std::vector& data) { + auto previousDatum = data.front(); + for (size_t i = 1; i < data.size(); ++i) { + auto& datum = data.at(i); + datum.timestamp = previousDatum.timestamp + previousDatum.dt; + datum.position = 0.5 * previousDatum.acceleration * + units::math::pow<2>(previousDatum.dt).value() + + previousDatum.velocity * previousDatum.dt.value() + + previousDatum.position; + datum.velocity = previousDatum.velocity + + previousDatum.acceleration * previousDatum.dt.value(); + + previousDatum = datum; + } +} + TEST(FilterTest, StepTrim) { - std::vector testData = { - {0_s, 1, 2, 0, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 0.25, 0}, - {2_s, 1, 2, 0, 5_ms, 10, 0}, {3_s, 1, 2, 3, 5_ms, 0.45, 0}, - {4_s, 1, 2, 9.6, 5_ms, 0, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0}, - {6_s, 1, 2, 0, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0}, - {8_s, 1, 2, 10, 5_ms, 0, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0}, - }; + { + std::vector forwardTestData = { + {0_s, 1, 0, 0, 1_s, 0}, {0_s, 1, 0, 0, 1_s, 0.25}, + {0_s, 1, 0, 0, 1_s, 10}, {0_s, 1, 0, 0, 1_s, 0.45}, + {0_s, 1, 0, 0, 1_s, 0}, {0_s, 1, 0, 0, 1_s, 0.15}, + {0_s, 1, 0, 0, 1_s, 0}, {0_s, 1, 0, 0, 1_s, 0.02}, + {0_s, 1, 0, 0, 1_s, 0}, {0_s, 1, 0, 0, 0_s, 0}, + }; - auto maxTime = 9_s; - auto minTime = maxTime; + FillStepVoltageData(forwardTestData); - sysid::AnalysisManager::Settings settings; - auto [tempMinTime, positionDelay, velocityDelay] = - sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime); - minTime = tempMinTime; + auto maxTime = 9_s; + auto minTime = maxTime; - EXPECT_EQ(4, settings.stepTestDuration.value()); - EXPECT_EQ(2, minTime.value()); + sysid::AnalysisManager::Settings settings; + auto [tempMinTime, positionDelay, velocityDelay] = + sysid::TrimStepVoltageData(&forwardTestData, &settings, minTime, + maxTime); + minTime = tempMinTime; + + EXPECT_EQ(3, settings.stepTestDuration.value()); + EXPECT_EQ(2, minTime.value()); + } + + { + std::vector backwardsTestData = { + {0_s, -1, 0, 0, 1_s, 0}, {0_s, -1, 0, 0, 1_s, -0.46}, + {0_s, -1, 0, 0, 1_s, -8}, {0_s, -1, 0, 0, 1_s, -0.32}, + {0_s, -1, 0, 0, 1_s, -0.12}, {0_s, -1, 0, 0, 1_s, -0.08}, + {0_s, -1, 0, 0, 1_s, -0.06}, {0_s, -1, 0, 0, 1_s, -0.02}, + {0_s, -1, 0, 0, 1_s, 0}, {0_s, -1, 0, 0, 0_s, 0}, + }; + + FillStepVoltageData(backwardsTestData); + + auto maxTime = 9_s; + auto minTime = maxTime; + + sysid::AnalysisManager::Settings settings; + auto [tempMinTime, positionDelay, velocityDelay] = + sysid::TrimStepVoltageData(&backwardsTestData, &settings, minTime, + maxTime); + minTime = tempMinTime; + + EXPECT_EQ(3, settings.stepTestDuration.value()); + EXPECT_EQ(2, minTime.value()); + } + + { + // Forward test but with an erroneous negative acceleration at the end + std::vector noisyTestData = { + {0_s, 1, 0, 0, 1_s, 0}, {0_s, 1, 0, 0, 1_s, 0.41}, + {0_s, 1, 0, 0, 1_s, 11.5}, {0_s, 1, 0, 0, 1_s, 1.2}, + {0_s, 1, 0, 0, 1_s, 0.34}, {0_s, 1, 0, 0, 1_s, 0.25}, + {0_s, 1, 0, 0, 1_s, 0.11}, {0_s, 1, 0, 0, 1_s, -0.08}, + {0_s, 1, 0, 0, 1_s, -12}, {0_s, 1, 0, 0, 0_s, 0}, + }; + + FillStepVoltageData(noisyTestData); + + auto maxTime = 9_s; + auto minTime = maxTime; + + sysid::AnalysisManager::Settings settings; + auto [tempMinTime, positionDelay, velocityDelay] = + sysid::TrimStepVoltageData(&noisyTestData, &settings, minTime, maxTime); + minTime = tempMinTime; + + // Expect trimming to reject the erroneous peak negative accel, + // correctly picking up the max positive accel instead. + EXPECT_EQ(4, settings.stepTestDuration.value()); + EXPECT_EQ(2, minTime.value()); + } } template