mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
SCRIPT Move subprojects
This commit is contained in:
PJ Reiniger
committed by
Peter Johnson
Peter Johnson
parent
8cfc158790
commit
a5492d30da
11
tools/sysid/src/test/native/cpp/Main.cpp
Normal file
11
tools/sysid/src/test/native/cpp/Main.cpp
Normal file
@@ -0,0 +1,11 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "sysid/analysis/AnalysisType.h"
|
||||
|
||||
TEST(AnalysisTypeTest, FromName) {
|
||||
EXPECT_EQ(sysid::analysis::kElevator, sysid::analysis::FromName("Elevator"));
|
||||
EXPECT_EQ(sysid::analysis::kArm, sysid::analysis::FromName("Arm"));
|
||||
EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Simple"));
|
||||
EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Random"));
|
||||
}
|
||||
@@ -0,0 +1,171 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "sysid/analysis/FeedbackAnalysis.h"
|
||||
#include "sysid/analysis/FeedbackControllerPreset.h"
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocitySystem1) {
|
||||
auto Kv = 3.060;
|
||||
auto Ka = 0.327;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
|
||||
sysid::presets::kDefault, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 2.11, 0.05);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocitySystem2) {
|
||||
auto Kv = 0.0693;
|
||||
auto Ka = 0.1170;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
|
||||
sysid::presets::kDefault, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 3.11, 0.05);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocitySystemWithSmallKa) {
|
||||
auto Kv = 3.060;
|
||||
auto Ka = 0.0;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
|
||||
sysid::presets::kDefault, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 0.00, 0.05);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocityConversion) {
|
||||
auto Kv = 0.0693;
|
||||
auto Ka = 0.1170;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
|
||||
sysid::presets::kDefault, params, Kv, Ka, 3.0 * 1023);
|
||||
|
||||
// This should have the same Kp as the test above, but scaled by a factor of 3
|
||||
// * 1023.
|
||||
EXPECT_NEAR(Kp, 3.11 / (3 * 1023), 0.005);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocityCTRE) {
|
||||
auto Kv = 1.97;
|
||||
auto Ka = 0.179;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(sysid::presets::kCTREv5,
|
||||
params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 259.21276731541178, 0.00005);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocityCTREConversion) {
|
||||
auto Kv = 1.97;
|
||||
auto Ka = 0.179;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(sysid::presets::kCTREv5,
|
||||
params, Kv, Ka, 3.0);
|
||||
|
||||
// This should have the same Kp as the test above, but scaled by a factor
|
||||
// of 3.
|
||||
EXPECT_NEAR(Kp, 259.21276731541178 / 3, 0.00005);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocityREV) {
|
||||
auto Kv = 1.97;
|
||||
auto Ka = 0.179;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
|
||||
sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 0.00241, 0.005);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, VelocityREVConversion) {
|
||||
auto Kv = 1.97;
|
||||
auto Ka = 0.179;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
|
||||
sysid::presets::kREVNEOBuiltIn, params, Kv, Ka, 3.0);
|
||||
|
||||
// This should have the same Kp as the test above, but scaled by a factor
|
||||
// of 3.
|
||||
EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, Position) {
|
||||
auto Kv = 3.060;
|
||||
auto Ka = 0.327;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
|
||||
sysid::presets::kDefault, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 6.41, 0.05);
|
||||
EXPECT_NEAR(Kd, 2.48, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, PositionWithSmallKa) {
|
||||
auto Kv = 3.060;
|
||||
auto Ka = 1e-10;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
|
||||
sysid::presets::kDefault, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 19.97, 0.05);
|
||||
EXPECT_NEAR(Kd, 0.00, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
|
||||
auto Kv = 3.060;
|
||||
auto Ka = 0.327;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
sysid::FeedbackControllerPreset preset{sysid::presets::kDefault};
|
||||
|
||||
preset.measurementDelay = 10_ms;
|
||||
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(preset, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 5.92, 0.05);
|
||||
EXPECT_NEAR(Kd, 2.12, 0.05);
|
||||
}
|
||||
|
||||
TEST(FeedbackAnalysisTest, PositionREV) {
|
||||
auto Kv = 3.060;
|
||||
auto Ka = 0.327;
|
||||
|
||||
sysid::LQRParameters params{1, 1.5, 7};
|
||||
|
||||
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
|
||||
sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
|
||||
|
||||
EXPECT_NEAR(Kp, 0.30202, 0.05);
|
||||
EXPECT_NEAR(Kd, 48.518, 0.05);
|
||||
}
|
||||
@@ -0,0 +1,292 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <bitset>
|
||||
#include <cmath>
|
||||
#include <span>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <units/time.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#include "sysid/analysis/AnalysisManager.h"
|
||||
#include "sysid/analysis/AnalysisType.h"
|
||||
#include "sysid/analysis/ArmSim.h"
|
||||
#include "sysid/analysis/ElevatorSim.h"
|
||||
#include "sysid/analysis/FeedforwardAnalysis.h"
|
||||
#include "sysid/analysis/SimpleMotorSim.h"
|
||||
|
||||
namespace {
|
||||
|
||||
enum Movements : uint32_t {
|
||||
kSlowForward,
|
||||
kSlowBackward,
|
||||
kFastForward,
|
||||
kFastBackward
|
||||
};
|
||||
|
||||
inline constexpr int kMovementCombinations = 16;
|
||||
|
||||
/**
|
||||
* Return simulated test data for a given simulation model.
|
||||
*
|
||||
* @tparam Model The model type.
|
||||
* @param model The simulation model.
|
||||
* @param movements Which movements to do.
|
||||
*/
|
||||
template <typename Model>
|
||||
sysid::Storage CollectData(Model& model, std::bitset<4> movements) {
|
||||
constexpr auto kUstep = 0.25_V / 1_s;
|
||||
constexpr units::volt_t kUmax = 7_V;
|
||||
constexpr units::second_t T = 5_ms;
|
||||
constexpr units::second_t kTestDuration = 5_s;
|
||||
|
||||
sysid::Storage storage;
|
||||
auto& [slowForward, slowBackward, fastForward, fastBackward] = storage;
|
||||
auto voltage = 0_V;
|
||||
|
||||
// Slow forward
|
||||
if (movements.test(Movements::kSlowForward)) {
|
||||
model.Reset();
|
||||
voltage = 0_V;
|
||||
for (int i = 0; i < (kTestDuration / T).value(); ++i) {
|
||||
slowForward.emplace_back(sysid::PreparedData{
|
||||
i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
|
||||
model.GetAcceleration(voltage), std::cos(model.GetPosition()),
|
||||
std::sin(model.GetPosition())});
|
||||
|
||||
model.Update(voltage, T);
|
||||
voltage += kUstep * T;
|
||||
}
|
||||
}
|
||||
|
||||
// Slow backward
|
||||
if (movements.test(Movements::kSlowBackward)) {
|
||||
model.Reset();
|
||||
voltage = 0_V;
|
||||
for (int i = 0; i < (kTestDuration / T).value(); ++i) {
|
||||
slowBackward.emplace_back(sysid::PreparedData{
|
||||
i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
|
||||
model.GetAcceleration(voltage), std::cos(model.GetPosition()),
|
||||
std::sin(model.GetPosition())});
|
||||
|
||||
model.Update(voltage, T);
|
||||
voltage -= kUstep * T;
|
||||
}
|
||||
}
|
||||
|
||||
// Fast forward
|
||||
if (movements.test(Movements::kFastForward)) {
|
||||
model.Reset();
|
||||
voltage = 0_V;
|
||||
for (int i = 0; i < (kTestDuration / T).value(); ++i) {
|
||||
fastForward.emplace_back(sysid::PreparedData{
|
||||
i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
|
||||
model.GetAcceleration(voltage), std::cos(model.GetPosition()),
|
||||
std::sin(model.GetPosition())});
|
||||
|
||||
model.Update(voltage, T);
|
||||
voltage = kUmax;
|
||||
}
|
||||
}
|
||||
|
||||
// Fast backward
|
||||
if (movements.test(Movements::kFastBackward)) {
|
||||
model.Reset();
|
||||
voltage = 0_V;
|
||||
for (int i = 0; i < (kTestDuration / T).value(); ++i) {
|
||||
fastBackward.emplace_back(sysid::PreparedData{
|
||||
i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
|
||||
model.GetAcceleration(voltage), std::cos(model.GetPosition()),
|
||||
std::sin(model.GetPosition())});
|
||||
|
||||
model.Update(voltage, T);
|
||||
voltage = -kUmax;
|
||||
}
|
||||
}
|
||||
|
||||
return storage;
|
||||
}
|
||||
|
||||
/**
|
||||
* Asserts success if the gains contain NaNs or are too far from their expected
|
||||
* values.
|
||||
*
|
||||
* @param expectedGains The expected feedforward gains.
|
||||
* @param actualGains The calculated feedforward gains.
|
||||
* @param tolerances The tolerances for the coefficient comparisons.
|
||||
*/
|
||||
testing::AssertionResult FitIsBad(std::span<const double> expectedGains,
|
||||
std::span<const double> actualGains,
|
||||
std::span<const double> tolerances) {
|
||||
// Check for NaN
|
||||
for (const auto& coeff : actualGains) {
|
||||
if (std::isnan(coeff)) {
|
||||
return testing::AssertionSuccess();
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < expectedGains.size(); ++i) {
|
||||
if (std::abs(expectedGains[i] - actualGains[i]) >= tolerances[i]) {
|
||||
return testing::AssertionSuccess();
|
||||
}
|
||||
}
|
||||
|
||||
auto result = testing::AssertionFailure();
|
||||
|
||||
result << "\n";
|
||||
for (size_t i = 0; i < expectedGains.size(); ++i) {
|
||||
if (i == 0) {
|
||||
result << "Ks";
|
||||
} else if (i == 1) {
|
||||
result << "Kv";
|
||||
} else if (i == 2) {
|
||||
result << "Ka";
|
||||
} else if (i == 3) {
|
||||
result << "Kg";
|
||||
} else if (i == 4) {
|
||||
result << "offset";
|
||||
}
|
||||
|
||||
result << ":\n";
|
||||
result << " expected " << expectedGains[i] << ",\n";
|
||||
result << " actual " << actualGains[i] << ",\n";
|
||||
result << " diff " << std::abs(expectedGains[i] - actualGains[i]) << "\n";
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Asserts that two arrays are equal.
|
||||
*
|
||||
* @param expected The expected array.
|
||||
* @param actual The actual array.
|
||||
* @param tolerances The tolerances for the element comparisons.
|
||||
*/
|
||||
void ExpectArrayNear(std::span<const double> expected,
|
||||
std::span<const double> actual,
|
||||
std::span<const double> tolerances) {
|
||||
// Check size
|
||||
const size_t size = expected.size();
|
||||
EXPECT_EQ(size, actual.size());
|
||||
EXPECT_EQ(size, tolerances.size());
|
||||
|
||||
// Check elements
|
||||
for (size_t i = 0; i < size; ++i) {
|
||||
EXPECT_NEAR(expected[i], actual[i], tolerances[i]) << "where i = " << i;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @tparam Model The model type.
|
||||
* @param model The simulation model.
|
||||
* @param type The analysis type.
|
||||
* @param expectedGains The expected feedforward gains.
|
||||
* @param tolerances The tolerances for the coefficient comparisons.
|
||||
*/
|
||||
template <typename Model>
|
||||
void RunTests(Model& model, const sysid::AnalysisType& type,
|
||||
std::span<const double> expectedGains,
|
||||
std::span<const double> tolerances) {
|
||||
// Iterate through all combinations of movements
|
||||
for (int movements = 0; movements < kMovementCombinations; ++movements) {
|
||||
try {
|
||||
auto ff =
|
||||
sysid::CalculateFeedforwardGains(CollectData(model, movements), type);
|
||||
|
||||
ExpectArrayNear(expectedGains, ff.coeffs, tolerances);
|
||||
} catch (sysid::InsufficientSamplesError&) {
|
||||
// If calculation threw an exception, confirm at least one of the gains
|
||||
// doesn't match
|
||||
auto ff = sysid::CalculateFeedforwardGains(CollectData(model, movements),
|
||||
type, false);
|
||||
EXPECT_TRUE(FitIsBad(expectedGains, ff.coeffs, tolerances));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(FeedforwardAnalysisTest, Arm) {
|
||||
{
|
||||
constexpr double Ks = 1.01;
|
||||
constexpr double Kv = 3.060;
|
||||
constexpr double Ka = 0.327;
|
||||
constexpr double Kg = 0.211;
|
||||
|
||||
for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
|
||||
sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
|
||||
|
||||
RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}},
|
||||
{{8e-3, 8e-3, 8e-3, 8e-3, 3e-2}});
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
constexpr double Ks = 0.547;
|
||||
constexpr double Kv = 0.0693;
|
||||
constexpr double Ka = 0.1170;
|
||||
constexpr double Kg = 0.122;
|
||||
|
||||
for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
|
||||
sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
|
||||
|
||||
RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}},
|
||||
{{8e-3, 8e-3, 8e-3, 8e-3, 5e-2}});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(FeedforwardAnalysisTest, Elevator) {
|
||||
{
|
||||
constexpr double Ks = 1.01;
|
||||
constexpr double Kv = 3.060;
|
||||
constexpr double Ka = 0.327;
|
||||
constexpr double Kg = -0.211;
|
||||
|
||||
sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
|
||||
|
||||
RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}},
|
||||
{{8e-3, 8e-3, 8e-3, 8e-3}});
|
||||
}
|
||||
|
||||
{
|
||||
constexpr double Ks = 0.547;
|
||||
constexpr double Kv = 0.0693;
|
||||
constexpr double Ka = 0.1170;
|
||||
constexpr double Kg = -0.122;
|
||||
|
||||
sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
|
||||
|
||||
RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}},
|
||||
{{8e-3, 8e-3, 8e-3, 8e-3}});
|
||||
}
|
||||
}
|
||||
|
||||
TEST(FeedforwardAnalysisTest, Simple) {
|
||||
{
|
||||
constexpr double Ks = 1.01;
|
||||
constexpr double Kv = 3.060;
|
||||
constexpr double Ka = 0.327;
|
||||
|
||||
sysid::SimpleMotorSim model{Ks, Kv, Ka};
|
||||
|
||||
RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}},
|
||||
{{8e-3, 8e-3, 8e-3}});
|
||||
}
|
||||
|
||||
{
|
||||
constexpr double Ks = 0.547;
|
||||
constexpr double Kv = 0.0693;
|
||||
constexpr double Ka = 0.1170;
|
||||
|
||||
sysid::SimpleMotorSim model{Ks, Kv, Ka};
|
||||
|
||||
RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}},
|
||||
{{8e-3, 8e-3, 8e-3}});
|
||||
}
|
||||
}
|
||||
233
tools/sysid/src/test/native/cpp/analysis/FilterTest.cpp
Normal file
233
tools/sysid/src/test/native/cpp/analysis/FilterTest.cpp
Normal file
@@ -0,0 +1,233 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "sysid/analysis/AnalysisManager.h"
|
||||
#include "sysid/analysis/FeedforwardAnalysis.h"
|
||||
#include "sysid/analysis/FilteringUtils.h"
|
||||
#include "sysid/analysis/Storage.h"
|
||||
|
||||
TEST(FilterTest, MedianFilter) {
|
||||
std::vector<sysid::PreparedData> testData{
|
||||
sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1},
|
||||
sysid::PreparedData{0_s, 0, 0, 10}, sysid::PreparedData{0_s, 0, 0, 5},
|
||||
sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 0},
|
||||
sysid::PreparedData{0_s, 0, 0, 1000}, sysid::PreparedData{0_s, 0, 0, 7},
|
||||
sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}};
|
||||
std::vector<sysid::PreparedData> expectedData{
|
||||
sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1},
|
||||
sysid::PreparedData{0_s, 0, 0, 5}, sysid::PreparedData{0_s, 0, 0, 5},
|
||||
sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 3},
|
||||
sysid::PreparedData{0_s, 0, 0, 7}, sysid::PreparedData{0_s, 0, 0, 7},
|
||||
sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}};
|
||||
|
||||
sysid::ApplyMedianFilter(&testData, 3);
|
||||
EXPECT_EQ(expectedData, testData);
|
||||
}
|
||||
|
||||
TEST(FilterTest, NoiseFloor) {
|
||||
std::vector<sysid::PreparedData> testData = {
|
||||
{0_s, 1, 2, 3, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 1, 0},
|
||||
{2_s, 1, 2, 3, 5_ms, 2, 0}, {3_s, 1, 2, 3, 5_ms, 5, 0},
|
||||
{4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
|
||||
{6_s, 1, 2, 3, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0},
|
||||
{8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0}};
|
||||
double noiseFloor =
|
||||
GetNoiseFloor(testData, 2, [](auto&& pt) { return pt.acceleration; });
|
||||
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> 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},
|
||||
};
|
||||
|
||||
FillStepVoltageData(forwardTestData);
|
||||
|
||||
auto maxTime = 9_s;
|
||||
auto minTime = maxTime;
|
||||
|
||||
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>
|
||||
void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
|
||||
double max) {
|
||||
static_assert(Samples % 2 != 0, "Number of samples must be odd.");
|
||||
|
||||
auto filter = sysid::CentralFiniteDifference<Derivative, Samples>(h);
|
||||
|
||||
for (int i = min / h.value(); i < max / h.value(); ++i) {
|
||||
// Let filter initialize
|
||||
if (i < static_cast<int>(min / h.value()) + Samples) {
|
||||
filter.Calculate(f(i * h.value()));
|
||||
continue;
|
||||
}
|
||||
|
||||
// For central finite difference, the derivative computed at this point is
|
||||
// half the window size in the past.
|
||||
// The order of accuracy is O(h^(N - d)) where N is number of stencil
|
||||
// points and d is order of derivative
|
||||
EXPECT_NEAR(dfdx((i - static_cast<int>((Samples - 1) / 2)) * h.value()),
|
||||
filter.Calculate(f(i * h.value())),
|
||||
std::pow(h.value(), Samples - Derivative));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Test central finite difference.
|
||||
*/
|
||||
TEST(LinearFilterOutputTest, CentralFiniteDifference) {
|
||||
constexpr auto h = 5_ms;
|
||||
|
||||
AssertCentralResults<1, 3>(
|
||||
[](double x) {
|
||||
// f(x) = x²
|
||||
return x * x;
|
||||
},
|
||||
[](double x) {
|
||||
// df/dx = 2x
|
||||
return 2.0 * x;
|
||||
},
|
||||
h, -20.0, 20.0);
|
||||
|
||||
AssertCentralResults<1, 3>(
|
||||
[](double x) {
|
||||
// f(x) = std::sin(x)
|
||||
return std::sin(x);
|
||||
},
|
||||
[](double x) {
|
||||
// df/dx = std::cos(x)
|
||||
return std::cos(x);
|
||||
},
|
||||
h, -20.0, 20.0);
|
||||
|
||||
AssertCentralResults<1, 3>(
|
||||
[](double x) {
|
||||
// f(x) = ln(x)
|
||||
return std::log(x);
|
||||
},
|
||||
[](double x) {
|
||||
// df/dx = 1 / x
|
||||
return 1.0 / x;
|
||||
},
|
||||
h, 1.0, 20.0);
|
||||
|
||||
AssertCentralResults<2, 5>(
|
||||
[](double x) {
|
||||
// f(x) = x^2
|
||||
return x * x;
|
||||
},
|
||||
[](double x) {
|
||||
// d²f/dx² = 2
|
||||
return 2.0;
|
||||
},
|
||||
h, -20.0, 20.0);
|
||||
|
||||
AssertCentralResults<2, 5>(
|
||||
[](double x) {
|
||||
// f(x) = std::sin(x)
|
||||
return std::sin(x);
|
||||
},
|
||||
[](double x) {
|
||||
// d²f/dx² = -std::sin(x)
|
||||
return -std::sin(x);
|
||||
},
|
||||
h, -20.0, 20.0);
|
||||
|
||||
AssertCentralResults<2, 5>(
|
||||
[](double x) {
|
||||
// f(x) = ln(x)
|
||||
return std::log(x);
|
||||
},
|
||||
[](double x) {
|
||||
// d²f/dx² = -1 / x²
|
||||
return -1.0 / (x * x);
|
||||
},
|
||||
h, 1.0, 20.0);
|
||||
}
|
||||
42
tools/sysid/src/test/native/cpp/analysis/OLSTest.cpp
Normal file
42
tools/sysid/src/test/native/cpp/analysis/OLSTest.cpp
Normal file
@@ -0,0 +1,42 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "sysid/analysis/OLS.h"
|
||||
|
||||
TEST(OLSTest, TwoVariablesTwoPoints) {
|
||||
// (1, 3) and (2, 5). Should produce y = 2x + 1.
|
||||
Eigen::MatrixXd X{{1.0, 1.0}, {1.0, 2.0}};
|
||||
Eigen::VectorXd y{{3.0}, {5.0}};
|
||||
|
||||
auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
|
||||
EXPECT_EQ(coeffs.size(), 2u);
|
||||
|
||||
EXPECT_NEAR(coeffs[0], 1.0, 1e-12);
|
||||
EXPECT_NEAR(coeffs[1], 2.0, 1e-12);
|
||||
EXPECT_DOUBLE_EQ(rSquared, 1.0);
|
||||
}
|
||||
|
||||
TEST(OLSTest, TwoVariablesFivePoints) {
|
||||
// (2, 4), (3, 5), (5, 7), (7, 10), (9, 15)
|
||||
// Should produce 1.518x + 0.305.
|
||||
Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 5}, {1, 7}, {1, 9}};
|
||||
Eigen::VectorXd y{{4}, {5}, {7}, {10}, {15}};
|
||||
|
||||
auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
|
||||
EXPECT_EQ(coeffs.size(), 2u);
|
||||
|
||||
EXPECT_NEAR(coeffs[0], 0.30487804878048774, 1e-12);
|
||||
EXPECT_NEAR(coeffs[1], 1.5182926829268293, 1e-12);
|
||||
EXPECT_DOUBLE_EQ(rSquared, 0.91906029466386019);
|
||||
}
|
||||
|
||||
#ifndef NDEBUG
|
||||
TEST(OLSTest, MalformedData) {
|
||||
Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 4}};
|
||||
Eigen::VectorXd y{{4}, {5}};
|
||||
EXPECT_DEATH(sysid::OLS(X, y), "");
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,12 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "sysid/analysis/TrackwidthAnalysis.h"
|
||||
|
||||
TEST(TrackwidthAnalysisTest, Calculate) {
|
||||
double result = sysid::CalculateTrackwidth(-0.5386, 0.5386, 90_deg);
|
||||
EXPECT_NEAR(result, 0.6858, 1E-4);
|
||||
}
|
||||
Reference in New Issue
Block a user