Refactor OLS (#522)

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

@@ -16,69 +16,81 @@
 using namespace sysid;
 
 /**
- * Populates OLS data for x_k+1 - x_k / tau = alpha x_k + beta u_k + gamma
- * sgn(x_k).
+ * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ).
  *
  * @param d List of characterization data.
  * @param type Type of system being identified.
  * @param X Vector representation of X in y = Xβ.
  * @param y Vector representation of y in y = Xβ.
  */
 static void PopulateOLSData(const std::vector<PreparedData>& d,
-                            const AnalysisType& type, std::vector<double>& X,
-                            std::vector<double>& y) {
-  for (const auto& pt : d) {
-    // Add the velocity term (for alpha)
-    X.push_back(pt.velocity);
+                            const AnalysisType& type,
+                            Eigen::Block<Eigen::MatrixXd> X,
+                            Eigen::VectorBlock<Eigen::VectorXd> y) {
+  for (size_t sample = 0; sample < d.size(); ++sample) {
+    const auto& pt = d[sample];
 
-    // Add the voltage term (for beta)
-    X.push_back(pt.voltage);
+    // Add the velocity term (for α)
+    X(sample, 0) = pt.velocity;
 
-    // Add the intercept term (for gamma)
-    X.push_back(std::copysign(1, pt.velocity));
+    // Add the voltage term (for β)
+    X(sample, 1) = pt.voltage;
+
+    // Add the intercept term (for γ)
+    X(sample, 2) = std::copysign(1, pt.velocity);
 
     // Add test-specific variables
     if (type == analysis::kElevator) {
       // Add the gravity term (for Kg)
-      X.push_back(1.0);
+      X(sample, 3) = 1.0;
     } else if (type == analysis::kArm) {
       // Add the cosine and sine terms (for Kg)
-      X.push_back(pt.cos);
-      X.push_back(pt.sin);
+      X(sample, 3) = pt.cos;
+      X(sample, 4) = pt.sin;
     }
 
     // Add the dependent variable (acceleration)
-    y.push_back(pt.acceleration);
+    y(sample) = pt.acceleration;
   }
 }
 
 std::tuple<std::vector<double>, double, double>
 sysid::CalculateFeedforwardGains(const Storage& data,
                                  const AnalysisType& type) {
-  // Create a raw vector of doubles with our data in it.
-  std::vector<double> X;
-  std::vector<double> y;
-
   // 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();
 
-  // 1 dependent variable, n independent variables in each observation
-  // Observations are stored serially
-  X.reserve(type.independentVariables * size);
-  y.reserve(size);
+  // Create a raw vector of doubles with our data in it.
+  Eigen::MatrixXd X{size, type.independentVariables};
+  Eigen::VectorXd y{size};
+
+  int rowOffset = 0;
+  PopulateOLSData(slowForward, type,
+                  X.block(rowOffset, 0, slowForward.size(), X.cols()),
+                  y.segment(rowOffset, slowForward.size()));
+
+  rowOffset += slowForward.size();
+  PopulateOLSData(slowBackward, type,
+                  X.block(rowOffset, 0, slowBackward.size(), X.cols()),
+                  y.segment(rowOffset, slowBackward.size()));
+
+  rowOffset += slowBackward.size();
+  PopulateOLSData(fastForward, type,
+                  X.block(rowOffset, 0, fastForward.size(), X.cols()),
+                  y.segment(rowOffset, fastForward.size()));
+
+  rowOffset += fastForward.size();
+  PopulateOLSData(fastBackward, type,
+                  X.block(rowOffset, 0, fastBackward.size(), X.cols()),
+                  y.segment(rowOffset, fastBackward.size()));
 
   // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel)
   // OLS performs best with the noisiest variable as the dependent var,
   // so we regress accel in terms of the other variables.
-  PopulateOLSData(slowForward, type, X, y);
-  PopulateOLSData(slowBackward, type, X, y);
-  PopulateOLSData(fastForward, type, X, y);
-  PopulateOLSData(fastBackward, type, X, y);
-
-  auto ols = sysid::OLS(X, type.independentVariables, y);
+  auto ols = sysid::OLS(X, y);
   double alpha = std::get<0>(ols)[0];  // -Kv/Ka
   double beta = std::get<0>(ols)[1];   // 1/Ka
   double gamma = std::get<0>(ols)[2];  // -Ks/Ka

sysid-application/src/main/native/cpp/analysis/OLS.cpp

@@ -13,11 +13,8 @@
 using namespace sysid;
 
 std::tuple<std::vector<double>, double, double> sysid::OLS(
-    const std::vector<double>& XData, size_t independentVariables,
-    const std::vector<double>& yData) {
-  // Perform some quick sanity checks regarding the size of the vector.
-  assert(XData.size() % independentVariables == 0);
-  assert(yData.size() % independentVariables == 0);
+    const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
+  assert(X.rows() == y.rows());
 
   // The linear model can be written as follows:
   // y = Xβ + u, where y is the dependent observed variable, X is the matrix
@@ -27,15 +24,6 @@ std::tuple<std::vector<double>, double, double> sysid::OLS(
   // We want to minimize u² = uᵀu = (y - Xβ)ᵀ(y - Xβ).
   // β = (XᵀX)⁻¹Xᵀy
 
-  // Create X matrix and y vector.
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      X(XData.data(), XData.size() / independentVariables,
-        independentVariables);
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      y(yData.data(), yData.size(), 1);
-
   // Calculate β that minimizes uᵀu.
   Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y);
 

sysid-application/src/main/native/include/sysid/analysis/OLS.h

@@ -8,18 +8,19 @@
 #include <tuple>
 #include <vector>
 
+#include <Eigen/Core>
+
 namespace sysid {
+
 /**
  * Performs ordinary least squares multiple regression on the provided data and
  * returns a vector of coefficients along with the r-squared (coefficient of
  * determination) and RMSE (stardard deviation of the residuals) of the fit.
  *
- * @param XData The independent data in y = Xβ. The data must be formatted as
- *   x₀, x₁, x₂, ..., in the vector.
- * @param independentVariables The number of independent variables (x values).
- * @param yData The dependent data in y = Xβ.
+ * @param X The independent data in y = Xβ.
+ * @param y The dependent data in y = Xβ.
  */
-std::tuple<std::vector<double>, double, double> OLS(
-    const std::vector<double>& XData, size_t independentVariables,
-    const std::vector<double>& yData);
+std::tuple<std::vector<double>, double, double> OLS(const Eigen::MatrixXd& X,
+                                                    const Eigen::VectorXd& y);
+
 }  // namespace sysid

sysid-application/src/test/native/cpp/analysis/OLSTest.cpp

@@ -2,18 +2,16 @@
 // 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 <vector>
-
 #include <gtest/gtest.h>
 
 #include "sysid/analysis/OLS.h"
 
 TEST(OLSTest, TwoVariablesTwoPoints) {
   // (1, 3) and (2, 5). Should produce y = 2x + 1.
-  std::vector<double> X{1.0, 1.0, 1.0, 2.0};
-  std::vector<double> y{3.0, 5.0};
+  Eigen::MatrixXd X{{1.0, 1.0}, {1.0, 2.0}};
+  Eigen::VectorXd y{{3.0}, {5.0}};
 
-  auto [coefficients, cod, rmse] = sysid::OLS(X, 2, y);
+  auto [coefficients, cod, rmse] = sysid::OLS(X, y);
   EXPECT_EQ(coefficients.size(), 2u);
 
   EXPECT_NEAR(coefficients[0], 1.0, 0.05);
@@ -24,10 +22,10 @@ TEST(OLSTest, TwoVariablesTwoPoints) {
 TEST(OLSTest, TwoVariablesFivePoints) {
   // (2, 4), (3, 5), (5, 7), (7, 10), (9, 15)
   // Should produce 1.518x + 0.305.
-  std::vector<double> X{1, 2, 1, 3, 1, 5, 1, 7, 1, 9};
-  std::vector<double> y{4, 5, 7, 10, 15};
+  Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 5}, {1, 7}, {1, 9}};
+  Eigen::VectorXd y{{4}, {5}, {7}, {10}, {15}};
 
-  auto [coefficients, cod, rmse] = sysid::OLS(X, 2, y);
+  auto [coefficients, cod, rmse] = sysid::OLS(X, y);
   EXPECT_EQ(coefficients.size(), 2u);
 
   EXPECT_NEAR(coefficients[0], 0.305, 0.05);
@@ -37,9 +35,8 @@ TEST(OLSTest, TwoVariablesFivePoints) {
 
 #ifndef NDEBUG
 TEST(OLSTest, MalformedData) {
-  std::vector<double> data{4, 1, 2, 5, 1};
-  std::vector<double> X{1, 2, 1};
-  std::vector<double> y{4, 5};
-  EXPECT_DEATH(sysid::OLS(X, 2, y), "");
+  Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 4}};
+  Eigen::VectorXd y{{4}, {5}};
+  EXPECT_DEATH(sysid::OLS(X, y), "");
 }
 #endif