From 3116f790ea95d815a509278c077f2ffc0d175868 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 12 Mar 2024 21:49:28 -0700 Subject: [PATCH] [sysid] Fix wrong position Kd with unnormalized time (#6433) --- .../main/native/cpp/analysis/FeedbackAnalysis.cpp | 7 ++++--- .../native/cpp/analysis/FeedbackAnalysisTest.cpp | 13 +++++++++++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 0a4f53a65c..7821812bd4 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -42,9 +42,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( controller.LatencyCompensate(system, preset.period, preset.measurementDelay); - return {controller.K(0, 0) * preset.outputConversionFactor, - controller.K(0, 1) * preset.outputConversionFactor / - (preset.normalized ? 1 : preset.period.value())}; + return { + controller.K(0, 0) * preset.outputConversionFactor, + controller.K(0, 1) * preset.outputConversionFactor / + (preset.normalized ? 1 : units::second_t{preset.period}.value())}; } // This is our special model to avoid instabilities in the LQR. diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp index 3e1474ef37..511950ed96 100644 --- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp @@ -130,3 +130,16 @@ TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) { 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); +}