Skip to content

Commit

Permalink
[sysid] Fix wrong position Kd with unnormalized time (#6433)
Browse files Browse the repository at this point in the history
  • Loading branch information
brettle authored Mar 13, 2024
1 parent 0e013dc commit 3116f79
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 3 deletions.
7 changes: 4 additions & 3 deletions sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
13 changes: 13 additions & 0 deletions sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

0 comments on commit 3116f79

Please sign in to comment.