Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[sysid] Fix peak acceleration filtering behavior in dynamic velocity test (#6207)

Browse Source
This commit is contained in:
HarryXChen
2024-01-12 20:05:50 -05:00
committed by GitHub
parent b482321c0d
commit 84e3a22baa
2 changed files with 92 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
View File

@@ -14,6 +14,7 @@
#include <frc/filter/LinearFilter.h>
#include <frc/filter/MedianFilter.h>
#include <units/math.h>
#include <wpi/MathExtras.h>
#include <wpi/StringExtras.h>
using namespace sysid;
@@ -138,7 +139,11 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* 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;

101
sysid/src/test/native/cpp/analysis/FilterTest.cpp
View File

@@ -43,25 +43,96 @@ TEST(FilterTest, NoiseFloor) {
EXPECT_NEAR(0.953, noiseFloor, 0.001);
}
void FillStepVoltageData(std::vector<sysid::PreparedData>& 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<sysid::PreparedData> 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<sysid::PreparedData> 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<sysid::PreparedData> 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<sysid::PreparedData> 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 <int Derivative, int Samples, typename F, typename DfDx>