From fad0f2eab6c0b67a019da7f0869210383abc215e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 23 Sep 2023 23:57:03 -0700 Subject: [PATCH] Refactor OLS (#522) --- .../cpp/analysis/FeedforwardAnalysis.cpp | 70 +++++++++++-------- .../src/main/native/cpp/analysis/OLS.cpp | 16 +---- .../main/native/include/sysid/analysis/OLS.h | 15 ++-- .../src/test/native/cpp/analysis/OLSTest.cpp | 21 +++--- 4 files changed, 60 insertions(+), 62 deletions(-) diff --git a/sysid-application/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/sysid-application/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp index 38badb3a..b7a9fce7 100644 --- a/sysid-application/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp +++ b/sysid-application/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp @@ -16,8 +16,7 @@ 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. @@ -25,60 +24,73 @@ using namespace sysid; * @param y Vector representation of y in y = Xβ. */ static void PopulateOLSData(const std::vector& d, - const AnalysisType& type, std::vector& X, - std::vector& y) { - for (const auto& pt : d) { - // Add the velocity term (for alpha) - X.push_back(pt.velocity); + const AnalysisType& type, + Eigen::Block X, + Eigen::VectorBlock 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, double, double> sysid::CalculateFeedforwardGains(const Storage& data, const AnalysisType& type) { - // Create a raw vector of doubles with our data in it. - std::vector X; - std::vector 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 diff --git a/sysid-application/src/main/native/cpp/analysis/OLS.cpp b/sysid-application/src/main/native/cpp/analysis/OLS.cpp index 128c6c54..d095a485 100644 --- a/sysid-application/src/main/native/cpp/analysis/OLS.cpp +++ b/sysid-application/src/main/native/cpp/analysis/OLS.cpp @@ -13,11 +13,8 @@ using namespace sysid; std::tuple, double, double> sysid::OLS( - const std::vector& XData, size_t independentVariables, - const std::vector& 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, 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> - X(XData.data(), XData.size() / independentVariables, - independentVariables); - Eigen::Map> - y(yData.data(), yData.size(), 1); - // Calculate β that minimizes uᵀu. Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y); diff --git a/sysid-application/src/main/native/include/sysid/analysis/OLS.h b/sysid-application/src/main/native/include/sysid/analysis/OLS.h index df8eb4f4..cf979041 100644 --- a/sysid-application/src/main/native/include/sysid/analysis/OLS.h +++ b/sysid-application/src/main/native/include/sysid/analysis/OLS.h @@ -8,18 +8,19 @@ #include #include +#include + 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, double, double> OLS( - const std::vector& XData, size_t independentVariables, - const std::vector& yData); +std::tuple, double, double> OLS(const Eigen::MatrixXd& X, + const Eigen::VectorXd& y); + } // namespace sysid diff --git a/sysid-application/src/test/native/cpp/analysis/OLSTest.cpp b/sysid-application/src/test/native/cpp/analysis/OLSTest.cpp index 439b2cc7..bf205165 100644 --- a/sysid-application/src/test/native/cpp/analysis/OLSTest.cpp +++ b/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 - #include #include "sysid/analysis/OLS.h" TEST(OLSTest, TwoVariablesTwoPoints) { // (1, 3) and (2, 5). Should produce y = 2x + 1. - std::vector X{1.0, 1.0, 1.0, 2.0}; - std::vector 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 X{1, 2, 1, 3, 1, 5, 1, 7, 1, 9}; - std::vector 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 data{4, 1, 2, 5, 1}; - std::vector X{1, 2, 1}; - std::vector 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