Least-square Polynomial Fitting using C++ Eigen Package | by Rahul Bhadani | Towards Data Science

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:
Residual in this case is given by
The objective of the least-square polynomial fitting is to minimize R2. 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:
In matrix notation, we can write Equation 3 as
Equation 4 can be solved by pre-multiplying by the transpose of T, thus the solution vector comes out to be
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.
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.
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:
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.