[sysid] Add SysId (#5672)

The source is copied from this commit:
625ff04784.

sysid/src/test/native/cpp/Main.cpp

@@ -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;
+}

sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp

@@ -0,0 +1,18 @@
+// 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::kDrivetrain,
+            sysid::analysis::FromName("Drivetrain"));
+  EXPECT_EQ(sysid::analysis::kDrivetrainAngular,
+            sysid::analysis::FromName("Drivetrain (Angular)"));
+  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"));
+}

sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp

@@ -0,0 +1,105 @@
+// 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, Velocity1) {
+  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, Velocity2) {
+  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, 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::kCTRECANCoder, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.000417, 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::kCTRECANCoder, 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.000417 / 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);
+}

sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp

@@ -0,0 +1,251 @@
+// 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 <cmath>
+
+#include <gtest/gtest.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/ArmSim.h"
+#include "sysid/analysis/ElevatorSim.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/SimpleMotorSim.h"
+
+/**
+ * Return simulated test data for a given simulation model.
+ *
+ * @param Ks Static friction gain.
+ * @param Kv Velocity gain.
+ * @param Ka Acceleration gain.
+ * @param Kg Gravity cosine gain.
+ */
+template <typename Model>
+sysid::Storage CollectData(Model& model) {
+  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;
+
+  // Slow forward test
+  auto 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 test
+  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 test
+  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 test
+  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;
+}
+
+TEST(FeedforwardAnalysisTest, Arm1) {
+  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};
+    auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                               sysid::analysis::kArm);
+    auto& gains = std::get<0>(ff);
+
+    EXPECT_NEAR(gains[0], Ks, 0.003);
+    EXPECT_NEAR(gains[1], Kv, 0.003);
+    EXPECT_NEAR(gains[2], Ka, 0.003);
+    EXPECT_NEAR(gains[3], Kg, 0.003);
+    EXPECT_NEAR(gains[4], offset, 0.007);
+  }
+}
+
+TEST(FeedforwardAnalysisTest, Arm2) {
+  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};
+    auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                               sysid::analysis::kArm);
+    auto& gains = std::get<0>(ff);
+
+    EXPECT_NEAR(gains[0], Ks, 0.003);
+    EXPECT_NEAR(gains[1], Kv, 0.003);
+    EXPECT_NEAR(gains[2], Ka, 0.003);
+    EXPECT_NEAR(gains[3], Kg, 0.003);
+    EXPECT_NEAR(gains[4], offset, 0.007);
+  }
+}
+
+TEST(FeedforwardAnalysisTest, Drivetrain1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kDrivetrain);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Drivetrain2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kDrivetrain);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, DrivetrainAngular1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(
+      CollectData(model), sysid::analysis::kDrivetrainAngular);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, DrivetrainAngular2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(
+      CollectData(model), sysid::analysis::kDrivetrainAngular);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Elevator1) {
+  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};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kElevator);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+  EXPECT_NEAR(gains[3], Kg, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Elevator2) {
+  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};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kElevator);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+  EXPECT_NEAR(gains[3], Kg, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Simple1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kSimple);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Simple2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kSimple);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}

sysid/src/test/native/cpp/analysis/FilterTest.cpp

@@ -0,0 +1,170 @@
+// 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);
+}
+
+TEST(FilterTest, StepTrim) {
+  std::vector<sysid::PreparedData> testData = {
+      {0_s, 1, 2, 3, 5_ms, 0, 0},    {1_s, 1, 2, 3, 5_ms, 0.25, 0},
+      {2_s, 1, 2, 3, 5_ms, 0.5, 0},  {3_s, 1, 2, 3, 5_ms, 0.45, 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},
+  };
+
+  std::vector<sysid::PreparedData> expectedData = {
+      {2_s, 1, 2, 3, 5_ms, 0.5, 0},
+      {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+      {4_s, 1, 2, 3, 5_ms, 0.35, 0},
+      {5_s, 1, 2, 3, 5_ms, 0.15, 0}};
+
+  auto maxTime = 9_s;
+  auto minTime = maxTime;
+
+  sysid::AnalysisManager::Settings settings;
+  auto [tempMinTime, positionDelay, velocityDelay] =
+      sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime);
+  minTime = tempMinTime;
+
+  EXPECT_EQ(expectedData[0].acceleration, testData[0].acceleration);
+  EXPECT_EQ(expectedData.back().acceleration, testData.back().acceleration);
+  EXPECT_EQ(5, 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);
+}

sysid/src/test/native/cpp/analysis/OLSTest.cpp

@@ -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 [coefficients, cod, rmse] = sysid::OLS(X, y);
+  EXPECT_EQ(coefficients.size(), 2u);
+
+  EXPECT_NEAR(coefficients[0], 1.0, 0.05);
+  EXPECT_NEAR(coefficients[1], 2.0, 0.05);
+  EXPECT_NEAR(cod, 1.0, 1E-4);
+}
+
+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 [coefficients, cod, rmse] = sysid::OLS(X, y);
+  EXPECT_EQ(coefficients.size(), 2u);
+
+  EXPECT_NEAR(coefficients[0], 0.305, 0.05);
+  EXPECT_NEAR(coefficients[1], 1.518, 0.05);
+  EXPECT_NEAR(cod, 0.985, 0.05);
+}
+
+#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

sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp

@@ -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);
+}