[sysid] Fix SSTO calculation

I misread the slide the equation came from.
wpilibsuite · Jan 23, 2024 · fbf51d6 · fbf51d6
1 parent 9d55941
commit fbf51d6
Showing 1 changed file with 5 additions and 5 deletions.
diff --git a/sysid/src/main/native/cpp/analysis/OLS.cpp b/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