[sysid] Check data quality before OLS (#6110)

sysid/CMakeLists.txt

@@ -19,6 +19,9 @@ elseif(APPLE)
 endif()
 
 add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX})
+if(MSVC)
+    target_compile_options(sysid PRIVATE /utf-8)
+endif()
 wpilib_link_macos_gui(sysid)
 wpilib_target_warnings(sysid)
 target_include_directories(sysid PRIVATE src/main/native/include)
@@ -35,6 +38,9 @@ if(WITH_TESTS)
     wpilib_link_macos_gui(sysid_test)
     target_sources(sysid_test PRIVATE ${sysid_src})
     target_compile_definitions(sysid_test PRIVATE RUNNING_SYSID_TESTS)
+    if(MSVC)
+        target_compile_options(sysid_test PRIVATE /utf-8)
+    endif()
     target_include_directories(sysid_test PRIVATE src/main/native/cpp src/main/native/include)
     target_link_libraries(sysid_test wpimath libglassnt libglass gtest)
 endif()

sysid/docs/arm-ols-with-angle-offset.md → sysid/docs/ols-derivations.md

@@ -1,28 +1,84 @@
-# Arm OLS with angle offset
+# OLS derivations
+
+## Simple/drivetrain
+
+Here's the ODE for a drivetrain.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+```
+
+### OLS setup
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, and `γ = -Ks/Ka`.
+```
+dx/dt = αx + βu + γ sgn(x)
+```
+
+### Feedforward gains
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+## Elevator
+
+Here's the ODE for an elevator.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+```
+
+### OLS setup
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, and `δ = -Kg/Ka`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ
+```
+
+### Feedforward gains
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, `Ka`, and `Kg`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+Kg = −δ/β
+```
+
+## Arm
+
+Here's the ODE for an arm:
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle)
+```
 
 If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit
 for `Kg` will be wrong. An angle offset should be added to the model like so.
 ```
 dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset)
 ```
+
 Use a trig identity to split the cosine into two terms.
 ```
 dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset))
 dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset)
 ```
+
 Reorder multiplicands so the offset trig is absorbed by the OLS terms.
 ```
 dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle)
 ```
 
-## OLS
+### OLS setup
 
 Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
 ```
 dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
 ```
 
-### Ks, Kv, Ka
+### Feedforward gains: Ks, Kv, Ka
 
 Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
 ```
@@ -31,7 +87,7 @@ Kv = -α/β
 Ka = 1/β
 ```
 
-### Kg
+### Feedforward gains: Kg
 
 Take the sum of squares of the OLS terms containing the angle offset. The angle
 offset trig functions will form a trig identity that cancels out. Then, just
@@ -44,14 +100,12 @@ solve for `Kg`.
 δ²+ε² = (Kg/Ka)² (1)
 δ²+ε² = (Kg/Ka)²
 √(δ²+ε²) = Kg/Ka
-√(δ²+ε²) = Kg β
-Kg = √(δ²+ε²)/β
+hypot(δ, ε) = Kg/Ka
+hypot(δ, ε) = Kg β
+Kg = hypot(δ, ε)/β
 ```
 
-As a sanity check, when the offset is zero, ε is zero and the equation for
-`Kg` simplifies to -δ/β, the equation previously used by SysId.
-
-### Angle offset
+### Feedforward gains: offset
 
 Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()`
 to preserve the angle quadrant. Maintaining the proper negative signs in the

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

@@ -4,8 +4,13 @@
 
 #include "sysid/analysis/FeedforwardAnalysis.h"
 
+#include <array>
+#include <bitset>
 #include <cmath>
 
+#include <Eigen/Eigenvalues>
+#include <fmt/format.h>
+#include <fmt/ranges.h>
 #include <units/math.h>
 #include <units/time.h>
 
@@ -16,7 +21,22 @@
 namespace sysid {
 
 /**
- * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ).
+ * 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.
@@ -27,35 +47,123 @@ 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];
 
-    // Add the velocity term (for α)
+    // Set the velocity term (for α)
     X(sample, 0) = pt.velocity;
 
-    // Add the voltage term (for β)
+    // Set the voltage term (for β)
     X(sample, 1) = pt.voltage;
 
-    // Add the intercept term (for γ)
+    // Set the intercept term (for γ)
     X(sample, 2) = std::copysign(1, pt.velocity);
 
-    // Add test-specific variables
+    // Set test-specific variables
     if (type == analysis::kElevator) {
-      // Add the gravity term (for Kg)
+      // Set the gravity term (for δ)
       X(sample, 3) = 1.0;
     } else if (type == analysis::kArm) {
-      // Add the cosine and sine terms (for Kg)
+      // Set the cosine and sine terms (for δ and ε)
       X(sample, 3) = pt.cos;
       X(sample, 4) = pt.sin;
     }
 
-    // Add the dependent variable (acceleration)
+    // 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 - 1 nonzero eigenvalues for good fit
+  for (int row = 0; row < eigvals.rows(); ++row) {
+    if (std::abs(eigvals(row)) <= threshold) {
+      // Find row of eigenvector with largest magnitude. This determines which
+      // gain is rank-deficient
+      int maxIndex;
+      eigvecs.col(row).cwiseAbs().maxCoeff(&maxIndex);
+
+      // Fit for α is rank-deficient
+      if (maxIndex == 0) {
+        badGains.set(1);
+      }
+      // Fit for β is rank-deficient
+      if (maxIndex == 1) {
+        badGains.set();
+        break;
+      }
+      // Fit for γ is rank-deficient
+      if (maxIndex == 2) {
+        badGains.set(0);
+      }
+      // Fit for δ is rank-deficient
+      if (maxIndex == 3) {
+        if (type == analysis::kElevator) {
+          badGains.set(3);
+        } else if (type == analysis::kArm) {
+          badGains.set(3);
+          badGains.set(4);
+        }
+      }
+      // Fit for ε is rank-deficient
+      if (maxIndex == 4) {
+        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) {
+                                    const AnalysisType& type,
+                                    bool throwOnBadData) {
   // Iterate through the data and add it to our raw vector.
   const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
 
@@ -86,32 +194,64 @@ OLSResult CalculateFeedforwardGains(const Storage& data,
                   X.block(rowOffset, 0, fastBackward.size(), X.cols()),
                   y.segment(rowOffset, fastBackward.size()));
 
-  // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel)
-  // OLS performs best with the noisiest variable as the dependent var,
-  // so we regress accel in terms of the other variables.
+  // Check quality of collected data
+  if (throwOnBadData) {
+    CheckOLSDataQuality(X, type);
+  }
+
+  std::vector<double> gains;
+  gains.reserve(X.rows());
+
   auto ols = OLS(X, y);
-  double alpha = ols.coeffs[0];  // -Kv/Ka
-  double beta = ols.coeffs[1];   // 1/Ka
-  double gamma = ols.coeffs[2];  // -Ks/Ka
 
-  // Initialize gains list with Ks, Kv, and Ka
-  std::vector<double> gains{-gamma / beta, -alpha / beta, 1 / beta};
+  // 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)
 
-  if (type == analysis::kElevator) {
-    // Add Kg to gains list
-    double delta = ols.coeffs[3];  // -Kg/Ka
-    gains.emplace_back(-delta / beta);
-  }
+    // α = -Kv/Ka
+    // β = 1/Ka
+    // γ = -Ks/Ka
+    double α = ols.coeffs[0];
+    double β = ols.coeffs[1];
+    double γ = ols.coeffs[2];
 
-  if (type == analysis::kArm) {
-    double delta = ols.coeffs[3];    // -Kg/Ka cos(offset)
-    double epsilon = ols.coeffs[4];  // Kg/Ka sin(offset)
+    // Ks = -γ/β
+    // Kv = -α/β
+    // Ka = 1/β
+    gains.emplace_back(-γ / β);
+    gains.emplace_back(-α / β);
+    gains.emplace_back(1 / β);
 
-    // Add Kg to gains list
-    gains.emplace_back(std::hypot(delta, epsilon) / beta);
+    if (type == analysis::kElevator) {
+      // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+      // dx/dt = αx + βu + γ sgn(x) + δ
 
-    // Add offset to gains list
-    gains.emplace_back(std::atan2(epsilon, -delta));
+      // δ = -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)                   NOLINT
+      //           + Kg/Ka sin(offset) sin(angle)                   NOLINT
+      // dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)   NOLINT
+
+      // δ = -Kg/Ka cos(offset)
+      // ε = Kg/Ka sin(offset)
+      double δ = ols.coeffs[3];
+      double ε = ols.coeffs[4];
+
+      // Kg = hypot(δ, ε)/β      NOLINT
+      // offset = atan2(ε, -δ)   NOLINT
+      gains.emplace_back(std::hypot(δ, ε) / β);
+      gains.emplace_back(std::atan2(ε, -δ));
+    }
   }
 
   // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)