[sysid] Add SysId (#5672)

The source is copied from this commit:
625ff04784.

sysid/src/main/native/cpp/analysis/AnalysisManager.cpp

@@ -0,0 +1,568 @@
+// 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 "sysid/analysis/AnalysisManager.h"
+
+#include <cmath>
+#include <cstddef>
+#include <functional>
+#include <stdexcept>
+
+#include <fmt/format.h>
+#include <units/angle.h>
+#include <units/math.h>
+#include <wpi/StringExtras.h>
+#include <wpi/StringMap.h>
+#include <wpi/raw_istream.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/JSONConverter.h"
+#include "sysid/analysis/TrackWidthAnalysis.h"
+
+using namespace sysid;
+
+/**
+ * Converts a raw data vector into a PreparedData vector with only the
+ * timestamp, voltage, position, and velocity fields filled out.
+ *
+ * @tparam S The size of the arrays in the raw data vector
+ * @tparam Timestamp The index of the Timestamp data in the raw data vector
+ * arrays
+ * @tparam Voltage The index of the Voltage data in the raw data vector arrays
+ * @tparam Position The index of the Position data in the raw data vector arrays
+ * @tparam Velocity The index of the Velocity data in the raw data vector arrays
+ *
+ * @param data A raw data vector
+ *
+ * @return A PreparedData vector
+ */
+template <size_t S, size_t Timestamp, size_t Voltage, size_t Position,
+          size_t Velocity>
+static std::vector<PreparedData> ConvertToPrepared(
+    const std::vector<std::array<double, S>>& data) {
+  std::vector<PreparedData> prepared;
+  for (int i = 0; i < static_cast<int>(data.size()) - 1; ++i) {
+    const auto& pt1 = data[i];
+    const auto& pt2 = data[i + 1];
+    prepared.emplace_back(PreparedData{
+        units::second_t{pt1[Timestamp]}, pt1[Voltage], pt1[Position],
+        pt1[Velocity], units::second_t{pt2[Timestamp] - pt1[Timestamp]}});
+  }
+  return prepared;
+}
+
+/**
+ * To preserve a raw copy of the data, this method saves a raw version
+ * in the dataset StringMap where the key of the raw data starts with "raw-"
+ * before the name of the original data.
+ *
+ * @tparam S The size of the array data that's being used
+ *
+ * @param dataset A reference to the dataset being used
+ */
+template <size_t S>
+static void CopyRawData(
+    wpi::StringMap<std::vector<std::array<double, S>>>* dataset) {
+  auto& data = *dataset;
+  // Loads the Raw Data
+  for (auto& it : data) {
+    auto key = it.first();
+    auto& dataset = it.getValue();
+
+    if (!wpi::contains(key, "raw")) {
+      data[fmt::format("raw-{}", key)] = dataset;
+      data[fmt::format("original-raw-{}", key)] = dataset;
+    }
+  }
+}
+
+/**
+ * Assigns the combines the various datasets into a single one for analysis.
+ *
+ * @param slowForward The slow forward dataset
+ * @param slowBackward The slow backward dataset
+ * @param fastForward The fast forward dataset
+ * @param fastBackward The fast backward dataset
+ */
+static Storage CombineDatasets(const std::vector<PreparedData>& slowForward,
+                               const std::vector<PreparedData>& slowBackward,
+                               const std::vector<PreparedData>& fastForward,
+                               const std::vector<PreparedData>& fastBackward) {
+  return Storage{slowForward, slowBackward, fastForward, fastBackward};
+}
+
+void AnalysisManager::PrepareGeneralData() {
+  using Data = std::array<double, 4>;
+  wpi::StringMap<std::vector<Data>> data;
+  wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+  // Store the raw data columns.
+  static constexpr size_t kTimeCol = 0;
+  static constexpr size_t kVoltageCol = 1;
+  static constexpr size_t kPosCol = 2;
+  static constexpr size_t kVelCol = 3;
+
+  WPI_INFO(m_logger, "{}", "Reading JSON data.");
+  // Get the major components from the JSON and store them inside a StringMap.
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    data[key] = m_json.at(key).get<std::vector<Data>>();
+  }
+
+  WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+  // Ensure that voltage and velocity have the same sign. Also multiply
+  // positions and velocities by the factor.
+  for (auto it = data.begin(); it != data.end(); ++it) {
+    for (auto&& pt : it->second) {
+      pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]);
+      pt[kPosCol] *= m_factor;
+      pt[kVelCol] *= m_factor;
+    }
+  }
+
+  WPI_INFO(m_logger, "{}", "Copying raw data.");
+  CopyRawData(&data);
+
+  WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
+  // Convert data to PreparedData structs
+  for (auto& it : data) {
+    auto key = it.first();
+    preparedData[key] =
+        ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>(
+            data[key]);
+  }
+
+  // Store the original datasets
+  m_originalDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(preparedData["original-raw-slow-forward"],
+                      preparedData["original-raw-slow-backward"],
+                      preparedData["original-raw-fast-forward"],
+                      preparedData["original-raw-fast-backward"]);
+
+  WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
+  sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+                              m_velocityDelays, m_minStepTime, m_maxStepTime,
+                              m_unit);
+
+  WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+  sysid::AccelFilter(&preparedData);
+
+  WPI_INFO(m_logger, "{}", "Storing datasets.");
+  // Store the raw datasets
+  m_rawDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(
+          preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
+          preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
+
+  // Store the filtered datasets
+  m_filteredDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(
+          preparedData["slow-forward"], preparedData["slow-backward"],
+          preparedData["fast-forward"], preparedData["fast-backward"]);
+
+  m_startTimes = {preparedData["raw-slow-forward"][0].timestamp,
+                  preparedData["raw-slow-backward"][0].timestamp,
+                  preparedData["raw-fast-forward"][0].timestamp,
+                  preparedData["raw-fast-backward"][0].timestamp};
+}
+
+void AnalysisManager::PrepareAngularDrivetrainData() {
+  using Data = std::array<double, 9>;
+  wpi::StringMap<std::vector<Data>> data;
+  wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+  // Store the relevant raw data columns.
+  static constexpr size_t kTimeCol = 0;
+  static constexpr size_t kLVoltageCol = 1;
+  static constexpr size_t kRVoltageCol = 2;
+  static constexpr size_t kLPosCol = 3;
+  static constexpr size_t kRPosCol = 4;
+  static constexpr size_t kLVelCol = 5;
+  static constexpr size_t kRVelCol = 6;
+  static constexpr size_t kAngleCol = 7;
+  static constexpr size_t kAngularRateCol = 8;
+
+  WPI_INFO(m_logger, "{}", "Reading JSON data.");
+  // Get the major components from the JSON and store them inside a StringMap.
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    data[key] = m_json.at(key).get<std::vector<Data>>();
+  }
+
+  WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+  // Ensure that voltage and velocity have the same sign. Also multiply
+  // positions and velocities by the factor.
+  for (auto it = data.begin(); it != data.end(); ++it) {
+    for (auto&& pt : it->second) {
+      pt[kLPosCol] *= m_factor;
+      pt[kRPosCol] *= m_factor;
+      pt[kLVelCol] *= m_factor;
+      pt[kRVelCol] *= m_factor;
+
+      // Stores the average voltages in the left voltage column.
+      // This aggregates the left and right voltages into a single voltage
+      // column for the ConvertToPrepared() method. std::copysign() ensures the
+      // polarity of the voltage matches the direction the robot turns.
+      pt[kLVoltageCol] = std::copysign(
+          (std::abs(pt[kLVoltageCol]) + std::abs(pt[kRVoltageCol])) / 2,
+          pt[kAngularRateCol]);
+
+      // ω = (v_r - v_l) / trackwidth
+      // v = ωr => v = ω * trackwidth / 2
+      // (v_r - v_l) / trackwidth * (trackwidth / 2) = (v_r - v_l) / 2
+      // However, since we know this is an angular test, the left and right
+      // wheel velocities will have opposite signs, allowing us to add their
+      // absolute values and get the same result (in terms of magnitude).
+      // std::copysign() is used to make sure the direction of the wheel
+      // velocities matches the direction the robot turns.
+      pt[kAngularRateCol] =
+          std::copysign((std::abs(pt[kRVelCol]) + std::abs(pt[kLVelCol])) / 2,
+                        pt[kAngularRateCol]);
+    }
+  }
+
+  WPI_INFO(m_logger, "{}", "Calculating trackwidth");
+  // Aggregating all the deltas from all the tests
+  double leftDelta = 0.0;
+  double rightDelta = 0.0;
+  double angleDelta = 0.0;
+  for (const auto& it : data) {
+    auto key = it.first();
+    auto& trackWidthData = data[key];
+    leftDelta += std::abs(trackWidthData.back()[kLPosCol] -
+                          trackWidthData.front()[kLPosCol]);
+    rightDelta += std::abs(trackWidthData.back()[kRPosCol] -
+                           trackWidthData.front()[kRPosCol]);
+    angleDelta += std::abs(trackWidthData.back()[kAngleCol] -
+                           trackWidthData.front()[kAngleCol]);
+  }
+  m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta,
+                                            units::radian_t{angleDelta});
+
+  WPI_INFO(m_logger, "{}", "Copying raw data.");
+  CopyRawData(&data);
+
+  WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
+  // Convert raw data to prepared data
+  for (const auto& it : data) {
+    auto key = it.first();
+    preparedData[key] = ConvertToPrepared<9, kTimeCol, kLVoltageCol, kAngleCol,
+                                          kAngularRateCol>(data[key]);
+  }
+
+  // Create the distinct datasets and store them
+  m_originalDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(preparedData["original-raw-slow-forward"],
+                      preparedData["original-raw-slow-backward"],
+                      preparedData["original-raw-fast-forward"],
+                      preparedData["original-raw-fast-backward"]);
+
+  WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
+  sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+                              m_velocityDelays, m_minStepTime, m_maxStepTime);
+
+  WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+  sysid::AccelFilter(&preparedData);
+
+  WPI_INFO(m_logger, "{}", "Storing datasets.");
+  // Create the distinct datasets and store them
+  m_rawDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(
+          preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
+          preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
+  m_filteredDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(
+          preparedData["slow-forward"], preparedData["slow-backward"],
+          preparedData["fast-forward"], preparedData["fast-backward"]);
+
+  m_startTimes = {preparedData["slow-forward"][0].timestamp,
+                  preparedData["slow-backward"][0].timestamp,
+                  preparedData["fast-forward"][0].timestamp,
+                  preparedData["fast-backward"][0].timestamp};
+}
+
+void AnalysisManager::PrepareLinearDrivetrainData() {
+  using Data = std::array<double, 9>;
+  wpi::StringMap<std::vector<Data>> data;
+  wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+  // Store the relevant raw data columns.
+  static constexpr size_t kTimeCol = 0;
+  static constexpr size_t kLVoltageCol = 1;
+  static constexpr size_t kRVoltageCol = 2;
+  static constexpr size_t kLPosCol = 3;
+  static constexpr size_t kRPosCol = 4;
+  static constexpr size_t kLVelCol = 5;
+  static constexpr size_t kRVelCol = 6;
+
+  // Get the major components from the JSON and store them inside a StringMap.
+  WPI_INFO(m_logger, "{}", "Reading JSON data.");
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    data[key] = m_json.at(key).get<std::vector<Data>>();
+  }
+
+  // Ensure that voltage and velocity have the same sign. Also multiply
+  // positions and velocities by the factor.
+  WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+  for (auto it = data.begin(); it != data.end(); ++it) {
+    for (auto&& pt : it->second) {
+      pt[kLVoltageCol] = std::copysign(pt[kLVoltageCol], pt[kLVelCol]);
+      pt[kRVoltageCol] = std::copysign(pt[kRVoltageCol], pt[kRVelCol]);
+      pt[kLPosCol] *= m_factor;
+      pt[kRPosCol] *= m_factor;
+      pt[kLVelCol] *= m_factor;
+      pt[kRVelCol] *= m_factor;
+    }
+  }
+
+  WPI_INFO(m_logger, "{}", "Copying raw data.");
+  CopyRawData(&data);
+
+  // Convert data to PreparedData
+  WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
+  for (auto& it : data) {
+    auto key = it.first();
+
+    preparedData[fmt::format("left-{}", key)] =
+        ConvertToPrepared<9, kTimeCol, kLVoltageCol, kLPosCol, kLVelCol>(
+            data[key]);
+    preparedData[fmt::format("right-{}", key)] =
+        ConvertToPrepared<9, kTimeCol, kRVoltageCol, kRPosCol, kRVelCol>(
+            data[key]);
+  }
+
+  // Create the distinct raw datasets and store them
+  auto originalSlowForward = AnalysisManager::DataConcat(
+      preparedData["left-original-raw-slow-forward"],
+      preparedData["right-original-raw-slow-forward"]);
+  auto originalSlowBackward = AnalysisManager::DataConcat(
+      preparedData["left-original-raw-slow-backward"],
+      preparedData["right-original-raw-slow-backward"]);
+  auto originalFastForward = AnalysisManager::DataConcat(
+      preparedData["left-original-raw-fast-forward"],
+      preparedData["right-original-raw-fast-forward"]);
+  auto originalFastBackward = AnalysisManager::DataConcat(
+      preparedData["left-original-raw-fast-backward"],
+      preparedData["right-original-raw-fast-backward"]);
+  m_originalDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(originalSlowForward, originalSlowBackward,
+                      originalFastForward, originalFastBackward);
+  m_originalDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
+      CombineDatasets(preparedData["left-original-raw-slow-forward"],
+                      preparedData["left-original-raw-slow-backward"],
+                      preparedData["left-original-raw-fast-forward"],
+                      preparedData["left-original-raw-fast-backward"]);
+  m_originalDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kRight)] =
+      CombineDatasets(preparedData["right-original-raw-slow-forward"],
+                      preparedData["right-original-raw-slow-backward"],
+                      preparedData["right-original-raw-fast-forward"],
+                      preparedData["right-original-raw-fast-backward"]);
+
+  WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
+  sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+                              m_velocityDelays, m_minStepTime, m_maxStepTime);
+
+  auto slowForward = AnalysisManager::DataConcat(
+      preparedData["left-slow-forward"], preparedData["right-slow-forward"]);
+  auto slowBackward = AnalysisManager::DataConcat(
+      preparedData["left-slow-backward"], preparedData["right-slow-backward"]);
+  auto fastForward = AnalysisManager::DataConcat(
+      preparedData["left-fast-forward"], preparedData["right-fast-forward"]);
+  auto fastBackward = AnalysisManager::DataConcat(
+      preparedData["left-fast-backward"], preparedData["right-fast-backward"]);
+
+  WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+  sysid::AccelFilter(&preparedData);
+
+  WPI_INFO(m_logger, "{}", "Storing datasets.");
+
+  // Create the distinct raw datasets and store them
+  auto rawSlowForward =
+      AnalysisManager::DataConcat(preparedData["left-raw-slow-forward"],
+                                  preparedData["right-raw-slow-forward"]);
+  auto rawSlowBackward =
+      AnalysisManager::DataConcat(preparedData["left-raw-slow-backward"],
+                                  preparedData["right-raw-slow-backward"]);
+  auto rawFastForward =
+      AnalysisManager::DataConcat(preparedData["left-raw-fast-forward"],
+                                  preparedData["right-raw-fast-forward"]);
+  auto rawFastBackward =
+      AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"],
+                                  preparedData["right-raw-fast-backward"]);
+
+  m_rawDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward,
+                      rawFastBackward);
+  m_rawDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
+      CombineDatasets(preparedData["left-raw-slow-forward"],
+                      preparedData["left-raw-slow-backward"],
+                      preparedData["left-raw-fast-forward"],
+                      preparedData["left-raw-fast-backward"]);
+  m_rawDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kRight)] =
+      CombineDatasets(preparedData["right-raw-slow-forward"],
+                      preparedData["right-raw-slow-backward"],
+                      preparedData["right-raw-fast-forward"],
+                      preparedData["right-raw-fast-backward"]);
+
+  // Create the distinct filtered datasets and store them
+  m_filteredDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+      CombineDatasets(slowForward, slowBackward, fastForward, fastBackward);
+  m_filteredDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
+      CombineDatasets(preparedData["left-slow-forward"],
+                      preparedData["left-slow-backward"],
+                      preparedData["left-fast-forward"],
+                      preparedData["left-fast-backward"]);
+  m_filteredDataset[static_cast<int>(
+      AnalysisManager::Settings::DrivetrainDataset::kRight)] =
+      CombineDatasets(preparedData["right-slow-forward"],
+                      preparedData["right-slow-backward"],
+                      preparedData["right-fast-forward"],
+                      preparedData["right-fast-backward"]);
+
+  m_startTimes = {
+      rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp,
+      rawFastForward.front().timestamp, rawFastBackward.front().timestamp};
+}
+
+AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
+    : m_logger{logger},
+      m_settings{settings},
+      m_type{analysis::kSimple},
+      m_unit{"Meters"},
+      m_factor{1} {}
+
+AnalysisManager::AnalysisManager(std::string_view path, Settings& settings,
+                                 wpi::Logger& logger)
+    : m_logger{logger}, m_settings{settings} {
+  {
+    // Read JSON from the specified path
+    std::error_code ec;
+    wpi::raw_fd_istream is{path, ec};
+
+    if (ec) {
+      throw FileReadingError(path);
+    }
+
+    is >> m_json;
+
+    WPI_INFO(m_logger, "Read {}", path);
+  }
+
+  // Check that we have a sysid JSON
+  if (m_json.find("sysid") == m_json.end()) {
+    // If it's not a sysid JSON, try converting it from frc-char format
+    std::string newPath = sysid::ConvertJSON(path, logger);
+
+    // Read JSON from the specified path
+    std::error_code ec;
+    wpi::raw_fd_istream is{newPath, ec};
+
+    if (ec) {
+      throw FileReadingError(newPath);
+    }
+
+    is >> m_json;
+
+    WPI_INFO(m_logger, "Read {}", newPath);
+  }
+
+  WPI_INFO(m_logger, "Parsing initial data of {}", path);
+  // Get the analysis type from the JSON.
+  m_type = sysid::analysis::FromName(m_json.at("test").get<std::string>());
+
+  // Get the rotation -> output units factor from the JSON.
+  m_unit = m_json.at("units").get<std::string>();
+  m_factor = m_json.at("unitsPerRotation").get<double>();
+  WPI_DEBUG(m_logger, "Parsing units per rotation as {} {} per rotation",
+            m_factor, m_unit);
+
+  // Reset settings for Dynamic Test Limits
+  m_settings.stepTestDuration = units::second_t{0.0};
+  m_settings.motionThreshold = std::numeric_limits<double>::infinity();
+}
+
+void AnalysisManager::PrepareData() {
+  WPI_INFO(m_logger, "Preparing {} data", m_type.name);
+  if (m_type == analysis::kDrivetrain) {
+    PrepareLinearDrivetrainData();
+  } else if (m_type == analysis::kDrivetrainAngular) {
+    PrepareAngularDrivetrainData();
+  } else {
+    PrepareGeneralData();
+  }
+  WPI_INFO(m_logger, "{}", "Finished Preparing Data");
+}
+
+AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() {
+  if (m_filteredDataset.empty()) {
+    throw sysid::InvalidDataError(
+        "There is no data to perform gain calculation on.");
+  }
+
+  WPI_INFO(m_logger, "{}", "Calculating Gains");
+  // Calculate feedforward gains from the data.
+  const auto& ff = sysid::CalculateFeedforwardGains(GetFilteredData(), m_type);
+  FeedforwardGains ffGains = {ff, m_trackWidth};
+
+  const auto& Ks = std::get<0>(ff)[0];
+  const auto& Kv = std::get<0>(ff)[1];
+  const auto& Ka = std::get<0>(ff)[2];
+
+  if (Ka <= 0 || Kv < 0) {
+    throw InvalidDataError(
+        fmt::format("The calculated feedforward gains of kS: {}, Kv: {}, Ka: "
+                    "{} are erroneous. Your Ka should be > 0 while your Kv and "
+                    "Ks constants should both >= 0. Try adjusting the "
+                    "filtering and trimming settings or collect better data.",
+                    Ks, Kv, Ka));
+  }
+
+  return ffGains;
+}
+
+sysid::FeedbackGains AnalysisManager::CalculateFeedback(
+    std::vector<double> ff) {
+  const auto& Kv = ff[1];
+  const auto& Ka = ff[2];
+  FeedbackGains fb;
+  if (m_settings.type == FeedbackControllerLoopType::kPosition) {
+    fb = sysid::CalculatePositionFeedbackGains(
+        m_settings.preset, m_settings.lqr, Kv, Ka,
+        m_settings.convertGainsToEncTicks
+            ? m_settings.gearing * m_settings.cpr * m_factor
+            : 1);
+  } else {
+    fb = sysid::CalculateVelocityFeedbackGains(
+        m_settings.preset, m_settings.lqr, Kv, Ka,
+        m_settings.convertGainsToEncTicks
+            ? m_settings.gearing * m_settings.cpr * m_factor
+            : 1);
+  }
+
+  return fb;
+}
+
+void AnalysisManager::OverrideUnits(std::string_view unit,
+                                    double unitsPerRotation) {
+  m_unit = unit;
+  m_factor = unitsPerRotation;
+}
+
+void AnalysisManager::ResetUnitsFromJSON() {
+  m_unit = m_json.at("units").get<std::string>();
+  m_factor = m_json.at("unitsPerRotation").get<double>();
+}

sysid/src/main/native/cpp/analysis/AnalysisType.cpp

@@ -0,0 +1,23 @@
+// 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 "sysid/analysis/AnalysisType.h"
+
+using namespace sysid;
+
+AnalysisType sysid::analysis::FromName(std::string_view name) {
+  if (name == "Drivetrain") {
+    return sysid::analysis::kDrivetrain;
+  }
+  if (name == "Drivetrain (Angular)") {
+    return sysid::analysis::kDrivetrainAngular;
+  }
+  if (name == "Elevator") {
+    return sysid::analysis::kElevator;
+  }
+  if (name == "Arm") {
+    return sysid::analysis::kArm;
+  }
+  return sysid::analysis::kSimple;
+}

sysid/src/main/native/cpp/analysis/ArmSim.cpp

@@ -0,0 +1,64 @@
+// 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 "sysid/analysis/ArmSim.h"
+
+#include <cmath>
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/system/NumericalIntegration.h>
+#include <wpi/MathExtras.h>
+
+using namespace sysid;
+
+ArmSim::ArmSim(double Ks, double Kv, double Ka, double Kg, double offset,
+               double initialPosition, double initialVelocity)
+    // u = Ks sgn(x) + Kv x + Ka a + Kg cos(theta)
+    // Ka a = u - Ks sgn(x) - Kv x - Kg cos(theta)
+    // a = 1/Ka u - Ks/Ka sgn(x) - Kv/Ka x - Kg/Ka cos(theta)
+    // a = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(theta)
+    // a = Ax + Bu + c sgn(x) + d cos(theta)
+    : m_A{-Kv / Ka},
+      m_B{1.0 / Ka},
+      m_c{-Ks / Ka},
+      m_d{-Kg / Ka},
+      m_offset{offset} {
+  Reset(initialPosition, initialVelocity);
+}
+
+void ArmSim::Update(units::volt_t voltage, units::second_t dt) {
+  // Returns arm acceleration under gravity
+  auto f = [=, this](
+               const Eigen::Vector<double, 2>& x,
+               const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
+    return Eigen::Vector<double, 2>{
+        x(1), (m_A * x.block<1, 1>(1, 0) + m_B * u + m_c * wpi::sgn(x(1)) +
+               m_d * std::cos(x(0) + m_offset))(0)};
+  };
+
+  // Max error is large because an accurate sim isn't as important as the sim
+  // finishing in a timely manner. Otherwise, the timestep can become absurdly
+  // small for ill-conditioned data (e.g., high velocities with sharp spikes in
+  // acceleration).
+  Eigen::Vector<double, 1> u{voltage.value()};
+  m_x = frc::RKDP(f, m_x, u, dt, 0.25);
+}
+
+double ArmSim::GetPosition() const {
+  return m_x(0);
+}
+
+double ArmSim::GetVelocity() const {
+  return m_x(1);
+}
+
+double ArmSim::GetAcceleration(units::volt_t voltage) const {
+  Eigen::Vector<double, 1> u{voltage.value()};
+  return (m_A * m_x.block<1, 1>(1, 0) + m_B * u +
+          m_c * wpi::sgn(GetVelocity()) + m_d * std::cos(m_x(0) + m_offset))(0);
+}
+
+void ArmSim::Reset(double position, double velocity) {
+  m_x = Eigen::Vector<double, 2>{position, velocity};
+}

sysid/src/main/native/cpp/analysis/ElevatorSim.cpp

@@ -0,0 +1,50 @@
+// 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 "sysid/analysis/ElevatorSim.h"
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/system/Discretization.h>
+#include <wpi/MathExtras.h>
+
+using namespace sysid;
+
+ElevatorSim::ElevatorSim(double Ks, double Kv, double Ka, double Kg,
+                         double initialPosition, double initialVelocity)
+    // dx/dt = Ax + Bu + c sgn(x) + d
+    : m_A{{0.0, 1.0}, {0.0, -Kv / Ka}},
+      m_B{0.0, 1.0 / Ka},
+      m_c{0.0, -Ks / Ka},
+      m_d{0.0, -Kg / Ka} {
+  Reset(initialPosition, initialVelocity);
+}
+
+void ElevatorSim::Update(units::volt_t voltage, units::second_t dt) {
+  Eigen::Vector<double, 1> u{voltage.value()};
+
+  // Given dx/dt = Ax + Bu + c sgn(x) + d,
+  // x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x) + d)
+  Eigen::Matrix<double, 2, 2> Ad;
+  Eigen::Matrix<double, 2, 1> Bd;
+  frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd);
+  m_x = Ad * m_x + Bd * u +
+        Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()) + m_d);
+}
+
+double ElevatorSim::GetPosition() const {
+  return m_x(0);
+}
+
+double ElevatorSim::GetVelocity() const {
+  return m_x(1);
+}
+
+double ElevatorSim::GetAcceleration(units::volt_t voltage) const {
+  Eigen::Vector<double, 1> u{voltage.value()};
+  return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()) + m_d)(1);
+}
+
+void ElevatorSim::Reset(double position, double velocity) {
+  m_x = Eigen::Vector<double, 2>{position, velocity};
+}

sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp

@@ -0,0 +1,78 @@
+// 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 "sysid/analysis/FeedbackAnalysis.h"
+
+#include <frc/controller/LinearQuadraticRegulator.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/acceleration.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+#include "sysid/analysis/FeedbackControllerPreset.h"
+
+using namespace sysid;
+
+using Kv_t = decltype(1_V / 1_mps);
+using Ka_t = decltype(1_V / 1_mps_sq);
+
+FeedbackGains sysid::CalculatePositionFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka, double encFactor) {
+  // If acceleration requires no effort, velocity becomes an input for position
+  // control. We choose an appropriate model in this case to avoid numerical
+  // instabilities in the LQR.
+  if (Ka > 1E-7) {
+    // Create a position system from our feedforward gains.
+    auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
+        Kv_t(Kv), Ka_t(Ka));
+    // Create an LQR with 2 states to control -- position and velocity.
+    frc::LinearQuadraticRegulator<2, 1> controller{
+        system, {params.qp, params.qv}, {params.r}, preset.period};
+    // Compensate for any latency from sensor measurements, filtering, etc.
+    controller.LatencyCompensate(system, preset.period, 0.0_s);
+
+    return {controller.K(0, 0) * preset.outputConversionFactor / encFactor,
+            controller.K(0, 1) * preset.outputConversionFactor /
+                (encFactor * (preset.normalized ? 1 : preset.period.value()))};
+  }
+
+  // This is our special model to avoid instabilities in the LQR.
+  auto system = frc::LinearSystem<1, 1, 1>(
+      Eigen::Matrix<double, 1, 1>{0.0}, Eigen::Matrix<double, 1, 1>{1.0},
+      Eigen::Matrix<double, 1, 1>{1.0}, Eigen::Matrix<double, 1, 1>{0.0});
+  // Create an LQR with one state -- position.
+  frc::LinearQuadraticRegulator<1, 1> controller{
+      system, {params.qp}, {params.r}, preset.period};
+  // Compensate for any latency from sensor measurements, filtering, etc.
+  controller.LatencyCompensate(system, preset.period, 0.0_s);
+
+  return {Kv * controller.K(0, 0) * preset.outputConversionFactor / encFactor,
+          0.0};
+}
+
+FeedbackGains sysid::CalculateVelocityFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka, double encFactor) {
+  // If acceleration for velocity control requires no effort, the feedback
+  // control gains approach zero. We special-case it here because numerical
+  // instabilities arise in LQR otherwise.
+  if (Ka < 1E-7) {
+    return {0.0, 0.0};
+  }
+
+  // Create a velocity system from our feedforward gains.
+  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
+      Kv_t(Kv), Ka_t(Ka));
+  // Create an LQR controller with 1 state -- velocity.
+  frc::LinearQuadraticRegulator<1, 1> controller{
+      system, {params.qv}, {params.r}, preset.period};
+  // Compensate for any latency from sensor measurements, filtering, etc.
+  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
+
+  return {controller.K(0, 0) * preset.outputConversionFactor /
+              (preset.outputVelocityTimeFactor * encFactor),
+          0.0};
+}

sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp

@@ -0,0 +1,120 @@
+// 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 "sysid/analysis/FeedforwardAnalysis.h"
+
+#include <cmath>
+
+#include <units/math.h>
+#include <units/time.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/OLS.h"
+
+using namespace sysid;
+
+/**
+ * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ).
+ *
+ * @param d List of characterization data.
+ * @param type Type of system being identified.
+ * @param X Vector representation of X in y = Xβ.
+ * @param y Vector representation of y in y = Xβ.
+ */
+static void PopulateOLSData(const std::vector<PreparedData>& d,
+                            const AnalysisType& type,
+                            Eigen::Block<Eigen::MatrixXd> X,
+                            Eigen::VectorBlock<Eigen::VectorXd> y) {
+  for (size_t sample = 0; sample < d.size(); ++sample) {
+    const auto& pt = d[sample];
+
+    // Add the velocity term (for α)
+    X(sample, 0) = pt.velocity;
+
+    // Add the voltage term (for β)
+    X(sample, 1) = pt.voltage;
+
+    // Add the intercept term (for γ)
+    X(sample, 2) = std::copysign(1, pt.velocity);
+
+    // Add test-specific variables
+    if (type == analysis::kElevator) {
+      // Add the gravity term (for Kg)
+      X(sample, 3) = 1.0;
+    } else if (type == analysis::kArm) {
+      // Add the cosine and sine terms (for Kg)
+      X(sample, 3) = pt.cos;
+      X(sample, 4) = pt.sin;
+    }
+
+    // Add the dependent variable (acceleration)
+    y(sample) = pt.acceleration;
+  }
+}
+
+std::tuple<std::vector<double>, double, double>
+sysid::CalculateFeedforwardGains(const Storage& data,
+                                 const AnalysisType& type) {
+  // Iterate through the data and add it to our raw vector.
+  const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
+
+  const auto size = slowForward.size() + slowBackward.size() +
+                    fastForward.size() + fastBackward.size();
+
+  // Create a raw vector of doubles with our data in it.
+  Eigen::MatrixXd X{size, type.independentVariables};
+  Eigen::VectorXd y{size};
+
+  int rowOffset = 0;
+  PopulateOLSData(slowForward, type,
+                  X.block(rowOffset, 0, slowForward.size(), X.cols()),
+                  y.segment(rowOffset, slowForward.size()));
+
+  rowOffset += slowForward.size();
+  PopulateOLSData(slowBackward, type,
+                  X.block(rowOffset, 0, slowBackward.size(), X.cols()),
+                  y.segment(rowOffset, slowBackward.size()));
+
+  rowOffset += slowBackward.size();
+  PopulateOLSData(fastForward, type,
+                  X.block(rowOffset, 0, fastForward.size(), X.cols()),
+                  y.segment(rowOffset, fastForward.size()));
+
+  rowOffset += fastForward.size();
+  PopulateOLSData(fastBackward, type,
+                  X.block(rowOffset, 0, fastBackward.size(), X.cols()),
+                  y.segment(rowOffset, fastBackward.size()));
+
+  // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel)
+  // OLS performs best with the noisiest variable as the dependent var,
+  // so we regress accel in terms of the other variables.
+  auto ols = sysid::OLS(X, y);
+  double alpha = std::get<0>(ols)[0];  // -Kv/Ka
+  double beta = std::get<0>(ols)[1];   // 1/Ka
+  double gamma = std::get<0>(ols)[2];  // -Ks/Ka
+
+  // Initialize gains list with Ks, Kv, and Ka
+  std::vector<double> gains{-gamma / beta, -alpha / beta, 1 / beta};
+
+  if (type == analysis::kElevator) {
+    // Add Kg to gains list
+    double delta = std::get<0>(ols)[3];  // -Kg/Ka
+    gains.emplace_back(-delta / beta);
+  }
+
+  if (type == analysis::kArm) {
+    double delta = std::get<0>(ols)[3];    // -Kg/Ka cos(offset)
+    double epsilon = std::get<0>(ols)[4];  // Kg/Ka sin(offset)
+
+    // Add Kg to gains list
+    gains.emplace_back(std::hypot(delta, epsilon) / beta);
+
+    // Add offset to gains list
+    gains.emplace_back(std::atan2(epsilon, -delta));
+  }
+
+  // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)
+  return std::tuple{gains, std::get<1>(ols), std::get<2>(ols)};
+}

sysid/src/main/native/cpp/analysis/FilteringUtils.cpp

@@ -0,0 +1,417 @@
+// 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 "sysid/analysis/FilteringUtils.h"
+
+#include <limits>
+#include <numbers>
+#include <numeric>
+#include <stdexcept>
+#include <vector>
+
+#include <fmt/format.h>
+#include <frc/filter/LinearFilter.h>
+#include <frc/filter/MedianFilter.h>
+#include <units/math.h>
+#include <wpi/StringExtras.h>
+
+using namespace sysid;
+
+/**
+ * Helper function that throws if it detects that the data vector is too small
+ * for an operation of a certain window size.
+ *
+ * @param data The data that is being used.
+ * @param window The window size for the operation.
+ * @param operation The operation we're checking the size for (for error
+ *                  throwing purposes).
+ */
+static void CheckSize(const std::vector<PreparedData>& data, size_t window,
+                      std::string_view operation) {
+  if (data.size() < window) {
+    throw sysid::InvalidDataError(
+        fmt::format("Not enough data to run {} which has a window size of {}.",
+                    operation, window));
+  }
+}
+
+/**
+ * Helper function that determines if a certain key is storing raw data.
+ *
+ * @param key The key of the dataset
+ *
+ * @return True, if the key corresponds to a raw dataset.
+ */
+static bool IsRaw(std::string_view key) {
+  return wpi::contains(key, "raw") && !wpi::contains(key, "original");
+}
+
+/**
+ * Helper function that determines if a certain key is storing filtered data.
+ *
+ * @param key The key of the dataset
+ *
+ * @return True, if the key corresponds to a filtered dataset.
+ */
+static bool IsFiltered(std::string_view key) {
+  return !wpi::contains(key, "raw") && !wpi::contains(key, "original");
+}
+
+/**
+ * Fills in the rest of the PreparedData Structs for a PreparedData Vector.
+ *
+ * @param data A reference to a vector of the raw data.
+ * @param unit The units that the data is in (rotations, radians, or degrees)
+ *             for arm mechanisms.
+ */
+static void PrepareMechData(std::vector<PreparedData>* data,
+                            std::string_view unit = "") {
+  constexpr size_t kWindow = 3;
+
+  CheckSize(*data, kWindow, "Acceleration Calculation");
+
+  // Calculates the cosine of the position data for single jointed arm analysis
+  for (size_t i = 0; i < data->size(); ++i) {
+    auto& pt = data->at(i);
+
+    double cos = 0.0;
+    double sin = 0.0;
+    if (unit == "Radians") {
+      cos = std::cos(pt.position);
+      sin = std::sin(pt.position);
+    } else if (unit == "Degrees") {
+      cos = std::cos(pt.position * std::numbers::pi / 180.0);
+      sin = std::sin(pt.position * std::numbers::pi / 180.0);
+    } else if (unit == "Rotations") {
+      cos = std::cos(pt.position * 2 * std::numbers::pi);
+      sin = std::sin(pt.position * 2 * std::numbers::pi);
+    }
+    pt.cos = cos;
+    pt.sin = sin;
+  }
+
+  auto derivative =
+      CentralFiniteDifference<1, kWindow>(GetMeanTimeDelta(*data));
+
+  // Load the derivative filter with the first value for accurate initial
+  // behavior
+  for (size_t i = 0; i < kWindow; ++i) {
+    derivative.Calculate(data->at(0).velocity);
+  }
+
+  for (size_t i = (kWindow - 1) / 2; i < data->size(); ++i) {
+    data->at(i - (kWindow - 1) / 2).acceleration =
+        derivative.Calculate(data->at(i).velocity);
+  }
+
+  // Fill in accelerations past end of derivative filter
+  for (size_t i = data->size() - (kWindow - 1) / 2; i < data->size(); ++i) {
+    data->at(i).acceleration = 0.0;
+  }
+}
+
+std::tuple<units::second_t, units::second_t, units::second_t>
+sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
+                           AnalysisManager::Settings* settings,
+                           units::second_t minStepTime,
+                           units::second_t maxStepTime) {
+  auto voltageBegins =
+      std::find_if(data->begin(), data->end(),
+                   [](auto& datum) { return std::abs(datum.voltage) > 0; });
+
+  units::second_t firstTimestamp = voltageBegins->timestamp;
+  double firstPosition = voltageBegins->position;
+
+  auto motionBegins = std::find_if(
+      data->begin(), data->end(), [settings, firstPosition](auto& datum) {
+        return std::abs(datum.position - firstPosition) >
+               (settings->motionThreshold * datum.dt.value());
+      });
+
+  units::second_t positionDelay;
+  if (motionBegins != data->end()) {
+    positionDelay = motionBegins->timestamp - firstTimestamp;
+  } else {
+    positionDelay = 0_s;
+  }
+
+  auto maxAccel = std::max_element(
+      data->begin(), data->end(), [](const auto& a, const auto& b) {
+        return std::abs(a.acceleration) < std::abs(b.acceleration);
+      });
+
+  units::second_t velocityDelay;
+  if (maxAccel != data->end()) {
+    velocityDelay = maxAccel->timestamp - firstTimestamp;
+
+    // Trim data before max acceleration
+    data->erase(data->begin(), maxAccel);
+  } else {
+    velocityDelay = 0_s;
+  }
+
+  minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime);
+
+  // If step duration hasn't been set yet, calculate a default (find the entry
+  // before the acceleration first hits zero)
+  if (settings->stepTestDuration <= minStepTime) {
+    // Get noise floor
+    const double accelNoiseFloor = GetNoiseFloor(
+        *data, kNoiseMeanWindow, [](auto&& pt) { return pt.acceleration; });
+    // Find latest element with nonzero acceleration
+    auto endIt = std::find_if(
+        data->rbegin(), data->rend(), [&](const PreparedData& entry) {
+          return std::abs(entry.acceleration) > accelNoiseFloor;
+        });
+
+    if (endIt != data->rend()) {
+      // Calculate default duration
+      settings->stepTestDuration = std::min(
+          endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
+          maxStepTime);
+    } else {
+      settings->stepTestDuration = maxStepTime;
+    }
+  }
+
+  // Find first entry greater than the step test duration
+  auto maxIt =
+      std::find_if(data->begin(), data->end(), [&](PreparedData entry) {
+        return entry.timestamp - data->front().timestamp + minStepTime >
+               settings->stepTestDuration;
+      });
+
+  // Trim data beyond desired step test duration
+  if (maxIt != data->end()) {
+    data->erase(maxIt, data->end());
+  }
+  return std::make_tuple(minStepTime, positionDelay, velocityDelay);
+}
+
+double sysid::GetNoiseFloor(
+    const std::vector<PreparedData>& data, int window,
+    std::function<double(const PreparedData&)> accessorFunction) {
+  double sum = 0.0;
+  size_t step = window / 2;
+  auto averageFilter = frc::LinearFilter<double>::MovingAverage(window);
+  for (size_t i = 0; i < data.size(); i++) {
+    double mean = averageFilter.Calculate(accessorFunction(data[i]));
+    if (i >= step) {
+      sum += std::pow(accessorFunction(data[i - step]) - mean, 2);
+    }
+  }
+  return std::sqrt(sum / (data.size() - step));
+}
+
+units::second_t sysid::GetMeanTimeDelta(const std::vector<PreparedData>& data) {
+  std::vector<units::second_t> dts;
+
+  for (const auto& pt : data) {
+    if (pt.dt > 0_s && pt.dt < 500_ms) {
+      dts.emplace_back(pt.dt);
+    }
+  }
+
+  return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size();
+}
+
+units::second_t sysid::GetMeanTimeDelta(const Storage& data) {
+  std::vector<units::second_t> dts;
+
+  for (const auto& pt : data.slowForward) {
+    if (pt.dt > 0_s && pt.dt < 500_ms) {
+      dts.emplace_back(pt.dt);
+    }
+  }
+
+  for (const auto& pt : data.slowBackward) {
+    if (pt.dt > 0_s && pt.dt < 500_ms) {
+      dts.emplace_back(pt.dt);
+    }
+  }
+
+  for (const auto& pt : data.fastForward) {
+    if (pt.dt > 0_s && pt.dt < 500_ms) {
+      dts.emplace_back(pt.dt);
+    }
+  }
+
+  for (const auto& pt : data.fastBackward) {
+    if (pt.dt > 0_s && pt.dt < 500_ms) {
+      dts.emplace_back(pt.dt);
+    }
+  }
+
+  return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size();
+}
+
+void sysid::ApplyMedianFilter(std::vector<PreparedData>* data, int window) {
+  CheckSize(*data, window, "Median Filter");
+
+  frc::MedianFilter<double> medianFilter(window);
+
+  // Load the median filter with the first value for accurate initial behavior
+  for (int i = 0; i < window; i++) {
+    medianFilter.Calculate(data->at(0).velocity);
+  }
+
+  for (size_t i = (window - 1) / 2; i < data->size(); i++) {
+    data->at(i - (window - 1) / 2).velocity =
+        medianFilter.Calculate(data->at(i).velocity);
+  }
+
+  // Run the median filter for the last half window of datapoints by loading the
+  // median filter with the last recorded velocity value
+  for (size_t i = data->size() - (window - 1) / 2; i < data->size(); i++) {
+    data->at(i).velocity =
+        medianFilter.Calculate(data->at(data->size() - 1).velocity);
+  }
+}
+
+/**
+ * Removes a substring from a string reference
+ *
+ * @param str The std::string_view that needs modification
+ * @param removeStr The substring that needs to be removed
+ *
+ * @return an std::string without the specified substring
+ */
+static std::string RemoveStr(std::string_view str, std::string_view removeStr) {
+  size_t idx = str.find(removeStr);
+  if (idx == std::string_view::npos) {
+    return std::string{str};
+  } else {
+    return fmt::format("{}{}", str.substr(0, idx),
+                       str.substr(idx + removeStr.size()));
+  }
+}
+
+/**
+ * Figures out the max duration of the Dynamic tests
+ *
+ * @param data The raw data String Map
+ *
+ * @return The maximum duration of the Dynamic Tests
+ */
+static units::second_t GetMaxStepTime(
+    wpi::StringMap<std::vector<PreparedData>>& data) {
+  auto maxStepTime = 0_s;
+  for (auto& it : data) {
+    auto key = it.first();
+    auto& dataset = it.getValue();
+
+    if (IsRaw(key) && wpi::contains(key, "fast")) {
+      auto duration = dataset.back().timestamp - dataset.front().timestamp;
+      if (duration > maxStepTime) {
+        maxStepTime = duration;
+      }
+    }
+  }
+  return maxStepTime;
+}
+
+void sysid::InitialTrimAndFilter(
+    wpi::StringMap<std::vector<PreparedData>>* data,
+    AnalysisManager::Settings* settings,
+    std::vector<units::second_t>& positionDelays,
+    std::vector<units::second_t>& velocityDelays, units::second_t& minStepTime,
+    units::second_t& maxStepTime, std::string_view unit) {
+  auto& preparedData = *data;
+
+  // Find the maximum Step Test Duration of the dynamic tests
+  maxStepTime = GetMaxStepTime(preparedData);
+
+  // Calculate Velocity Threshold if it hasn't been set yet
+  if (settings->motionThreshold == std::numeric_limits<double>::infinity()) {
+    for (auto& it : preparedData) {
+      auto key = it.first();
+      auto& dataset = it.getValue();
+      if (wpi::contains(key, "slow")) {
+        settings->motionThreshold =
+            std::min(settings->motionThreshold,
+                     GetNoiseFloor(dataset, kNoiseMeanWindow,
+                                   [](auto&& pt) { return pt.velocity; }));
+      }
+    }
+  }
+
+  for (auto& it : preparedData) {
+    auto key = it.first();
+    auto& dataset = it.getValue();
+
+    // Trim quasistatic test data to remove all points where voltage is zero or
+    // velocity < motion threshold.
+    if (wpi::contains(key, "slow")) {
+      dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
+                                   [&](const auto& pt) {
+                                     return std::abs(pt.voltage) <= 0 ||
+                                            std::abs(pt.velocity) <
+                                                settings->motionThreshold;
+                                   }),
+                    dataset.end());
+
+      // Confirm there's still data
+      if (dataset.empty()) {
+        throw sysid::NoQuasistaticDataError();
+      }
+    }
+
+    // Apply Median filter
+    if (IsFiltered(key) && settings->medianWindow > 1) {
+      ApplyMedianFilter(&dataset, settings->medianWindow);
+    }
+
+    // Recalculate Accel and Cosine
+    PrepareMechData(&dataset, unit);
+
+    // Trims filtered Dynamic Test Data
+    if (IsFiltered(key) && wpi::contains(key, "fast")) {
+      // Get the filtered dataset name
+      auto filteredKey = RemoveStr(key, "raw-");
+
+      // Trim Filtered Data
+      auto [tempMinStepTime, positionDelay, velocityDelay] =
+          TrimStepVoltageData(&preparedData[filteredKey], settings, minStepTime,
+                              maxStepTime);
+
+      positionDelays.emplace_back(positionDelay);
+      velocityDelays.emplace_back(velocityDelay);
+
+      // Set the Raw Data to start at the same time as the Filtered Data
+      auto startTime = preparedData[filteredKey].front().timestamp;
+      auto rawStart =
+          std::find_if(preparedData[key].begin(), preparedData[key].end(),
+                       [&](auto&& pt) { return pt.timestamp == startTime; });
+      preparedData[key].erase(preparedData[key].begin(), rawStart);
+
+      // Confirm there's still data
+      if (preparedData[key].empty()) {
+        throw sysid::NoDynamicDataError();
+      }
+    }
+  }
+}
+
+void sysid::AccelFilter(wpi::StringMap<std::vector<PreparedData>>* data) {
+  auto& preparedData = *data;
+
+  // Remove points with acceleration = 0
+  for (auto& it : preparedData) {
+    auto& dataset = it.getValue();
+
+    for (size_t i = 0; i < dataset.size(); i++) {
+      if (dataset.at(i).acceleration == 0.0) {
+        dataset.erase(dataset.begin() + i);
+        i--;
+      }
+    }
+  }
+
+  // Confirm there's still data
+  if (std::any_of(preparedData.begin(), preparedData.end(),
+                  [](const auto& it) { return it.getValue().empty(); })) {
+    throw sysid::InvalidDataError(
+        "Acceleration filtering has removed all data.");
+  }
+}

sysid/src/main/native/cpp/analysis/JSONConverter.cpp

@@ -0,0 +1,165 @@
+// 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 "sysid/analysis/JSONConverter.h"
+
+#include <stdexcept>
+#include <string>
+
+#include <fmt/core.h>
+#include <fmt/format.h>
+#include <wpi/Logger.h>
+#include <wpi/fmt/raw_ostream.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+
+// Sizes of the arrays for new sysid data.
+static constexpr size_t kDrivetrainSize = 9;
+static constexpr size_t kGeneralSize = 4;
+
+// Indices for the old data.
+static constexpr size_t kTimestampCol = 0;
+static constexpr size_t kLVoltsCol = 3;
+static constexpr size_t kRVoltsCol = 4;
+static constexpr size_t kLPosCol = 5;
+static constexpr size_t kRPosCol = 6;
+static constexpr size_t kLVelCol = 7;
+static constexpr size_t kRVelCol = 8;
+
+static wpi::json GetJSON(std::string_view path, wpi::Logger& logger) {
+  std::error_code ec;
+  wpi::raw_fd_istream input{path, ec};
+
+  if (ec) {
+    throw std::runtime_error(fmt::format("Unable to read: {}", path));
+  }
+
+  wpi::json json;
+  input >> json;
+  WPI_INFO(logger, "Read frc-characterization JSON from {}", path);
+  return json;
+}
+
+std::string sysid::ConvertJSON(std::string_view path, wpi::Logger& logger) {
+  wpi::json ojson = GetJSON(path, logger);
+
+  auto type = sysid::analysis::FromName(ojson.at("test").get<std::string>());
+  auto factor = ojson.at("unitsPerRotation").get<double>();
+  auto unit = ojson.at("units").get<std::string>();
+
+  wpi::json json;
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    if (type == analysis::kDrivetrain) {
+      // Get the old data; create a vector for the new data; reserve the
+      // appropriate size for the new data.
+      auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
+      std::vector<std::array<double, kDrivetrainSize>> data;
+      data.reserve(odata.size());
+
+      // Transfer the data.
+      for (auto&& pt : odata) {
+        data.push_back(std::array<double, kDrivetrainSize>{
+            pt[kTimestampCol], pt[kLVoltsCol], pt[kRVoltsCol], pt[kLPosCol],
+            pt[kRPosCol], pt[kLVelCol], pt[kRVelCol], 0.0, 0.0});
+      }
+      json[key] = data;
+    } else {
+      // Get the old data; create a vector for the new data; reserve the
+      // appropriate size for the new data.
+      auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
+      std::vector<std::array<double, kGeneralSize>> data;
+      data.reserve(odata.size());
+
+      // Transfer the data.
+      for (auto&& pt : odata) {
+        data.push_back(std::array<double, kGeneralSize>{
+            pt[kTimestampCol], pt[kLVoltsCol], pt[kLPosCol], pt[kLVelCol]});
+      }
+      json[key] = data;
+    }
+  }
+  json["units"] = unit;
+  json["unitsPerRotation"] = factor;
+  json["test"] = type.name;
+  json["sysid"] = true;
+
+  // Write the new file with "_new" appended to it.
+  path.remove_suffix(std::string_view{".json"}.size());
+  std::string loc = fmt::format("{}_new.json", path);
+
+  sysid::SaveFile(json.dump(2), std::filesystem::path{loc});
+
+  WPI_INFO(logger, "Wrote new JSON to: {}", loc);
+  return loc;
+}
+
+std::string sysid::ToCSV(std::string_view path, wpi::Logger& logger) {
+  wpi::json json = GetJSON(path, logger);
+
+  auto type = sysid::analysis::FromName(json.at("test").get<std::string>());
+  auto factor = json.at("unitsPerRotation").get<double>();
+  auto unit = json.at("units").get<std::string>();
+  std::string_view abbreviation = GetAbbreviation(unit);
+
+  std::error_code ec;
+  // Naming: {sysid-json-name}(Test, Units).csv
+  path.remove_suffix(std::string_view{".json"}.size());
+  std::string loc = fmt::format("{} ({}, {}).csv", path, type.name, unit);
+  wpi::raw_fd_ostream outputFile{loc, ec};
+
+  if (ec) {
+    throw std::runtime_error("Unable to write to: " + loc);
+  }
+
+  fmt::print(outputFile, "Timestamp (s),Test,");
+  if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
+    fmt::print(
+        outputFile,
+        "Left Volts (V),Right Volts (V),Left Position ({0}),Right "
+        "Position ({0}),Left Velocity ({0}/s),Right Velocity ({0}/s),Gyro "
+        "Position (deg),Gyro Rate (deg/s)\n",
+        abbreviation);
+  } else {
+    fmt::print(outputFile, "Volts (V),Position({0}),Velocity ({0}/s)\n",
+               abbreviation);
+  }
+  outputFile << "\n";
+
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
+      auto tempData =
+          json.at(key).get<std::vector<std::array<double, kDrivetrainSize>>>();
+      for (auto&& pt : tempData) {
+        fmt::print(outputFile, "{},{},{},{},{},{},{},{},{},{}\n",
+                   pt[0],                           // Timestamp
+                   key,                             // Test
+                   pt[1], pt[2],                    // Left and Right Voltages
+                   pt[3] * factor, pt[4] * factor,  // Left and Right Positions
+                   pt[5] * factor, pt[6] * factor,  // Left and Right Velocity
+                   pt[7], pt[8]  // Gyro Position and Velocity
+        );
+      }
+    } else {
+      auto tempData =
+          json.at(key).get<std::vector<std::array<double, kGeneralSize>>>();
+      for (auto&& pt : tempData) {
+        fmt::print(outputFile, "{},{},{},{},{}\n",
+                   pt[0],           // Timestamp,
+                   key,             // Test
+                   pt[1],           // Voltage
+                   pt[2] * factor,  // Position
+                   pt[3] * factor   // Velocity
+        );
+      }
+    }
+  }
+  outputFile.flush();
+  WPI_INFO(logger, "Wrote CSV to: {}", loc);
+  return loc;
+}

sysid/src/main/native/cpp/analysis/OLS.cpp

@@ -0,0 +1,48 @@
+// 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 "sysid/analysis/OLS.h"
+
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+
+using namespace sysid;
+
+std::tuple<std::vector<double>, double, double> sysid::OLS(
+    const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
+  assert(X.rows() == y.rows());
+
+  // The linear model can be written as follows:
+  // y = Xβ + u, where y is the dependent observed variable, X is the matrix
+  // of independent variables, β is a vector of coefficients, and u is a
+  // vector of residuals.
+
+  // We want to minimize u² = uᵀu = (y - Xβ)ᵀ(y - Xβ).
+  // β = (XᵀX)⁻¹Xᵀy
+
+  // Calculate β that minimizes uᵀu.
+  Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y);
+
+  // We will now calculate R² or the coefficient of determination, which
+  // tells us how much of the total variation (variation in y) can be
+  // explained by the regression model.
+
+  // We will first calculate the sum of the squares of the error, or the
+  // variation in error (SSE).
+  double SSE = (y - X * beta).squaredNorm();
+
+  int n = X.cols();
+
+  // Now we will calculate the total variation in y, known as SSTO.
+  double SSTO = ((y.transpose() * y) - (1.0 / n) * (y.transpose() * y)).value();
+
+  double rSquared = (SSTO - SSE) / SSTO;
+  double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - 3.0));
+  double RMSE = std::sqrt(SSE / n);
+
+  return {{beta.data(), beta.data() + beta.rows()}, adjRSquared, RMSE};
+}

sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp

@@ -0,0 +1,47 @@
+// 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 "sysid/analysis/SimpleMotorSim.h"
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/system/Discretization.h>
+#include <wpi/MathExtras.h>
+
+using namespace sysid;
+
+SimpleMotorSim::SimpleMotorSim(double Ks, double Kv, double Ka,
+                               double initialPosition, double initialVelocity)
+    // dx/dt = Ax + Bu + c sgn(x)
+    : m_A{{0.0, 1.0}, {0.0, -Kv / Ka}}, m_B{0.0, 1.0 / Ka}, m_c{0.0, -Ks / Ka} {
+  Reset(initialPosition, initialVelocity);
+}
+
+void SimpleMotorSim::Update(units::volt_t voltage, units::second_t dt) {
+  Eigen::Vector<double, 1> u{voltage.value()};
+
+  // Given dx/dt = Ax + Bu + c sgn(x),
+  // x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x))
+  Eigen::Matrix<double, 2, 2> Ad;
+  Eigen::Matrix<double, 2, 1> Bd;
+  frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd);
+  m_x = Ad * m_x + Bd * u +
+        Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()));
+}
+
+double SimpleMotorSim::GetPosition() const {
+  return m_x(0);
+}
+
+double SimpleMotorSim::GetVelocity() const {
+  return m_x(1);
+}
+
+double SimpleMotorSim::GetAcceleration(units::volt_t voltage) const {
+  Eigen::Vector<double, 1> u{voltage.value()};
+  return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()))(1);
+}
+
+void SimpleMotorSim::Reset(double position, double velocity) {
+  m_x = Eigen::Vector<double, 2>{position, velocity};
+}

sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.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 "sysid/analysis/TrackWidthAnalysis.h"
+
+#include <cmath>
+
+double sysid::CalculateTrackWidth(double l, double r, units::radian_t accum) {
+  // The below comes from solving ω = (vr − vl) / 2r for 2r.
+  return (std::abs(r) + std::abs(l)) / std::abs(accum.value());
+}