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

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.
-  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};
-
-  if (type == analysis::kElevator) {
-    // Add Kg to gains list
-    double delta = ols.coeffs[3];  // -Kg/Ka
-    gains.emplace_back(-delta / beta);
+  // Check quality of collected data
+  if (throwOnBadData) {
+    CheckOLSDataQuality(X, type);
   }
 
-  if (type == analysis::kArm) {
-    double delta = ols.coeffs[3];    // -Kg/Ka cos(offset)
-    double epsilon = ols.coeffs[4];  // Kg/Ka sin(offset)
+  std::vector<double> gains;
+  gains.reserve(X.rows());
 
-    // Add Kg to gains list
-    gains.emplace_back(std::hypot(delta, epsilon) / beta);
+  auto ols = OLS(X, y);
 
-    // Add offset to gains list
-    gains.emplace_back(std::atan2(epsilon, -delta));
+  // 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)                   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)