SCRIPT Move subprojects

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

@@ -0,0 +1,213 @@
+// 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>
+#include <utility>
+#include <string_view>
+
+#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 <wpi/Logger.h>
+#include <wpi/print.h>
+#include <wpigui.h>
+#include <wpigui_openurl.h>
+
+#include "sysid/view/Analyzer.h"
+#include "sysid/view/DataSelector.h"
+#include "sysid/view/LogLoader.h"
+#include "sysid/view/UILayout.h"
+
+namespace gui = wpi::gui;
+
+static std::unique_ptr<glass::WindowManager> gWindowManager;
+
+glass::Window* gLogLoaderWindow;
+glass::Window* gDataSelectorWindow;
+glass::Window* gAnalyzerWindow;
+glass::Window* gProgramLogWindow;
+static glass::MainMenuBar gMainMenu;
+
+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
+    wpi::print(stderr, "{}{} ({}:{})\n", lvl, msg, filename, line);
+#endif
+  });
+
+  gLogger.set_min_level(wpi::WPI_LOG_DEBUG);
+
+  // Initialize window manager and add views.
+  auto& storage = glass::GetStorageRoot().GetChild("SysId");
+  gWindowManager = std::make_unique<glass::WindowManager>(storage);
+  gWindowManager->GlobalInit();
+
+  auto logLoader = std::make_unique<sysid::LogLoader>(storage, gLogger);
+  auto dataSelector = std::make_unique<sysid::DataSelector>(storage, gLogger);
+  auto analyzer = std::make_unique<sysid::Analyzer>(storage, gLogger);
+
+  logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); });
+  dataSelector->testdata = [_analyzer = analyzer.get(),
+                            ds = dataSelector.get()](auto data) {
+    _analyzer->m_data = data;
+    _analyzer->SetMissingTests(ds->m_missingTests);
+    _analyzer->AnalyzeData();
+  };
+
+  gLogLoaderWindow =
+      gWindowManager->AddWindow("Log Loader", std::move(logLoader));
+
+  gDataSelectorWindow =
+      gWindowManager->AddWindow("Data Selector", std::move(dataSelector));
+
+  gAnalyzerWindow = gWindowManager->AddWindow("Analyzer", std::move(analyzer));
+
+  gProgramLogWindow = gWindowManager->AddWindow(
+      "Program Log", std::make_unique<glass::LogView>(&gLog));
+
+  // Set default positions and sizes for windows.
+
+  // Logger window position/size
+  gLogLoaderWindow->SetDefaultPos(sysid::kLogLoaderWindowPos.x,
+                                  sysid::kLogLoaderWindowPos.y);
+  gLogLoaderWindow->SetDefaultSize(sysid::kLogLoaderWindowSize.x,
+                                   sysid::kLogLoaderWindowSize.y);
+
+  // Data selector window position/size
+  gDataSelectorWindow->SetDefaultPos(sysid::kDataSelectorWindowPos.x,
+                                     sysid::kDataSelectorWindowPos.y);
+  gDataSelectorWindow->SetDefaultSize(sysid::kDataSelectorWindowSize.x,
+                                      sysid::kDataSelectorWindowSize.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();
+
+  // 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();
+    }
+
+    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 (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

tools/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

tools/sysid/src/main/native/cpp/Util.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/Util.h"
+
+#include <filesystem>
+#include <stdexcept>
+
+#include <IconsFontAwesome6.h>
+#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::CreateErrorTooltip(const char* text) {
+  ImGui::SameLine();
+  ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f),
+                     ICON_FA_TRIANGLE_EXCLAMATION);
+
+  if (ImGui::IsItemHovered()) {
+    ImGui::BeginTooltip();
+    ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f);
+    ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s", 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();
+  }
+}
+
+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;
+  // NOLINTNEXTLINE(build/include_what_you_use)
+  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;
+}

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

@@ -0,0 +1,290 @@
+// 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 <limits>
+#include <utility>
+#include <vector>
+
+#include <fmt/format.h>
+#include <units/angle.h>
+#include <wpi/MathExtras.h>
+#include <wpi/StringExtras.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/FilteringUtils.h"
+
+using namespace sysid;
+
+static double Lerp(units::second_t time,
+                   std::vector<MotorData::Run::Sample<double>>& data) {
+  auto next = std::find_if(data.begin(), data.end(), [&](const auto& entry) {
+    return entry.time > time;
+  });
+
+  if (next == data.begin()) {
+    next++;
+  }
+
+  if (next == data.end()) {
+    next--;
+  }
+
+  const auto prev = next - 1;
+
+  return wpi::Lerp(prev->measurement, next->measurement,
+                   (time - prev->time) / (next->time - prev->time));
+}
+
+/**
+ * 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
+ */
+static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
+  std::vector<PreparedData> prepared;
+  // assume we've selected down to a single contiguous run by this point
+  auto run = data.runs[0];
+
+  for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
+    const auto& currentVoltage = run.voltage[i];
+    const auto& nextVoltage = run.voltage[i + 1];
+
+    auto currentPosition = Lerp(currentVoltage.time, run.position);
+
+    auto currentVelocity = Lerp(currentVoltage.time, run.velocity);
+
+    prepared.emplace_back(PreparedData{currentVoltage.time,
+                                       currentVoltage.measurement.value(),
+                                       currentPosition, currentVelocity,
+                                       nextVoltage.time - currentVoltage.time});
+  }
+
+  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
+ */
+static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
+  auto& data = *dataset;
+  // Loads the Raw Data
+  for (auto& it : data) {
+    auto& key = it.first;
+    auto& motorData = it.second;
+
+    if (!wpi::contains(key, "raw")) {
+      data[fmt::format("raw-{}", key)] = motorData;
+      data[fmt::format("original-raw-{}", key)] = motorData;
+    }
+  }
+}
+
+/**
+ * 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() {
+  wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+  WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+
+  WPI_INFO(m_logger, "{}", "Copying raw data.");
+  CopyRawData(&m_data.motorData);
+
+  WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
+  // Convert data to PreparedData structs
+  for (auto& it : m_data.motorData) {
+    auto key = it.first;
+    preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
+    WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
+  }
+
+  // Store the original datasets
+  m_originalDataset =
+      CombineDatasets(preparedData["original-raw-quasistatic-forward"],
+                      preparedData["original-raw-quasistatic-reverse"],
+                      preparedData["original-raw-dynamic-forward"],
+                      preparedData["original-raw-dynamic-reverse"]);
+
+  WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
+  sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+                              m_velocityDelays, m_minStepTime, m_maxStepTime,
+                              m_data.distanceUnit);
+
+  WPI_INFO(m_logger, "{}", m_minStepTime);
+  WPI_INFO(m_logger, "{}", m_maxStepTime);
+
+  WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+  sysid::AccelFilter(&preparedData);
+
+  WPI_INFO(m_logger, "{}", "Storing datasets.");
+  // Store the raw datasets
+  m_rawDataset = CombineDatasets(preparedData["raw-quasistatic-forward"],
+                                 preparedData["raw-quasistatic-reverse"],
+                                 preparedData["raw-dynamic-forward"],
+                                 preparedData["raw-dynamic-reverse"]);
+
+  // Store the filtered datasets
+  m_filteredDataset = CombineDatasets(
+      preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"],
+      preparedData["dynamic-forward"], preparedData["dynamic-reverse"]);
+
+  m_startTimes = {preparedData["raw-quasistatic-forward"][0].timestamp,
+                  preparedData["raw-quasistatic-reverse"][0].timestamp,
+                  preparedData["raw-dynamic-forward"][0].timestamp,
+                  preparedData["raw-dynamic-reverse"][0].timestamp};
+}
+
+AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
+    : m_logger{logger}, m_settings{settings} {}
+
+AnalysisManager::AnalysisManager(TestData data, Settings& settings,
+                                 wpi::Logger& logger)
+    : m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
+  // Reset settings for Dynamic Test Limits
+  m_settings.stepTestDuration = 0_s;
+  m_settings.velocityThreshold = std::numeric_limits<double>::infinity();
+}
+
+void AnalysisManager::PrepareData() {
+  //  WPI_INFO(m_logger, "Preparing {} data", m_data.mechanismType.name);
+
+  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& analysisType = m_data.mechanismType;
+  const auto& ff =
+      sysid::CalculateFeedforwardGains(GetFilteredData(), analysisType, false);
+
+  const auto& Ks = ff.coeffs[0];
+  FeedforwardGain KsGain = {
+      .gain = Ks, .descriptor = "Voltage needed to overcome static friction."};
+  if (Ks < 0) {
+    KsGain.isValidGain = false;
+    KsGain.errorMessage = fmt::format(
+        "Calculated Ks gain of: {0:.3f} is erroneous! Ks should be >= 0.", Ks);
+  }
+
+  const auto& Kv = ff.coeffs[1];
+  FeedforwardGain KvGain = {
+      .gain = Kv,
+      .descriptor =
+          "Voltage needed to hold/cruise at a constant velocity while "
+          "overcoming the counter-electromotive force and any additional "
+          "friction."};
+  if (Kv < 0) {
+    KvGain.isValidGain = false;
+    KvGain.errorMessage = fmt::format(
+        "Calculated Kv gain of: {0:.3f} is erroneous! Kv should be >= 0.", Kv);
+  }
+
+  const auto& Ka = ff.coeffs[2];
+  FeedforwardGain KaGain = {
+      .gain = Ka,
+      .descriptor =
+          "Voltage needed to induce a given acceleration in the motor shaft."};
+  if (Ka <= 0) {
+    KaGain.isValidGain = false;
+    KaGain.errorMessage = fmt::format(
+        "Calculated Ka gain of: {0:.3f} is erroneous! Ka should be > 0.", Ka);
+  }
+
+  if (analysisType == analysis::kSimple) {
+    return FeedforwardGains{
+        .olsResult = ff, .Ks = KsGain, .Kv = KvGain, .Ka = KaGain};
+  }
+
+  if (analysisType == analysis::kElevator || analysisType == analysis::kArm) {
+    const auto& Kg = ff.coeffs[3];
+    FeedforwardGain KgGain = {
+        Kg, "Voltage needed to counteract the force of gravity."};
+    if (Kg < 0) {
+      KgGain.isValidGain = false;
+      KgGain.errorMessage = fmt::format(
+          "Calculated Kg gain of: {0:.3f} is erroneous! Kg should be >= 0.",
+          Ka);
+    }
+
+    // Elevator analysis only requires Kg
+    if (analysisType == analysis::kElevator) {
+      return FeedforwardGains{.olsResult = ff,
+                              .Ks = KsGain,
+                              .Kv = KvGain,
+                              .Ka = KaGain,
+                              .Kg = KgGain};
+    } else {
+      // Arm analysis requires Kg and an angle offset
+      FeedforwardGain offset = {
+          .gain = ff.coeffs[4],
+          .descriptor =
+              "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."};
+      return FeedforwardGains{ff, KsGain, KvGain, KaGain, KgGain, offset};
+    }
+  }
+
+  return FeedforwardGains{.olsResult = ff};
+}
+
+sysid::FeedbackGains AnalysisManager::CalculateFeedback(
+    const FeedforwardGain& Kv, const FeedforwardGain& Ka) {
+  FeedbackGains fb;
+  if (m_settings.type == FeedbackControllerLoopType::kPosition) {
+    fb = sysid::CalculatePositionFeedbackGains(
+        m_settings.preset, m_settings.lqr, Kv.gain, Ka.gain);
+  } else {
+    fb = sysid::CalculateVelocityFeedbackGains(
+        m_settings.preset, m_settings.lqr, Kv.gain, Ka.gain);
+  }
+
+  return fb;
+}
+
+void AnalysisManager::OverrideUnits(std::string_view unit) {
+  m_data.distanceUnit = unit;
+}
+
+void AnalysisManager::ResetUnitsFromJSON() {}

tools/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};
+}

tools/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};
+}

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

@@ -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.
+
+#include "sysid/analysis/FeedbackAnalysis.h"
+
+#include <cmath>
+
+#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);
+using Matrix1d = Eigen::Matrix<double, 1, 1>;
+
+FeedbackGains sysid::CalculatePositionFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
+  // If acceleration for position control requires no effort, velocity becomes
+  // an input. We choose an appropriate model in this case to avoid numerical
+  // instabilities in the LQR.
+  if (std::abs(Ka) < 1e-7) {
+    // System has position state and velocity input
+    frc::LinearSystem<1, 1, 1> system{Matrix1d{0.0}, Matrix1d{1.0},
+                                      Matrix1d{1.0}, Matrix1d{0.0}};
+
+    frc::LinearQuadraticRegulator<1, 1> controller{
+        system, {params.qp}, {params.r}, preset.period};
+    controller.LatencyCompensate(system, preset.period,
+                                 preset.measurementDelay);
+
+    return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
+  }
+
+  auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
+
+  frc::LinearQuadraticRegulator<2, 1> controller{
+      system, {params.qp, params.qv}, {params.r}, preset.period};
+  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
+
+  return {controller.K(0, 0) * preset.outputConversionFactor,
+          controller.K(0, 1) * preset.outputConversionFactor /
+              (preset.normalized ? 1 : units::second_t{preset.period}.value())};
+}
+
+FeedbackGains sysid::CalculateVelocityFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka, double encFactor) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
+  // If acceleration for velocity control requires no effort, the feedback
+  // control gains approach zero. We special-case it here to avoid numerical
+  // instabilities in LQR.
+  if (std::abs(Ka) < 1E-7) {
+    return {0.0, 0.0};
+  }
+
+  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
+  frc::LinearQuadraticRegulator<1, 1> controller{
+      system, {params.qv}, {params.r}, preset.period};
+  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
+
+  return {controller.K(0, 0) * preset.outputConversionFactor /
+              (preset.outputVelocityTimeFactor * encFactor),
+          0.0};
+}

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

@@ -0,0 +1,274 @@
+// 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 <array>
+#include <bitset>
+#include <cmath>
+#include <string>
+#include <vector>
+
+#include <Eigen/Eigenvalues>
+#include <fmt/format.h>
+#include <fmt/ranges.h>
+#include <units/math.h>
+#include <units/time.h>
+
+#include "sysid/analysis/OLS.h"
+
+namespace sysid {
+
+/**
+ * Populates OLS data for the following models:
+ *
+ * Simple, Drivetrain, DrivetrainAngular:
+ *
+ *   (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ)
+ *
+ * Elevator:
+ *
+ *   (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ
+ *
+ * Arm:
+ *
+ *   (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ cos(angle) + ε sin(angle)
+ *
+ * OLS performs best with the noisiest variable as the dependent variable, so we
+ * regress acceleration in terms of the other variables.
+ *
+ * @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) {
+  // Fill in X and y row-wise
+  for (size_t sample = 0; sample < d.size(); ++sample) {
+    const auto& pt = d[sample];
+
+    // Set the velocity term (for α)
+    X(sample, 0) = pt.velocity;
+
+    // Set the voltage term (for β)
+    X(sample, 1) = pt.voltage;
+
+    // Set the intercept term (for γ)
+    X(sample, 2) = std::copysign(1, pt.velocity);
+
+    // Set test-specific variables
+    if (type == analysis::kElevator) {
+      // Set the gravity term (for δ)
+      X(sample, 3) = 1.0;
+    } else if (type == analysis::kArm) {
+      // Set the cosine and sine terms (for δ and ε)
+      X(sample, 3) = pt.cos;
+      X(sample, 4) = pt.sin;
+    }
+
+    // Set the dependent variable (acceleration)
+    y(sample) = pt.acceleration;
+  }
+}
+
+/**
+ * Throws an InsufficientSamplesError if the collected data is poor for OLS.
+ *
+ * @param X The collected data in matrix form for OLS.
+ * @param type The analysis type.
+ */
+static void CheckOLSDataQuality(const Eigen::MatrixXd& X,
+                                const AnalysisType& type) {
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigSolver{X.transpose() * X};
+  const Eigen::VectorXd& eigvals = eigSolver.eigenvalues();
+  const Eigen::MatrixXd& eigvecs = eigSolver.eigenvectors();
+
+  // Bits are Ks, Kv, Ka, Kg, offset
+  std::bitset<5> badGains;
+
+  constexpr double threshold = 10.0;
+
+  // For n x n matrix XᵀX, need n nonzero eigenvalues for good fit
+  for (int row = 0; row < eigvals.rows(); ++row) {
+    // Find row of eigenvector with largest magnitude. This determines the
+    // primary regression variable that corresponds to the eigenvalue.
+    int maxIndex;
+    double maxCoeff = eigvecs.col(row).cwiseAbs().maxCoeff(&maxIndex);
+
+    // Check whether the eigenvector component along the regression variable's
+    // direction is below the threshold. If it is, the regression variable's fit
+    // is bad.
+    if (std::abs(eigvals(row) * maxCoeff) <= threshold) {
+      // Fit for α is bad
+      if (maxIndex == 0) {
+        // Affects Kv
+        badGains.set(1);
+      }
+
+      // Fit for β is bad
+      if (maxIndex == 1) {
+        // Affects all gains
+        badGains.set();
+        break;
+      }
+
+      // Fit for γ is bad
+      if (maxIndex == 2) {
+        // Affects Ks
+        badGains.set(0);
+      }
+
+      // Fit for δ is bad
+      if (maxIndex == 3) {
+        if (type == analysis::kElevator) {
+          // Affects Kg
+          badGains.set(3);
+        } else if (type == analysis::kArm) {
+          // Affects Kg and offset
+          badGains.set(3);
+          badGains.set(4);
+        }
+      }
+
+      // Fit for ε is bad
+      if (maxIndex == 4) {
+        // Affects Kg and offset
+        badGains.set(3);
+        badGains.set(4);
+      }
+    }
+  }
+
+  // If any gains are bad, throw an error
+  if (badGains.any()) {
+    // Create list of bad gain names
+    constexpr std::array gainNames{"Ks", "Kv", "Ka", "Kg", "offset"};
+    std::vector<std::string_view> badGainsList;
+    for (size_t i = 0; i < badGains.size(); ++i) {
+      if (badGains.test(i)) {
+        badGainsList.emplace_back(gainNames[i]);
+      }
+    }
+
+    std::string error = fmt::format("Insufficient samples to compute {}.\n\n",
+                                    fmt::join(badGainsList, ", "));
+
+    // If all gains are bad, the robot may not have moved
+    if (badGains.all()) {
+      error += "Either no data was collected or the robot didn't move.\n\n";
+    }
+
+    // Append guidance for fixing the data
+    error +=
+        "Ensure the data has:\n\n"
+        "  * at least 2 steady-state velocity events to separate Ks from Kv\n"
+        "  * at least 1 acceleration event to find Ka\n"
+        "  * for elevators, enough vertical motion to measure gravity\n"
+        "  * for arms, enough range of motion to measure gravity and encoder "
+        "offset\n";
+    throw InsufficientSamplesError{error};
+  }
+}
+
+OLSResult CalculateFeedforwardGains(const Storage& data,
+                                    const AnalysisType& type,
+                                    bool throwOnBadData) {
+  // 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()));
+
+  // Check quality of collected data
+  if (throwOnBadData) {
+    CheckOLSDataQuality(X, type);
+  }
+
+  std::vector<double> gains;
+  gains.reserve(X.rows());
+
+  auto ols = OLS(X, y);
+
+  // Calculate feedforward gains
+  //
+  // See docs/ols-derivations.md for more details.
+  {
+    // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+    // dx/dt = αx + βu + γ sgn(x)
+
+    // α = -Kv/Ka
+    // β = 1/Ka
+    // γ = -Ks/Ka
+    double α = ols.coeffs[0];
+    double β = ols.coeffs[1];
+    double γ = ols.coeffs[2];
+
+    // Ks = -γ/β
+    // Kv = -α/β
+    // Ka = 1/β
+    gains.emplace_back(-γ / β);
+    gains.emplace_back(-α / β);
+    gains.emplace_back(1 / β);
+
+    if (type == analysis::kElevator) {
+      // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+      // dx/dt = αx + βu + γ sgn(x) + δ
+
+      // δ = -Kg/Ka
+      double δ = ols.coeffs[3];
+
+      // Kg = -δ/β
+      gains.emplace_back(-δ / β);
+    }
+
+    if (type == analysis::kArm) {
+      // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+      //           - Kg/Ka cos(offset) cos(angle)
+      //           + Kg/Ka sin(offset) sin(angle)
+      // dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
+
+      // δ = -Kg/Ka cos(offset)
+      // ε = Kg/Ka sin(offset)
+      double δ = ols.coeffs[3];
+      double ε = ols.coeffs[4];
+
+      // Kg = hypot(δ, ε)/β
+      // offset = atan2(ε, -δ)
+      gains.emplace_back(std::hypot(δ, ε) / β);
+      gains.emplace_back(std::atan2(ε, -δ));
+    }
+  }
+
+  // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)
+  return OLSResult{gains, ols.rSquared, ols.rmse};
+}
+
+}  // namespace sysid

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

@@ -0,0 +1,443 @@
+// 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 <algorithm>
+#include <functional>
+#include <limits>
+#include <numbers>
+#include <numeric>
+#include <string>
+#include <tuple>
+#include <vector>
+
+#include <fmt/format.h>
+#include <frc/filter/LinearFilter.h>
+#include <frc/filter/MedianFilter.h>
+#include <units/math.h>
+#include <wpi/MathExtras.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->velocityThreshold * 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) {
+        // Since we don't know if its a forward or backwards test here, we use
+        // the sign of each point's velocity to determine how to compare their
+        // accelerations.
+        return wpi::sgn(a.velocity) * a.acceleration <
+               wpi::sgn(b.velocity) * b.acceleration;
+      });
+
+  // Current limiting can delay onset of the peak acceleration, so we need to
+  // find the first acceleration *near* the max.  Magic number tolerance here
+  // because this whole file is tech debt already
+  auto accelBegins = std::find_if(
+      data->begin(), data->end(), [&maxAccel](const auto& measurement) {
+        return wpi::sgn(measurement.velocity) * measurement.acceleration >
+               0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration;
+      });
+
+  units::second_t velocityDelay;
+  if (accelBegins != data->end()) {
+    velocityDelay = accelBegins->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 test duration not yet specified, calculate default
+  if (settings->stepTestDuration == 0_s) {
+    // Find maximum speed reached
+    const auto maxSpeed =
+        GetMaxSpeed(*data, [](auto&& pt) { return pt.velocity; });
+    // Find place where 90% of maximum speed exceeded
+    auto endIt = std::find_if(
+        data->begin(), data->end(), [&](const PreparedData& entry) {
+          return std::abs(entry.velocity) > maxSpeed * 0.9;
+        });
+
+    if (endIt != data->end()) {
+      settings->stepTestDuration =
+          std::min(endIt->timestamp - data->front().timestamp + minStepTime,
+                   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 >
+               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));
+}
+
+double sysid::GetMaxSpeed(
+    const std::vector<PreparedData>& data,
+    std::function<double(const PreparedData&)> accessorFunction) {
+  double max = 0.0;
+  for (size_t i = 0; i < data.size(); i++) {
+    max = std::max(max, std::abs(accessorFunction(data[i])));
+  }
+  return max;
+}
+
+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.second;
+
+    if (IsRaw(key) && wpi::contains(key, "dynamic")) {
+      if (!dataset.empty()) {
+        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->velocityThreshold == std::numeric_limits<double>::infinity()) {
+    for (auto& it : preparedData) {
+      auto& key = it.first;
+      auto& dataset = it.second;
+      if (wpi::contains(key, "quasistatic")) {
+        settings->velocityThreshold =
+            std::min(settings->velocityThreshold,
+                     GetNoiseFloor(dataset, kNoiseMeanWindow,
+                                   [](auto&& pt) { return pt.velocity; }));
+      }
+    }
+  }
+
+  for (auto& it : preparedData) {
+    auto& key = it.first;
+    auto& dataset = it.second;
+
+    // Trim quasistatic test data to remove all points where voltage is zero or
+    // velocity < velocity threshold.
+    if (wpi::contains(key, "quasistatic")) {
+      dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
+                                   [&](const auto& pt) {
+                                     return std::abs(pt.voltage) <= 0 ||
+                                            std::abs(pt.velocity) <
+                                                settings->velocityThreshold;
+                                   }),
+                    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, "dynamic")) {
+      // 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.second;
+
+    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.second.empty(); })) {
+    throw sysid::InvalidDataError(
+        "Acceleration filtering has removed all data.");
+  }
+}

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

@@ -0,0 +1,88 @@
+// 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 <cassert>
+#include <cmath>
+
+#include <Eigen/Cholesky>
+
+namespace sysid {
+
+OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
+  assert(X.rows() == y.rows());
+
+  // The linear regression model can be written as follows:
+  //
+  //   y = Xβ + ε
+  //
+  // where y is the dependent observed variable, X is the matrix of independent
+  // variables, β is a vector of coefficients, and ε is a vector of residuals.
+  //
+  // We want to find the value of β that minimizes εᵀε.
+  //
+  //   ε = y − Xβ
+  //   εᵀε = (y − Xβ)ᵀ(y − Xβ)
+  //
+  //   β̂ = argmin (y − Xβ)ᵀ(y − Xβ)
+  //         β
+  //
+  // Take the partial derivative of the cost function with respect to β and set
+  // it equal to zero, then solve for β̂ .
+  //
+  //   0 = −2Xᵀ(y − Xβ̂)
+  //   0 = Xᵀ(y − Xβ̂)
+  //   0 = Xᵀy − XᵀXβ̂
+  //   XᵀXβ̂ = Xᵀy
+  //   β̂  = (XᵀX)⁻¹Xᵀy
+
+  // β = (XᵀX)⁻¹Xᵀy
+  //
+  // XᵀX is guaranteed to be symmetric positive definite, so an LLT
+  // decomposition can be used.
+  Eigen::MatrixXd β = (X.transpose() * X).llt().solve(X.transpose() * y);
+
+  // Error sum of squares
+  double SSE = (y - X * β).squaredNorm();
+
+  // Sample size
+  int n = X.rows();
+
+  // Number of explanatory variables
+  int p = β.rows();
+
+  // Total sum of squares (total variation in y)
+  //
+  // From slide 24 of
+  // http://www.stat.columbia.edu/~fwood/Teaching/w4315/Fall2009/lecture_11:
+  //
+  //   SSTO = yᵀy - 1/n yᵀJy
+  //
+  // where J is a matrix of ones.
+  double SSTO =
+      (y.transpose() * y - 1.0 / y.rows() * y.transpose() *
+                               Eigen::MatrixXd::Ones(y.rows(), y.rows()) * y)
+          .value();
+
+  // R² or the coefficient of determination, which represents how much of the
+  // total variation (variation in y) can be explained by the regression model
+  double rSquared = 1.0 - SSE / SSTO;
+
+  // Adjusted R²
+  //
+  //                       n − 1
+  //   R̅² = 1 − (1 − R²) ---------
+  //                     n − p − 1
+  //
+  // See https://en.wikipedia.org/wiki/Coefficient_of_determination#Adjusted_R2
+  double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - p - 1.0));
+
+  // Root-mean-square error
+  double RMSE = std::sqrt(SSE / n);
+
+  return {{β.data(), β.data() + β.size()}, adjRSquared, RMSE};
+}
+
+}  // namespace sysid

tools/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};
+}

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

@@ -0,0 +1,759 @@
+// 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 <memory>
+#include <numbers>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <fmt/format.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_logger(logger) {
+  // Fill the StringMap with preset values.
+  m_presets["Default"] = presets::kDefault;
+  m_presets["WPILib"] = presets::kWPILib;
+  m_presets["CTRE Phoenix 5"] = presets::kCTREv5;
+  m_presets["CTRE Phoenix 6"] = presets::kCTREv6;
+  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& feedforwardGains = m_manager->CalculateFeedforward();
+    m_feedforwardGains = feedforwardGains;
+    m_accelRSquared = feedforwardGains.olsResult.rSquared;
+    m_accelRMSE = feedforwardGains.olsResult.rmse;
+    m_settings.preset.measurementDelay =
+        m_settings.type == FeedbackControllerLoopType::kPosition
+            // Clamp feedback measurement delay to ≥ 0
+            ? units::math::max(0_s, m_manager->GetPositionDelay())
+            : units::math::max(0_s, m_manager->GetVelocityDelay());
+    PrepareGraphs();
+  } catch (const sysid::InvalidDataError& e) {
+    m_state = AnalyzerState::kGeneralDataError;
+    HandleError(e.what());
+  } catch (const sysid::NoQuasistaticDataError& e) {
+    m_state = AnalyzerState::kVelocityThresholdError;
+    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() {
+  WPI_INFO(m_logger, "{}", "Updating feedback gains");
+
+  const auto& Kv = m_feedforwardGains.Kv;
+  const auto& Ka = m_feedforwardGains.Ka;
+  if (Kv.isValidGain && Ka.isValidGain) {
+    const auto& fb = m_manager->CalculateFeedback(Kv, Ka);
+    m_timescale = units::second_t{Ka.gain / Kv.gain};
+    m_timescaleValid = true;
+    m_Kp = fb.Kp;
+    m_Kd = fb.Kd;
+  } else {
+    m_timescaleValid = false;
+  }
+}
+
+bool Analyzer::DisplayDouble(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::kVelocityThresholdError ||
+         m_state == AnalyzerState::kTestDurationError ||
+         m_state == AnalyzerState::kGeneralDataError ||
+         m_state == AnalyzerState::kFileError;
+}
+
+bool Analyzer::IsDataErrorState() {
+  return m_state == AnalyzerState::kVelocityThresholdError ||
+         m_state == AnalyzerState::kTestDurationError ||
+         m_state == AnalyzerState::kGeneralDataError;
+}
+
+void Analyzer::ResetData() {
+  m_plot.ResetData();
+  m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
+  m_feedforwardGains = AnalysisManager::FeedforwardGains{};
+  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::kWaitingForData;
+    return true;
+  }
+
+  ImGui::Spacing();
+  ImGui::Text(
+      "Units:              %s\n"
+      "Type:               %s",
+      std::string(unit).c_str(), type.name);
+
+  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];
+
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+      m_manager->OverrideUnits(unit);
+      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/10 native distance unit
+  m_settings.lqr.qp = 0.1;
+  // Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
+  m_settings.lqr.qv =
+      0.25 * (12.0 - m_feedforwardGains.Ks.gain) / m_feedforwardGains.Kv.gain;
+}
+
+void Analyzer::Display() {
+  DisplayGraphs();
+
+  switch (m_state) {
+    case AnalyzerState::kWaitingForData: {
+      ImGui::Text(
+          "SysId is currently in theoretical analysis mode.\n"
+          "To analyze recorded test data, select a "
+          "data file (.wpilog).");
+      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::kWaitingForData;
+        return;
+      }
+      break;
+    }
+    case AnalyzerState::kMissingTestsError: {
+      CreateErrorPopup(m_errorPopup, m_exception);
+      if (!m_errorPopup) {
+        m_state = AnalyzerState::kWaitingForData;
+      }
+      break;
+    }
+    case AnalyzerState::kGeneralDataError:
+    case AnalyzerState::kTestDurationError:
+    case AnalyzerState::kVelocityThresholdError: {
+      CreateErrorPopup(m_errorPopup, m_exception);
+      if (DisplayResetAndUnitOverride()) {
+        return;
+      }
+      float beginX = ImGui::GetCursorPosX();
+      float beginY = ImGui::GetCursorPosY();
+      DisplayFeedforwardParameters(beginX, beginY);
+      break;
+    }
+  }
+}
+
+void Analyzer::PrepareData() {
+  WPI_INFO(m_logger, "{}", "Preparing data");
+  try {
+    if (m_missingTests.size() > 0) {
+      throw sysid::MissingTestsError{m_missingTests};
+    }
+    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::kVelocityThresholdError;
+    HandleError(e.what());
+  } catch (const sysid::NoDynamicDataError& e) {
+    m_state = AnalyzerState::kTestDurationError;
+    HandleError(e.what());
+  } catch (const sysid::MissingTestsError& e) {
+    m_state = AnalyzerState::kMissingTestsError;
+    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_feedforwardGains,
+                     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;
+  PrepareRawGraphs();
+}
+
+void Analyzer::SetMissingTests(const std::vector<std::string>& missingTests) {
+  m_missingTests = missingTests;
+}
+
+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) {
+      DisplayDouble("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();
+      DisplayDouble("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.");
+
+      DisplayDouble("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();
+      DisplayDouble("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::AnalyzeData() {
+  m_manager = std::make_unique<AnalysisManager>(m_data, m_settings, m_logger);
+  PrepareData();
+  m_dataset = 0;
+  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 = 1.1;
+  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::kVelocityThresholdError) {
+    // 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.velocityThreshold;
+    if (ImGui::InputDouble("Velocity Threshold", &threshold, 0.0, 0.0, "%.3f",
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+      m_settings.velocityThreshold = 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 (DisplayDouble("Kv", &m_feedforwardGains.Kv.gain, false)) {
+    UpdateFeedbackGains();
+  }
+
+  SetPosition(beginX, beginY, 0, 1);
+  if (DisplayDouble("Ka", &m_feedforwardGains.Ka.gain, false)) {
+    UpdateFeedbackGains();
+  }
+
+  SetPosition(beginX, beginY, 0, 2);
+  // Show Timescale
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayDouble("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::DisplayFeedforwardGain(const char* text,
+                                      AnalysisManager::FeedforwardGain& ffGain,
+                                      bool readOnly = true) {
+  DisplayDouble(text, &ffGain.gain, readOnly);
+  if (!ffGain.isValidGain) {
+    // Display invalid gain message with warning and tooltip
+    CreateErrorTooltip(ffGain.errorMessage.c_str());
+  }
+
+  // Display descriptor message as tooltip, whether the gain is valid or not
+  CreateTooltip(ffGain.descriptor.c_str());
+}
+
+void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
+  SetPosition(beginX, beginY, 0, 0);
+  DisplayFeedforwardGain("Ks", m_feedforwardGains.Ks);
+
+  SetPosition(beginX, beginY, 0, 1);
+  DisplayFeedforwardGain("Kv", m_feedforwardGains.Kv);
+
+  SetPosition(beginX, beginY, 0, 2);
+  DisplayFeedforwardGain("Ka", m_feedforwardGains.Ka);
+
+  SetPosition(beginX, beginY, 0, 3);
+  // Show Timescale
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayDouble("Response Timescale (ms)",
+                reinterpret_cast<double*>(&m_timescale));
+  if (!m_timescaleValid) {
+    CreateErrorTooltip(
+        "Response timescale calculation invalid. Ensure that calculated gains "
+        "are valid.");
+  }
+  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();
+  DisplayDouble("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();
+  DisplayDouble("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) {
+    DisplayFeedforwardGain("Kg", m_feedforwardGains.Kg);
+  } else if (m_manager->GetAnalysisType() == analysis::kArm) {
+    DisplayFeedforwardGain("Kg", m_feedforwardGains.Kg);
+
+    double offset;
+    auto unit = m_manager->GetUnit();
+    if (unit == "Radians") {
+      offset = m_feedforwardGains.offset.gain;
+    } else if (unit == "Degrees") {
+      offset = m_feedforwardGains.offset.gain / std::numbers::pi * 180.0;
+    } else if (unit == "Rotations") {
+      offset = m_feedforwardGains.offset.gain / (2 * std::numbers::pi);
+    }
+    DisplayDouble(
+        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.");
+  }
+  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);
+    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.");
+
+  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::kWaitingForData) {
+      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);
+  DisplayDouble("Kp", &m_Kp);
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  DisplayDouble("Kd", &m_Kd);
+
+  // Come back to the starting y pos.
+  ImGui::SetCursorPosY(beginY);
+
+  if (m_selectedLoopType == 0) {
+    std::string unit;
+    if (m_state != AnalyzerState::kWaitingForData) {
+      unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit()));
+    }
+
+    ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+    if (DisplayDouble(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::kWaitingForData) {
+    unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit()));
+  }
+
+  ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+  if (DisplayDouble(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 (DisplayDouble("Max Control Effort (V)", &m_settings.lqr.r, false)) {
+    if (m_settings.lqr.r > 0) {
+      UpdateFeedbackGains();
+    }
+  }
+}

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

@@ -0,0 +1,534 @@
+// 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 <functional>
+#include <mutex>
+#include <utility>
+#include <vector>
+
+#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 AnalysisManager::FeedforwardGains& 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.Ks.gain;
+  const auto& Kv = ffGains.Kv.gain;
+  const auto& Ka = ffGains.Ka.gain;
+
+  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.Kg.gain;
+      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.Kg.gain;
+      const auto& offset = ffGains.offset.gain;
+      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.Kg.gain;
+      accelPortion -= Kg / Ka;
+    } else if (type == analysis::kArm) {
+      const auto& Kg = ffGains.Kg.gain;
+      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.Kg.gain;
+      accelPortion -= Kg / Ka;
+    } else if (type == analysis::kArm) {
+      const auto& Kg = ffGains.Kg.gain;
+      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};
+}

tools/sysid/src/main/native/cpp/view/DataSelector.cpp

@@ -0,0 +1,312 @@
+// 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/DataSelector.h"
+
+#include <algorithm>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <fmt/format.h>
+#include <imgui.h>
+#include <wpi/Logger.h>
+#include <wpi/StringExtras.h>
+#include <wpi/datalog/DataLogReader.h>
+#include <wpi/datalog/DataLogReaderThread.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+using namespace sysid;
+
+static constexpr const char* kAnalysisTypes[] = {"Elevator", "Arm", "Simple"};
+
+static bool EmitEntryTarget(const char* name, bool isString,
+                            const wpi::log::DataLogReaderEntry** entry) {
+  if (*entry) {
+    auto text =
+        fmt::format("{}: {} ({})", name, (*entry)->name, (*entry)->type);
+    ImGui::TextUnformatted(text.c_str());
+  } else {
+    ImGui::Text("%s: <none (DROP HERE)> (%s)", name,
+                isString ? "string" : "number");
+  }
+  bool rv = false;
+  if (ImGui::BeginDragDropTarget()) {
+    if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(
+            isString ? "DataLogEntryString" : "DataLogEntry")) {
+      assert(payload->DataSize == sizeof(const wpi::log::DataLogReaderEntry*));
+      *entry =
+          *static_cast<const wpi::log::DataLogReaderEntry**>(payload->Data);
+      rv = true;
+    }
+    ImGui::EndDragDropTarget();
+  }
+  return rv;
+}
+
+void DataSelector::Display() {
+  using namespace std::chrono_literals;
+
+  // building test data is modal (due to async access)
+  if (m_testdataFuture.valid()) {
+    if (m_testdataFuture.wait_for(0s) == std::future_status::ready) {
+      TestData data = m_testdataFuture.get();
+      for (auto&& motordata : data.motorData) {
+        m_testdataStats.emplace_back(
+            fmt::format("Test State: {}", motordata.first));
+        int i = 0;
+        for (auto&& run : motordata.second.runs) {
+          m_testdataStats.emplace_back(fmt::format(
+              "  Run {} samples: {} Volt {} Pos {} Vel", ++i,
+              run.voltage.size(), run.position.size(), run.velocity.size()));
+        }
+      }
+      if (testdata) {
+        testdata(std::move(data));
+      }
+    }
+    ImGui::Text("Loading data...");
+    return;
+  }
+
+  if (!m_testdataStats.empty()) {
+    for (auto&& line : m_testdataStats) {
+      ImGui::TextUnformatted(line.c_str());
+    }
+    if (ImGui::Button("Ok")) {
+      m_testdataStats.clear();
+    }
+    return;
+  }
+
+  if (EmitEntryTarget("Test State", true, &m_testStateEntry)) {
+    m_testsFuture =
+        std::async(std::launch::async, [testStateEntry = m_testStateEntry] {
+          return LoadTests(*testStateEntry);
+        });
+  }
+
+  if (!m_testStateEntry) {
+    return;
+  }
+
+  if (m_testsFuture.valid() &&
+      m_testsFuture.wait_for(0s) == std::future_status::ready) {
+    m_tests = m_testsFuture.get();
+    for (auto it = m_tests.begin(); it != m_tests.end();) {
+      if (it->first != "quasistatic" && it->first != "dynamic") {
+        WPI_WARNING(m_logger, "Unrecognized test {}, removing", it->first);
+        it = m_tests.erase(it);
+        continue;
+      }
+      for (auto it2 = it->second.begin(); it2 != it->second.end();) {
+        auto direction = wpi::rsplit(it2->first, '-').second;
+        if (direction != "forward" && direction != "reverse") {
+          WPI_WARNING(m_logger, "Unrecognized direction {}, removing",
+                      direction);
+          it2 = it->second.erase(it2);
+          continue;
+        }
+        WPI_INFO(m_logger, "Loaded test state {}", it2->first);
+        m_executedTests.insert(it2->first);
+        ++it2;
+      }
+      if (it->second.empty()) {
+        WPI_WARNING(m_logger, "No data for test {}, removing", it->first);
+        it = m_tests.erase(it);
+        continue;
+      }
+      ++it;
+    }
+    WPI_INFO(m_logger, "Loaded {} tests", m_tests.size());
+  }
+
+  if (m_tests.empty()) {
+    if (m_testsFuture.valid()) {
+      ImGui::TextUnformatted("Reading tests...");
+    } else {
+      ImGui::TextUnformatted("No tests found");
+    }
+    return;
+  }
+
+  if (m_executedTests.size() < 4 && !m_testCountValidated) {
+    for (auto test : kValidTests) {
+      if (!m_executedTests.contains(test)) {
+        m_missingTests.push_back(test);
+        m_testCountValidated = true;
+      }
+    }
+  }
+
+#if 0
+  // Test filtering
+  if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) {
+    for (auto&& test : m_tests) {
+      if (ImGui::Selectable(test.first.c_str(), test.first == m_selectedTest)) {
+        m_selectedTest = test.first;
+      }
+    }
+    ImGui::EndCombo();
+  }
+#endif
+
+  ImGui::Combo("Analysis Type", &m_selectedAnalysis, kAnalysisTypes,
+               IM_ARRAYSIZE(kAnalysisTypes));
+
+  // DND targets
+  EmitEntryTarget("Velocity", false, &m_velocityEntry);
+  EmitEntryTarget("Position", false, &m_positionEntry);
+  EmitEntryTarget("Voltage", false, &m_voltageEntry);
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+  ImGui::Combo("Units", &m_selectedUnit, kUnits, IM_ARRAYSIZE(kUnits));
+
+  ImGui::InputDouble("Velocity scaling", &m_velocityScale);
+  ImGui::InputDouble("Position scaling", &m_positionScale);
+
+  if (/*!m_selectedTest.empty() &&*/ m_velocityEntry && m_positionEntry &&
+      m_voltageEntry) {
+    if (ImGui::Button("Load")) {
+      m_testdataFuture =
+          std::async(std::launch::async, [this] { return BuildTestData(); });
+    }
+  }
+}
+
+void DataSelector::Reset() {
+  m_testsFuture = {};
+  m_tests.clear();
+  m_selectedTest.clear();
+  m_testStateEntry = nullptr;
+  m_velocityEntry = nullptr;
+  m_positionEntry = nullptr;
+  m_voltageEntry = nullptr;
+  m_testdataFuture = {};
+}
+
+DataSelector::Tests DataSelector::LoadTests(
+    const wpi::log::DataLogReaderEntry& testStateEntry) {
+  Tests tests;
+  for (auto&& range : testStateEntry.ranges) {
+    std::string_view prevState;
+    Runs* curRuns = nullptr;
+    wpi::log::DataLogReader::iterator lastStart = range.begin();
+    int64_t ts = lastStart->GetTimestamp();
+    for (auto it = range.begin(), end = range.end(); it != end; ++it) {
+      ts = it->GetTimestamp();
+      std::string_view testState;
+      if (it->GetEntry() != testStateEntry.entry ||
+          !it->GetString(&testState)) {
+        continue;
+      }
+
+      // track runs as iterator ranges of the same test
+      if (testState != prevState) {
+        if (curRuns) {
+          curRuns->emplace_back(lastStart->GetTimestamp(), ts);
+        }
+        lastStart = it;
+      }
+      prevState = testState;
+
+      if (testState == "none") {
+        curRuns = nullptr;
+        continue;
+      }
+
+      auto [testName, direction] = wpi::rsplit(testState, '-');
+      auto testIt = tests.find(testName);
+      if (testIt == tests.end()) {
+        testIt = tests.emplace(std::string{testName}, State{}).first;
+      }
+      auto stateIt = testIt->second.find(testState);
+      if (stateIt == testIt->second.end()) {
+        stateIt = testIt->second.emplace(std::string{testState}, Runs{}).first;
+      }
+      curRuns = &stateIt->second;
+    }
+
+    if (curRuns) {
+      curRuns->emplace_back(lastStart->GetTimestamp(), ts);
+    }
+  }
+  return tests;
+}
+
+template <typename T>
+static void AddSamples(std::vector<MotorData::Run::Sample<T>>& samples,
+                       const std::vector<std::pair<int64_t, double>>& data,
+                       int64_t tsbegin, int64_t tsend) {
+  // data is sorted, so do a binary search for tsbegin and tsend
+  auto begin = std::lower_bound(
+      data.begin(), data.end(), tsbegin,
+      [](const auto& datapoint, double val) { return datapoint.first < val; });
+  auto end = std::lower_bound(
+      begin, data.end(), tsend,
+      [](const auto& datapoint, double val) { return datapoint.first < val; });
+
+  for (auto it = begin; it != end; ++it) {
+    samples.emplace_back(units::second_t{it->first * 1.0e-6}, T{it->second});
+  }
+}
+
+static std::vector<std::pair<int64_t, double>> GetData(
+    const wpi::log::DataLogReaderEntry& entry, double scale) {
+  std::vector<std::pair<int64_t, double>> rv;
+  bool isDouble = entry.type == "double";
+  for (auto&& range : entry.ranges) {
+    for (auto&& record : range) {
+      if (record.GetEntry() != entry.entry) {
+        continue;
+      }
+      if (isDouble) {
+        double val;
+        if (record.GetDouble(&val)) {
+          rv.emplace_back(record.GetTimestamp(), val * scale);
+        }
+      } else {
+        float val;
+        if (record.GetFloat(&val)) {
+          rv.emplace_back(record.GetTimestamp(),
+                          static_cast<double>(val * scale));
+        }
+      }
+    }
+  }
+
+  std::sort(rv.begin(), rv.end(),
+            [](const auto& a, const auto& b) { return a.first < b.first; });
+  return rv;
+}
+
+TestData DataSelector::BuildTestData() {
+  TestData data;
+  data.distanceUnit = kUnits[m_selectedUnit];
+  data.mechanismType = analysis::FromName(kAnalysisTypes[m_selectedAnalysis]);
+
+  // read and sort the entire dataset first; this is memory hungry but
+  // dramatically speeds up splitting it into runs.
+  auto voltageData = GetData(*m_voltageEntry, 1.0);
+  auto positionData = GetData(*m_positionEntry, m_positionScale);
+  auto velocityData = GetData(*m_velocityEntry, m_velocityScale);
+
+  for (auto&& test : m_tests) {
+    for (auto&& state : test.second) {
+      auto& motorData = data.motorData[state.first];
+      for (auto [tsbegin, tsend] : state.second) {
+        auto& run = motorData.runs.emplace_back();
+        AddSamples(run.voltage, voltageData, tsbegin, tsend);
+        AddSamples(run.position, positionData, tsbegin, tsend);
+        AddSamples(run.velocity, velocityData, tsbegin, tsend);
+      }
+    }
+  }
+
+  return data;
+}

tools/sysid/src/main/native/cpp/view/LogLoader.cpp

@@ -0,0 +1,213 @@
+// 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/LogLoader.h"
+
+#include <algorithm>
+#include <memory>
+#include <span>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <imgui.h>
+#include <imgui_stdlib.h>
+#include <portable-file-dialogs.h>
+#include <wpi/SmallVector.h>
+#include <wpi/SpanExtras.h>
+#include <wpi/StringExtras.h>
+#include <wpi/datalog/DataLogReaderThread.h>
+#include <wpi/fs.h>
+
+using namespace sysid;
+
+LogLoader::LogLoader(glass::Storage& storage, wpi::Logger& logger) {}
+
+LogLoader::~LogLoader() = default;
+
+void LogLoader::Display() {
+  if (ImGui::Button("Open data log file...")) {
+    m_opener = std::make_unique<pfd::open_file>(
+        "Select Data Log", "",
+        std::vector<std::string>{"DataLog Files", "*.wpilog"});
+  }
+
+  // Handle opening the file
+  if (m_opener && m_opener->ready(0)) {
+    if (!m_opener->result().empty()) {
+      m_filename = m_opener->result()[0];
+
+      auto fileBuffer = wpi::MemoryBuffer::GetFile(m_filename);
+      if (!fileBuffer) {
+        ImGui::OpenPopup("Error");
+        m_error = fmt::format("Could not open file: {}",
+                              fileBuffer.error().message());
+        return;
+      }
+
+      wpi::log::DataLogReader reader{std::move(*fileBuffer)};
+      if (!reader.IsValid()) {
+        ImGui::OpenPopup("Error");
+        m_error = "Not a valid datalog file";
+        return;
+      }
+      unload();
+      m_reader =
+          std::make_unique<wpi::log::DataLogReaderThread>(std::move(reader));
+      m_entryTree.clear();
+    }
+    m_opener.reset();
+  }
+
+  // Handle errors
+  ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+  if (ImGui::BeginPopupModal("Error")) {
+    ImGui::PushTextWrapPos(0.0f);
+    ImGui::TextUnformatted(m_error.c_str());
+    ImGui::PopTextWrapPos();
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  if (!m_reader) {
+    return;
+  }
+
+  // Summary info
+  ImGui::TextUnformatted(fs::path{m_filename}.stem().string().c_str());
+  ImGui::Text("%u records, %u entries%s", m_reader->GetNumRecords(),
+              m_reader->GetNumEntries(),
+              m_reader->IsDone() ? "" : " (working)");
+
+  if (!m_reader->IsDone()) {
+    return;
+  }
+
+  bool refilter = ImGui::InputText("Filter", &m_filter);
+
+  // Display tree of entries
+  if (m_entryTree.empty() || refilter) {
+    RebuildEntryTree();
+  }
+
+  ImGui::BeginTable(
+      "Entries", 2,
+      ImGuiTableFlags_Borders | ImGuiTableFlags_SizingStretchProp);
+  ImGui::TableSetupColumn("Name");
+  ImGui::TableSetupColumn("Type");
+  // ImGui::TableSetupColumn("Metadata");
+  ImGui::TableHeadersRow();
+  DisplayEntryTree(m_entryTree);
+  ImGui::EndTable();
+}
+
+void LogLoader::RebuildEntryTree() {
+  m_entryTree.clear();
+  wpi::SmallVector<std::string_view, 16> parts;
+  m_reader->ForEachEntryName([&](const wpi::log::DataLogReaderEntry& entry) {
+    // only show double/float/string entries (TODO: support struct/protobuf)
+    if (entry.type != "double" && entry.type != "float" &&
+        entry.type != "string") {
+      return;
+    }
+
+    // filter on name
+    if (!m_filter.empty() && !wpi::contains_lower(entry.name, m_filter)) {
+      return;
+    }
+
+    parts.clear();
+    // split on first : if one is present
+    auto [prefix, mainpart] = wpi::split(entry.name, ':');
+    if (mainpart.empty() || wpi::contains(prefix, '/')) {
+      mainpart = entry.name;
+    } else {
+      parts.emplace_back(prefix);
+    }
+    wpi::split(mainpart, '/', -1, false,
+               [&](auto part) { parts.emplace_back(part); });
+
+    // ignore a raw "/" key
+    if (parts.empty()) {
+      return;
+    }
+
+    // get to leaf
+    auto nodes = &m_entryTree;
+    for (auto part : wpi::drop_back(std::span{parts.begin(), parts.end()})) {
+      auto it =
+          std::find_if(nodes->begin(), nodes->end(),
+                       [&](const auto& node) { return node.name == part; });
+      if (it == nodes->end()) {
+        nodes->emplace_back(part);
+        // path is from the beginning of the string to the end of the current
+        // part; this works because part is a reference to the internals of
+        // entry.name
+        nodes->back().path.assign(
+            entry.name.data(), part.data() + part.size() - entry.name.data());
+        it = nodes->end() - 1;
+      }
+      nodes = &it->children;
+    }
+
+    auto it = std::find_if(nodes->begin(), nodes->end(), [&](const auto& node) {
+      return node.name == parts.back();
+    });
+    if (it == nodes->end()) {
+      nodes->emplace_back(parts.back());
+      // no need to set path, as it's identical to entry.name
+      it = nodes->end() - 1;
+    }
+    it->entry = &entry;
+  });
+}
+
+static void EmitEntry(const std::string& name,
+                      const wpi::log::DataLogReaderEntry& entry) {
+  ImGui::TableNextColumn();
+  ImGui::Selectable(name.c_str());
+  if (ImGui::BeginDragDropSource()) {
+    auto entryPtr = &entry;
+    ImGui::SetDragDropPayload(
+        entry.type == "string" ? "DataLogEntryString" : "DataLogEntry",
+        &entryPtr,
+        sizeof(entryPtr));  // NOLINT
+    ImGui::TextUnformatted(entry.name.data(),
+                           entry.name.data() + entry.name.size());
+    ImGui::EndDragDropSource();
+  }
+  ImGui::TableNextColumn();
+  ImGui::TextUnformatted(entry.type.data(),
+                         entry.type.data() + entry.type.size());
+#if 0
+  ImGui::TableNextColumn();
+  ImGui::TextUnformatted(entry.metadata.data(),
+                         entry.metadata.data() + entry.metadata.size());
+#endif
+}
+
+void LogLoader::DisplayEntryTree(const std::vector<EntryTreeNode>& tree) {
+  for (auto&& node : tree) {
+    if (node.entry) {
+      EmitEntry(node.name, *node.entry);
+    }
+
+    if (!node.children.empty()) {
+      ImGui::TableNextColumn();
+      bool open = ImGui::TreeNodeEx(node.name.c_str(),
+                                    ImGuiTreeNodeFlags_SpanFullWidth);
+      ImGui::TableNextColumn();
+#if 0
+      ImGui::TableNextColumn();
+#endif
+      if (open) {
+        DisplayEntryTree(node.children);
+        ImGui::TreePop();
+      }
+    }
+  }
+}

tools/sysid/src/main/native/include/sysid/Util.h

@@ -0,0 +1,113 @@
+// 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 <stdexcept>
+#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 {
+inline 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);
+
+/**
+ * Displays an error tooltip beside the widget that this method is called after
+ * with the provided text.
+ *
+ * @param text The text to show in the error tooltip.
+ */
+void CreateErrorTooltip(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.
+ */
+constexpr std::string_view 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");
+  }
+}
+
+/**
+ * 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

tools/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h

@@ -0,0 +1,351 @@
+// 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 <string>
+#include <string_view>
+#include <vector>
+
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/StringMap.h>
+#include <wpi/json.h>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/analysis/OLS.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:
+  // This contains data for each test (e.g. quasistatic-forward,
+  // quasistatic-backward, etc) indexed by test name
+  TestData m_data;
+  /**
+   * Represents settings for an instance of the analysis manager. This contains
+   * information about the feedback controller preset, loop type, velocity
+   * threshold, acceleration window size, LQR parameters, and the selected
+   * dataset.
+   */
+  struct Settings {
+    /**
+     * 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 velocity threshold (units/s) for trimming quasistatic test data.
+     */
+    double velocityThreshold = 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;
+  };
+
+  struct FeedforwardGain {
+    /**
+     * The feedforward gain.
+     */
+    double gain = 1;
+
+    /**
+     * Descriptor attached to the feedforward gain.
+     */
+    std::string descriptor = "Feedforward gain.";
+
+    /**
+     * Whether the feedforward gain is valid.
+     */
+    bool isValidGain = true;
+
+    /**
+     * Error message attached to the feedforward gain.
+     */
+    std::string errorMessage = "No error.";
+  };
+
+  /**
+   * Stores feedforward gains.
+   */
+  struct FeedforwardGains {
+    /**
+     * Stores the raw OLSResult from analysis.
+     */
+    OLSResult olsResult;
+
+    /**
+     * The static gain Ks.
+     */
+    FeedforwardGain Ks = {};
+
+    /**
+     * The velocity gain kV.
+     */
+    FeedforwardGain Kv = {};
+
+    /**
+     * The acceleration gain kA.
+     */
+    FeedforwardGain Ka = {};
+
+    /**
+     * The gravity gain Kg.
+     */
+    FeedforwardGain Kg = {};
+
+    /**
+     * The offset (arm).
+     */
+    FeedforwardGain offset = {};
+  };
+
+  /**
+   * Exception for File Reading Errors.
+   */
+  class FileReadingError : public std::exception {
+   public:
+    /**
+     * 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);
+    }
+
+    const char* what() const noexcept override { return msg.c_str(); }
+
+   private:
+    /**
+     * The path of the file that was opened.
+     */
+    std::string msg;
+  };
+
+  /**
+   * The keys (which contain sysid data) that are in the JSON to analyze.
+   */
+  static constexpr const char* kJsonDataKeys[] = {
+      "quasistatic-forward", "quasistatic-reverse", "dynamic-forward",
+      "dynamic-reverse"};
+
+  /**
+   * 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 data     The data from the SysId routine.
+   * @param settings The settings for this instance of the analysis manager.
+   * @param logger   The logger instance to use for log data.
+   */
+  AnalysisManager(TestData data, 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(const FeedforwardGain& Kv,
+                                  const FeedforwardGain& Ka);
+
+  /**
+   * Overrides the units in the JSON with the user-provided ones.
+   *
+   * @param unit             The unit to output gains in.
+   */
+  void OverrideUnits(std::string_view unit);
+
+  /**
+   * 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_data.mechanismType; }
+
+  /**
+   * Returns the units of analysis.
+   *
+   * @return The units of analysis.
+   */
+  std::string_view GetUnit() const { return m_data.distanceUnit; }
+
+  /**
+   * Returns a reference to the iterator of the currently selected raw dataset.
+   * 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; }
+
+  /**
+   * Returns a reference to the iterator of the currently selected filtered
+   * dataset. 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; }
+
+  /**
+   * Returns the original dataset.
+   *
+   * @return The original (untouched) dataset
+   */
+  Storage& GetOriginalData() { return m_originalDataset; }
+
+  /**
+   * 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.empty(); }
+
+ private:
+  wpi::Logger& m_logger;
+
+  Storage m_originalDataset;
+  Storage m_rawDataset;
+  Storage 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;
+
+  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;
+
+  void PrepareGeneralData();
+};
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/analysis/AnalysisType.h

@@ -0,0 +1,69 @@
+// 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 {
+inline constexpr AnalysisType kElevator{4, 4, "Elevator"};
+inline constexpr AnalysisType kArm{5, 4, "Arm"};
+inline constexpr AnalysisType kSimple{3, 4, "Simple"};
+
+constexpr AnalysisType FromName(std::string_view name) {
+  if (name == "Elevator") {
+    return sysid::analysis::kElevator;
+  }
+  if (name == "Arm") {
+    return sysid::analysis::kArm;
+  }
+  return sysid::analysis::kSimple;
+}
+}  // namespace analysis
+}  // namespace sysid

tools/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      Sample 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

tools/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      Sample 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

tools/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.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
+
+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.
+ */
+FeedbackGains CalculatePositionFeedbackGains(
+    const FeedbackControllerPreset& preset, const LQRParameters& params,
+    double Kv, double Ka);
+
+/**
+ * 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

tools/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h

@@ -0,0 +1,160 @@
+// 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 {
+inline constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s};
+
+inline constexpr FeedbackControllerPreset kWPILib{kDefault};
+
+// 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.
+ */
+inline constexpr FeedbackControllerPreset kCTREv5{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 6 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
+ * Phoenix 6 devices make use of Kalman filters default-tuned to lowest latency,
+ * which in testing is roughly 1 millisecond
+ */
+inline constexpr FeedbackControllerPreset kCTREv6{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.
+ */
+inline 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.
+ */
+inline 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.
+ */
+inline constexpr FeedbackControllerPreset kVenom{4096.0 / 12.0, 60.0, 1_ms,
+                                                 false, 5_ms};
+}  // namespace presets
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h

@@ -0,0 +1,52 @@
+// 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 "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/OLS.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+/**
+ * Exception for data that doesn't sample enough of the state-input space.
+ */
+class InsufficientSamplesError : public std::exception {
+ public:
+  /**
+   * Constructs an InsufficientSamplesError.
+   *
+   * @param message The error message
+   */
+  explicit InsufficientSamplesError(std::string_view message) {
+    m_message = message;
+  }
+
+  const char* what() const noexcept override { return m_message.c_str(); }
+
+ private:
+  /**
+   * Stores the error message
+   */
+  std::string m_message;
+};
+
+/**
+ * Calculates feedforward gains given the data and the type of analysis to
+ * perform.
+ *
+ * @param data The OLS input data.
+ * @param type The analysis type.
+ * @param throwOnRankDeficiency Whether to throw if the fit is going to be poor.
+ *   This option is provided for unit testing purposes.
+ */
+OLSResult CalculateFeedforwardGains(const Storage& data,
+                                    const AnalysisType& type,
+                                    bool throwOnRankDeficiency = true);
+
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/analysis/FilteringUtils.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 <exception>
+#include <functional>
+#include <string>
+#include <string_view>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+#include <fmt/format.h>
+#include <fmt/ranges.h>
+#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.
+ */
+class InvalidDataError : public std::exception {
+ public:
+  /**
+   * 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 velocity threshold, test duration, and/or window size to "
+        "try to fix this issue.",
+        message);
+  }
+
+  const char* what() const noexcept override { return m_message.c_str(); }
+
+ private:
+  /**
+   * Stores the error message
+   */
+  std::string m_message;
+};
+
+/**
+ * Exception for Quasistatic Data being completely removed.
+ */
+class NoQuasistaticDataError : public std::exception {
+ public:
+  const char* what() const noexcept override {
+    return "Quasistatic test trimming removed all data. Please adjust your "
+           "velocity threshold and double check "
+           "your units and test data to make sure that the robot is reporting "
+           "reasonable values.";
+  }
+};
+
+/**
+ * Exception for not all tests being present.
+ */
+class MissingTestsError : public std::exception {
+ public:
+  explicit MissingTestsError(std::vector<std::string> MissingTests)
+      : missingTests(std::move(MissingTests)) {
+    errorString = fmt::format(
+        "The following tests were not detected: {}. Make sure to perform all "
+        "four tests as described in the SysId documentation.",
+        fmt::join(missingTests, ", "));
+  }
+  const char* what() const noexcept override { return errorString.c_str(); }
+
+ private:
+  std::vector<std::string> missingTests;
+  std::string errorString;
+};
+
+/**
+ * Exception for Dynamic Data being completely removed.
+ */
+class NoDynamicDataError : public std::exception {
+ public:
+  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);
+
+double GetMaxSpeed(const std::vector<PreparedData>& data,
+                   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

tools/sysid/src/main/native/include/sysid/analysis/OLS.h

@@ -0,0 +1,32 @@
+// 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 <Eigen/Core>
+
+namespace sysid {
+
+struct OLSResult {
+  /// Regression coefficients.
+  std::vector<double> coeffs;
+
+  /// R² (coefficient of determination)
+  double rSquared = 0.0;
+
+  /// Root-mean-square error
+  double rmse = 0.0;
+};
+
+/**
+ * Performs ordinary least squares multiple regression on the provided data.
+ *
+ * @param X The independent data in y = Xβ.
+ * @param y The dependent data in y = Xβ.
+ */
+OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);
+
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h

@@ -0,0 +1,75 @@
+// 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      Sample 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

tools/sysid/src/main/native/include/sysid/analysis/Storage.h

@@ -0,0 +1,129 @@
+// 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 <vector>
+
+#include <units/time.h>
+#include <units/voltage.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/AnalysisType.h"
+
+namespace sysid {
+
+struct MotorData {
+  // name of the *motor*, not the test
+  std::string name;
+
+  // Data for a single contiguous motor test
+  // Timestamps are not necessarily aligned!
+  struct Run {
+    template <typename T>
+      requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+    struct Sample {
+      Sample(units::second_t time, T measurement)
+          : time{time}, measurement{measurement} {}
+      units::second_t time;
+      T measurement;
+    };
+    std::vector<Sample<units::volt_t>> voltage;
+    std::vector<Sample<double>> position;
+    std::vector<Sample<double>> velocity;
+  };
+
+  std::vector<Run> runs;
+};
+
+struct TestData {
+  std::string distanceUnit;
+  AnalysisType mechanismType;
+  wpi::StringMap<MotorData> motorData;
+};
+
+/**
+ * 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

tools/sysid/src/main/native/include/sysid/analysis/TrackwidthAnalysis.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 <gcem.hpp>
+#include <units/angle.h>
+
+namespace sysid {
+
+/**
+ * Calculates the trackwidth 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.
+ */
+constexpr double CalculateTrackwidth(double l, double r,
+                                     units::radian_t accum) {
+  // The below comes from solving ω = (vr − vl) / 2r for 2r.
+  return (gcem::abs(r) + gcem::abs(l)) / gcem::abs(accum.value());
+}
+
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/view/Analyzer.h

@@ -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.
+
+#pragma once
+
+#include <memory>
+#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/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:
+  TestData m_data;
+  /**
+   * The different display and processing states for the GUI
+   */
+  enum class AnalyzerState {
+    kWaitingForData,
+    kNominalDisplay,
+    kVelocityThresholdError,
+    kTestDurationError,
+    kGeneralDataError,
+    kMissingTestsError,
+    kFileError
+  };
+  /**
+   * The different motor controller timing presets that can be used.
+   */
+  static constexpr const char* kPresetNames[] = {"Default",
+                                                 "WPILib",
+                                                 "CTRE Phoenix 5",
+                                                 "CTRE Phoenix 6",
+                                                 "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(); };
+
+  /**
+   * Analyzes the selected data.
+   */
+  void AnalyzeData();
+
+  /**
+   * Used by DataSelector to import any missing tests.
+   */
+  void SetMissingTests(const std::vector<std::string>& missingTests);
+
+ private:
+  /**
+   * 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();
+
+  /**
+   * 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 double on ImGui.
+   */
+  bool DisplayDouble(const char* text, double* data, bool readOnly);
+
+  /**
+   * Displays a Feedforward gain, including the gain itself along with its
+   * validity and message.
+   */
+  void DisplayFeedforwardGain(const char* text,
+                              AnalysisManager::FeedforwardGain& ffGain,
+                              bool readOnly);
+
+  /**
+   * Handles errors when they pop up.
+   */
+  void HandleError(std::string_view msg);
+
+  // State of the Display GUI
+  AnalyzerState m_state = AnalyzerState::kWaitingForData;
+
+  // Stores the exception message.
+  std::string m_exception;
+  std::vector<std::string> m_missingTests;
+
+  // 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.
+  AnalysisManager::FeedforwardGains m_feedforwardGains;
+  double m_accelRSquared;
+  double m_accelRMSE;
+  double m_Kp;
+  double m_Kd;
+  units::millisecond_t m_timescale;
+  bool m_timescaleValid = false;
+
+  // Units
+  int m_selectedOverrideUnit = 0;
+
+  // Data analysis
+  std::unique_ptr<AnalysisManager> m_manager;
+  int m_dataset = 0;
+  float m_stepTestDuration = 0;
+
+  // 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

tools/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h

@@ -0,0 +1,205 @@
+// 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/AnalysisManager.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           Feedforward gains (Ks, Kv, Ka, optionally
+   *                     Kg and offset).
+   * @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 AnalysisManager::FeedforwardGains& 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

tools/sysid/src/main/native/include/sysid/view/DataSelector.h

@@ -0,0 +1,90 @@
+// 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 <future>
+#include <map>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <glass/View.h>
+#include <wpi/StringMap.h>
+#include <wpi/datalog/DataLogReaderThread.h>
+
+#include "sysid/analysis/Storage.h"
+
+namespace glass {
+class Storage;
+}  // namespace glass
+
+namespace wpi {
+namespace log {
+class DataLogReaderEntry;
+}  // namespace log
+class Logger;
+}  // namespace wpi
+
+namespace sysid {
+/**
+ * Helps with loading datalog files.
+ */
+class DataSelector : public glass::View {
+ public:
+  /**
+   * Creates a data selector widget
+   *
+   * @param logger The program logger
+   */
+  explicit DataSelector(glass::Storage& storage, wpi::Logger& logger)
+      : m_logger{logger} {}
+
+  /**
+   * Displays the log loader window.
+   */
+  void Display() override;
+
+  /**
+   * Resets view. Must be called whenever the DataLogReader goes away, as this
+   * class keeps references to DataLogReaderEntry objects.
+   */
+  void Reset();
+
+  /**
+   * Called when new test data is loaded.
+   */
+  std::function<void(TestData)> testdata;
+  std::vector<std::string> m_missingTests;
+
+ private:
+  wpi::Logger& m_logger;
+  using Runs = std::vector<std::pair<int64_t, int64_t>>;
+  using State = std::map<std::string, Runs, std::less<>>;   // full name
+  using Tests = std::map<std::string, State, std::less<>>;  // e.g. "dynamic"
+  std::future<Tests> m_testsFuture;
+  Tests m_tests;
+  std::string m_selectedTest;
+  const wpi::log::DataLogReaderEntry* m_testStateEntry = nullptr;
+  const wpi::log::DataLogReaderEntry* m_velocityEntry = nullptr;
+  const wpi::log::DataLogReaderEntry* m_positionEntry = nullptr;
+  const wpi::log::DataLogReaderEntry* m_voltageEntry = nullptr;
+  double m_velocityScale = 1.0;
+  double m_positionScale = 1.0;
+  int m_selectedUnit = 0;
+  int m_selectedAnalysis = 0;
+  std::future<TestData> m_testdataFuture;
+  std::vector<std::string> m_testdataStats;
+  std::set<std::string> kValidTests = {"quasistatic-forward",
+                                       "quasistatic-reverse", "dynamic-forward",
+                                       "dynamic-reverse"};
+  std::set<std::string> m_executedTests;
+  bool m_testCountValidated = false;
+
+  static Tests LoadTests(const wpi::log::DataLogReaderEntry& testStateEntry);
+  TestData BuildTestData();
+};
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/view/LogLoader.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
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <glass/View.h>
+#include <wpi/Signal.h>
+
+namespace glass {
+class Storage;
+}  // namespace glass
+
+namespace pfd {
+class open_file;
+}  // namespace pfd
+
+namespace wpi {
+namespace log {
+class DataLogReaderEntry;
+class DataLogReaderThread;
+}  // namespace log
+class Logger;
+}  // namespace wpi
+
+namespace sysid {
+/**
+ * Helps with loading datalog files.
+ */
+class LogLoader : public glass::View {
+ public:
+  /**
+   * Creates a log loader widget
+   *
+   * @param logger The program logger
+   */
+  explicit LogLoader(glass::Storage& storage, wpi::Logger& logger);
+
+  ~LogLoader() override;
+
+  /**
+   * Displays the log loader window.
+   */
+  void Display() override;
+
+  /**
+   * Signal called when the current file is unloaded (invalidates any
+   * LogEntry*).
+   */
+  wpi::sig::Signal<> unload;
+
+ private:
+  // wpi::Logger& m_logger;
+
+  std::string m_filename;
+  std::unique_ptr<pfd::open_file> m_opener;
+  std::unique_ptr<wpi::log::DataLogReaderThread> m_reader;
+
+  std::string m_error;
+
+  std::string m_filter;
+
+  struct EntryTreeNode {
+    explicit EntryTreeNode(std::string_view name) : name{name} {}
+    std::string name;  // name of just this node
+    std::string path;  // full path if entry is nullptr
+    const wpi::log::DataLogReaderEntry* entry = nullptr;
+    std::vector<EntryTreeNode> children;  // children, sorted by name
+  };
+  std::vector<EntryTreeNode> m_entryTree;
+
+  void RebuildEntryTree();
+  void DisplayEntryTree(const std::vector<EntryTreeNode>& nodes);
+};
+}  // namespace sysid

tools/sysid/src/main/native/include/sysid/view/UILayout.h

@@ -0,0 +1,98 @@
+// 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 kLogLoaderWindowPos = kLeftColPos;
+inline constexpr Vector2d kLogLoaderWindowSize{kLeftColSize.x, 450};
+inline constexpr Vector2d kDataSelectorWindowPos =
+    kLogLoaderWindowPos + Vector2d{0, kLogLoaderWindowSize.y + kWindowGap};
+inline constexpr Vector2d kDataSelectorWindowSize{
+    kLeftColSize.x, kAppWindowSize.y - kWindowGap - kDataSelectorWindowPos.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

tools/sysid/src/main/native/win/sysid.rc

@@ -0,0 +1 @@
+IDI_ICON1 ICON "sysid.ico"