diff --git a/sysid/src/main/generate/WPILibVersion.cpp.in b/sysid/src/main/generate/WPILibVersion.cpp.in
new file mode 100644
index 0000000..b0a4490
--- /dev/null
+++ b/sysid/src/main/generate/WPILibVersion.cpp.in
@@ -0,0 +1,7 @@
+/*
+ * Autogenerated file! Do not manually edit this file. This version is regenerated
+ * any time the publish task is run, or when this file is deleted.
+ */
+const char* GetWPILibVersion() {
+  return "${wpilib_version}";
+}
diff --git a/sysid/src/main/native/cpp/App.cpp b/sysid/src/main/native/cpp/App.cpp
new file mode 100644
index 0000000..947ea43
--- /dev/null
+++ b/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
diff --git a/sysid/src/main/native/cpp/Main.cpp b/sysid/src/main/native/cpp/Main.cpp
new file mode 100644
index 0000000..6fba848
--- /dev/null
+++ b/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
diff --git a/sysid/src/main/native/cpp/Util.cpp b/sysid/src/main/native/cpp/Util.cpp
new file mode 100644
index 0000000..a9acc53
--- /dev/null
+++ b/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;
+}
diff --git a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
new file mode 100644
index 0000000..b15cae8
--- /dev/null
+++ b/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/MemoryBuffer.h>
+#include <wpi/StringExtras.h>
+#include <wpi/StringMap.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;
+    std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+        wpi::MemoryBuffer::GetFile(path, ec);
+    if (fileBuffer == nullptr || ec) {
+      throw FileReadingError(path);
+    }
+
+    m_json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+
+    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;
+    std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+        wpi::MemoryBuffer::GetFile(path, ec);
+    if (fileBuffer == nullptr || ec) {
+      throw FileReadingError(newPath);
+    }
+
+    m_json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+
+    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>();
+}
diff --git a/sysid/src/main/native/cpp/analysis/AnalysisType.cpp b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
new file mode 100644
index 0000000..18b461f
--- /dev/null
+++ b/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;
+}
diff --git a/sysid/src/main/native/cpp/analysis/ArmSim.cpp b/sysid/src/main/native/cpp/analysis/ArmSim.cpp
new file mode 100644
index 0000000..a72f505
--- /dev/null
+++ b/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};
+}
diff --git a/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp b/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp
new file mode 100644
index 0000000..5cfcabe
--- /dev/null
+++ b/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};
+}
diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
new file mode 100644
index 0000000..0691aaf
--- /dev/null
+++ b/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};
+}
diff --git a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
new file mode 100644
index 0000000..b7a9fce
--- /dev/null
+++ b/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)};
+}
diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
new file mode 100644
index 0000000..6c66ef8
--- /dev/null
+++ b/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.");
+  }
+}
diff --git a/sysid/src/main/native/cpp/analysis/JSONConverter.cpp b/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
new file mode 100644
index 0000000..9bab8c6
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
@@ -0,0 +1,164 @@
+// 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/MemoryBuffer.h>
+#include <wpi/fmt/raw_ostream.h>
+#include <wpi/json.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;
+  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+      wpi::MemoryBuffer::GetFile(path, ec);
+  if (fileBuffer == nullptr || ec) {
+    throw std::runtime_error(fmt::format("Unable to read: {}", path));
+  }
+
+  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+  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;
+}
diff --git a/sysid/src/main/native/cpp/analysis/OLS.cpp b/sysid/src/main/native/cpp/analysis/OLS.cpp
new file mode 100644
index 0000000..d095a48
--- /dev/null
+++ b/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};
+}
diff --git a/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp b/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp
new file mode 100644
index 0000000..58a8b30
--- /dev/null
+++ b/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};
+}
diff --git a/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp b/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp
new file mode 100644
index 0000000..eebfe8c
--- /dev/null
+++ b/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());
+}
diff --git a/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
new file mode 100644
index 0000000..ac97cdb
--- /dev/null
+++ b/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;
+}
diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp
new file mode 100644
index 0000000..3270918
--- /dev/null
+++ b/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();
+    }
+  }
+}
diff --git a/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp b/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp
new file mode 100644
index 0000000..d8afbaf
--- /dev/null
+++ b/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};
+}
diff --git a/sysid/src/main/native/cpp/view/JSONConverter.cpp b/sysid/src/main/native/cpp/view/JSONConverter.cpp
new file mode 100644
index 0000000..88eaa6a
--- /dev/null
+++ b/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);
+}
diff --git a/sysid/src/main/native/cpp/view/Logger.cpp b/sysid/src/main/native/cpp/view/Logger.cpp
new file mode 100644
index 0000000..5e7773d
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/sysid/src/main/native/include/sysid/Util.h b/sysid/src/main/native/include/sysid/Util.h
new file mode 100644
index 0000000..38cf8b2
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/Util.h
@@ -0,0 +1,89 @@
+// 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.
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <filesystem>
+#include <string>
+#include <string_view>
+
+// The generated AppleScript by portable-file-dialogs for just *.json does not
+// work correctly because it is a uniform type identifier. This means that
+// "public." needs to be prepended to it.
+#ifdef __APPLE__
+#define SYSID_PFD_JSON_EXT "*.public.json"
+#else
+#define SYSID_PFD_JSON_EXT "*.json"
+#endif
+
+#ifdef _WIN32
+#define LAUNCH "gradlew"
+#define LAUNCH_DETACHED "start /b gradlew"
+#define DETACHED_SUFFIX ""
+#else
+#define LAUNCH "./gradlew"
+#define LAUNCH_DETACHED "./gradlew"
+#define DETACHED_SUFFIX "&"
+#endif
+
+// Based on https://gcc.gnu.org/onlinedocs/cpp/Stringizing.html
+#define EXPAND_STRINGIZE(s) STRINGIZE(s)
+#define STRINGIZE(s) #s
+
+namespace sysid {
+static constexpr const char* kUnits[] = {"Meters",  "Feet",      "Inches",
+                                         "Radians", "Rotations", "Degrees"};
+
+/**
+ * Displays a tooltip beside the widget that this method is called after with
+ * the provided text.
+ *
+ * @param text The text to show in the tooltip.
+ */
+void CreateTooltip(const char* text);
+
+/**
+ * Utility function to launch an error popup if an exception is detected.
+ *
+ * @param isError True if an exception is detected
+ * @param errorMessage The error message associated with the exception
+ */
+void CreateErrorPopup(bool& isError, std::string_view errorMessage);
+
+/**
+ * Returns the abbreviation for the unit.
+ *
+ * @param unit The unit to return the abbreviation for.
+ * @return The abbreviation for the unit.
+ */
+std::string_view GetAbbreviation(std::string_view unit);
+
+/**
+ * Saves a file with the provided contents to a specified location.
+ *
+ * @param contents The file contents.
+ * @param path The file location.
+ */
+void SaveFile(std::string_view contents, const std::filesystem::path& path);
+
+/**
+ * Concatenates all the arrays passed as arguments and returns the result.
+ *
+ * @tparam Type The array element type.
+ * @tparam Sizes The array sizes.
+ * @param arrays Parameter pack of arrays to concatenate.
+ */
+template <typename Type, size_t... Sizes>
+constexpr auto ArrayConcat(const std::array<Type, Sizes>&... arrays) {
+  std::array<Type, (Sizes + ...)> result;
+  size_t index = 0;
+
+  ((std::copy_n(arrays.begin(), Sizes, result.begin() + index), index += Sizes),
+   ...);
+
+  return result;
+}
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
new file mode 100644
index 0000000..d572578
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
@@ -0,0 +1,358 @@
+// 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.
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <exception>
+#include <limits>
+#include <numeric>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <tuple>
+#include <vector>
+
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/json.h>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+/**
+ * Manages analysis of data. Each instance of this class represents a JSON file
+ * that is read from storage.
+ */
+class AnalysisManager {
+ public:
+  /**
+   * Represents settings for an instance of the analysis manager. This contains
+   * information about the feedback controller preset, loop type, motion
+   * threshold, acceleration window size, LQR parameters, and the selected
+   * dataset.
+   */
+  struct Settings {
+    enum class DrivetrainDataset { kCombined = 0, kLeft = 1, kRight = 2 };
+    /**
+     * The feedback controller preset used to calculate gains.
+     */
+    FeedbackControllerPreset preset = presets::kDefault;
+
+    /**
+     * The feedback controller loop type (position or velocity).
+     */
+    FeedbackControllerLoopType type = FeedbackControllerLoopType::kVelocity;
+
+    /**
+     * LQR parameters used for feedback gain calculation.
+     */
+    LQRParameters lqr{1, 1.5, 7};
+
+    /**
+     * The motion threshold (units/s) for trimming quasistatic test data.
+     */
+    double motionThreshold = 0.2;
+
+    /**
+     * The window size for the median filter.
+     */
+    int medianWindow = 1;
+
+    /**
+     * The duration of the dynamic test that should be considered. A value of
+     * zero indicates it needs to be set to the default.
+     */
+    units::second_t stepTestDuration = 0_s;
+
+    /**
+     * The conversion factor of counts per revolution.
+     */
+    int cpr = 1440;
+
+    /**
+     * The conversion factor of gearing.
+     */
+    double gearing = 1;
+
+    /**
+     * Whether or not the gains should be in the encoder's units (mainly for use
+     * in a smart motor controller).
+     */
+    bool convertGainsToEncTicks = false;
+
+    DrivetrainDataset dataset = DrivetrainDataset::kCombined;
+  };
+
+  /**
+   * Stores feedforward.
+   */
+  struct FeedforwardGains {
+    /**
+     * Stores the Feedforward gains.
+     */
+    std::tuple<std::vector<double>, double, double> ffGains;
+
+    /**
+     * Stores the trackwidth for angular drivetrain tests.
+     */
+    std::optional<double> trackWidth;
+  };
+
+  /**
+   * Exception for File Reading Errors.
+   */
+  struct FileReadingError : public std::exception {
+    /**
+     * Creates a FileReadingError object
+     *
+     * @param path The path of the file attempted to open
+     */
+    explicit FileReadingError(std::string_view path) {
+      msg = fmt::format("Unable to read: {}", path);
+    }
+
+    /**
+     * The path of the file that was opened.
+     */
+    std::string msg;
+    const char* what() const noexcept override { return msg.c_str(); }
+  };
+
+  /**
+   * The keys (which contain sysid data) that are in the JSON to analyze.
+   */
+  static constexpr const char* kJsonDataKeys[] = {
+      "slow-forward", "slow-backward", "fast-forward", "fast-backward"};
+
+  /**
+   * Concatenates a list of vectors. The contents of the source vectors are
+   * copied (not moved) into the new vector. Also sorts the datapoints by
+   * timestamp to assist with future simulation.
+   *
+   * @param sources The source vectors.
+   * @return The concatenated vector
+   */
+  template <typename... Sources>
+  static std::vector<PreparedData> DataConcat(const Sources&... sources) {
+    std::vector<PreparedData> result;
+    (result.insert(result.end(), sources.begin(), sources.end()), ...);
+
+    // Sort data by timestamp to remove the possibility of negative dts in
+    // future simulations.
+    std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) {
+      return a.timestamp < b.timestamp;
+    });
+
+    return result;
+  }
+
+  /**
+   * Constructs an instance of the analysis manager for theoretical analysis,
+   * containing settings and gains but no data.
+   *
+   * @param settings The settings for this instance of the analysis manager.
+   * @param logger The logger instance to use for log data.
+   */
+  AnalysisManager(Settings& settings, wpi::Logger& logger);
+
+  /**
+   * Constructs an instance of the analysis manager with the given path (to the
+   * JSON) and analysis manager settings.
+   *
+   * @param path     The path to the JSON containing the sysid data.
+   * @param settings The settings for this instance of the analysis manager.
+   * @param logger   The logger instance to use for log data.
+   */
+  AnalysisManager(std::string_view path, Settings& settings,
+                  wpi::Logger& logger);
+
+  /**
+   * Prepares data from the JSON and stores the output in Storage member
+   * variables.
+   */
+  void PrepareData();
+
+  /**
+   * Calculates the gains with the latest data (from the pointers in the
+   * settings struct that this instance was constructed with).
+   *
+   * @return The latest feedforward gains and trackwidth (if needed).
+   */
+  FeedforwardGains CalculateFeedforward();
+
+  /**
+   * Calculates feedback gains from the given feedforward gains.
+   *
+   * @param ff The feedforward gains.
+   * @return The calculated feedback gains.
+   */
+  FeedbackGains CalculateFeedback(std::vector<double> ff);
+
+  /**
+   * Overrides the units in the JSON with the user-provided ones.
+   *
+   * @param unit             The unit to output gains in.
+   * @param unitsPerRotation The conversion factor between rotations and the
+   *                         selected unit.
+   */
+  void OverrideUnits(std::string_view unit, double unitsPerRotation);
+
+  /**
+   * Resets the units back to those defined in the JSON.
+   */
+  void ResetUnitsFromJSON();
+
+  /**
+   * Returns the analysis type of the current instance (read from the JSON).
+   *
+   * @return The analysis type.
+   */
+  const AnalysisType& GetAnalysisType() const { return m_type; }
+
+  /**
+   * Returns the units of analysis.
+   *
+   * @return The units of analysis.
+   */
+  std::string_view GetUnit() const { return m_unit; }
+
+  /**
+   * Returns the factor (a.k.a. units per rotation) for analysis.
+   *
+   * @return The factor (a.k.a. units per rotation) for analysis.
+   */
+  double GetFactor() const { return m_factor; }
+
+  /**
+   * Returns a reference to the iterator of the currently selected raw datset.
+   * Unfortunately, due to ImPlot internals, the reference cannot be const so
+   * the user should be careful not to change any data.
+   *
+   * @return A reference to the raw internal data.
+   */
+  Storage& GetRawData() {
+    return m_rawDataset[static_cast<int>(m_settings.dataset)];
+  }
+
+  /**
+   * Returns a reference to the iterator of the currently selected filtered
+   * datset. Unfortunately, due to ImPlot internals, the reference cannot be
+   * const so the user should be careful not to change any data.
+   *
+   * @return A reference to the filtered internal data.
+   */
+  Storage& GetFilteredData() {
+    return m_filteredDataset[static_cast<int>(m_settings.dataset)];
+  }
+
+  /**
+   * Returns the original dataset.
+   *
+   * @return The original (untouched) dataset
+   */
+  Storage& GetOriginalData() {
+    return m_originalDataset[static_cast<int>(m_settings.dataset)];
+  }
+
+  /**
+   * Returns the minimum duration of the Step Voltage Test of the currently
+   * stored data.
+   *
+   * @return The minimum step test duration.
+   */
+  units::second_t GetMinStepTime() const { return m_minStepTime; }
+
+  /**
+   * Returns the maximum duration of the Step Voltage Test of the currently
+   * stored data.
+   *
+   * @return  Maximum step test duration
+   */
+  units::second_t GetMaxStepTime() const { return m_maxStepTime; }
+
+  /**
+   * Returns the estimated time delay of the measured position, including
+   * CAN delays.
+   *
+   * @return Position delay in milliseconds
+   */
+  units::millisecond_t GetPositionDelay() const {
+    return std::accumulate(m_positionDelays.begin(), m_positionDelays.end(),
+                           0_s) /
+           m_positionDelays.size();
+  }
+
+  /**
+   * Returns the estimated time delay of the measured velocity, including
+   * CAN delays.
+   *
+   * @return Velocity delay in milliseconds
+   */
+  units::millisecond_t GetVelocityDelay() const {
+    return std::accumulate(m_velocityDelays.begin(), m_velocityDelays.end(),
+                           0_s) /
+           m_positionDelays.size();
+  }
+
+  /**
+   * Returns the different start times of the recorded tests.
+   *
+   * @return The start times for each test
+   */
+  const std::array<units::second_t, 4>& GetStartTimes() const {
+    return m_startTimes;
+  }
+
+  bool HasData() const {
+    return !m_originalDataset[static_cast<int>(
+                                  Settings::DrivetrainDataset::kCombined)]
+                .empty();
+  }
+
+ private:
+  wpi::Logger& m_logger;
+
+  // This is used to store the various datasets (i.e. Combined, Forward,
+  // Backward, etc.)
+  wpi::json m_json;
+
+  std::array<Storage, 3> m_originalDataset;
+  std::array<Storage, 3> m_rawDataset;
+  std::array<Storage, 3> m_filteredDataset;
+
+  // Stores the various start times of the different tests.
+  std::array<units::second_t, 4> m_startTimes;
+
+  // The settings for this instance. This contains pointers to the feedback
+  // controller preset, LQR parameters, acceleration window size, etc.
+  Settings& m_settings;
+
+  // Miscellaneous data from the JSON -- the analysis type, the units, and the
+  // units per rotation.
+  AnalysisType m_type;
+  std::string m_unit;
+  double m_factor;
+
+  units::second_t m_minStepTime{0};
+  units::second_t m_maxStepTime{std::numeric_limits<double>::infinity()};
+  std::vector<units::second_t> m_positionDelays;
+  std::vector<units::second_t> m_velocityDelays;
+
+  // Stores an optional track width if we are doing the drivetrain angular test.
+  std::optional<double> m_trackWidth;
+
+  void PrepareGeneralData();
+
+  void PrepareAngularDrivetrainData();
+
+  void PrepareLinearDrivetrainData();
+};
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisType.h b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
new file mode 100644
index 0000000..7feedb3
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
@@ -0,0 +1,63 @@
+// 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.
+
+#pragma once
+
+#include <cstddef>
+#include <string_view>
+
+namespace sysid {
+
+/**
+ * Stores the type of Analysis and relevant information about the analysis.
+ */
+struct AnalysisType {
+  /**
+   * The number of independent variables for feedforward analysis.
+   */
+  size_t independentVariables;
+
+  /**
+   * The number of fields in the raw data within the mechanism's JSON.
+   */
+  size_t rawDataSize;
+
+  /**
+   * Display name for the analysis type.
+   */
+  const char* name;
+
+  /**
+   * Compares equality of two AnalysisType structs.
+   *
+   * @param rhs Another AnalysisType
+   * @return True if the two analysis types are equal
+   */
+  constexpr bool operator==(const AnalysisType& rhs) const {
+    return std::string_view{name} == rhs.name &&
+           independentVariables == rhs.independentVariables &&
+           rawDataSize == rhs.rawDataSize;
+  }
+
+  /**
+   * Compares inequality of two AnalysisType structs.
+   *
+   * @param rhs Another AnalysisType
+   * @return True if the two analysis types are not equal
+   */
+  constexpr bool operator!=(const AnalysisType& rhs) const {
+    return !operator==(rhs);
+  }
+};
+
+namespace analysis {
+constexpr AnalysisType kDrivetrain{3, 9, "Drivetrain"};
+constexpr AnalysisType kDrivetrainAngular{3, 9, "Drivetrain (Angular)"};
+constexpr AnalysisType kElevator{4, 4, "Elevator"};
+constexpr AnalysisType kArm{5, 4, "Arm"};
+constexpr AnalysisType kSimple{3, 4, "Simple"};
+
+AnalysisType FromName(std::string_view name);
+}  // namespace analysis
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/ArmSim.h b/sysid/src/main/native/include/sysid/analysis/ArmSim.h
new file mode 100644
index 0000000..af6c10e
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/ArmSim.h
@@ -0,0 +1,79 @@
+// 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.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace sysid {
+/**
+ * Simulation of an Arm mechanism based off of a model from SysId Feedforward
+ * gains.
+ */
+class ArmSim {
+ public:
+  /**
+   * @param Ks              Static friction gain.
+   * @param Kv              Velocity gain.
+   * @param Ka              Acceleration gain.
+   * @param Kg              Gravity cosine gain.
+   * @param offset          Arm position offset.
+   * @param initialPosition Initial arm position.
+   * @param initialVelocity Initial arm velocity.
+   */
+  ArmSim(double Ks, double Kv, double Ka, double Kg, double offset = 0.0,
+         double initialPosition = 0.0, double initialVelocity = 0.0);
+
+  /**
+   * Simulates affine state-space system:
+   * dx/dt = Ax + Bu + c sgn(x) + d cos(theta)
+   * forward dt seconds.
+   *
+   * @param voltage Voltage to apply over the timestep.
+   * @param dt      Sammple period.
+   */
+  void Update(units::volt_t voltage, units::second_t dt);
+
+  /**
+   * Returns the position.
+   *
+   * @return The current position
+   */
+  double GetPosition() const;
+
+  /**
+   * Returns the velocity.
+   *
+   * @return The current velocity
+   */
+  double GetVelocity() const;
+
+  /**
+   * Returns the acceleration for the current state and given input.
+   *
+   * @param voltage The voltage that is being applied to the mechanism / input
+   * @return The acceleration given the state and input
+   */
+  double GetAcceleration(units::volt_t voltage) const;
+
+  /**
+   * Resets model position and velocity.
+   *
+   * @param position The position the mechanism should be reset to
+   * @param velocity The velocity the mechanism should be reset to
+   */
+  void Reset(double position = 0.0, double velocity = 0.0);
+
+ private:
+  Eigen::Matrix<double, 1, 1> m_A;
+  Eigen::Matrix<double, 1, 1> m_B;
+  Eigen::Vector<double, 1> m_c;
+  Eigen::Vector<double, 1> m_d;
+  Eigen::Vector<double, 2> m_x;
+  double m_offset;
+};
+
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h
new file mode 100644
index 0000000..2d0c5f6
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h
@@ -0,0 +1,76 @@
+// 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.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace sysid {
+/**
+ * Simulation of an Elevator mechanism based off of a model from SysId
+ * Feedforward gains.
+ */
+class ElevatorSim {
+ public:
+  /**
+   * @param Ks              Static friction gain.
+   * @param Kv              Velocity gain.
+   * @param Ka              Acceleration gain.
+   * @param Kg              Gravity gain.
+   * @param initialPosition Initial elevator position.
+   * @param initialVelocity Initial elevator velocity.
+   */
+  ElevatorSim(double Ks, double Kv, double Ka, double Kg,
+              double initialPosition = 0.0, double initialVelocity = 0.0);
+
+  /**
+   * Simulates affine state-space system dx/dt = Ax + Bu + c sgn(x) + d forward
+   * dt seconds.
+   *
+   * @param voltage Voltage to apply over the timestep.
+   * @param dt      Sammple period.
+   */
+  void Update(units::volt_t voltage, units::second_t dt);
+
+  /**
+   * Returns the position.
+   *
+   * @return The current position
+   */
+  double GetPosition() const;
+
+  /**
+   * Returns the velocity.
+   *
+   * @return The current velocity
+   */
+  double GetVelocity() const;
+
+  /**
+   * Returns the acceleration for the current state and given input.
+   *
+   * @param voltage The voltage that is being applied to the mechanism / input
+   * @return The acceleration given the state and input
+   */
+  double GetAcceleration(units::volt_t voltage) const;
+
+  /**
+   * Resets model position and velocity.
+   *
+   * @param position The position the mechanism should be reset to
+   * @param velocity The velocity the mechanism should be reset to
+   */
+  void Reset(double position = 0.0, double velocity = 0.0);
+
+ private:
+  Eigen::Matrix<double, 2, 2> m_A;
+  Eigen::Matrix<double, 2, 1> m_B;
+  Eigen::Vector<double, 2> m_c;
+  Eigen::Vector<double, 2> m_d;
+  Eigen::Vector<double, 2> m_x;
+};
+
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h
new file mode 100644
index 0000000..51754a2
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h
@@ -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.
+
+#pragma once
+
+namespace sysid {
+
+struct FeedbackControllerPreset;
+
+/**
+ * Represents parameters used to calculate optimal feedback gains using a
+ * linear-quadratic regulator (LQR).
+ */
+struct LQRParameters {
+  /**
+   * The maximum allowable deviation in position.
+   */
+  double qp;
+
+  /**
+   * The maximum allowable deviation in velocity.
+   */
+  double qv;
+
+  /**
+   * The maximum allowable control effort.
+   */
+  double r;
+};
+
+/**
+ * Stores feedback controller gains.
+ */
+struct FeedbackGains {
+  /**
+   * The calculated Proportional gain
+   */
+  double Kp;
+
+  /**
+   * The calculated Derivative gain
+   */
+  double Kd;
+};
+
+/**
+ * Calculates position feedback gains for the given controller preset, LQR
+ * controller gain parameters and feedforward gains.
+ *
+ * @param preset           The feedback controller preset.
+ * @param params           The parameters for calculating optimal feedback
+ *                         gains.
+ * @param Kv               Velocity feedforward gain.
+ * @param Ka               Acceleration feedforward gain.
+ * @param encFactor        The factor to convert the gains from output units to
+ *                         encoder units. This is usually encoder EPR * gearing
+ *                         * units per rotation.
+ */
+FeedbackGains CalculatePositionFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka, double encFactor = 1.0);
+
+/**
+ * Calculates velocity feedback gains for the given controller preset, LQR
+ * controller gain parameters and feedforward gains.
+ *
+ * @param preset           The feedback controller preset.
+ * @param params           The parameters for calculating optimal feedback
+ *                         gains.
+ * @param Kv               Velocity feedforward gain.
+ * @param Ka               Acceleration feedforward gain.
+ * @param encFactor        The factor to convert the gains from output units to
+ *                         encoder units. This is usually encoder EPR * gearing
+ *                         * units per rotation.
+ */
+FeedbackGains CalculateVelocityFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka, double encFactor = 1.0);
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
new file mode 100644
index 0000000..4b13c6c
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
@@ -0,0 +1,164 @@
+// 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.
+
+#pragma once
+
+#include <units/time.h>
+
+namespace sysid {
+/**
+ * Represents a preset for a specific feedback controller. This includes info
+ * about the max controller output, the controller period, whether the gains
+ * are time-normalized, and whether there are measurement delays from sensors or
+ * onboard filtering.
+ */
+struct FeedbackControllerPreset {
+  /**
+   * The conversion factor between volts and the final controller output.
+   */
+  double outputConversionFactor;
+
+  /**
+   * The conversion factor for using controller output for velocity gains. This
+   * is necessary as some controllers do velocity controls with different time
+   * units.
+   */
+  double outputVelocityTimeFactor;
+
+  /**
+   * The period at which the controller runs.
+   */
+  units::millisecond_t period;
+
+  /**
+   * Whether the controller gains are time-normalized.
+   */
+  bool normalized;
+
+  /**
+   * The measurement delay in the encoder measurements.
+   */
+  units::millisecond_t measurementDelay;
+
+  /**
+   * Checks equality between two feedback controller presets.
+   *
+   * @param rhs Another FeedbackControllerPreset
+   * @return If the two presets are equal
+   */
+  constexpr bool operator==(const FeedbackControllerPreset& rhs) const {
+    return outputConversionFactor == rhs.outputConversionFactor &&
+           outputVelocityTimeFactor == rhs.outputVelocityTimeFactor &&
+           period == rhs.period && normalized == rhs.normalized &&
+           measurementDelay == rhs.measurementDelay;
+  }
+
+  /**
+   * Checks inequality between two feedback controller presets.
+   *
+   * @param rhs Another FeedbackControllerPreset
+   * @return If the two presets are not equal
+   */
+  constexpr bool operator!=(const FeedbackControllerPreset& rhs) const {
+    return !operator==(rhs);
+  }
+};
+
+/**
+ * The loop type for the feedback controller.
+ */
+enum class FeedbackControllerLoopType { kPosition, kVelocity };
+
+namespace presets {
+constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s};
+
+constexpr FeedbackControllerPreset kWPILibNew{kDefault};
+constexpr FeedbackControllerPreset kWPILibOld{1.0 / 12.0, 1.0, 50_ms, false,
+                                              0_s};
+
+// Measurement delay from a moving average filter:
+//
+// A moving average filter with a window size of N is an FIR filter with N taps.
+// The average delay of a moving average filter with N taps and a period between
+// samples of T is (N - 1)/2 T.
+//
+// Proof:
+// N taps with delays of 0 .. (N - 1) T
+//
+// average delay = (sum 0 .. N - 1) / N T
+// = (sum 1 .. N - 1) / N T
+//
+// note: sum 1 .. n = n(n + 1) / 2
+//
+// = (N - 1)((N - 1) + 1) / (2N) T
+// = (N - 1)N / (2N) T
+// = (N - 1)/2 T
+
+// Measurement delay from a backward finite difference:
+//
+// Velocities calculated via a backward finite difference with a period of T
+// look like:
+//
+// velocity = (position at timestep k - position at timestep k-1) / T
+//
+// The average delay is T / 2.
+//
+// Proof:
+// average delay = (0 ms + T) / 2
+//               = T / 2
+
+/**
+ * https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#changing-velocity-measurement-parameters
+ *
+ * Backward finite difference delay = 100 ms / 2 = 50 ms.
+ *
+ * 64-tap moving average delay = (64 - 1) / 2 * 1 ms = 31.5 ms.
+ *
+ * Total delay = 50 ms + 31.5 ms = 81.5 ms.
+ */
+constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms, true,
+                                                 81.5_ms};
+constexpr FeedbackControllerPreset kCTREDefault{1023.0 / 12.0, 0.1, 1_ms, false,
+                                                81.5_ms};
+/**
+ * https://api.ctr-electronics.com/phoenixpro/release/cpp/classctre_1_1phoenixpro_1_1hardware_1_1core_1_1_core_c_a_ncoder.html#a718a1a214b58d3c4543e88e3cb51ade5
+ *
+ * Phoenix Pro uses standard units and Voltage output. This means the output
+ * is 1.0, time factor is 1.0, and closed loop operates at 1 millisecond. All
+ * Pro devices make use of Kalman filters default-tuned to lowest latency, which
+ * in testing is roughly 1 millisecond
+ */
+constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true, 1_ms};
+
+/**
+ * https://github.com/wpilibsuite/sysid/issues/258#issuecomment-1010658237
+ *
+ * 8-sample moving average with 32 ms between samples.
+ *
+ * Total delay = 8-tap moving average delay = (8 - 1) / 2 * 32 ms = 112 ms.
+ */
+constexpr FeedbackControllerPreset kREVNEOBuiltIn{1.0 / 12.0, 60.0, 1_ms, false,
+                                                  112_ms};
+
+/**
+ * https://www.revrobotics.com/content/sw/max/sw-docs/cpp/classrev_1_1_c_a_n_encoder.html#a7e6ce792bc0c0558fb944771df572e6a
+ *
+ * Backward finite difference delay = 100 ms / 2 = 50 ms.
+ *
+ * 64-tap moving average delay = (64 - 1) / 2 * 1 ms = 31.5 ms.
+ *
+ * Total delay = 50 ms + 31.5 ms = 81.5 ms.
+ */
+constexpr FeedbackControllerPreset kREVNonNEO{1.0 / 12.0, 60.0, 1_ms, false,
+                                              81.5_ms};
+
+/**
+ * https://github.com/wpilibsuite/sysid/pull/138#issuecomment-841734229
+ *
+ * Backward finite difference delay = 10 ms / 2 = 5 ms.
+ */
+constexpr FeedbackControllerPreset kVenom{4096.0 / 12.0, 60.0, 1_ms, false,
+                                          5_ms};
+}  // namespace presets
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
new file mode 100644
index 0000000..fc9e47c
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
@@ -0,0 +1,25 @@
+// 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.
+
+#pragma once
+
+#include <tuple>
+#include <vector>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+/**
+ * Calculates feedforward gains given the data and the type of analysis to
+ * perform.
+ *
+ * @return Tuple containing the coefficients of the analysis along with the
+ *         r-squared (coefficient of determination) and RMSE (standard deviation
+ * of the residuals) of the fit.
+ */
+std::tuple<std::vector<double>, double, double> CalculateFeedforwardGains(
+    const Storage& data, const AnalysisType& type);
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
new file mode 100644
index 0000000..9030c00
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
@@ -0,0 +1,210 @@
+// 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.
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <exception>
+#include <functional>
+#include <string>
+#include <string_view>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+#include <frc/filter/LinearFilter.h>
+#include <units/time.h>
+#include <wpi/StringMap.h>
+#include <wpi/array.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+constexpr int kNoiseMeanWindow = 9;
+
+/**
+ * Exception for Invalid Data Errors in which we can't pin the cause of error to
+ * any one specific setting of the GUI.
+ */
+struct InvalidDataError : public std::exception {
+  /**
+   * Creates an InvalidDataError Exception. It adds additional steps after the
+   * initial error message to inform users in the ways that they could fix their
+   * data.
+   *
+   * @param message The error message
+   */
+  explicit InvalidDataError(std::string_view message) {
+    m_message = fmt::format(
+        "{}. Please verify that your units and data is reasonable and then "
+        "adjust your motion threshold, test duration, and/or window size to "
+        "try to fix this issue.",
+        message);
+  }
+
+  /**
+   * Stores the error message
+   */
+  std::string m_message;
+  const char* what() const noexcept override { return m_message.c_str(); }
+};
+
+/**
+ * Exception for Quasistatic Data being completely removed.
+ */
+struct NoQuasistaticDataError : public std::exception {
+  const char* what() const noexcept override {
+    return "Quasistatic test trimming removed all data. Please adjust your "
+           "motion threshold and double check "
+           "your units and test data to make sure that the robot is reporting "
+           "reasonable values.";
+  }
+};
+
+/**
+ * Exception for Dynamic Data being completely removed.
+ */
+struct NoDynamicDataError : public std::exception {
+  const char* what() const noexcept override {
+    return "Dynamic test trimming removed all data. Please adjust your test "
+           "duration and double check "
+           "your units and test data to make sure that the robot is reporting "
+           "reasonable values.";
+  }
+};
+
+/**
+ * Calculates the expected acceleration noise to be used as the floor of the
+ * Voltage Trim. This is done by taking the standard deviation from the moving
+ * average values of each point.
+ *
+ * @param data the prepared data vector containing acceleration data
+ * @param window the size of the window for the moving average
+ * @param accessorFunction a function that accesses the desired data from the
+ *                         PreparedData struct.
+ * @return The expected acceleration noise
+ */
+double GetNoiseFloor(
+    const std::vector<PreparedData>& data, int window,
+    std::function<double(const PreparedData&)> accessorFunction);
+
+/**
+ * Reduces noise in velocity data by applying a median filter.
+ *
+ * @tparam S The size of the raw data array
+ * @tparam Velocity The index of the velocity entry in the raw data.
+ * @param data the vector of arrays representing sysid data (must contain
+ *             velocity data)
+ * @param window the size of the window of the median filter (must be odd)
+ */
+void ApplyMedianFilter(std::vector<PreparedData>* data, int window);
+
+/**
+ * Trims the step voltage data to discard all points before the maximum
+ * acceleration and after reaching stead-state velocity. Also trims the end of
+ * the test based off of user specified test durations, but it will determine a
+ * default duration if the requested duration is less than the minimum step test
+ * duration.
+ *
+ * @param data A pointer to the step voltage data.
+ * @param settings A pointer to the settings of an analysis manager object.
+ * @param minStepTime The current minimum step test duration as one of the
+ *                    trimming procedures will remove this amount from the start
+ *                    of the test.
+ * @param maxStepTime The maximum step test duration.
+ * @return The updated minimum step test duration.
+ */
+std::tuple<units::second_t, units::second_t, units::second_t>
+TrimStepVoltageData(std::vector<PreparedData>* data,
+                    AnalysisManager::Settings* settings,
+                    units::second_t minStepTime, units::second_t maxStepTime);
+
+/**
+ * Compute the mean time delta of the given data.
+ *
+ * @param data A reference to all of the collected PreparedData
+ * @return The mean time delta for all the data points
+ */
+units::second_t GetMeanTimeDelta(const std::vector<PreparedData>& data);
+
+/**
+ * Compute the mean time delta of the given data.
+ *
+ * @param data A reference to all of the collected PreparedData
+ * @return The mean time delta for all the data points
+ */
+units::second_t GetMeanTimeDelta(const Storage& data);
+
+/**
+ * Creates a central finite difference filter that computes the nth
+ * derivative of the input given the specified number of samples.
+ *
+ * Since this requires data from the future, it should only be used in offline
+ * filtering scenarios.
+ *
+ * For example, a first derivative filter that uses two samples and a sample
+ * period of 20 ms would be
+ *
+ * <pre><code>
+ * CentralFiniteDifference<1, 2>(20_ms);
+ * </code></pre>
+ *
+ * @tparam Derivative The order of the derivative to compute.
+ * @tparam Samples    The number of samples to use to compute the given
+ *                    derivative. This must be odd and one more than the order
+ *                    of derivative or higher.
+ * @param period      The period in seconds between samples taken by the user.
+ */
+template <int Derivative, int Samples>
+frc::LinearFilter<double> CentralFiniteDifference(units::second_t period) {
+  static_assert(Samples % 2 != 0, "Number of samples must be odd.");
+
+  // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
+  wpi::array<int, Samples> stencil{wpi::empty_array};
+  for (int i = 0; i < Samples; ++i) {
+    stencil[i] = -(Samples - 1) / 2 + i;
+  }
+
+  return frc::LinearFilter<double>::FiniteDifference<Derivative, Samples>(
+      stencil, period);
+}
+
+/**
+ * Trims the quasistatic tests, applies a median filter to the velocity data,
+ * calculates acceleration and cosine (arm only) data, and trims the dynamic
+ * tests.
+ *
+ * @param data A pointer to a data vector recently created by the
+ *             ConvertToPrepared method
+ * @param settings A reference to the analysis settings
+ * @param positionDelays A reference to the vector of computed position signal
+ * delays.
+ * @param velocityDelays A reference to the vector of computed velocity signal
+ * delays.
+ * @param minStepTime A reference to the minimum dynamic test duration as one of
+ *                    the trimming procedures will remove this amount from the
+ *                    start of the test.
+ * @param maxStepTime A reference to the maximum dynamic test duration
+ * @param unit The angular unit that the arm test is in (only for calculating
+ *             cosine data)
+ */
+void 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 = "");
+
+/**
+ * Removes all points with acceleration = 0.
+ *
+ * @param data A pointer to a PreparedData vector
+ */
+void AccelFilter(wpi::StringMap<std::vector<PreparedData>>* data);
+
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/JSONConverter.h b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
new file mode 100644
index 0000000..7581d25
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
@@ -0,0 +1,24 @@
+// 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.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <wpi/Logger.h>
+
+namespace sysid {
+/**
+ * Converts a JSON from the old frc-characterization format to the new sysid
+ * format.
+ *
+ * @param path   The path to the old JSON.
+ * @param logger The logger instance for log messages.
+ * @return The full file path of the newly saved JSON.
+ */
+std::string ConvertJSON(std::string_view path, wpi::Logger& logger);
+
+std::string ToCSV(std::string_view path, wpi::Logger& logger);
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/OLS.h b/sysid/src/main/native/include/sysid/analysis/OLS.h
new file mode 100644
index 0000000..cf97904
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/OLS.h
@@ -0,0 +1,26 @@
+// 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.
+
+#pragma once
+
+#include <cstddef>
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Core>
+
+namespace sysid {
+
+/**
+ * Performs ordinary least squares multiple regression on the provided data and
+ * returns a vector of coefficients along with the r-squared (coefficient of
+ * determination) and RMSE (stardard deviation of the residuals) of the fit.
+ *
+ * @param X The independent data in y = Xβ.
+ * @param y The dependent data in y = Xβ.
+ */
+std::tuple<std::vector<double>, double, double> OLS(const Eigen::MatrixXd& X,
+                                                    const Eigen::VectorXd& y);
+
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h
new file mode 100644
index 0000000..a452c64
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h
@@ -0,0 +1,74 @@
+// 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.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace sysid {
+/**
+ * Simulation of a Simple Motor mechanism based off of a model from SysId
+ * Feedforward gains.
+ */
+class SimpleMotorSim {
+ public:
+  /**
+   * @param Ks              Static friction gain.
+   * @param Kv              Velocity gain.
+   * @param Ka              Acceleration gain.
+   * @param initialPosition Initial flywheel position.
+   * @param initialVelocity Initial flywheel velocity.
+   */
+  SimpleMotorSim(double Ks, double Kv, double Ka, double initialPosition = 0.0,
+                 double initialVelocity = 0.0);
+
+  /**
+   * Simulates affine state-space system dx/dt = Ax + Bu + c sgn(x) forward dt
+   * seconds.
+   *
+   * @param voltage Voltage to apply over the timestep.
+   * @param dt      Sammple period.
+   */
+  void Update(units::volt_t voltage, units::second_t dt);
+
+  /**
+   * Returns the position.
+   *
+   * @return The current position
+   */
+  double GetPosition() const;
+
+  /**
+   * Returns the velocity.
+   *
+   * @return The current velocity
+   */
+  double GetVelocity() const;
+
+  /**
+   * Returns the acceleration for the current state and given input.
+   *
+   * @param voltage The voltage that is being applied to the mechanism / input
+   * @return The acceleration given the state and input
+   */
+  double GetAcceleration(units::volt_t voltage) const;
+
+  /**
+   * Resets model position and velocity.
+   *
+   * @param position The position the mechanism should be reset to
+   * @param velocity The velocity the mechanism should be reset to
+   */
+  void Reset(double position = 0.0, double velocity = 0.0);
+
+ private:
+  Eigen::Matrix<double, 2, 2> m_A;
+  Eigen::Matrix<double, 2, 1> m_B;
+  Eigen::Vector<double, 2> m_c;
+  Eigen::Vector<double, 2> m_x;
+};
+
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/Storage.h b/sysid/src/main/native/include/sysid/analysis/Storage.h
new file mode 100644
index 0000000..52899a0
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/Storage.h
@@ -0,0 +1,95 @@
+// 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.
+
+#pragma once
+
+#include <vector>
+
+#include <units/time.h>
+
+namespace sysid {
+
+/**
+ * Represents each data point after it is cleaned and various parameters are
+ * calculated.
+ */
+struct PreparedData {
+  /**
+   * The timestamp of the data point.
+   */
+  units::second_t timestamp;
+
+  /**
+   * The voltage of the data point.
+   */
+  double voltage;
+
+  /**
+   * The position of the data point.
+   */
+  double position;
+
+  /**
+   * The velocity of the data point.
+   */
+  double velocity;
+
+  /**
+   * The difference in timestamps between this point and the next point.
+   */
+  units::second_t dt = 0_s;
+
+  /**
+   * The acceleration of the data point
+   */
+  double acceleration = 0.0;
+
+  /**
+   * The cosine value of the data point. This is only used for arm data where we
+   * take the cosine of the position.
+   */
+  double cos = 0.0;
+
+  /**
+   * The sine value of the data point. This is only used for arm data where we
+   * take the sine of the position.
+   */
+  double sin = 0.0;
+
+  /**
+   * Equality operator between PreparedData structs
+   *
+   * @param rhs Another PreparedData struct
+   * @return If the other struct is equal to this one
+   */
+  constexpr bool operator==(const PreparedData& rhs) const {
+    return timestamp == rhs.timestamp && voltage == rhs.voltage &&
+           position == rhs.position && velocity == rhs.velocity &&
+           dt == rhs.dt && acceleration == rhs.acceleration && cos == rhs.cos;
+  }
+};
+
+/**
+ * Storage used by the analysis manger.
+ */
+struct Storage {
+  /**
+   * Dataset for slow (aka quasistatic) test
+   */
+  std::vector<PreparedData> slowForward;
+  std::vector<PreparedData> slowBackward;
+
+  /**
+   * Dataset for fast (aka dynamic) test
+   */
+  std::vector<PreparedData> fastForward;
+  std::vector<PreparedData> fastBackward;
+
+  bool empty() const {
+    return slowForward.empty() || slowBackward.empty() || fastForward.empty() ||
+           fastBackward.empty();
+  }
+};
+
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h b/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h
new file mode 100644
index 0000000..b022072
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h
@@ -0,0 +1,19 @@
+// 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.
+
+#pragma once
+
+#include <units/angle.h>
+
+namespace sysid {
+/**
+ * Calculates the track width given the left distance, right distance, and
+ * accumulated gyro angle.
+ *
+ * @param l     The distance traveled by the left side of the drivetrain.
+ * @param r     The distance traveled by the right side of the drivetrain.
+ * @param accum The accumulated gyro angle.
+ */
+double CalculateTrackWidth(double l, double r, units::radian_t accum);
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
new file mode 100644
index 0000000..85ee09e
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
@@ -0,0 +1,237 @@
+// 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.
+
+#pragma once
+
+#include <array>
+#include <cstddef>
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringTopic.h>
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/SmallVector.h>
+#include <wpi/json.h>
+
+#include "sysid/analysis/AnalysisType.h"
+
+namespace sysid {
+/**
+ * This class is responsible for collecting data from the robot and storing it
+ * inside a JSON.
+ */
+class TelemetryManager {
+ public:
+  /**
+   * Represents settings for an instance of the TelemetryManager class. This
+   * contains information about the quasistatic ramp rate for slow tests, the
+   * step voltage for fast tests, and the mechanism type for characterization.
+   */
+  struct Settings {
+    /**
+     * The rate at which the voltage should increase during the quasistatic test
+     * (V/s).
+     */
+    double quasistaticRampRate = 0.25;
+
+    /**
+     * The voltage that the dynamic test should run at (V).
+     */
+    double stepVoltage = 7.0;
+
+    /**
+     * The units the mechanism moves per recorded rotation. The sysid project
+     * will be recording things in rotations of the shaft so the
+     * unitsPerRotation is to convert those measurements to the units the user
+     * wants to use.
+     */
+    double unitsPerRotation = 1.0;
+
+    /**
+     * The name of the units used.
+     * Valid units:  "Meters", "Feet", "Inches", "Radians", "Degrees",
+     * "Rotations"
+     */
+    std::string units = "Meters";
+
+    /**
+     * The type of mechanism that will be analyzed.
+     * Supported mechanisms: Drivetrain, Angular Drivetrain, Elevator, Arm,
+     * Simple motor.
+     */
+    AnalysisType mechanism = analysis::kDrivetrain;
+  };
+
+  /**
+   * Constructs an instance of the telemetry manager with the provided settings
+   * and NT instance to collect data over.
+   *
+   * @param settings The settings for this instance of the telemetry manager.
+   * @param logger   The logger instance to use for log data.
+   * @param instance The NT instance to collect data over. The default value of
+   *                 this parameter should suffice in production; it should only
+   *                 be changed during unit testing.
+   */
+  explicit TelemetryManager(const Settings& settings, wpi::Logger& logger,
+                            nt::NetworkTableInstance instance =
+                                nt::NetworkTableInstance::GetDefault());
+
+  /**
+   * Begins a test with the given parameters.
+   *
+   * @param name The name of the test.
+   */
+  void BeginTest(std::string_view name);
+
+  /**
+   * Ends the currently running test. If there is no test running, this is a
+   * no-op.
+   */
+  void EndTest();
+
+  /**
+   * Updates the telemetry manager -- this adds a new autospeed entry and
+   * collects newest data from the robot. This must be called periodically by
+   * the user.
+   */
+  void Update();
+
+  /**
+   * Registers a callback that's called by the TelemetryManager when there is a
+   * message to display to the user.
+   *
+   * @param callback Callback function that runs based off of the message
+   */
+  void RegisterDisplayCallback(std::function<void(std::string_view)> callback) {
+    m_callbacks.emplace_back(std::move(callback));
+  }
+
+  /**
+   * Saves a JSON with the stored data at the given location.
+   *
+   * @param location The location to save the JSON at (this is the folder that
+   *                 should contain the saved JSON).
+   * @return The full file path of the saved JSON.
+   */
+  std::string SaveJSON(std::string_view location);
+
+  /**
+   * Returns whether a test is currently running.
+   *
+   * @return Whether a test is currently running.
+   */
+  bool IsActive() const { return m_isRunningTest; }
+
+  /**
+   * Returns whether the specified test is running or has run.
+   *
+   * @param name The test to check.
+   *
+   * @return Whether the specified test is running or has run.
+   */
+  bool HasRunTest(std::string_view name) const {
+    return std::find(m_tests.cbegin(), m_tests.cend(), name) != m_tests.end();
+  }
+
+  /**
+   * Gets the size of the stored data.
+   *
+   * @return The size of the stored data
+   */
+  size_t GetCurrentDataSize() const { return m_params.data.size(); }
+
+ private:
+  enum class State { WaitingForEnable, RunningTest, WaitingForData };
+
+  /**
+   * Stores information about a currently running test. This information
+   * includes whether the robot will be traveling quickly (dynamic) or slowly
+   * (quasistatic), the direction of movement, the start time of the test,
+   * whether the robot is enabled, the current speed of the robot, and the
+   * collected data.
+   */
+  struct TestParameters {
+    bool fast = false;
+    bool forward = false;
+    bool rotate = false;
+
+    State state = State::WaitingForEnable;
+
+    double enableStart = 0.0;
+    double disableStart = 0.0;
+
+    bool enabled = false;
+    double speed = 0.0;
+
+    std::string raw;
+    std::vector<std::vector<double>> data{};
+    bool overflow = false;
+    bool mechError = false;
+
+    TestParameters() = default;
+    TestParameters(bool fast, bool forward, bool rotate, State state)
+        : fast{fast}, forward{forward}, rotate{rotate}, state{state} {}
+  };
+
+  // Settings for this instance.
+  const Settings& m_settings;
+
+  // Logger.
+  wpi::Logger& m_logger;
+
+  // Test parameters for the currently running test.
+  TestParameters m_params;
+  bool m_isRunningTest = false;
+
+  // A list of running or already run tests.
+  std::vector<std::string> m_tests;
+
+  // Stores the test data.
+  wpi::json m_data;
+
+  // Display callbacks.
+  wpi::SmallVector<std::function<void(std::string_view)>, 1> m_callbacks;
+
+  // NetworkTables instance and entries.
+  nt::NetworkTableInstance m_inst;
+  std::shared_ptr<nt::NetworkTable> table = m_inst.GetTable("SmartDashboard");
+  nt::DoublePublisher m_voltageCommand =
+      table->GetDoubleTopic("SysIdVoltageCommand").Publish();
+  nt::StringPublisher m_testType =
+      table->GetStringTopic("SysIdTestType").Publish();
+  nt::BooleanPublisher m_rotate =
+      table->GetBooleanTopic("SysIdRotate").Publish();
+  nt::StringPublisher m_mechanism =
+      table->GetStringTopic("SysIdTest").Publish();
+  nt::BooleanPublisher m_overflowPub =
+      table->GetBooleanTopic("SysIdOverflow").Publish();
+  nt::BooleanSubscriber m_overflowSub =
+      table->GetBooleanTopic("SysIdOverflow").Subscribe(false);
+  nt::BooleanPublisher m_mechErrorPub =
+      table->GetBooleanTopic("SysIdWrongMech").Publish();
+  nt::BooleanSubscriber m_mechErrorSub =
+      table->GetBooleanTopic("SysIdWrongMech").Subscribe(false);
+  nt::StringSubscriber m_telemetry =
+      table->GetStringTopic("SysIdTelemetry").Subscribe("");
+  nt::IntegerSubscriber m_fmsControlData =
+      m_inst.GetTable("FMSInfo")
+          ->GetIntegerTopic("FMSControlData")
+          .Subscribe(0);
+  nt::DoublePublisher m_ackNumberPub =
+      table->GetDoubleTopic("SysIdAckNumber").Publish();
+  nt::DoubleSubscriber m_ackNumberSub =
+      table->GetDoubleTopic("SysIdAckNumber").Subscribe(0);
+
+  int m_ackNumber;
+};
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/Analyzer.h b/sysid/src/main/native/include/sysid/view/Analyzer.h
new file mode 100644
index 0000000..2f30f61
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/Analyzer.h
@@ -0,0 +1,261 @@
+// 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.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <thread>
+#include <vector>
+
+#include <glass/View.h>
+#include <implot.h>
+#include <portable-file-dialogs.h>
+#include <units/time.h>
+#include <units/voltage.h>
+#include <wpi/Logger.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/view/AnalyzerPlot.h"
+
+struct ImPlotPoint;
+
+namespace glass {
+class Storage;
+}  // namespace glass
+
+namespace sysid {
+/**
+ * The Analyzer GUI takes care of providing the user with a user interface to
+ * load their data, visualize the data, adjust certain variables, and then view
+ * the calculated gains.
+ */
+class Analyzer : public glass::View {
+ public:
+  /**
+   * The different display and processing states for the GUI
+   */
+  enum class AnalyzerState {
+    kWaitingForJSON,
+    kNominalDisplay,
+    kMotionThresholdError,
+    kTestDurationError,
+    kGeneralDataError,
+    kFileError
+  };
+  /**
+   * The different motor controller timing presets that can be used.
+   */
+  static constexpr const char* kPresetNames[] = {"Default",
+                                                 "WPILib (2020-)",
+                                                 "WPILib (Pre-2020)",
+                                                 "CANCoder",
+                                                 "CTRE (Pro)",
+                                                 "CTRE",
+                                                 "REV Brushless Encoder Port",
+                                                 "REV Brushed Encoder Port",
+                                                 "REV Data Port",
+                                                 "Venom"};
+
+  /**
+   * The different control loops that can be used.
+   */
+  static constexpr const char* kLoopTypes[] = {"Position", "Velocity"};
+
+  /**
+   * Linear drivetrain analysis subsets
+   */
+  static constexpr const char* kDatasets[] = {"Combined", "Left", "Right"};
+
+  /**
+   * Creates the Analyzer widget
+   *
+   * @param storage Glass Storage
+   * @param logger The program logger
+   */
+  Analyzer(glass::Storage& storage, wpi::Logger& logger);
+
+  /**
+   * Displays the analyzer widget
+   */
+  void Display() override;
+
+  ~Analyzer() override { AbortDataPrep(); };
+
+ private:
+  /**
+   * Handles the logic for selecting a json to analyze
+   */
+  void SelectFile();
+
+  /**
+   * Kills the data preparation thread
+   */
+  void AbortDataPrep();
+
+  /**
+   * Displays the settings to adjust trimming and filtering for feedforward
+   * gains.
+   */
+  void DisplayFeedforwardParameters(float beginX, float beginY);
+
+  /**
+   * Displays the graphs of the data.
+   */
+  void DisplayGraphs();
+
+  /**
+   * Displays the file selection widget.
+   */
+  void DisplayFileSelector();
+
+  /**
+   * Resets the current analysis data.
+   */
+  void ResetData();
+
+  /**
+   * Sets up the reset button and Unit override buttons.
+   *
+   * @return True if the tool had been reset.
+   */
+  bool DisplayResetAndUnitOverride();
+
+  /**
+   * Prepares the data for analysis.
+   */
+  void PrepareData();
+
+  /**
+   * Sets up the graphs to display Raw Data.
+   */
+  void PrepareRawGraphs();
+
+  /**
+   * Sets up the graphs to display filtered/processed data.
+   */
+  void PrepareGraphs();
+
+  /**
+   * True if the stored state is associated with an error.
+   */
+  bool IsErrorState();
+
+  /**
+   * True if the stored state is associated with a data processing error.
+   */
+  bool IsDataErrorState();
+
+  /**
+   * Displays inputs to allow the collecting of theoretical feedforward gains.
+   */
+  void CollectFeedforwardGains(float beginX, float beginY);
+
+  /**
+   * Displays calculated feedforward gains.
+   */
+  void DisplayFeedforwardGains(float beginX, float beginY);
+
+  /**
+   * Displays calculated feedback gains.
+   */
+  void DisplayFeedbackGains();
+
+  /**
+   * Estimates ideal step test duration, qp, and qv for the LQR based off of the
+   * data given
+   */
+  void ConfigParamsOnFileSelect();
+
+  /**
+   * Updates feedforward gains from the analysis manager.
+   */
+  void UpdateFeedforwardGains();
+
+  /**
+   * Updates feedback gains from the analysis manager.
+   */
+  void UpdateFeedbackGains();
+
+  /**
+   * Handles logic of displaying a gain on ImGui
+   */
+  bool DisplayGain(const char* text, double* data, bool readOnly);
+
+  /**
+   * Handles errors when they pop up.
+   */
+  void HandleError(std::string_view msg);
+
+  // State of the Display GUI
+  AnalyzerState m_state = AnalyzerState::kWaitingForJSON;
+
+  // Stores the exception message.
+  std::string m_exception;
+
+  bool m_calcDefaults = false;
+
+  // This is true if the error popup needs to be displayed
+  bool m_errorPopup = false;
+
+  // Everything related to feedback controller calculations.
+  AnalysisManager::Settings m_settings;
+  wpi::StringMap<FeedbackControllerPreset> m_presets;
+
+  int m_selectedLoopType = 1;
+  int m_selectedPreset = 0;
+
+  // Feedforward and feedback gains.
+  std::vector<double> m_ff;
+  double m_accelRSquared;
+  double m_accelRMSE;
+  double m_Kp;
+  double m_Kd;
+  units::millisecond_t m_timescale;
+
+  // Track width
+  std::optional<double> m_trackWidth;
+
+  // Units
+  int m_selectedOverrideUnit = 0;
+  double m_conversionFactor = 0.0;
+
+  // Data analysis
+  std::unique_ptr<AnalysisManager> m_manager;
+  int m_dataset = 0;
+  int m_window = 8;
+  double m_threshold = 0.2;
+  float m_stepTestDuration = 0.0;
+
+  double m_gearingNumerator = 1.0;
+  double m_gearingDenominator = 1.0;
+
+  bool combinedGraphFit = false;
+
+  // File manipulation
+  std::unique_ptr<pfd::open_file> m_selector;
+  std::string m_location;
+
+  // Logger
+  wpi::Logger& m_logger;
+
+  // Plot
+  AnalyzerPlot m_plot{m_logger};
+  bool m_prevPlotsLoaded = false;
+
+  // Stores graph scroll bar position and states for keeping track of scroll
+  // positions after loading graphs
+  float m_graphScroll = 0;
+
+  std::atomic<bool> m_abortDataPrep{false};
+  std::thread m_dataThread;
+};
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h b/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h
new file mode 100644
index 0000000..e5fbe8c
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h
@@ -0,0 +1,203 @@
+// 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.
+
+#pragma once
+
+#include <array>
+#include <atomic>
+#include <functional>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <imgui.h>
+#include <implot.h>
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/spinlock.h>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+/**
+ * Class that helps with plotting data in the analyzer view.
+ */
+class AnalyzerPlot {
+ public:
+  float m_pointSize = 1.25;
+  // 0 for forward, 1 for reverse
+  int m_direction = 0;
+
+  /**
+   * Constructs an instance of the analyzer plot helper and allocates memory for
+   * all data vectors.
+   *
+   * @param logger The program logger
+   */
+  explicit AnalyzerPlot(wpi::Logger& logger);
+
+  /**
+   * Sets the data to be displayed on the plots.
+   *
+   * @param rawData      Raw data storage.
+   * @param filteredData Filtered data storage.
+   * @param unit         Unit of the dataset
+   * @param ff           List of feedforward gains (Ks, Kv, Ka, and optionally
+   *                     Kg).
+   * @param startTimes   Array of dataset start times.
+   * @param type         Type of analysis.
+   * @param abort        Aborts analysis early if set to true from another
+   *                     thread.
+   */
+  void SetData(const Storage& rawData, const Storage& filteredData,
+               std::string_view unit, const std::vector<double>& ff,
+               const std::array<units::second_t, 4>& startTimes,
+               AnalysisType type, std::atomic<bool>& abort);
+
+  /**
+   * Utility method to plot the raw time series data
+   *
+   * @param rawSlow The raw slow (quasistatic) test data
+   * @param rawFast The raw fast (dynamic) test data
+   * @param abort   Aborts analysis early if set to true from another thread
+   */
+  void SetRawTimeData(const std::vector<PreparedData>& rawSlow,
+                      const std::vector<PreparedData>& rawFast,
+                      std::atomic<bool>& abort);
+
+  /**
+   * Utility method to reset everything before generating the points to plot.
+   */
+  void ResetData();
+
+  /**
+   * Utility method to get set the graph labels based off of the units
+   *
+   * @param unit Unit of the dataset
+   */
+  void SetGraphLabels(std::string_view unit);
+
+  /**
+   * Sets up only the raw time series data to be plotted. This is mainly
+   * intended to be used if the filtered data has issues with it.
+   *
+   * @param data    The raw data.
+   * @param unit    Unit of the dataset.
+   * @param abort   Aborts analysis early if set to true from another thread.
+   */
+  void SetRawData(const Storage& data, std::string_view unit,
+                  std::atomic<bool>& abort);
+
+  /**
+   * Displays time domain plots.
+   *
+   * @return Returns true if plots aren't in the loading state
+   */
+  bool DisplayPlots();
+
+  /**
+   * Sets certain flags to true so that the GUI automatically fits the plots
+   */
+  void FitPlots();
+
+  /**
+   * Gets the pointer to the stored Root Mean Squared Error for display
+   *
+   * @return A pointer to the RMSE
+   */
+  double* GetSimRMSE();
+
+  /**
+   * Gets the pointer to the stored simulated velocity R-squared for display
+   *
+   * @return A pointer to the R-squared
+   */
+  double* GetSimRSquared();
+
+ private:
+  // The maximum size of each vector (dataset to plot)
+  static constexpr size_t kMaxSize = 2048;
+
+  struct FilteredDataVsTimePlot {
+    std::vector<ImPlotPoint> rawData;
+    std::vector<ImPlotPoint> filteredData;
+
+    // Simulated time domain data
+    std::vector<std::vector<ImPlotPoint>> simData;
+
+    // Stores whether this was the first call to Plot() since setting data
+    bool fitNextPlot = false;
+
+    FilteredDataVsTimePlot();
+
+    /**
+     * Plots data and fit line.
+     *
+     * @param title Plot title.
+     * @param size Plot size.
+     * @param yLabel Y axis label.
+     * @param pointSize The size of the data point markers (in pixels).
+     */
+    void Plot(const char* title, const ImVec2& size, const char* yLabel,
+              float pointSize);
+
+    /**
+     * Clears plot.
+     */
+    void Clear();
+  };
+
+  struct DataWithFitLinePlot {
+    std::vector<ImPlotPoint> data;
+    std::array<ImPlotPoint, 2> fitLine;
+
+    // Stores whether this was the first call to Plot() since setting data
+    bool fitNextPlot = false;
+
+    DataWithFitLinePlot();
+
+    /**
+     * Plots data and fit line.
+     *
+     * @param title Plot title.
+     * @param size Plot size.
+     * @param xLabel X axis label.
+     * @param yLabel Y axis label.
+     * @param fitX True if X axis should be autofitted.
+     * @param fitY True if Y axis should be autofitted.
+     * @param pointSize The size of the data point markers (in pixels).
+     * @param setup Callback within BeginPlot() block that performs custom plot
+     *              setup.
+     */
+    void Plot(
+        const char* title, const ImVec2& size, const char* xLabel,
+        const char* yLabel, bool fitX, bool fitY, float pointSize,
+        std::function<void()> setup = [] {});
+
+    /**
+     * Clears plot.
+     */
+    void Clear();
+  };
+
+  std::string m_velocityLabel;
+  std::string m_accelerationLabel;
+  std::string m_velPortionAccelLabel;
+
+  // Thread safety
+  wpi::spinlock m_mutex;
+
+  // Logger
+  wpi::Logger& m_logger;
+
+  FilteredDataVsTimePlot m_quasistaticData;
+  FilteredDataVsTimePlot m_dynamicData;
+  DataWithFitLinePlot m_regressionData;
+  DataWithFitLinePlot m_timestepData;
+
+  double m_RMSE;
+  double m_accelRSquared;
+};
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/JSONConverter.h b/sysid/src/main/native/include/sysid/view/JSONConverter.h
new file mode 100644
index 0000000..89bfa32
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/JSONConverter.h
@@ -0,0 +1,57 @@
+// 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.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+
+#include <glass/View.h>
+#include <portable-file-dialogs.h>
+#include <wpi/Logger.h>
+
+namespace sysid {
+/**
+ * Helps with converting different JSONs into different formats. Primarily
+ * enables users to convert an old 2020 FRC-Characterization JSON into a SysId
+ * JSON or a SysId JSON into a CSV file.
+ */
+class JSONConverter {
+ public:
+  /**
+   * Creates a JSONConverter widget
+   *
+   * @param logger The program logger
+   */
+  explicit JSONConverter(wpi::Logger& logger) : m_logger(logger) {}
+
+  /**
+   * Function to display the SysId JSON to CSV converter.
+   */
+  void DisplayCSVConvert();
+
+ private:
+  /**
+   * Helper method to display a specific JSON converter
+   *
+   * @param tooltip The tooltip describing the JSON converter
+   * @param converter The function that takes a filename path and performs the
+   *                  previously specifid JSON conversion.
+   */
+  void DisplayConverter(
+      const char* tooltip,
+      std::function<std::string(std::string_view, wpi::Logger&)> converter);
+
+  wpi::Logger& m_logger;
+
+  std::string m_location;
+  std::unique_ptr<pfd::open_file> m_opener;
+
+  std::string m_exception;
+
+  double m_timestamp = 0;
+};
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/Logger.h b/sysid/src/main/native/include/sysid/view/Logger.h
new file mode 100644
index 0000000..d06d650
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/Logger.h
@@ -0,0 +1,82 @@
+// 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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <glass/DataSource.h>
+#include <glass/View.h>
+#include <glass/networktables/NetworkTablesSettings.h>
+#include <portable-file-dialogs.h>
+#include <wpi/Logger.h>
+
+#include "sysid/telemetry/TelemetryManager.h"
+
+namespace glass {
+class Storage;
+}  // namespace glass
+
+namespace sysid {
+/**
+ * The logger GUI takes care of running the system idenfitication tests over
+ * NetworkTables and logging the data. This data is then stored in a JSON file
+ * which can be used for analysis.
+ */
+class Logger : public glass::View {
+ public:
+  /**
+   * Makes a logger widget.
+   *
+   * @param storage The glass storage object
+   * @param logger A logger object that keeps track of the program's logs
+   */
+  Logger(glass::Storage& storage, wpi::Logger& logger);
+
+  /**
+   * Displays the logger widget.
+   */
+  void Display() override;
+
+  /**
+   * The different mechanism / analysis types that are supported.
+   */
+  static constexpr const char* kTypes[] = {"Drivetrain", "Drivetrain (Angular)",
+                                           "Arm", "Elevator", "Simple"};
+
+  /**
+   * The different units that are supported.
+   */
+  static constexpr const char* kUnits[] = {"Meters",  "Feet",      "Inches",
+                                           "Radians", "Rotations", "Degrees"};
+
+ private:
+  /**
+   * Handles the logic of selecting a folder to save the SysId JSON to
+   */
+  void SelectDataFolder();
+
+  wpi::Logger& m_logger;
+
+  TelemetryManager::Settings m_settings;
+  int m_selectedType = 0;
+  int m_selectedUnit = 0;
+
+  std::unique_ptr<TelemetryManager> m_manager =
+      std::make_unique<TelemetryManager>(m_settings, m_logger);
+
+  std::unique_ptr<pfd::select_folder> m_selector;
+  std::string m_jsonLocation;
+
+  glass::NetworkTablesSettings m_ntSettings;
+
+  bool m_isRotationalUnits = false;
+
+  std::string m_popupText;
+
+  std::string m_opened;
+  std::string m_exception;
+};
+}  // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/UILayout.h b/sysid/src/main/native/include/sysid/view/UILayout.h
new file mode 100644
index 0000000..732a1aa
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/UILayout.h
@@ -0,0 +1,95 @@
+// 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.
+
+#pragma once
+
+#include <imgui.h>
+
+namespace sysid {
+/**
+ * constexpr shim for ImVec2.
+ */
+struct Vector2d {
+  /**
+   * X coordinate.
+   */
+  float x = 0;
+
+  /**
+   * Y coordinate.
+   */
+  float y = 0;
+
+  /**
+   * Vector2d addition operator.
+   *
+   * @param rhs Vector to add.
+   * @return Sum of two vectors.
+   */
+  constexpr Vector2d operator+(const Vector2d& rhs) const {
+    return Vector2d{x + rhs.x, y + rhs.y};
+  }
+
+  /**
+   * Vector2d subtraction operator.
+   *
+   * @param rhs Vector to subtract.
+   * @return Difference of two vectors.
+   */
+  constexpr Vector2d operator-(const Vector2d& rhs) const {
+    return Vector2d{x - rhs.x, y - rhs.y};
+  }
+
+  /**
+   * Conversion operator to ImVec2.
+   */
+  explicit operator ImVec2() const { return ImVec2{x, y}; }
+};
+
+// App window size
+inline constexpr Vector2d kAppWindowSize{1280, 720};
+
+// Menubar height
+inline constexpr int kMenubarHeight = 20;
+
+// Gap between window edges
+inline constexpr int kWindowGap = 5;
+
+// Left column position and size
+inline constexpr Vector2d kLeftColPos{kWindowGap, kMenubarHeight + kWindowGap};
+inline constexpr Vector2d kLeftColSize{
+    310, kAppWindowSize.y - kLeftColPos.y - kWindowGap};
+
+// Left column contents
+inline constexpr Vector2d kLoggerWindowPos = kLeftColPos;
+inline constexpr Vector2d kLoggerWindowSize{
+    kLeftColSize.x, kAppWindowSize.y - kWindowGap - kLoggerWindowPos.y};
+
+// Center column position and size
+inline constexpr Vector2d kCenterColPos =
+    kLeftColPos + Vector2d{kLeftColSize.x + kWindowGap, 0};
+inline constexpr Vector2d kCenterColSize{
+    360, kAppWindowSize.y - kLeftColPos.y - kWindowGap};
+
+// Center column contents
+inline constexpr Vector2d kAnalyzerWindowPos = kCenterColPos;
+inline constexpr Vector2d kAnalyzerWindowSize{kCenterColSize.x, 550};
+inline constexpr Vector2d kProgramLogWindowPos =
+    kAnalyzerWindowPos + Vector2d{0, kAnalyzerWindowSize.y + kWindowGap};
+inline constexpr Vector2d kProgramLogWindowSize{
+    kCenterColSize.x, kAppWindowSize.y - kWindowGap - kProgramLogWindowPos.y};
+
+// Right column position and size
+inline constexpr Vector2d kRightColPos =
+    kCenterColPos + Vector2d{kCenterColSize.x + kWindowGap, 0};
+inline constexpr Vector2d kRightColSize =
+    kAppWindowSize - kRightColPos - Vector2d{kWindowGap, kWindowGap};
+
+// Right column contents
+inline constexpr Vector2d kDiagnosticPlotWindowPos = kRightColPos;
+inline constexpr Vector2d kDiagnosticPlotWindowSize = kRightColSize;
+
+// Text box width as a multiple of the font size
+inline constexpr int kTextBoxWidthMultiple = 10;
+}  // namespace sysid
diff --git a/sysid/src/main/native/mac/sysid.icns b/sysid/src/main/native/mac/sysid.icns
new file mode 100644
index 0000000..53f0d69
--- /dev/null
+++ b/sysid/src/main/native/mac/sysid.icns
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-128.png b/sysid/src/main/native/resources/sysid-128.png
new file mode 100644
index 0000000..1da8dfe
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-128.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-16.png b/sysid/src/main/native/resources/sysid-16.png
new file mode 100644
index 0000000..851739f
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-16.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-256.png b/sysid/src/main/native/resources/sysid-256.png
new file mode 100644
index 0000000..a66aa94
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-256.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-32.png b/sysid/src/main/native/resources/sysid-32.png
new file mode 100644
index 0000000..3332bd1
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-32.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-48.png b/sysid/src/main/native/resources/sysid-48.png
new file mode 100644
index 0000000..bee41b7
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-48.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-512.png b/sysid/src/main/native/resources/sysid-512.png
new file mode 100644
index 0000000..5fc1d8e
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-512.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-64.png b/sysid/src/main/native/resources/sysid-64.png
new file mode 100644
index 0000000..efb193d
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-64.png
Binary files differ
diff --git a/sysid/src/main/native/win/sysid.ico b/sysid/src/main/native/win/sysid.ico
new file mode 100644
index 0000000..89f6d3f
--- /dev/null
+++ b/sysid/src/main/native/win/sysid.ico
Binary files differ
diff --git a/sysid/src/main/native/win/sysid.rc b/sysid/src/main/native/win/sysid.rc
new file mode 100644
index 0000000..21d11e8
--- /dev/null
+++ b/sysid/src/main/native/win/sysid.rc
@@ -0,0 +1 @@
+IDI_ICON1 ICON "sysid.ico"
diff --git a/sysid/src/test/native/cpp/Main.cpp b/sysid/src/test/native/cpp/Main.cpp
new file mode 100644
index 0000000..e993c1f
--- /dev/null
+++ b/sysid/src/test/native/cpp/Main.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}
diff --git a/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
new file mode 100644
index 0000000..0abb2a1
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/AnalysisType.h"
+
+TEST(AnalysisTypeTest, FromName) {
+  EXPECT_EQ(sysid::analysis::kDrivetrain,
+            sysid::analysis::FromName("Drivetrain"));
+  EXPECT_EQ(sysid::analysis::kDrivetrainAngular,
+            sysid::analysis::FromName("Drivetrain (Angular)"));
+  EXPECT_EQ(sysid::analysis::kElevator, sysid::analysis::FromName("Elevator"));
+  EXPECT_EQ(sysid::analysis::kArm, sysid::analysis::FromName("Arm"));
+  EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Simple"));
+  EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Random"));
+}
diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
new file mode 100644
index 0000000..44f664c
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+
+TEST(FeedbackAnalysisTest, Velocity1) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 2.11, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, Velocity2) {
+  auto Kv = 0.0693;
+  auto Ka = 0.1170;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 3.11, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityConversion) {
+  auto Kv = 0.0693;
+  auto Ka = 0.1170;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka, 3.0 * 1023);
+
+  // This should have the same Kp as the test above, but scaled by a factor of 3
+  // * 1023.
+  EXPECT_NEAR(Kp, 3.11 / (3 * 1023), 0.005);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityCTRE) {
+  auto Kv = 1.97;
+  auto Ka = 0.179;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kCTRECANCoder, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.000417, 0.00005);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityCTREConversion) {
+  auto Kv = 1.97;
+  auto Ka = 0.179;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kCTRECANCoder, params, Kv, Ka, 3.0);
+
+  // This should have the same Kp as the test above, but scaled by a factor
+  // of 3.
+  EXPECT_NEAR(Kp, 0.000417 / 3, 0.00005);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityREV) {
+  auto Kv = 1.97;
+  auto Ka = 0.179;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.00241, 0.005);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityREVConversion) {
+  auto Kv = 1.97;
+  auto Ka = 0.179;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kREVNEOBuiltIn, params, Kv, Ka, 3.0);
+
+  // This should have the same Kp as the test above, but scaled by a factor
+  // of 3.
+  EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
diff --git a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
new file mode 100644
index 0000000..a52840d
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
@@ -0,0 +1,251 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/ArmSim.h"
+#include "sysid/analysis/ElevatorSim.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/SimpleMotorSim.h"
+
+/**
+ * Return simulated test data for a given simulation model.
+ *
+ * @param Ks Static friction gain.
+ * @param Kv Velocity gain.
+ * @param Ka Acceleration gain.
+ * @param Kg Gravity cosine gain.
+ */
+template <typename Model>
+sysid::Storage CollectData(Model& model) {
+  constexpr auto kUstep = 0.25_V / 1_s;
+  constexpr units::volt_t kUmax = 7_V;
+  constexpr units::second_t T = 5_ms;
+  constexpr units::second_t kTestDuration = 5_s;
+
+  sysid::Storage storage;
+  auto& [slowForward, slowBackward, fastForward, fastBackward] = storage;
+
+  // Slow forward test
+  auto voltage = 0_V;
+  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+    slowForward.emplace_back(sysid::PreparedData{
+        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+        std::sin(model.GetPosition())});
+
+    model.Update(voltage, T);
+    voltage += kUstep * T;
+  }
+
+  // Slow backward test
+  model.Reset();
+  voltage = 0_V;
+  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+    slowBackward.emplace_back(sysid::PreparedData{
+        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+        std::sin(model.GetPosition())});
+
+    model.Update(voltage, T);
+    voltage -= kUstep * T;
+  }
+
+  // Fast forward test
+  model.Reset();
+  voltage = 0_V;
+  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+    fastForward.emplace_back(sysid::PreparedData{
+        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+        std::sin(model.GetPosition())});
+
+    model.Update(voltage, T);
+    voltage = kUmax;
+  }
+
+  // Fast backward test
+  model.Reset();
+  voltage = 0_V;
+  for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+    fastBackward.emplace_back(sysid::PreparedData{
+        i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+        model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+        std::sin(model.GetPosition())});
+
+    model.Update(voltage, T);
+    voltage = -kUmax;
+  }
+
+  return storage;
+}
+
+TEST(FeedforwardAnalysisTest, Arm1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+  constexpr double Kg = 0.211;
+
+  for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+    sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+    auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                               sysid::analysis::kArm);
+    auto& gains = std::get<0>(ff);
+
+    EXPECT_NEAR(gains[0], Ks, 0.003);
+    EXPECT_NEAR(gains[1], Kv, 0.003);
+    EXPECT_NEAR(gains[2], Ka, 0.003);
+    EXPECT_NEAR(gains[3], Kg, 0.003);
+    EXPECT_NEAR(gains[4], offset, 0.007);
+  }
+}
+
+TEST(FeedforwardAnalysisTest, Arm2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+  constexpr double Kg = 0.122;
+
+  for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+    sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+    auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                               sysid::analysis::kArm);
+    auto& gains = std::get<0>(ff);
+
+    EXPECT_NEAR(gains[0], Ks, 0.003);
+    EXPECT_NEAR(gains[1], Kv, 0.003);
+    EXPECT_NEAR(gains[2], Ka, 0.003);
+    EXPECT_NEAR(gains[3], Kg, 0.003);
+    EXPECT_NEAR(gains[4], offset, 0.007);
+  }
+}
+
+TEST(FeedforwardAnalysisTest, Drivetrain1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kDrivetrain);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Drivetrain2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kDrivetrain);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, DrivetrainAngular1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(
+      CollectData(model), sysid::analysis::kDrivetrainAngular);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, DrivetrainAngular2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(
+      CollectData(model), sysid::analysis::kDrivetrainAngular);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Elevator1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+  constexpr double Kg = -0.211;
+
+  sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kElevator);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+  EXPECT_NEAR(gains[3], Kg, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Elevator2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+  constexpr double Kg = -0.122;
+
+  sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kElevator);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+  EXPECT_NEAR(gains[3], Kg, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Simple1) {
+  constexpr double Ks = 1.01;
+  constexpr double Kv = 3.060;
+  constexpr double Ka = 0.327;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kSimple);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Simple2) {
+  constexpr double Ks = 0.547;
+  constexpr double Kv = 0.0693;
+  constexpr double Ka = 0.1170;
+
+  sysid::SimpleMotorSim model{Ks, Kv, Ka};
+  auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+                                             sysid::analysis::kSimple);
+  auto& gains = std::get<0>(ff);
+
+  EXPECT_NEAR(gains[0], Ks, 0.003);
+  EXPECT_NEAR(gains[1], Kv, 0.003);
+  EXPECT_NEAR(gains[2], Ka, 0.003);
+}
diff --git a/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/sysid/src/test/native/cpp/analysis/FilterTest.cpp
new file mode 100644
index 0000000..a7b0349
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/FilterTest.cpp
@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <array>
+#include <cmath>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/Storage.h"
+
+TEST(FilterTest, MedianFilter) {
+  std::vector<sysid::PreparedData> testData{
+      sysid::PreparedData{0_s, 0, 0, 0},    sysid::PreparedData{0_s, 0, 0, 1},
+      sysid::PreparedData{0_s, 0, 0, 10},   sysid::PreparedData{0_s, 0, 0, 5},
+      sysid::PreparedData{0_s, 0, 0, 3},    sysid::PreparedData{0_s, 0, 0, 0},
+      sysid::PreparedData{0_s, 0, 0, 1000}, sysid::PreparedData{0_s, 0, 0, 7},
+      sysid::PreparedData{0_s, 0, 0, 6},    sysid::PreparedData{0_s, 0, 0, 5}};
+  std::vector<sysid::PreparedData> expectedData{
+      sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1},
+      sysid::PreparedData{0_s, 0, 0, 5}, sysid::PreparedData{0_s, 0, 0, 5},
+      sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 3},
+      sysid::PreparedData{0_s, 0, 0, 7}, sysid::PreparedData{0_s, 0, 0, 7},
+      sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}};
+
+  sysid::ApplyMedianFilter(&testData, 3);
+  EXPECT_EQ(expectedData, testData);
+}
+
+TEST(FilterTest, NoiseFloor) {
+  std::vector<sysid::PreparedData> testData = {
+      {0_s, 1, 2, 3, 5_ms, 0, 0},    {1_s, 1, 2, 3, 5_ms, 1, 0},
+      {2_s, 1, 2, 3, 5_ms, 2, 0},    {3_s, 1, 2, 3, 5_ms, 5, 0},
+      {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
+      {6_s, 1, 2, 3, 5_ms, 0, 0},    {7_s, 1, 2, 3, 5_ms, 0.02, 0},
+      {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0}};
+  double noiseFloor =
+      GetNoiseFloor(testData, 2, [](auto&& pt) { return pt.acceleration; });
+  EXPECT_NEAR(0.953, noiseFloor, 0.001);
+}
+
+TEST(FilterTest, StepTrim) {
+  std::vector<sysid::PreparedData> testData = {
+      {0_s, 1, 2, 3, 5_ms, 0, 0},    {1_s, 1, 2, 3, 5_ms, 0.25, 0},
+      {2_s, 1, 2, 3, 5_ms, 0.5, 0},  {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+      {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
+      {6_s, 1, 2, 3, 5_ms, 0, 0},    {7_s, 1, 2, 3, 5_ms, 0.02, 0},
+      {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0},
+  };
+
+  std::vector<sysid::PreparedData> expectedData = {
+      {2_s, 1, 2, 3, 5_ms, 0.5, 0},
+      {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+      {4_s, 1, 2, 3, 5_ms, 0.35, 0},
+      {5_s, 1, 2, 3, 5_ms, 0.15, 0}};
+
+  auto maxTime = 9_s;
+  auto minTime = maxTime;
+
+  sysid::AnalysisManager::Settings settings;
+  auto [tempMinTime, positionDelay, velocityDelay] =
+      sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime);
+  minTime = tempMinTime;
+
+  EXPECT_EQ(expectedData[0].acceleration, testData[0].acceleration);
+  EXPECT_EQ(expectedData.back().acceleration, testData.back().acceleration);
+  EXPECT_EQ(5, settings.stepTestDuration.value());
+  EXPECT_EQ(2, minTime.value());
+}
+
+template <int Derivative, int Samples, typename F, typename DfDx>
+void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+                          double max) {
+  static_assert(Samples % 2 != 0, "Number of samples must be odd.");
+
+  auto filter = sysid::CentralFiniteDifference<Derivative, Samples>(h);
+
+  for (int i = min / h.value(); i < max / h.value(); ++i) {
+    // Let filter initialize
+    if (i < static_cast<int>(min / h.value()) + Samples) {
+      filter.Calculate(f(i * h.value()));
+      continue;
+    }
+
+    // For central finite difference, the derivative computed at this point is
+    // half the window size in the past.
+    // The order of accuracy is O(h^(N - d)) where N is number of stencil
+    // points and d is order of derivative
+    EXPECT_NEAR(dfdx((i - static_cast<int>((Samples - 1) / 2)) * h.value()),
+                filter.Calculate(f(i * h.value())),
+                std::pow(h.value(), Samples - Derivative));
+  }
+}
+
+/**
+ * Test central finite difference.
+ */
+TEST(LinearFilterOutputTest, CentralFiniteDifference) {
+  constexpr auto h = 5_ms;
+
+  AssertCentralResults<1, 3>(
+      [](double x) {
+        // f(x) = x²
+        return x * x;
+      },
+      [](double x) {
+        // df/dx = 2x
+        return 2.0 * x;
+      },
+      h, -20.0, 20.0);
+
+  AssertCentralResults<1, 3>(
+      [](double x) {
+        // f(x) = std::sin(x)
+        return std::sin(x);
+      },
+      [](double x) {
+        // df/dx = std::cos(x)
+        return std::cos(x);
+      },
+      h, -20.0, 20.0);
+
+  AssertCentralResults<1, 3>(
+      [](double x) {
+        // f(x) = ln(x)
+        return std::log(x);
+      },
+      [](double x) {
+        // df/dx = 1 / x
+        return 1.0 / x;
+      },
+      h, 1.0, 20.0);
+
+  AssertCentralResults<2, 5>(
+      [](double x) {
+        // f(x) = x^2
+        return x * x;
+      },
+      [](double x) {
+        // d²f/dx² = 2
+        return 2.0;
+      },
+      h, -20.0, 20.0);
+
+  AssertCentralResults<2, 5>(
+      [](double x) {
+        // f(x) = std::sin(x)
+        return std::sin(x);
+      },
+      [](double x) {
+        // d²f/dx² = -std::sin(x)
+        return -std::sin(x);
+      },
+      h, -20.0, 20.0);
+
+  AssertCentralResults<2, 5>(
+      [](double x) {
+        // f(x) = ln(x)
+        return std::log(x);
+      },
+      [](double x) {
+        // d²f/dx² = -1 / x²
+        return -1.0 / (x * x);
+      },
+      h, 1.0, 20.0);
+}
diff --git a/sysid/src/test/native/cpp/analysis/OLSTest.cpp b/sysid/src/test/native/cpp/analysis/OLSTest.cpp
new file mode 100644
index 0000000..bf20516
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/OLSTest.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/OLS.h"
+
+TEST(OLSTest, TwoVariablesTwoPoints) {
+  // (1, 3) and (2, 5). Should produce y = 2x + 1.
+  Eigen::MatrixXd X{{1.0, 1.0}, {1.0, 2.0}};
+  Eigen::VectorXd y{{3.0}, {5.0}};
+
+  auto [coefficients, cod, rmse] = sysid::OLS(X, y);
+  EXPECT_EQ(coefficients.size(), 2u);
+
+  EXPECT_NEAR(coefficients[0], 1.0, 0.05);
+  EXPECT_NEAR(coefficients[1], 2.0, 0.05);
+  EXPECT_NEAR(cod, 1.0, 1E-4);
+}
+
+TEST(OLSTest, TwoVariablesFivePoints) {
+  // (2, 4), (3, 5), (5, 7), (7, 10), (9, 15)
+  // Should produce 1.518x + 0.305.
+  Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 5}, {1, 7}, {1, 9}};
+  Eigen::VectorXd y{{4}, {5}, {7}, {10}, {15}};
+
+  auto [coefficients, cod, rmse] = sysid::OLS(X, y);
+  EXPECT_EQ(coefficients.size(), 2u);
+
+  EXPECT_NEAR(coefficients[0], 0.305, 0.05);
+  EXPECT_NEAR(coefficients[1], 1.518, 0.05);
+  EXPECT_NEAR(cod, 0.985, 0.05);
+}
+
+#ifndef NDEBUG
+TEST(OLSTest, MalformedData) {
+  Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 4}};
+  Eigen::VectorXd y{{4}, {5}};
+  EXPECT_DEATH(sysid::OLS(X, y), "");
+}
+#endif
diff --git a/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp
new file mode 100644
index 0000000..e7b662f
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/TrackWidthAnalysis.h"
+
+TEST(TrackWidthAnalysisTest, Calculate) {
+  double result = sysid::CalculateTrackWidth(-0.5386, 0.5386, 90_deg);
+  EXPECT_NEAR(result, 0.6858, 1E-4);
+}