SCRIPT Move subprojects

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