The world’s leading publication for data science, AI, and ML professionals.

Least-square Polynomial Fitting using C++ Eigen Package

How to do matrix operations in a fast way

Image Copyright: Author
Image Copyright: Author

Often while working with sensor data (or signal), we find that data are often not clean and exhibit a significant amount of noise. Such noise makes it harder to perform further mathematical operations such as differentiation, integration, convolution, etc. Further, such noise poses a great challenge if we are meant to use such signals for real-time operations such as controlling an autonomous vehicle, a robotic arm, or an industrial plant, as noise tends to amplify in any downstream mathematical operations.

In such a case, one generic approach is to smooth out the data to remove the noise. We seek smoothing of such data in a real-time manner for applications in control engineering such as intelligent control of autonomous vehicles or robots. A number of methods have already been developed to smoothen out signals in a real-time manner, for example, Kalman Filter, Extended Kalman Filter, and their variants. However, designing a Kalman Filter requires knowledge of system dynamics which might or might not be known. Under such circumstances, a simpler approach is to perform least-squares polynomial fitting of last n data-points received.

The mathematics of least-squares polynomial fitting is very simple. Consider a set of n datapoints

In such a case, a polynomial fit of order k can be written as:

Equation 1
Equation 1

Residual in this case is given by

Equation 2
Equation 2

The objective of the least-square polynomial fitting is to minimize . The usual approach is to take the partial derivative of Equation 2 with respect to coefficients a and equate to zero. This leads to a system of k equations. Such a system of equations comes out as Vandermonde matrix equations which can be simplified and written as follows:

Equation 3
Equation 3

In matrix notation, we can write Equation 3 as

Equation 4
Equation 4

Equation 4 can be solved by pre-multiplying by the transpose of T, thus the solution vector comes out to be

Equation 5
Equation 5

Implementation of Least Square Polynomial Fit

As said in the beginning, the application of concern here is a real-time system that may have to deal with safety-critical systems such as autonomous vehicles and robotic arms. For such systems, the speed of the implementation matter where the target is usually an embedded system. Hence, a common choice of the programming language for implementation in C++. In this article, we use the Eigen package written in C++ for solving Equation 5.

For sample data points obtained from the speed profile of a passenger car, the implementation code is provided below. I merely fitted a cubic polynomial. Note that fitting a polynomial of higher degree is computation expensive and might not be needed at all.

Compiling and Running the Above Program

The above code is written with the Linux system in the mind. A prerequisite for compilation is the availability of a g++ compiler preferable g++ 8 or above with C++ standard 11 or above. Further, we assume the Eigen package is installed in the system. An interested user can download the Eigen package, version 3 tarball from https://eigen.tuxfamily.org/dox/ and extract it in the desired folder. In my case, the Eigen package is at /usr/include/eigen3 with a directory structure that looks like as shown in Figure 1 and Figure 2. Eigen package allows performing matrix mathematics very fast which is desirable for real-time systems.

Figure 1
Figure 1
Figure 2
Figure 2

Once you have set up your Eigen package, you can compile eigen_polyfit.cpp program by g++ command and execute it as

g++ -I /usr/include/eigen3 eigen_polyfit.cpp && ./a.out

For visualization, I created a scatter plot shown below:

Figure 3
Figure 3

As you can see, the fitted data points are smoother compared to the original data points and we can expect its derivative to be smoother as well.

References

  1. https://web.archive.org/web/20180527223004/http://dovgalecs.com:80/blog/eigen-how-to-get-in-and-out-data-from-eigen-matrix/
  2. https://stackoverflow.com/questions/8443102/convert-eigen-matrix-to-c-array/29865019
  3. https://iamfaisalkhan.com/matrix-manipulations-using-eigen-cplusplus/
  4. https://mathworld.wolfram.com/LeastSquaresFittingPolynomial.html
  5. https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
  6. https://stackoverflow.com/questions/40510135/map-two-dimensional-array-to-eigenmatrix

Related Articles