[sysid] Fix SSTO calculation (wpilibsuite#6301)

Author: calcmogul

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

@@ -60,11 +60,11 @@ OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
   //
   //   SSTO = yᵀy - 1/n yᵀJy
   //
-  // Let J = I.
-  //
-  //   SSTO = yᵀy - 1/n yᵀy
-  //   SSTO = (n − 1)/n yᵀy
-  double SSTO = (n - 1.0) / n * (y.transpose() * y).value();
+  // 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

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

@@ -14,9 +14,9 @@ TEST(OLSTest, TwoVariablesTwoPoints) {
   auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
   EXPECT_EQ(coeffs.size(), 2u);
 
-  EXPECT_NEAR(coeffs[0], 1.0, 0.05);
-  EXPECT_NEAR(coeffs[1], 2.0, 0.05);
-  EXPECT_NEAR(rSquared, 1.0, 1e-4);
+  EXPECT_DOUBLE_EQ(coeffs[0], 1.0);
+  EXPECT_DOUBLE_EQ(coeffs[1], 2.0);
+  EXPECT_DOUBLE_EQ(rSquared, 1.0);
 }
 
 TEST(OLSTest, TwoVariablesFivePoints) {
@@ -28,9 +28,9 @@ TEST(OLSTest, TwoVariablesFivePoints) {
   auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
   EXPECT_EQ(coeffs.size(), 2u);
 
-  EXPECT_NEAR(coeffs[0], 0.305, 0.05);
-  EXPECT_NEAR(coeffs[1], 1.518, 0.05);
-  EXPECT_NEAR(rSquared, 0.985, 0.05);
+  EXPECT_DOUBLE_EQ(coeffs[0], 0.30487804878048774);
+  EXPECT_DOUBLE_EQ(coeffs[1], 1.5182926829268293);
+  EXPECT_DOUBLE_EQ(rSquared, 0.91906029466386019);
 }
 
 #ifndef NDEBUG