[sysid] Add SysId (#5672)

The source is copied from this commit:
625ff04784.

sysid/src/main/native/cpp/App.cpp

@@ -0,0 +1,219 @@
+// 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 <cstdio>
+
+#ifndef RUNNING_SYSID_TESTS
+
+#include <filesystem>
+#include <memory>
+#include <string_view>
+
+#include <fmt/format.h>
+#include <glass/Context.h>
+#include <glass/MainMenuBar.h>
+#include <glass/Storage.h>
+#include <glass/Window.h>
+#include <glass/WindowManager.h>
+#include <glass/other/Log.h>
+#include <imgui.h>
+#include <uv.h>
+#include <wpi/Logger.h>
+#include <wpigui.h>
+#include <wpigui_openurl.h>
+
+#include "sysid/view/Analyzer.h"
+#include "sysid/view/JSONConverter.h"
+#include "sysid/view/Logger.h"
+#include "sysid/view/UILayout.h"
+
+namespace gui = wpi::gui;
+
+static std::unique_ptr<glass::WindowManager> gWindowManager;
+
+glass::Window* gLoggerWindow;
+glass::Window* gAnalyzerWindow;
+glass::Window* gProgramLogWindow;
+static glass::MainMenuBar gMainMenu;
+
+std::unique_ptr<sysid::JSONConverter> gJSONConverter;
+
+glass::LogData gLog;
+wpi::Logger gLogger;
+
+const char* GetWPILibVersion();
+
+namespace sysid {
+std::string_view GetResource_sysid_16_png();
+std::string_view GetResource_sysid_32_png();
+std::string_view GetResource_sysid_48_png();
+std::string_view GetResource_sysid_64_png();
+std::string_view GetResource_sysid_128_png();
+std::string_view GetResource_sysid_256_png();
+std::string_view GetResource_sysid_512_png();
+}  // namespace sysid
+
+void Application(std::string_view saveDir) {
+  // Create the wpigui (along with Dear ImGui) and Glass contexts.
+  gui::CreateContext();
+  glass::CreateContext();
+
+  // Add icons
+  gui::AddIcon(sysid::GetResource_sysid_16_png());
+  gui::AddIcon(sysid::GetResource_sysid_32_png());
+  gui::AddIcon(sysid::GetResource_sysid_48_png());
+  gui::AddIcon(sysid::GetResource_sysid_64_png());
+  gui::AddIcon(sysid::GetResource_sysid_128_png());
+  gui::AddIcon(sysid::GetResource_sysid_256_png());
+  gui::AddIcon(sysid::GetResource_sysid_512_png());
+
+  glass::SetStorageName("sysid");
+  glass::SetStorageDir(saveDir.empty() ? gui::GetPlatformSaveFileDir()
+                                       : saveDir);
+
+  // Add messages from the global sysid logger into the Log window.
+  gLogger.SetLogger([](unsigned int level, const char* file, unsigned int line,
+                       const char* msg) {
+    const char* lvl = "";
+    if (level >= wpi::WPI_LOG_CRITICAL) {
+      lvl = "CRITICAL: ";
+    } else if (level >= wpi::WPI_LOG_ERROR) {
+      lvl = "ERROR: ";
+    } else if (level >= wpi::WPI_LOG_WARNING) {
+      lvl = "WARNING: ";
+    } else if (level >= wpi::WPI_LOG_INFO) {
+      lvl = "INFO: ";
+    } else if (level >= wpi::WPI_LOG_DEBUG) {
+      lvl = "DEBUG: ";
+    }
+    std::string filename = std::filesystem::path{file}.filename().string();
+    gLog.Append(fmt::format("{}{} ({}:{})\n", lvl, msg, filename, line));
+#ifndef NDEBUG
+    fmt::print(stderr, "{}{} ({}:{})\n", lvl, msg, filename, line);
+#endif
+  });
+
+  gLogger.set_min_level(wpi::WPI_LOG_DEBUG);
+  // Set the number of workers for the libuv threadpool.
+  uv_os_setenv("UV_THREADPOOL_SIZE", "6");
+
+  // Initialize window manager and add views.
+  auto& storage = glass::GetStorageRoot().GetChild("SysId");
+  gWindowManager = std::make_unique<glass::WindowManager>(storage);
+  gWindowManager->GlobalInit();
+
+  gLoggerWindow = gWindowManager->AddWindow(
+      "Logger", std::make_unique<sysid::Logger>(storage, gLogger));
+
+  gAnalyzerWindow = gWindowManager->AddWindow(
+      "Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
+
+  gProgramLogWindow = gWindowManager->AddWindow(
+      "Program Log", std::make_unique<glass::LogView>(&gLog));
+
+  // Set default positions and sizes for windows.
+
+  // Logger window position/size
+  gLoggerWindow->SetDefaultPos(sysid::kLoggerWindowPos.x,
+                               sysid::kLoggerWindowPos.y);
+  gLoggerWindow->SetDefaultSize(sysid::kLoggerWindowSize.x,
+                                sysid::kLoggerWindowSize.y);
+
+  // Analyzer window position/size
+  gAnalyzerWindow->SetDefaultPos(sysid::kAnalyzerWindowPos.x,
+                                 sysid::kAnalyzerWindowPos.y);
+  gAnalyzerWindow->SetDefaultSize(sysid::kAnalyzerWindowSize.x,
+                                  sysid::kAnalyzerWindowSize.y);
+
+  // Program log window position/size
+  gProgramLogWindow->SetDefaultPos(sysid::kProgramLogWindowPos.x,
+                                   sysid::kProgramLogWindowPos.y);
+  gProgramLogWindow->SetDefaultSize(sysid::kProgramLogWindowSize.x,
+                                    sysid::kProgramLogWindowSize.y);
+  gProgramLogWindow->DisableRenamePopup();
+
+  gJSONConverter = std::make_unique<sysid::JSONConverter>(gLogger);
+
+  // Configure save file.
+  gui::ConfigurePlatformSaveFile("sysid.ini");
+
+  // Add menu bar.
+  gui::AddLateExecute([] {
+    ImGui::BeginMainMenuBar();
+    gMainMenu.WorkspaceMenu();
+    gui::EmitViewMenu();
+
+    if (ImGui::BeginMenu("Widgets")) {
+      gWindowManager->DisplayMenu();
+      ImGui::EndMenu();
+    }
+
+    bool about = false;
+    if (ImGui::BeginMenu("Info")) {
+      if (ImGui::MenuItem("About")) {
+        about = true;
+      }
+      ImGui::EndMenu();
+    }
+
+    bool toCSV = false;
+    if (ImGui::BeginMenu("JSON Converters")) {
+      if (ImGui::MenuItem("JSON to CSV Converter")) {
+        toCSV = true;
+      }
+
+      ImGui::EndMenu();
+    }
+
+    if (ImGui::BeginMenu("Docs")) {
+      if (ImGui::MenuItem("Online documentation")) {
+        wpi::gui::OpenURL(
+            "https://docs.wpilib.org/en/stable/docs/software/pathplanning/"
+            "system-identification/");
+      }
+
+      ImGui::EndMenu();
+    }
+
+    ImGui::EndMainMenuBar();
+
+    if (toCSV) {
+      ImGui::OpenPopup("SysId JSON to CSV Converter");
+      toCSV = false;
+    }
+
+    if (ImGui::BeginPopupModal("SysId JSON to CSV Converter")) {
+      gJSONConverter->DisplayCSVConvert();
+      if (ImGui::Button("Close")) {
+        ImGui::CloseCurrentPopup();
+      }
+      ImGui::EndPopup();
+    }
+
+    if (about) {
+      ImGui::OpenPopup("About");
+      about = false;
+    }
+    if (ImGui::BeginPopupModal("About")) {
+      ImGui::Text("SysId: System Identification for Robot Mechanisms");
+      ImGui::Separator();
+      ImGui::Text("v%s", GetWPILibVersion());
+      ImGui::Separator();
+      ImGui::Text("Save location: %s", glass::GetStorageDir().c_str());
+      if (ImGui::Button("Close")) {
+        ImGui::CloseCurrentPopup();
+      }
+      ImGui::EndPopup();
+    }
+  });
+
+  gui::Initialize("System Identification", sysid::kAppWindowSize.x,
+                  sysid::kAppWindowSize.y);
+  gui::Main();
+
+  glass::DestroyContext();
+  gui::DestroyContext();
+}
+
+#endif

sysid/src/main/native/cpp/Main.cpp

@@ -0,0 +1,29 @@
+// 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 <string_view>
+
+#ifndef RUNNING_SYSID_TESTS
+
+void Application(std::string_view saveDir);
+
+#ifdef _WIN32
+int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
+                      int nCmdShow) {
+  int argc = __argc;
+  char** argv = __argv;
+#else
+int main(int argc, char** argv) {
+#endif
+  std::string_view saveDir;
+  if (argc == 2) {
+    saveDir = argv[1];
+  }
+
+  Application(saveDir);
+
+  return 0;
+}
+
+#endif

sysid/src/main/native/cpp/Util.cpp

@@ -0,0 +1,80 @@
+// 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/Util.h"
+
+#include <filesystem>
+#include <stdexcept>
+
+#include <imgui.h>
+#include <wpi/raw_ostream.h>
+
+void sysid::CreateTooltip(const char* text) {
+  ImGui::SameLine();
+  ImGui::TextDisabled(" (?)");
+
+  if (ImGui::IsItemHovered()) {
+    ImGui::BeginTooltip();
+    ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f);
+    ImGui::TextUnformatted(text);
+    ImGui::PopTextWrapPos();
+    ImGui::EndTooltip();
+  }
+}
+
+void sysid::CreateErrorPopup(bool& isError, std::string_view errorMessage) {
+  if (isError) {
+    ImGui::OpenPopup("Exception Caught!");
+  }
+
+  // Handle exceptions.
+  ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+  if (ImGui::BeginPopupModal("Exception Caught!")) {
+    ImGui::PushTextWrapPos(0.0f);
+    ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
+                       errorMessage.data());
+    ImGui::PopTextWrapPos();
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+      isError = false;
+    }
+    ImGui::EndPopup();
+  }
+}
+
+std::string_view sysid::GetAbbreviation(std::string_view unit) {
+  if (unit == "Meters") {
+    return "m";
+  } else if (unit == "Feet") {
+    return "ft";
+  } else if (unit == "Inches") {
+    return "in";
+  } else if (unit == "Radians") {
+    return "rad";
+  } else if (unit == "Degrees") {
+    return "deg";
+  } else if (unit == "Rotations") {
+    return "rot";
+  } else {
+    throw std::runtime_error("Invalid Unit");
+  }
+}
+
+void sysid::SaveFile(std::string_view contents,
+                     const std::filesystem::path& path) {
+  // Create the path if it doesn't already exist.
+  std::filesystem::create_directories(path.root_directory());
+
+  // Open a fd_ostream to write to file.
+  std::error_code ec;
+  wpi::raw_fd_ostream ostream{path.string(), ec};
+
+  // Check error code.
+  if (ec) {
+    throw std::runtime_error("Cannot write to file: " + ec.message());
+  }
+
+  // Write contents.
+  ostream << contents;
+}

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

sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp

@@ -0,0 +1,275 @@
+// 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/telemetry/TelemetryManager.h"
+
+#include <algorithm>
+#include <cctype>  // for ::tolower
+#include <numbers>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+#include <fmt/chrono.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/Logger.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/timestamp.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+
+using namespace sysid;
+
+TelemetryManager::TelemetryManager(const Settings& settings,
+                                   wpi::Logger& logger,
+                                   nt::NetworkTableInstance instance)
+    : m_settings(settings), m_logger(logger), m_inst(instance) {}
+
+void TelemetryManager::BeginTest(std::string_view name) {
+  // Create a new test params instance for this test.
+  m_params =
+      TestParameters{name.starts_with("fast"), name.ends_with("forward"),
+                     m_settings.mechanism == analysis::kDrivetrainAngular,
+                     State::WaitingForEnable};
+
+  // Add this test to the list of running tests and set the running flag.
+  m_tests.push_back(std::string{name});
+  m_isRunningTest = true;
+
+  // Set the Voltage Command Entry
+  m_voltageCommand.Set((m_params.fast ? m_settings.stepVoltage
+                                      : m_settings.quasistaticRampRate) *
+                       (m_params.forward ? 1 : -1));
+
+  // Set the test type
+  m_testType.Set(m_params.fast ? "Dynamic" : "Quasistatic");
+
+  // Set the rotate entry
+  m_rotate.Set(m_params.rotate);
+
+  // Set the current mechanism in NT.
+  m_mechanism.Set(m_settings.mechanism.name);
+  // Set Overflow to False
+  m_overflowPub.Set(false);
+  // Set Mechanism Error to False
+  m_mechErrorPub.Set(false);
+  m_inst.Flush();
+
+  // Display the warning message.
+  for (auto&& func : m_callbacks) {
+    func(
+        "Please enable the robot in autonomous mode, and then "
+        "disable it "
+        "before it runs out of space. \n Note: The robot will "
+        "continue "
+        "to move until you disable it - It is your "
+        "responsibility to "
+        "ensure it does not hit anything!");
+  }
+
+  WPI_INFO(m_logger, "Started {} test.", m_tests.back());
+}
+
+void TelemetryManager::EndTest() {
+  // If there is no test running, this is a no-op
+  if (!m_isRunningTest) {
+    return;
+  }
+
+  // Disable the running flag and store the data in the JSON.
+  m_isRunningTest = false;
+  m_data[m_tests.back()] = m_params.data;
+
+  // Call the cancellation callbacks.
+  for (auto&& func : m_callbacks) {
+    std::string msg;
+    if (m_params.mechError) {
+      msg +=
+          "\nERROR: The robot indicated that you are using the wrong project "
+          "for characterizing your mechanism. \nThis most likely means you "
+          "are trying to characterize a mechanism like a Drivetrain with a "
+          "deployed config for a General Mechanism (e.g. Arm, Flywheel, and "
+          "Elevator) or vice versa. Please double check your settings and "
+          "try again.";
+    } else if (!m_params.data.empty()) {
+      std::string units = m_settings.units;
+      std::transform(m_settings.units.begin(), m_settings.units.end(),
+                     units.begin(), ::tolower);
+
+      if (std::string_view{m_settings.mechanism.name}.starts_with(
+              "Drivetrain")) {
+        double p = (m_params.data.back()[3] - m_params.data.front()[3]) *
+                   m_settings.unitsPerRotation;
+        double s = (m_params.data.back()[4] - m_params.data.front()[4]) *
+                   m_settings.unitsPerRotation;
+        double g = m_params.data.back()[7] - m_params.data.front()[7];
+
+        msg = fmt::format(
+            "The left and right encoders traveled {} {} and {} {} "
+            "respectively.\nThe gyro angle delta was {} degrees.",
+            p, units, s, units, g * 180.0 / std::numbers::pi);
+      } else {
+        double p = (m_params.data.back()[2] - m_params.data.front()[2]) *
+                   m_settings.unitsPerRotation;
+        msg = fmt::format("The encoder reported traveling {} {}.", p, units);
+      }
+
+      if (m_params.overflow) {
+        msg +=
+            "\nNOTE: the robot stopped recording data early because the entry "
+            "storage was exceeded.";
+      }
+    } else {
+      msg = "No data was detected.";
+    }
+    func(msg);
+  }
+
+  // Remove previously run test from list of tests if no data was detected.
+  if (m_params.data.empty()) {
+    m_tests.pop_back();
+  }
+
+  // Send a zero command over NT.
+  m_voltageCommand.Set(0.0);
+  m_inst.Flush();
+}
+
+void TelemetryManager::Update() {
+  // If there is no test running, these is nothing to update.
+  if (!m_isRunningTest) {
+    return;
+  }
+
+  // Update the NT entries that we're reading.
+
+  int currAckNumber = m_ackNumberSub.Get();
+  std::string telemetryValue;
+
+  // Get the FMS Control Word.
+  for (auto tsValue : m_fmsControlData.ReadQueue()) {
+    uint32_t ctrl = tsValue.value;
+    m_params.enabled = ctrl & 0x01;
+  }
+
+  // Get the string in the data field.
+  for (auto tsValue : m_telemetry.ReadQueue()) {
+    telemetryValue = tsValue.value;
+  }
+
+  // Get the overflow flag
+  for (auto tsValue : m_overflowSub.ReadQueue()) {
+    m_params.overflow = tsValue.value;
+  }
+
+  // Get the mechanism error flag
+  for (auto tsValue : m_mechErrorSub.ReadQueue()) {
+    m_params.mechError = tsValue.value;
+  }
+
+  // Go through our state machine.
+  if (m_params.state == State::WaitingForEnable) {
+    if (m_params.enabled) {
+      m_params.enableStart = wpi::Now() * 1E-6;
+      m_params.state = State::RunningTest;
+      m_ackNumber = currAckNumber;
+      WPI_INFO(m_logger, "{}", "Transitioned to running test state.");
+    }
+  }
+
+  if (m_params.state == State::RunningTest) {
+    // If for some reason we've disconnected, end the test.
+    if (!m_inst.IsConnected()) {
+      WPI_WARNING(m_logger, "{}",
+                  "NT connection was dropped when executing the test. The test "
+                  "has been canceled.");
+      EndTest();
+    }
+
+    // If the robot has disabled, then we can move on to the next step.
+    if (!m_params.enabled) {
+      m_params.disableStart = wpi::Now() * 1E-6;
+      m_params.state = State::WaitingForData;
+      WPI_INFO(m_logger, "{}", "Transitioned to waiting for data.");
+    }
+  }
+
+  if (m_params.state == State::WaitingForData) {
+    double now = wpi::Now() * 1E-6;
+    m_voltageCommand.Set(0.0);
+    m_inst.Flush();
+
+    // Process valid data
+    if (!telemetryValue.empty() && m_ackNumber < currAckNumber) {
+      m_params.raw = std::move(telemetryValue);
+      m_ackNumber = currAckNumber;
+    }
+
+    // We have the data that we need, so we can parse it and end the test.
+    if (!m_params.raw.empty() &&
+        wpi::starts_with(m_params.raw, m_tests.back())) {
+      // Remove test type from start of string
+      m_params.raw.erase(0, m_params.raw.find(';') + 1);
+
+      // Clean up the string -- remove spaces if there are any.
+      m_params.raw.erase(
+          std::remove_if(m_params.raw.begin(), m_params.raw.end(), ::isspace),
+          m_params.raw.end());
+
+      // Split the string into individual components.
+      wpi::SmallVector<std::string_view, 16> res;
+      wpi::split(m_params.raw, res, ',');
+
+      // Convert each string to double.
+      std::vector<double> values;
+      values.reserve(res.size());
+      for (auto&& str : res) {
+        values.push_back(wpi::parse_float<double>(str).value());
+      }
+
+      // Add the values to our result vector.
+      for (size_t i = 0; i < values.size() - m_settings.mechanism.rawDataSize;
+           i += m_settings.mechanism.rawDataSize) {
+        std::vector<double> d(m_settings.mechanism.rawDataSize);
+
+        std::copy_n(std::make_move_iterator(values.begin() + i),
+                    m_settings.mechanism.rawDataSize, d.begin());
+        m_params.data.push_back(std::move(d));
+      }
+
+      WPI_INFO(m_logger,
+               "Received data with size: {} for the {} test in {} seconds.",
+               m_params.data.size(), m_tests.back(),
+               m_params.data.back()[0] - m_params.data.front()[0]);
+      m_ackNumberPub.Set(++m_ackNumber);
+      EndTest();
+    }
+
+    // If we timed out, end the test and let the user know.
+    if (now - m_params.disableStart > 5.0) {
+      WPI_WARNING(m_logger, "{}",
+                  "TelemetryManager did not receieve data 5 seconds after "
+                  "completing the test...");
+      EndTest();
+    }
+  }
+}
+
+std::string TelemetryManager::SaveJSON(std::string_view location) {
+  m_data["test"] = m_settings.mechanism.name;
+  m_data["units"] = m_settings.units;
+  m_data["unitsPerRotation"] = m_settings.unitsPerRotation;
+  m_data["sysid"] = true;
+
+  std::string loc = fmt::format("{}/sysid_data{:%Y%m%d-%H%M%S}.json", location,
+                                std::chrono::system_clock::now());
+
+  sysid::SaveFile(m_data.dump(2), std::filesystem::path{loc});
+  WPI_INFO(m_logger, "Wrote JSON to: {}", loc);
+
+  return loc;
+}

sysid/src/main/native/cpp/view/Analyzer.cpp

@@ -0,0 +1,851 @@
+// 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/view/Analyzer.h"
+
+#include <algorithm>
+#include <exception>
+#include <filesystem>
+#include <numbers>
+#include <thread>
+
+#include <fmt/core.h>
+#include <glass/Context.h>
+#include <glass/Storage.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <wpi/json.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/view/UILayout.h"
+
+using namespace sysid;
+
+Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger)
+    : m_location(""), m_logger(logger) {
+  // Fill the StringMap with preset values.
+  m_presets["Default"] = presets::kDefault;
+  m_presets["WPILib (2020-)"] = presets::kWPILibNew;
+  m_presets["WPILib (Pre-2020)"] = presets::kWPILibOld;
+  m_presets["CANCoder"] = presets::kCTRECANCoder;
+  m_presets["CTRE"] = presets::kCTREDefault;
+  m_presets["CTRE (Pro)"] = presets::kCTREProDefault;
+  m_presets["REV Brushless Encoder Port"] = presets::kREVNEOBuiltIn;
+  m_presets["REV Brushed Encoder Port"] = presets::kREVNonNEO;
+  m_presets["REV Data Port"] = presets::kREVNonNEO;
+  m_presets["Venom"] = presets::kVenom;
+
+  ResetData();
+  UpdateFeedbackGains();
+}
+
+void Analyzer::UpdateFeedforwardGains() {
+  WPI_INFO(m_logger, "{}", "Gain calc");
+  try {
+    const auto& [ff, trackWidth] = m_manager->CalculateFeedforward();
+    m_ff = std::get<0>(ff);
+    m_accelRSquared = std::get<1>(ff);
+    m_accelRMSE = std::get<2>(ff);
+    m_trackWidth = trackWidth;
+    m_settings.preset.measurementDelay =
+        m_settings.type == FeedbackControllerLoopType::kPosition
+            ? m_manager->GetPositionDelay()
+            : m_manager->GetVelocityDelay();
+    m_conversionFactor = m_manager->GetFactor();
+    PrepareGraphs();
+  } catch (const sysid::InvalidDataError& e) {
+    m_state = AnalyzerState::kGeneralDataError;
+    HandleError(e.what());
+  } catch (const sysid::NoQuasistaticDataError& e) {
+    m_state = AnalyzerState::kMotionThresholdError;
+    HandleError(e.what());
+  } catch (const sysid::NoDynamicDataError& e) {
+    m_state = AnalyzerState::kTestDurationError;
+    HandleError(e.what());
+  } catch (const AnalysisManager::FileReadingError& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  } catch (const wpi::json::exception& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  } catch (const std::exception& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  }
+}
+
+void Analyzer::UpdateFeedbackGains() {
+  if (m_ff[1] > 0 && m_ff[2] > 0) {
+    const auto& fb = m_manager->CalculateFeedback(m_ff);
+    m_timescale = units::second_t{m_ff[2] / m_ff[1]};
+    m_Kp = fb.Kp;
+    m_Kd = fb.Kd;
+  }
+}
+
+bool Analyzer::DisplayGain(const char* text, double* data,
+                           bool readOnly = true) {
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+  if (readOnly) {
+    return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G",
+                              ImGuiInputTextFlags_ReadOnly);
+  } else {
+    return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G");
+  }
+}
+
+static void SetPosition(double beginX, double beginY, double xShift,
+                        double yShift) {
+  ImGui::SetCursorPos(ImVec2(beginX + xShift * 10 * ImGui::GetFontSize(),
+                             beginY + yShift * 1.75 * ImGui::GetFontSize()));
+}
+
+bool Analyzer::IsErrorState() {
+  return m_state == AnalyzerState::kMotionThresholdError ||
+         m_state == AnalyzerState::kTestDurationError ||
+         m_state == AnalyzerState::kGeneralDataError ||
+         m_state == AnalyzerState::kFileError;
+}
+
+bool Analyzer::IsDataErrorState() {
+  return m_state == AnalyzerState::kMotionThresholdError ||
+         m_state == AnalyzerState::kTestDurationError ||
+         m_state == AnalyzerState::kGeneralDataError;
+}
+
+void Analyzer::DisplayFileSelector() {
+  // Get the current width of the window. This will be used to scale
+  // our UI elements.
+  float width = ImGui::GetContentRegionAvail().x;
+
+  // Show the file location along with an option to choose.
+  if (ImGui::Button("Select")) {
+    m_selector = std::make_unique<pfd::open_file>(
+        "Select Data", "",
+        std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
+  }
+  ImGui::SameLine();
+  ImGui::SetNextItemWidth(width - ImGui::CalcTextSize("Select").x -
+                          ImGui::GetFontSize() * 5);
+  ImGui::InputText("##location", &m_location, ImGuiInputTextFlags_ReadOnly);
+}
+
+void Analyzer::ResetData() {
+  m_plot.ResetData();
+  m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
+  m_location = "";
+  m_ff = std::vector<double>{1, 1, 1};
+  UpdateFeedbackGains();
+}
+
+bool Analyzer::DisplayResetAndUnitOverride() {
+  auto type = m_manager->GetAnalysisType();
+  auto unit = m_manager->GetUnit();
+
+  float width = ImGui::GetContentRegionAvail().x;
+  ImGui::SameLine(width - ImGui::CalcTextSize("Reset").x);
+  if (ImGui::Button("Reset")) {
+    ResetData();
+    m_state = AnalyzerState::kWaitingForJSON;
+    return true;
+  }
+
+  if (type == analysis::kDrivetrain) {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+    if (ImGui::Combo("Dataset", &m_dataset, kDatasets, 3)) {
+      m_settings.dataset =
+          static_cast<AnalysisManager::Settings::DrivetrainDataset>(m_dataset);
+      PrepareData();
+    }
+    ImGui::SameLine();
+  } else {
+    m_settings.dataset =
+        AnalysisManager::Settings::DrivetrainDataset::kCombined;
+  }
+
+  ImGui::Spacing();
+  ImGui::Text(
+      "Units:              %s\n"
+      "Units Per Rotation: %.4f\n"
+      "Type:               %s",
+      std::string(unit).c_str(), m_conversionFactor, type.name);
+
+  if (type == analysis::kDrivetrainAngular) {
+    ImGui::SameLine();
+    sysid::CreateTooltip(
+        "Here, the units and units per rotation represent what the wheel "
+        "positions and velocities were captured in. The track width value "
+        "will reflect the unit selected here. However, the Kv and Ka will "
+        "always be in Vs/rad and Vs^2 / rad respectively.");
+  }
+
+  if (ImGui::Button("Override Units")) {
+    ImGui::OpenPopup("Override Units");
+  }
+
+  auto size = ImGui::GetIO().DisplaySize;
+  ImGui::SetNextWindowSize(ImVec2(size.x / 4, size.y * 0.2));
+  if (ImGui::BeginPopupModal("Override Units")) {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+    ImGui::Combo("Units", &m_selectedOverrideUnit, kUnits,
+                 IM_ARRAYSIZE(kUnits));
+    unit = kUnits[m_selectedOverrideUnit];
+
+    if (unit == "Degrees") {
+      m_conversionFactor = 360.0;
+    } else if (unit == "Radians") {
+      m_conversionFactor = 2 * std::numbers::pi;
+    } else if (unit == "Rotations") {
+      m_conversionFactor = 1.0;
+    }
+
+    bool isRotational = m_selectedOverrideUnit > 2;
+
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+    ImGui::InputDouble(
+        "Units Per Rotation", &m_conversionFactor, 0.0, 0.0, "%.4f",
+        isRotational ? ImGuiInputTextFlags_ReadOnly : ImGuiInputTextFlags_None);
+
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+      m_manager->OverrideUnits(unit, m_conversionFactor);
+      PrepareData();
+    }
+
+    ImGui::EndPopup();
+  }
+
+  ImGui::SameLine();
+  if (ImGui::Button("Reset Units from JSON")) {
+    m_manager->ResetUnitsFromJSON();
+    PrepareData();
+  }
+
+  return false;
+}
+
+void Analyzer::ConfigParamsOnFileSelect() {
+  WPI_INFO(m_logger, "{}", "Configuring Params");
+  m_stepTestDuration = m_settings.stepTestDuration.to<float>();
+
+  // Estimate qp as 1/8 * units-per-rot
+  m_settings.lqr.qp = 0.125 * m_manager->GetFactor();
+  // Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
+  m_settings.lqr.qv = 0.25 * (12.0 - m_ff[0]) / m_ff[1];
+}
+
+void Analyzer::Display() {
+  DisplayFileSelector();
+  DisplayGraphs();
+
+  switch (m_state) {
+    case AnalyzerState::kWaitingForJSON: {
+      ImGui::Text(
+          "SysId is currently in theoretical analysis mode.\n"
+          "To analyze recorded test data, select a "
+          "data JSON.");
+      sysid::CreateTooltip(
+          "Theoretical feedback gains can be calculated from a "
+          "physical model of the mechanism being controlled. "
+          "Theoretical gains for several common mechanisms can "
+          "be obtained from ReCalc (https://reca.lc).");
+      ImGui::Spacing();
+      ImGui::Spacing();
+
+      ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+      if (ImGui::CollapsingHeader("Feedforward Gains (Theoretical)")) {
+        float beginX = ImGui::GetCursorPosX();
+        float beginY = ImGui::GetCursorPosY();
+        CollectFeedforwardGains(beginX, beginY);
+      }
+      ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+      if (ImGui::CollapsingHeader("Feedback Analysis")) {
+        DisplayFeedbackGains();
+      }
+      break;
+    }
+    case AnalyzerState::kNominalDisplay: {  // Allow the user to select which
+                                            // data set they want analyzed and
+                                            // add a
+      // reset button. Also show the units and the units per rotation.
+      if (DisplayResetAndUnitOverride()) {
+        return;
+      }
+      ImGui::Spacing();
+      ImGui::Spacing();
+
+      ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+      if (ImGui::CollapsingHeader("Feedforward Analysis")) {
+        float beginX = ImGui::GetCursorPosX();
+        float beginY = ImGui::GetCursorPosY();
+        DisplayFeedforwardGains(beginX, beginY);
+      }
+      ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+      if (ImGui::CollapsingHeader("Feedback Analysis")) {
+        DisplayFeedbackGains();
+      }
+      break;
+    }
+    case AnalyzerState::kFileError: {
+      CreateErrorPopup(m_errorPopup, m_exception);
+      if (!m_errorPopup) {
+        m_state = AnalyzerState::kWaitingForJSON;
+        return;
+      }
+      break;
+    }
+    case AnalyzerState::kGeneralDataError:
+    case AnalyzerState::kTestDurationError:
+    case AnalyzerState::kMotionThresholdError: {
+      CreateErrorPopup(m_errorPopup, m_exception);
+      if (DisplayResetAndUnitOverride()) {
+        return;
+      }
+      float beginX = ImGui::GetCursorPosX();
+      float beginY = ImGui::GetCursorPosY();
+      DisplayFeedforwardParameters(beginX, beginY);
+      break;
+    }
+  }
+
+  // Periodic functions
+  try {
+    SelectFile();
+  } catch (const AnalysisManager::FileReadingError& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  } catch (const wpi::json::exception& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  }
+}
+
+void Analyzer::PrepareData() {
+  try {
+    m_manager->PrepareData();
+    UpdateFeedforwardGains();
+    UpdateFeedbackGains();
+  } catch (const sysid::InvalidDataError& e) {
+    m_state = AnalyzerState::kGeneralDataError;
+    HandleError(e.what());
+  } catch (const sysid::NoQuasistaticDataError& e) {
+    m_state = AnalyzerState::kMotionThresholdError;
+    HandleError(e.what());
+  } catch (const sysid::NoDynamicDataError& e) {
+    m_state = AnalyzerState::kTestDurationError;
+    HandleError(e.what());
+  } catch (const AnalysisManager::FileReadingError& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  } catch (const wpi::json::exception& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  } catch (const std::exception& e) {
+    m_state = AnalyzerState::kFileError;
+    HandleError(e.what());
+  }
+}
+
+void Analyzer::PrepareRawGraphs() {
+  if (m_manager->HasData()) {
+    AbortDataPrep();
+    m_dataThread = std::thread([&] {
+      m_plot.SetRawData(m_manager->GetOriginalData(), m_manager->GetUnit(),
+                        m_abortDataPrep);
+    });
+  }
+}
+
+void Analyzer::PrepareGraphs() {
+  if (m_manager->HasData()) {
+    WPI_INFO(m_logger, "{}", "Graph state");
+    AbortDataPrep();
+    m_dataThread = std::thread([&] {
+      m_plot.SetData(m_manager->GetRawData(), m_manager->GetFilteredData(),
+                     m_manager->GetUnit(), m_ff, m_manager->GetStartTimes(),
+                     m_manager->GetAnalysisType(), m_abortDataPrep);
+    });
+    UpdateFeedbackGains();
+    m_state = AnalyzerState::kNominalDisplay;
+  }
+}
+
+void Analyzer::HandleError(std::string_view msg) {
+  m_exception = msg;
+  m_errorPopup = true;
+  if (m_state == AnalyzerState::kFileError) {
+    m_location = "";
+  }
+  PrepareRawGraphs();
+}
+
+void Analyzer::DisplayGraphs() {
+  ImGui::SetNextWindowPos(ImVec2{kDiagnosticPlotWindowPos},
+                          ImGuiCond_FirstUseEver);
+  ImGui::SetNextWindowSize(ImVec2{kDiagnosticPlotWindowSize},
+                           ImGuiCond_FirstUseEver);
+  ImGui::Begin("Diagnostic Plots");
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+  if (ImGui::SliderFloat("Point Size", &m_plot.m_pointSize, 1, 2, "%.2f")) {
+    if (!IsErrorState()) {
+      PrepareGraphs();
+    } else {
+      PrepareRawGraphs();
+    }
+  }
+
+  ImGui::SameLine();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+  const char* items[] = {"Forward", "Backward"};
+  if (ImGui::Combo("Direction", &m_plot.m_direction, items, 2)) {
+    if (!IsErrorState()) {
+      PrepareGraphs();
+    } else {
+      PrepareRawGraphs();
+    }
+  }
+
+  // If the plots were already loaded, store the scroll position. Else go to
+  // the last recorded scroll position if they have just been initialized
+  bool plotsLoaded = m_plot.DisplayPlots();
+  if (plotsLoaded) {
+    if (m_prevPlotsLoaded) {
+      m_graphScroll = ImGui::GetScrollY();
+    } else {
+      ImGui::SetScrollY(m_graphScroll);
+    }
+
+    // If a JSON is selected
+    if (m_state == AnalyzerState::kNominalDisplay) {
+      DisplayGain("Acceleration R²", &m_accelRSquared);
+      CreateTooltip(
+          "The coefficient of determination of the OLS fit of acceleration "
+          "versus velocity and voltage.  Acceleration is extremely noisy, "
+          "so this is generally quite small.");
+
+      ImGui::SameLine();
+      DisplayGain("Acceleration RMSE", &m_accelRMSE);
+      CreateTooltip(
+          "The standard deviation of the residuals from the predicted "
+          "acceleration."
+          "This can be interpreted loosely as the mean measured disturbance "
+          "from the \"ideal\" system equation.");
+
+      DisplayGain("Sim velocity R²", m_plot.GetSimRSquared());
+      CreateTooltip(
+          "The coefficient of determination the simulated velocity. "
+          "Velocity is much less-noisy than acceleration, so this "
+          "is pretty close to 1 for a decent fit.");
+
+      ImGui::SameLine();
+      DisplayGain("Sim velocity RMSE", m_plot.GetSimRMSE());
+      CreateTooltip(
+          "The standard deviation of the residuals from the simulated velocity "
+          "predictions - essentially the size of the mean error of the "
+          "simulated model "
+          "in the recorded velocity units.");
+    }
+  }
+  m_prevPlotsLoaded = plotsLoaded;
+
+  ImGui::End();
+}
+
+void Analyzer::SelectFile() {
+  // If the selector exists and is ready with a result, we can store it.
+  if (m_selector && m_selector->ready() && !m_selector->result().empty()) {
+    // Store the location of the file and reset the selector.
+    WPI_INFO(m_logger, "Opening File: {}", m_selector->result()[0]);
+    m_location = m_selector->result()[0];
+    m_selector.reset();
+    WPI_INFO(m_logger, "{}", "Opened File");
+    m_manager =
+        std::make_unique<AnalysisManager>(m_location, m_settings, m_logger);
+    PrepareData();
+    m_dataset = 0;
+    m_settings.dataset =
+        AnalysisManager::Settings::DrivetrainDataset::kCombined;
+    ConfigParamsOnFileSelect();
+    UpdateFeedbackGains();
+  }
+}
+
+void Analyzer::AbortDataPrep() {
+  if (m_dataThread.joinable()) {
+    m_abortDataPrep = true;
+    m_dataThread.join();
+    m_abortDataPrep = false;
+  }
+}
+
+void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) {
+  // Increase spacing to not run into trackwidth in the normal analyzer view
+  constexpr double kHorizontalOffset = 0.9;
+  SetPosition(beginX, beginY, kHorizontalOffset, 0);
+
+  bool displayAll =
+      !IsErrorState() || m_state == AnalyzerState::kGeneralDataError;
+
+  if (displayAll) {
+    // Wait for enter before refresh so double digit entries like "15" don't
+    // prematurely refresh with "1". That can cause display stuttering.
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+    int window = m_settings.medianWindow;
+    if (ImGui::InputInt("Window Size", &window, 0, 0,
+                        ImGuiInputTextFlags_EnterReturnsTrue)) {
+      m_settings.medianWindow = std::clamp(window, 1, 15);
+      PrepareData();
+    }
+
+    CreateTooltip(
+        "The number of samples in the velocity median "
+        "filter's sliding window.");
+  }
+
+  if (displayAll || m_state == AnalyzerState::kMotionThresholdError) {
+    // Wait for enter before refresh so decimal inputs like "0.2" don't
+    // prematurely refresh with a velocity threshold of "0".
+    SetPosition(beginX, beginY, kHorizontalOffset, 1);
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+    double threshold = m_settings.motionThreshold;
+    if (ImGui::InputDouble("Velocity Threshold", &threshold, 0.0, 0.0, "%.3f",
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+      m_settings.motionThreshold = std::max(0.0, threshold);
+      PrepareData();
+    }
+    CreateTooltip("Velocity data below this threshold will be ignored.");
+  }
+
+  if (displayAll || m_state == AnalyzerState::kTestDurationError) {
+    SetPosition(beginX, beginY, kHorizontalOffset, 2);
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+    if (ImGui::SliderFloat("Test Duration", &m_stepTestDuration,
+                           m_manager->GetMinStepTime().value(),
+                           m_manager->GetMaxStepTime().value(), "%.2f")) {
+      m_settings.stepTestDuration = units::second_t{m_stepTestDuration};
+      PrepareData();
+    }
+  }
+}
+
+void Analyzer::CollectFeedforwardGains(float beginX, float beginY) {
+  SetPosition(beginX, beginY, 0, 0);
+  if (DisplayGain("Kv", &m_ff[1], false)) {
+    UpdateFeedbackGains();
+  }
+
+  SetPosition(beginX, beginY, 0, 1);
+  if (DisplayGain("Ka", &m_ff[2], false)) {
+    UpdateFeedbackGains();
+  }
+
+  SetPosition(beginX, beginY, 0, 2);
+  // Show Timescale
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayGain("Response Timescale (ms)",
+              reinterpret_cast<double*>(&m_timescale));
+  CreateTooltip(
+      "The characteristic timescale of the system response in milliseconds. "
+      "Both the control loop period and total signal delay should be "
+      "at least 3-5 times shorter than this to optimally control the "
+      "system.");
+}
+
+void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
+  SetPosition(beginX, beginY, 0, 0);
+  DisplayGain("Ks", &m_ff[0]);
+
+  SetPosition(beginX, beginY, 0, 1);
+  DisplayGain("Kv", &m_ff[1]);
+
+  SetPosition(beginX, beginY, 0, 2);
+  DisplayGain("Ka", &m_ff[2]);
+
+  SetPosition(beginX, beginY, 0, 3);
+  // Show Timescale
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayGain("Response Timescale (ms)",
+              reinterpret_cast<double*>(&m_timescale));
+  CreateTooltip(
+      "The characteristic timescale of the system response in milliseconds. "
+      "Both the control loop period and total signal delay should be "
+      "at least 3-5 times shorter than this to optimally control the "
+      "system.");
+
+  SetPosition(beginX, beginY, 0, 4);
+  auto positionDelay = m_manager->GetPositionDelay();
+  DisplayGain("Position Measurement Delay (ms)",
+              reinterpret_cast<double*>(&positionDelay));
+  CreateTooltip(
+      "The average elapsed time between the first application of "
+      "voltage and the first detected change in mechanism position "
+      "in the step-voltage tests.  This includes CAN delays, and "
+      "may overestimate the true delay for on-motor-controller "
+      "feedback loops by up to 20ms.");
+
+  SetPosition(beginX, beginY, 0, 5);
+  auto velocityDelay = m_manager->GetVelocityDelay();
+  DisplayGain("Velocity Measurement Delay (ms)",
+              reinterpret_cast<double*>(&velocityDelay));
+  CreateTooltip(
+      "The average elapsed time between the first application of "
+      "voltage and the maximum calculated mechanism acceleration "
+      "in the step-voltage tests.  This includes CAN delays, and "
+      "may overestimate the true delay for on-motor-controller "
+      "feedback loops by up to 20ms.");
+
+  SetPosition(beginX, beginY, 0, 6);
+
+  if (m_manager->GetAnalysisType() == analysis::kElevator) {
+    DisplayGain("Kg", &m_ff[3]);
+  } else if (m_manager->GetAnalysisType() == analysis::kArm) {
+    DisplayGain("Kg", &m_ff[3]);
+
+    double offset;
+    auto unit = m_manager->GetUnit();
+    if (unit == "Radians") {
+      offset = m_ff[4];
+    } else if (unit == "Degrees") {
+      offset = m_ff[4] / std::numbers::pi * 180.0;
+    } else if (unit == "Rotations") {
+      offset = m_ff[4] / (2 * std::numbers::pi);
+    }
+    DisplayGain(
+        fmt::format("Angle offset to horizontal ({})", GetAbbreviation(unit))
+            .c_str(),
+        &offset);
+    CreateTooltip(
+        "This is the angle offset which, when added to the angle measurement, "
+        "zeroes it out when the arm is horizontal. This is needed for the arm "
+        "feedforward to work.");
+  } else if (m_trackWidth) {
+    DisplayGain("Track Width", &*m_trackWidth);
+  }
+  double endY = ImGui::GetCursorPosY();
+
+  DisplayFeedforwardParameters(beginX, beginY);
+  ImGui::SetCursorPosY(endY);
+}
+
+void Analyzer::DisplayFeedbackGains() {
+  // Allow the user to select a feedback controller preset.
+  ImGui::Spacing();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+  if (ImGui::Combo("Gain Preset", &m_selectedPreset, kPresetNames,
+                   IM_ARRAYSIZE(kPresetNames))) {
+    m_settings.preset = m_presets[kPresetNames[m_selectedPreset]];
+    m_settings.type = FeedbackControllerLoopType::kVelocity;
+    m_selectedLoopType =
+        static_cast<int>(FeedbackControllerLoopType::kVelocity);
+    m_settings.convertGainsToEncTicks = m_selectedPreset > 2;
+    UpdateFeedbackGains();
+  }
+  ImGui::SameLine();
+  sysid::CreateTooltip(
+      "Gain presets represent how feedback gains are calculated for your "
+      "specific feedback controller:\n\n"
+      "Default, WPILib (2020-): For use with the new WPILib PIDController "
+      "class.\n"
+      "WPILib (Pre-2020): For use with the old WPILib PIDController class.\n"
+      "CTRE: For use with CTRE units. These are the default units that ship "
+      "with CTRE motor controllers.\n"
+      "REV (Brushless): For use with NEO and NEO 550 motors on a SPARK MAX.\n"
+      "REV (Brushed): For use with brushed motors connected to a SPARK MAX.");
+
+  if (m_settings.preset != m_presets[kPresetNames[m_selectedPreset]]) {
+    ImGui::SameLine();
+    ImGui::TextDisabled("(modified)");
+  }
+
+  // Show our feedback controller preset values.
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  double value = m_settings.preset.outputConversionFactor * 12;
+  if (ImGui::InputDouble("Max Controller Output", &value, 0.0, 0.0, "%.1f") &&
+      value > 0) {
+    m_settings.preset.outputConversionFactor = value / 12.0;
+    UpdateFeedbackGains();
+  }
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  value = m_settings.preset.outputVelocityTimeFactor;
+  if (ImGui::InputDouble("Velocity Denominator Units (s)", &value, 0.0, 0.0,
+                         "%.1f") &&
+      value > 0) {
+    m_settings.preset.outputVelocityTimeFactor = value;
+    UpdateFeedbackGains();
+  }
+
+  sysid::CreateTooltip(
+      "This represents the denominator of the velocity unit used by the "
+      "feedback controller. For example, CTRE uses 100 ms = 0.1 s.");
+
+  auto ShowPresetValue = [](const char* text, double* data,
+                            float cursorX = 0.0f) {
+    if (cursorX > 0) {
+      ImGui::SetCursorPosX(cursorX);
+    }
+
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+    return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G");
+  };
+
+  // Show controller period.
+  if (ShowPresetValue("Controller Period (ms)",
+                      reinterpret_cast<double*>(&m_settings.preset.period))) {
+    if (m_settings.preset.period > 0_s &&
+        m_settings.preset.measurementDelay >= 0_s) {
+      UpdateFeedbackGains();
+    }
+  }
+
+  // Show whether the controller gains are time-normalized.
+  if (ImGui::Checkbox("Time-Normalized?", &m_settings.preset.normalized)) {
+    UpdateFeedbackGains();
+  }
+
+  // Show position/velocity measurement delay.
+  if (ShowPresetValue(
+          "Measurement Delay (ms)",
+          reinterpret_cast<double*>(&m_settings.preset.measurementDelay))) {
+    if (m_settings.preset.period > 0_s &&
+        m_settings.preset.measurementDelay >= 0_s) {
+      UpdateFeedbackGains();
+    }
+  }
+
+  sysid::CreateTooltip(
+      "The average measurement delay of the process variable in milliseconds. "
+      "This may depend on your encoder settings and choice of motor "
+      "controller. Default velocity filtering windows are quite long "
+      "on many motor controllers, so be careful that this value is "
+      "accurate if the characteristic timescale of the mechanism "
+      "is small.");
+
+  // Add CPR and Gearing for converting Feedback Gains
+  ImGui::Separator();
+  ImGui::Spacing();
+
+  if (ImGui::Checkbox("Convert Gains to Encoder Counts",
+                      &m_settings.convertGainsToEncTicks)) {
+    UpdateFeedbackGains();
+  }
+  sysid::CreateTooltip(
+      "Whether the feedback gains should be in terms of encoder counts or "
+      "output units. Because smart motor controllers usually don't have "
+      "direct access to the output units (i.e. m/s for a drivetrain), they "
+      "perform feedback on the encoder counts directly. If you are using a "
+      "PID Controller on the RoboRIO, you are probably performing feedback "
+      "on the output units directly.\n\nNote that if you have properly set "
+      "up position and velocity conversion factors with the SPARK MAX, you "
+      "can leave this box unchecked. The motor controller will perform "
+      "feedback on the output directly.");
+
+  if (m_settings.convertGainsToEncTicks) {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+    if (ImGui::InputDouble("##Numerator", &m_gearingNumerator, 0.0, 0.0, "%.4f",
+                           ImGuiInputTextFlags_EnterReturnsTrue) &&
+        m_gearingNumerator > 0) {
+      m_settings.gearing = m_gearingNumerator / m_gearingDenominator;
+      UpdateFeedbackGains();
+    }
+    ImGui::SameLine();
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+    if (ImGui::InputDouble("##Denominator", &m_gearingDenominator, 0.0, 0.0,
+                           "%.4f", ImGuiInputTextFlags_EnterReturnsTrue) &&
+        m_gearingDenominator > 0) {
+      m_settings.gearing = m_gearingNumerator / m_gearingDenominator;
+      UpdateFeedbackGains();
+    }
+    sysid::CreateTooltip(
+        "The gearing between the encoder and the motor shaft (# of encoder "
+        "turns / # of motor shaft turns).");
+
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+    if (ImGui::InputInt("CPR", &m_settings.cpr, 0, 0,
+                        ImGuiInputTextFlags_EnterReturnsTrue) &&
+        m_settings.cpr > 0) {
+      UpdateFeedbackGains();
+    }
+    sysid::CreateTooltip(
+        "The counts per rotation of your encoder. This is the number of counts "
+        "reported in user code when the encoder is rotated exactly once. Some "
+        "common values for various motors/encoders are:\n\n"
+        "Falcon 500: 2048\nNEO: 1\nCTRE Mag Encoder / CANCoder: 4096\nREV "
+        "Through Bore Encoder: 8192\n");
+  }
+
+  ImGui::Separator();
+  ImGui::Spacing();
+
+  // Allow the user to select a loop type.
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+  if (ImGui::Combo("Loop Type", &m_selectedLoopType, kLoopTypes,
+                   IM_ARRAYSIZE(kLoopTypes))) {
+    m_settings.type =
+        static_cast<FeedbackControllerLoopType>(m_selectedLoopType);
+    if (m_state == AnalyzerState::kWaitingForJSON) {
+      m_settings.preset.measurementDelay = 0_ms;
+    } else {
+      if (m_settings.type == FeedbackControllerLoopType::kPosition) {
+        m_settings.preset.measurementDelay = m_manager->GetPositionDelay();
+      } else {
+        m_settings.preset.measurementDelay = m_manager->GetVelocityDelay();
+      }
+    }
+    UpdateFeedbackGains();
+  }
+
+  ImGui::Spacing();
+
+  // Show Kp and Kd.
+  float beginY = ImGui::GetCursorPosY();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayGain("Kp", &m_Kp);
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayGain("Kd", &m_Kd);
+
+  // Come back to the starting y pos.
+  ImGui::SetCursorPosY(beginY);
+
+  if (m_selectedLoopType == 0) {
+    std::string unit;
+    if (m_state != AnalyzerState::kWaitingForJSON) {
+      unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit()));
+    }
+
+    ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+    if (DisplayGain(fmt::format("Max Position Error{}", unit).c_str(),
+                    &m_settings.lqr.qp, false)) {
+      if (m_settings.lqr.qp > 0) {
+        UpdateFeedbackGains();
+      }
+    }
+  }
+
+  std::string unit;
+  if (m_state != AnalyzerState::kWaitingForJSON) {
+    unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit()));
+  }
+
+  ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+  if (DisplayGain(fmt::format("Max Velocity Error{}", unit).c_str(),
+                  &m_settings.lqr.qv, false)) {
+    if (m_settings.lqr.qv > 0) {
+      UpdateFeedbackGains();
+    }
+  }
+  ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+  if (DisplayGain("Max Control Effort (V)", &m_settings.lqr.r, false)) {
+    if (m_settings.lqr.r > 0) {
+      UpdateFeedbackGains();
+    }
+  }
+}

sysid/src/main/native/cpp/view/AnalyzerPlot.cpp

@@ -0,0 +1,531 @@
+// 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/view/AnalyzerPlot.h"
+
+#include <algorithm>
+#include <cmath>
+#include <mutex>
+
+#include <fmt/format.h>
+#include <units/math.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/ArmSim.h"
+#include "sysid/analysis/ElevatorSim.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/SimpleMotorSim.h"
+
+using namespace sysid;
+
+static ImPlotPoint Getter(int idx, void* data) {
+  return static_cast<ImPlotPoint*>(data)[idx];
+}
+
+template <typename Model>
+static std::vector<std::vector<ImPlotPoint>> PopulateTimeDomainSim(
+    const std::vector<PreparedData>& data,
+    const std::array<units::second_t, 4>& startTimes, size_t step, Model model,
+    double* simSquaredErrorSum, double* squaredVariationSum,
+    int* timeSeriesPoints) {
+  // Create the vector of ImPlotPoints that will contain our simulated data.
+  std::vector<std::vector<ImPlotPoint>> pts;
+  std::vector<ImPlotPoint> tmp;
+
+  auto startTime = data[0].timestamp;
+
+  tmp.emplace_back(startTime.value(), data[0].velocity);
+
+  model.Reset(data[0].position, data[0].velocity);
+  units::second_t t = 0_s;
+
+  for (size_t i = 1; i < data.size(); ++i) {
+    const auto& now = data[i];
+    const auto& pre = data[i - 1];
+
+    t += now.timestamp - pre.timestamp;
+
+    // If the current time stamp and previous time stamp are across a test's
+    // start timestamp, it is the start of a new test and the model needs to be
+    // reset.
+    if (std::find(startTimes.begin(), startTimes.end(), now.timestamp) !=
+        startTimes.end()) {
+      pts.emplace_back(std::move(tmp));
+      model.Reset(now.position, now.velocity);
+      continue;
+    }
+
+    model.Update(units::volt_t{pre.voltage}, now.timestamp - pre.timestamp);
+    tmp.emplace_back((startTime + t).value(), model.GetVelocity());
+    *simSquaredErrorSum += std::pow(now.velocity - model.GetVelocity(), 2);
+    *squaredVariationSum += std::pow(now.velocity, 2);
+    ++(*timeSeriesPoints);
+  }
+
+  pts.emplace_back(std::move(tmp));
+  return pts;
+}
+
+AnalyzerPlot::AnalyzerPlot(wpi::Logger& logger) : m_logger(logger) {}
+
+void AnalyzerPlot::SetRawTimeData(const std::vector<PreparedData>& rawSlow,
+                                  const std::vector<PreparedData>& rawFast,
+                                  std::atomic<bool>& abort) {
+  auto rawSlowStep = std::ceil(rawSlow.size() * 1.0 / kMaxSize * 4);
+  auto rawFastStep = std::ceil(rawFast.size() * 1.0 / kMaxSize * 4);
+  // Populate Raw Slow Time Series Data
+  for (size_t i = 0; i < rawSlow.size(); i += rawSlowStep) {
+    if (abort) {
+      return;
+    }
+    m_quasistaticData.rawData.emplace_back((rawSlow[i].timestamp).value(),
+                                           rawSlow[i].velocity);
+  }
+
+  // Populate Raw fast Time Series Data
+  for (size_t i = 0; i < rawFast.size(); i += rawFastStep) {
+    if (abort) {
+      return;
+    }
+    m_dynamicData.rawData.emplace_back((rawFast[i].timestamp).value(),
+                                       rawFast[i].velocity);
+  }
+}
+
+void AnalyzerPlot::ResetData() {
+  m_quasistaticData.Clear();
+  m_dynamicData.Clear();
+  m_regressionData.Clear();
+  m_timestepData.Clear();
+
+  FitPlots();
+}
+
+void AnalyzerPlot::SetGraphLabels(std::string_view unit) {
+  std::string_view abbreviation = GetAbbreviation(unit);
+  m_velocityLabel = fmt::format("Velocity ({}/s)", abbreviation);
+  m_accelerationLabel = fmt::format("Acceleration ({}/s²)", abbreviation);
+  m_velPortionAccelLabel =
+      fmt::format("Velocity-Portion Accel ({}/s²)", abbreviation);
+}
+
+void AnalyzerPlot::SetRawData(const Storage& data, std::string_view unit,
+                              std::atomic<bool>& abort) {
+  const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
+  const auto& slow = m_direction == 0 ? slowForward : slowBackward;
+  const auto& fast = m_direction == 0 ? fastForward : fastBackward;
+
+  SetGraphLabels(unit);
+
+  std::scoped_lock lock(m_mutex);
+
+  ResetData();
+  SetRawTimeData(slow, fast, abort);
+}
+
+void AnalyzerPlot::SetData(const Storage& rawData, const Storage& filteredData,
+                           std::string_view unit,
+                           const std::vector<double>& ffGains,
+                           const std::array<units::second_t, 4>& startTimes,
+                           AnalysisType type, std::atomic<bool>& abort) {
+  double simSquaredErrorSum = 0;
+  double squaredVariationSum = 0;
+  int timeSeriesPoints = 0;
+
+  const auto& Ks = ffGains[0];
+  const auto& Kv = ffGains[1];
+  const auto& Ka = ffGains[2];
+
+  auto& [slowForward, slowBackward, fastForward, fastBackward] = filteredData;
+  auto& [rawSlowForward, rawSlowBackward, rawFastForward, rawFastBackward] =
+      rawData;
+
+  const auto slow = AnalysisManager::DataConcat(slowForward, slowBackward);
+  const auto fast = AnalysisManager::DataConcat(fastForward, fastBackward);
+  const auto rawSlow =
+      AnalysisManager::DataConcat(rawSlowForward, rawSlowBackward);
+  const auto rawFast =
+      AnalysisManager::DataConcat(rawFastForward, rawFastBackward);
+
+  SetGraphLabels(unit);
+
+  std::scoped_lock lock(m_mutex);
+
+  ResetData();
+
+  // Calculate step sizes to ensure that we only use the memory that we
+  // allocated.
+  auto slowStep = std::ceil(slow.size() * 1.0 / kMaxSize * 4);
+  auto fastStep = std::ceil(fast.size() * 1.0 / kMaxSize * 4);
+
+  units::second_t dtMean = GetMeanTimeDelta(filteredData);
+
+  // Velocity-vs-time plots
+  {
+    const auto& slow = m_direction == 0 ? slowForward : slowBackward;
+    const auto& fast = m_direction == 0 ? fastForward : fastBackward;
+    const auto& rawSlow = m_direction == 0 ? rawSlowForward : rawSlowBackward;
+    const auto& rawFast = m_direction == 0 ? rawFastForward : rawFastBackward;
+
+    // Populate quasistatic time-domain graphs
+    for (size_t i = 0; i < slow.size(); i += slowStep) {
+      if (abort) {
+        return;
+      }
+
+      m_quasistaticData.filteredData.emplace_back((slow[i].timestamp).value(),
+                                                  slow[i].velocity);
+
+      if (i > 0) {
+        // If the current timestamp is not in the startTimes array, it is the
+        // during a test and should be included. If it is in the startTimes
+        // array, it is the beginning of a new test and the dt will be inflated.
+        // Therefore we skip those to exclude that dt and effectively reset dt
+        // calculations.
+        if (slow[i].dt > 0_s &&
+            std::find(startTimes.begin(), startTimes.end(),
+                      slow[i].timestamp) == startTimes.end()) {
+          m_timestepData.data.emplace_back(
+              (slow[i].timestamp).value(),
+              units::millisecond_t{slow[i].dt}.value());
+        }
+      }
+    }
+
+    // Populate dynamic time-domain graphs
+    for (size_t i = 0; i < fast.size(); i += fastStep) {
+      if (abort) {
+        return;
+      }
+
+      m_dynamicData.filteredData.emplace_back((fast[i].timestamp).value(),
+                                              fast[i].velocity);
+
+      if (i > 0) {
+        // If the current timestamp is not in the startTimes array, it is the
+        // during a test and should be included. If it is in the startTimes
+        // array, it is the beginning of a new test and the dt will be inflated.
+        // Therefore we skip those to exclude that dt and effectively reset dt
+        // calculations.
+        if (fast[i].dt > 0_s &&
+            std::find(startTimes.begin(), startTimes.end(),
+                      fast[i].timestamp) == startTimes.end()) {
+          m_timestepData.data.emplace_back(
+              (fast[i].timestamp).value(),
+              units::millisecond_t{fast[i].dt}.value());
+        }
+      }
+    }
+
+    SetRawTimeData(rawSlow, rawFast, abort);
+
+    // Populate simulated time domain data
+    if (type == analysis::kElevator) {
+      const auto& Kg = ffGains[3];
+      m_quasistaticData.simData = PopulateTimeDomainSim(
+          rawSlow, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg},
+          &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+      m_dynamicData.simData = PopulateTimeDomainSim(
+          rawFast, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg},
+          &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+    } else if (type == analysis::kArm) {
+      const auto& Kg = ffGains[3];
+      const auto& offset = ffGains[4];
+      m_quasistaticData.simData = PopulateTimeDomainSim(
+          rawSlow, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset},
+          &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+      m_dynamicData.simData = PopulateTimeDomainSim(
+          rawFast, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset},
+          &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+    } else {
+      m_quasistaticData.simData = PopulateTimeDomainSim(
+          rawSlow, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka},
+          &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+      m_dynamicData.simData = PopulateTimeDomainSim(
+          rawFast, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka},
+          &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+    }
+  }
+
+  // Acceleration-vs-velocity plot
+
+  // Find minimum velocity of slow and fast datasets, then find point for line
+  // of best fit
+  auto slowMinVel =
+      std::min_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) {
+        return a.velocity < b.velocity;
+      })->velocity;
+  auto fastMinVel =
+      std::min_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) {
+        return a.velocity < b.velocity;
+      })->velocity;
+  auto minVel = std::min(slowMinVel, fastMinVel);
+  m_regressionData.fitLine[0] = ImPlotPoint{minVel, -Kv / Ka * minVel};
+
+  // Find maximum velocity of slow and fast datasets, then find point for line
+  // of best fit
+  auto slowMaxVel =
+      std::max_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) {
+        return a.velocity < b.velocity;
+      })->velocity;
+  auto fastMaxVel =
+      std::max_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) {
+        return a.velocity < b.velocity;
+      })->velocity;
+  auto maxVel = std::max(slowMaxVel, fastMaxVel);
+  m_regressionData.fitLine[1] = ImPlotPoint{maxVel, -Kv / Ka * maxVel};
+
+  // Populate acceleration vs velocity graph
+  for (size_t i = 0; i < slow.size(); i += slowStep) {
+    if (abort) {
+      return;
+    }
+
+    // Calculate portion of acceleration caused by back-EMF
+    double accelPortion = slow[i].acceleration - 1.0 / Ka * slow[i].voltage +
+                          std::copysign(Ks / Ka, slow[i].velocity);
+
+    if (type == analysis::kElevator) {
+      const auto& Kg = ffGains[3];
+      accelPortion -= Kg / Ka;
+    } else if (type == analysis::kArm) {
+      const auto& Kg = ffGains[3];
+      accelPortion -= Kg / Ka * slow[i].cos;
+    }
+
+    m_regressionData.data.emplace_back(slow[i].velocity, accelPortion);
+  }
+  for (size_t i = 0; i < fast.size(); i += fastStep) {
+    if (abort) {
+      return;
+    }
+
+    // Calculate portion of voltage that corresponds to change in acceleration.
+    double accelPortion = fast[i].acceleration - 1.0 / Ka * fast[i].voltage +
+                          std::copysign(Ks / Ka, fast[i].velocity);
+
+    if (type == analysis::kElevator) {
+      const auto& Kg = ffGains[3];
+      accelPortion -= Kg / Ka;
+    } else if (type == analysis::kArm) {
+      const auto& Kg = ffGains[3];
+      accelPortion -= Kg / Ka * fast[i].cos;
+    }
+
+    m_regressionData.data.emplace_back(fast[i].velocity, accelPortion);
+  }
+
+  // Timestep-vs-time plot
+
+  for (size_t i = 0; i < slow.size(); i += slowStep) {
+    if (i > 0) {
+      // If the current timestamp is not in the startTimes array, it is the
+      // during a test and should be included. If it is in the startTimes
+      // array, it is the beginning of a new test and the dt will be inflated.
+      // Therefore we skip those to exclude that dt and effectively reset dt
+      // calculations.
+      if (slow[i].dt > 0_s &&
+          std::find(startTimes.begin(), startTimes.end(), slow[i].timestamp) ==
+              startTimes.end()) {
+        m_timestepData.data.emplace_back(
+            (slow[i].timestamp).value(),
+            units::millisecond_t{slow[i].dt}.value());
+      }
+    }
+  }
+
+  for (size_t i = 0; i < fast.size(); i += fastStep) {
+    if (i > 0) {
+      // If the current timestamp is not in the startTimes array, it is the
+      // during a test and should be included. If it is in the startTimes
+      // array, it is the beginning of a new test and the dt will be inflated.
+      // Therefore we skip those to exclude that dt and effectively reset dt
+      // calculations.
+      if (fast[i].dt > 0_s &&
+          std::find(startTimes.begin(), startTimes.end(), fast[i].timestamp) ==
+              startTimes.end()) {
+        m_timestepData.data.emplace_back(
+            (fast[i].timestamp).value(),
+            units::millisecond_t{fast[i].dt}.value());
+      }
+    }
+  }
+
+  auto minTime =
+      units::math::min(slow.front().timestamp, fast.front().timestamp);
+  m_timestepData.fitLine[0] =
+      ImPlotPoint{minTime.value(), units::millisecond_t{dtMean}.value()};
+
+  auto maxTime = units::math::max(slow.back().timestamp, fast.back().timestamp);
+  m_timestepData.fitLine[1] =
+      ImPlotPoint{maxTime.value(), units::millisecond_t{dtMean}.value()};
+
+  // RMSE = std::sqrt(sum((x_i - x^_i)^2) / N) where sum represents the sum of
+  // all time series points, x_i represents the velocity at a timestep, x^_i
+  // represents the prediction at the timestep, and N represents the number of
+  // points
+  m_RMSE = std::sqrt(simSquaredErrorSum / timeSeriesPoints);
+  m_accelRSquared =
+      1 - m_RMSE / std::sqrt(squaredVariationSum / timeSeriesPoints);
+  FitPlots();
+}
+
+void AnalyzerPlot::FitPlots() {
+  // Set the "fit" flag to true.
+  m_quasistaticData.fitNextPlot = true;
+  m_dynamicData.fitNextPlot = true;
+  m_regressionData.fitNextPlot = true;
+  m_timestepData.fitNextPlot = true;
+}
+
+double* AnalyzerPlot::GetSimRMSE() {
+  return &m_RMSE;
+}
+
+double* AnalyzerPlot::GetSimRSquared() {
+  return &m_accelRSquared;
+}
+
+static void PlotSimData(std::vector<std::vector<ImPlotPoint>>& data) {
+  for (auto&& pts : data) {
+    ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5);
+    ImPlot::PlotLineG("Simulation", Getter, pts.data(), pts.size());
+  }
+}
+
+bool AnalyzerPlot::DisplayPlots() {
+  std::unique_lock lock(m_mutex, std::defer_lock);
+
+  if (!lock.try_lock()) {
+    ImGui::Text("Loading %c",
+                "|/-\\"[static_cast<int>(ImGui::GetTime() / 0.05f) & 3]);
+    return false;
+  }
+
+  ImVec2 plotSize = ImGui::GetContentRegionAvail();
+
+  // Fit two plots horizontally
+  plotSize.x = (plotSize.x - ImGui::GetStyle().ItemSpacing.x) / 2.f;
+
+  // Fit two plots vertically while leaving room for three text boxes
+  const float textBoxHeight = ImGui::GetFontSize() * 1.75;
+  plotSize.y =
+      (plotSize.y - textBoxHeight * 3 - ImGui::GetStyle().ItemSpacing.y) / 2.f;
+
+  m_quasistaticData.Plot("Quasistatic Velocity vs. Time", plotSize,
+                         m_velocityLabel.c_str(), m_pointSize);
+  ImGui::SameLine();
+  m_dynamicData.Plot("Dynamic Velocity vs. Time", plotSize,
+                     m_velocityLabel.c_str(), m_pointSize);
+
+  m_regressionData.Plot("Acceleration vs. Velocity", plotSize,
+                        m_velocityLabel.c_str(), m_velPortionAccelLabel.c_str(),
+                        true, true, m_pointSize);
+  ImGui::SameLine();
+  m_timestepData.Plot("Timesteps vs. Time", plotSize, "Time (s)",
+                      "Timestep duration (ms)", true, false, m_pointSize,
+                      [] { ImPlot::SetupAxisLimits(ImAxis_Y1, 0, 50); });
+
+  return true;
+}
+
+AnalyzerPlot::FilteredDataVsTimePlot::FilteredDataVsTimePlot() {
+  rawData.reserve(kMaxSize);
+  filteredData.reserve(kMaxSize);
+  simData.reserve(kMaxSize);
+}
+
+void AnalyzerPlot::FilteredDataVsTimePlot::Plot(const char* title,
+                                                const ImVec2& size,
+                                                const char* yLabel,
+                                                float pointSize) {
+  // Generate Sim vs Filtered Plot
+  if (fitNextPlot) {
+    ImPlot::SetNextAxesToFit();
+  }
+
+  if (ImPlot::BeginPlot(title, size)) {
+    ImPlot::SetupAxis(ImAxis_X1, "Time (s)", ImPlotAxisFlags_NoGridLines);
+    ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines);
+    ImPlot::SetupLegend(ImPlotLocation_NorthEast);
+
+    // Plot Raw Data
+    ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
+    ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
+    ImPlot::PlotScatterG("Raw Data", Getter, rawData.data(), rawData.size());
+
+    // Plot Filtered Data after Raw data
+    ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
+    ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
+    ImPlot::PlotScatterG("Filtered Data", Getter, filteredData.data(),
+                         filteredData.size());
+
+    // Plot Simulation Data for Velocity Data
+    PlotSimData(simData);
+
+    // Disable constant resizing
+    if (fitNextPlot) {
+      fitNextPlot = false;
+    }
+
+    ImPlot::EndPlot();
+  }
+}
+
+void AnalyzerPlot::FilteredDataVsTimePlot::Clear() {
+  rawData.clear();
+  filteredData.clear();
+  simData.clear();
+}
+
+AnalyzerPlot::DataWithFitLinePlot::DataWithFitLinePlot() {
+  data.reserve(kMaxSize);
+}
+
+void AnalyzerPlot::DataWithFitLinePlot::Plot(const char* title,
+                                             const ImVec2& size,
+                                             const char* xLabel,
+                                             const char* yLabel, bool fitX,
+                                             bool fitY, float pointSize,
+                                             std::function<void()> setup) {
+  if (fitNextPlot) {
+    if (fitX && fitY) {
+      ImPlot::SetNextAxesToFit();
+    } else if (fitX && !fitY) {
+      ImPlot::SetNextAxisToFit(ImAxis_X1);
+    } else if (!fitX && fitY) {
+      ImPlot::SetNextAxisToFit(ImAxis_Y1);
+    }
+  }
+
+  if (ImPlot::BeginPlot(title, size)) {
+    setup();
+    ImPlot::SetupAxis(ImAxis_X1, xLabel, ImPlotAxisFlags_NoGridLines);
+    ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines);
+    ImPlot::SetupLegend(ImPlotLocation_NorthEast);
+
+    // Get a reference to the data that we are plotting.
+    ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
+    ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
+    ImPlot::PlotScatterG("Filtered Data", Getter, data.data(), data.size());
+
+    ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5);
+    ImPlot::PlotLineG("Fit", Getter, fitLine.data(), fitLine.size());
+
+    ImPlot::EndPlot();
+
+    if (fitNextPlot) {
+      fitNextPlot = false;
+    }
+  }
+}
+
+void AnalyzerPlot::DataWithFitLinePlot::Clear() {
+  data.clear();
+
+  // Reset line of best fit
+  fitLine[0] = ImPlotPoint{0, 0};
+  fitLine[1] = ImPlotPoint{0, 0};
+}

sysid/src/main/native/cpp/view/JSONConverter.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/JSONConverter.h"
+#include "sysid/view/JSONConverter.h"
+
+#include <exception>
+
+#include <imgui.h>
+#include <portable-file-dialogs.h>
+#include <wpi/timestamp.h>
+
+#include "sysid/Util.h"
+
+using namespace sysid;
+
+void JSONConverter::DisplayConverter(
+    const char* tooltip,
+    std::function<std::string(std::string_view, wpi::Logger&)> converter) {
+  if (ImGui::Button(tooltip)) {
+    m_opener = std::make_unique<pfd::open_file>(
+        tooltip, "", std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
+  }
+
+  if (m_opener && m_opener->ready()) {
+    if (!m_opener->result().empty()) {
+      m_location = m_opener->result()[0];
+      try {
+        converter(m_location, m_logger);
+        m_timestamp = wpi::Now() * 1E-6;
+      } catch (const std::exception& e) {
+        ImGui::OpenPopup("Exception Caught!");
+        m_exception = e.what();
+      }
+    }
+    m_opener.reset();
+  }
+
+  if (wpi::Now() * 1E-6 - m_timestamp < 5) {
+    ImGui::SameLine();
+    ImGui::Text("Saved!");
+  }
+
+  // Handle exceptions.
+  ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+  if (ImGui::BeginPopupModal("Exception Caught!")) {
+    ImGui::PushTextWrapPos(0.0f);
+    ImGui::Text(
+        "An error occurred when parsing the JSON. This most likely means that "
+        "the JSON data is incorrectly formatted.");
+    ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
+                       m_exception.c_str());
+    ImGui::PopTextWrapPos();
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+}
+
+void JSONConverter::DisplayCSVConvert() {
+  DisplayConverter("Select SysId JSON", sysid::ToCSV);
+}

sysid/src/main/native/cpp/view/Logger.cpp

@@ -0,0 +1,222 @@
+// 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/view/Logger.h"
+
+#include <exception>
+#include <numbers>
+
+#include <glass/Context.h>
+#include <glass/Storage.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <networktables/NetworkTable.h>
+#include <units/angle.h>
+#include <wpigui.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/view/UILayout.h"
+
+using namespace sysid;
+
+Logger::Logger(glass::Storage& storage, wpi::Logger& logger)
+    : m_logger{logger}, m_ntSettings{"sysid", storage} {
+  wpi::gui::AddEarlyExecute([&] { m_ntSettings.Update(); });
+
+  m_ntSettings.EnableServerOption(false);
+}
+
+void Logger::Display() {
+  // Get the current width of the window. This will be used to scale
+  // our UI elements.
+  float width = ImGui::GetContentRegionAvail().x;
+
+  // Add team number input and apply button for NT connection.
+  m_ntSettings.Display();
+
+  // Reset and clear the internal manager state.
+  ImGui::SameLine();
+  if (ImGui::Button("Reset Telemetry")) {
+    m_settings = TelemetryManager::Settings{};
+    m_manager = std::make_unique<TelemetryManager>(m_settings, m_logger);
+    m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
+  }
+
+  // Add NT connection indicator.
+  static ImVec4 kColorDisconnected{1.0f, 0.4f, 0.4f, 1.0f};
+  static ImVec4 kColorConnected{0.2f, 1.0f, 0.2f, 1.0f};
+  ImGui::SameLine();
+  bool ntConnected = nt::NetworkTableInstance::GetDefault().IsConnected();
+  ImGui::TextColored(ntConnected ? kColorConnected : kColorDisconnected,
+                     ntConnected ? "NT Connected" : "NT Disconnected");
+
+  // Create a Section for project configuration
+  ImGui::Separator();
+  ImGui::Spacing();
+  ImGui::Text("Project Parameters");
+
+  // Add a dropdown for mechanism type.
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+
+  if (ImGui::Combo("Mechanism", &m_selectedType, kTypes,
+                   IM_ARRAYSIZE(kTypes))) {
+    m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
+  }
+
+  // Add Dropdown for Units
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+  if (ImGui::Combo("Unit Type", &m_selectedUnit, kUnits,
+                   IM_ARRAYSIZE(kUnits))) {
+    m_settings.units = kUnits[m_selectedUnit];
+  }
+
+  sysid::CreateTooltip(
+      "This is the type of units that your gains will be in. For example, if "
+      "you want your flywheel gains in terms of radians, then use the radians "
+      "unit. On the other hand, if your drivetrain will use gains in meters, "
+      "choose meters.");
+
+  // Rotational units have fixed Units per rotations
+  m_isRotationalUnits =
+      (m_settings.units == "Rotations" || m_settings.units == "Degrees" ||
+       m_settings.units == "Radians");
+  if (m_settings.units == "Degrees") {
+    m_settings.unitsPerRotation = 360.0;
+  } else if (m_settings.units == "Radians") {
+    m_settings.unitsPerRotation = 2 * std::numbers::pi;
+  } else if (m_settings.units == "Rotations") {
+    m_settings.unitsPerRotation = 1.0;
+  }
+
+  // Units Per Rotations entry
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+  ImGui::InputDouble("Units Per Rotation", &m_settings.unitsPerRotation, 0.0f,
+                     0.0f, "%.4f",
+                     m_isRotationalUnits ? ImGuiInputTextFlags_ReadOnly
+                                         : ImGuiInputTextFlags_None);
+  sysid::CreateTooltip(
+      "The logger assumes that the code will be sending recorded motor shaft "
+      "rotations over NetworkTables. This value will then be multiplied by the "
+      "units per rotation to get the measurement in the units you "
+      "specified.\n\nFor non-rotational units (e.g. meters), this value is "
+      "usually the wheel diameter times pi (should not include gearing).");
+  // Create a section for voltage parameters.
+  ImGui::Separator();
+  ImGui::Spacing();
+  ImGui::Text("Voltage Parameters");
+
+  auto CreateVoltageParameters = [this](const char* text, double* data,
+                                        float min, float max) {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+    ImGui::PushItemFlag(ImGuiItemFlags_Disabled,
+                        m_manager && m_manager->IsActive());
+    float value = static_cast<float>(*data);
+    if (ImGui::SliderFloat(text, &value, min, max, "%.2f")) {
+      *data = value;
+    }
+    ImGui::PopItemFlag();
+  };
+
+  CreateVoltageParameters("Quasistatic Ramp Rate (V/s)",
+                          &m_settings.quasistaticRampRate, 0.10f, 0.60f);
+  sysid::CreateTooltip(
+      "This is the rate at which the voltage will increase during the "
+      "quasistatic test.");
+
+  CreateVoltageParameters("Dynamic Step Voltage (V)", &m_settings.stepVoltage,
+                          0.0f, 10.0f);
+  sysid::CreateTooltip(
+      "This is the voltage that will be applied for the "
+      "dynamic voltage (acceleration) tests.");
+
+  // Create a section for tests.
+  ImGui::Separator();
+  ImGui::Spacing();
+  ImGui::Text("Tests");
+
+  auto CreateTest = [this, width](const char* text, const char* itext) {
+    // Display buttons if we have an NT connection.
+    if (nt::NetworkTableInstance::GetDefault().IsConnected()) {
+      // Create button to run tests.
+      if (ImGui::Button(text)) {
+        // Open the warning message.
+        ImGui::OpenPopup("Warning");
+        m_manager->BeginTest(itext);
+        m_opened = text;
+      }
+      if (m_opened == text && ImGui::BeginPopupModal("Warning")) {
+        ImGui::TextWrapped("%s", m_popupText.c_str());
+        if (ImGui::Button(m_manager->IsActive() ? "End Test" : "Close")) {
+          m_manager->EndTest();
+          ImGui::CloseCurrentPopup();
+          m_opened = "";
+        }
+        ImGui::EndPopup();
+      }
+    } else {
+      // Show disabled text when there is no connection.
+      ImGui::TextDisabled("%s", text);
+    }
+
+    // Show whether the tests were run or not.
+    bool run = m_manager->HasRunTest(itext);
+    ImGui::SameLine(width * 0.7);
+    ImGui::Text(run ? "Run" : "Not Run");
+  };
+
+  CreateTest("Quasistatic Forward", "slow-forward");
+  CreateTest("Quasistatic Backward", "slow-backward");
+  CreateTest("Dynamic Forward", "fast-forward");
+  CreateTest("Dynamic Backward", "fast-backward");
+
+  m_manager->RegisterDisplayCallback(
+      [this](const auto& str) { m_popupText = str; });
+
+  // Display the path to where the JSON will be saved and a button to select the
+  // location.
+  ImGui::Separator();
+  ImGui::Spacing();
+  ImGui::Text("Save Location");
+  if (ImGui::Button("Choose")) {
+    m_selector = std::make_unique<pfd::select_folder>("Select Folder");
+  }
+  ImGui::SameLine();
+  ImGui::InputText("##savelocation", &m_jsonLocation,
+                   ImGuiInputTextFlags_ReadOnly);
+
+  // Add button to save.
+  ImGui::SameLine(width * 0.9);
+  if (ImGui::Button("Save")) {
+    try {
+      m_manager->SaveJSON(m_jsonLocation);
+    } catch (const std::exception& e) {
+      ImGui::OpenPopup("Exception Caught!");
+      m_exception = e.what();
+    }
+  }
+
+  // Handle exceptions.
+  if (ImGui::BeginPopupModal("Exception Caught!")) {
+    ImGui::Text("%s", m_exception.c_str());
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  // Run periodic methods.
+  SelectDataFolder();
+  m_ntSettings.Update();
+  m_manager->Update();
+}
+
+void Logger::SelectDataFolder() {
+  // If the selector exists and is ready with a result, we can store it.
+  if (m_selector && m_selector->ready()) {
+    m_jsonLocation = m_selector->result();
+    m_selector.reset();
+  }
+}