Last active
December 6, 2025 21:38
-
-
Save ksjgh/4d5050d0e9afc5fdb5908734335138d0 to your computer and use it in GitHub Desktop.
Polynomial fitting
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // In this quiz you'll fit a polynomial to waypoints. | |
| #include <iostream> | |
| #include "Dense" | |
| using namespace Eigen; | |
| ///////////////////////////////////////////////////////////////////////////////////////////// | |
| // Fit a polynomial. | |
| // Adapted from | |
| // https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716 | |
| Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int order) | |
| { | |
| assert(xvals.size() == yvals.size()); | |
| assert(order >= 1 && order <= xvals.size() - 1); | |
| Eigen::MatrixXd A(xvals.size(), order + 1); | |
| for (int i = 0; i < xvals.size(); i++) { | |
| A(i, 0) = 1.0; | |
| } | |
| for (int j = 0; j < xvals.size(); j++) { | |
| for (int i = 0; i < order; i++) { | |
| A(j, i + 1) = A(j, i) * xvals(j); | |
| } | |
| } | |
| auto Q = A.householderQr(); | |
| auto result = Q.solve(yvals); | |
| return result; | |
| } | |
| ///////////////////////////////////////////////////////////////////////////////////////////// | |
| // Evaluate a polynomial. | |
| double polyeval(Eigen::VectorXd coeffs, double x) { | |
| double result = 0.0; | |
| for (int i = 0; i < coeffs.size(); i++) { | |
| result += coeffs[i] * pow(x, i); | |
| } | |
| return result; | |
| } | |
| ///////////////////////////////////////////////////////////////////////////////////////////// | |
| ///////// Test block | |
| int main() | |
| { | |
| Eigen::VectorXd xvals(6); | |
| Eigen::VectorXd yvals(6); | |
| // x waypoint coordinates | |
| xvals << 9.261977, -2.06803, -19.6663, -36.868, -51.6263, -66.3482; | |
| // y waypoint coordinates | |
| yvals << 5.17, -2.25, -15.306, -29.46, -42.85, -57.6116; | |
| // TODO: use `polyfit` to fit a third order polynomial to the (x, y) | |
| // coordinates. | |
| auto coeff = polyfit(xvals,yvals,3); | |
| for (double x = 0; x <= 20; x += 1.0) { | |
| // TODO: use `polyeval` to evaluate the x values. | |
| std::cout << polyeval(coeff,x) << std::endl; | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Hey @ksjgh! Thanks for providing this snippet, it has been really helpful to me! Would you be kind enough to add a license?