165 lines
4.1 KiB
C
165 lines
4.1 KiB
C
/**
|
||
* @brief 多项式拟合，求解拟合系数。 y=a[0]+a[1]*x^1+a[2]*x^2+...+a[n-1]*x^(n-1)
|
||
*
|
||
* @param xn 输入：自变量。
|
||
* @param yn 输入：因变量。
|
||
* @param n 输入：采样点数量。
|
||
* @param order 输入：拟合次数，最大次数为 5。
|
||
* @param an 输出：拟合系数。
|
||
* @return int 输出：成功返回 0，失败返回 -1。
|
||
*/
|
||
int Polyfit(const double* const xn,
|
||
const double* const yn,
|
||
const unsigned int n,
|
||
const unsigned int order,
|
||
double* const an)
|
||
{
|
||
// Declarations...
|
||
// ----------------------------------
|
||
enum {maxOrder = 5};
|
||
|
||
double B[maxOrder+1] = {0.0};
|
||
double P[((maxOrder+1) * 2)+1] = {0.0};
|
||
double A[(maxOrder + 1)*2*(maxOrder + 1)] = {0.0};
|
||
|
||
double x, y, powx;
|
||
|
||
unsigned int ii, jj, kk;
|
||
|
||
// Verify initial conditions....
|
||
// ----------------------------------
|
||
|
||
// This method requires that the n >
|
||
// (order+1)
|
||
if (n <= order)
|
||
return -1;
|
||
|
||
// This method has imposed an arbitrary bound of
|
||
// order <= maxOrder. Increase maxOrder if necessary.
|
||
if (order > maxOrder)
|
||
return -1;
|
||
|
||
// Begin Code...
|
||
// ----------------------------------
|
||
|
||
// Identify the column vector
|
||
for (ii = 0; ii < n; ii++)
|
||
{
|
||
x = xn[ii];
|
||
y = yn[ii];
|
||
powx = 1;
|
||
|
||
for (jj = 0; jj < (order + 1); jj++)
|
||
{
|
||
B[jj] = B[jj] + (y * powx);
|
||
powx = powx * x;
|
||
}
|
||
}
|
||
|
||
// Initialize the PowX array
|
||
P[0] = n;
|
||
|
||
// Compute the sum of the Powers of X
|
||
for (ii = 0; ii < n; ii++)
|
||
{
|
||
x = xn[ii];
|
||
powx = xn[ii];
|
||
|
||
for (jj = 1; jj < ((2 * (order + 1)) + 1); jj++)
|
||
{
|
||
P[jj] = P[jj] + powx;
|
||
powx = powx * x;
|
||
}
|
||
}
|
||
|
||
// Initialize the reduction matrix
|
||
//
|
||
for (ii = 0; ii < (order + 1); ii++)
|
||
{
|
||
for (jj = 0; jj < (order + 1); jj++)
|
||
{
|
||
A[(ii * (2 * (order + 1))) + jj] = P[ii+jj];
|
||
}
|
||
|
||
A[(ii*(2 * (order + 1))) + (ii + (order + 1))] = 1;
|
||
}
|
||
|
||
// Move the Identity matrix portion of the redux matrix
|
||
// to the left side (find the inverse of the left side
|
||
// of the redux matrix
|
||
for (ii = 0; ii < (order + 1); ii++)
|
||
{
|
||
x = A[(ii * (2 * (order + 1))) + ii];
|
||
if (x != 0.0)
|
||
{
|
||
for (kk = 0; kk < (2 * (order + 1)); kk++)
|
||
{
|
||
A[(ii * (2 * (order + 1))) + kk] =
|
||
A[(ii * (2 * (order + 1))) + kk] / x;
|
||
}
|
||
|
||
for (jj = 0; jj < (order + 1); jj++)
|
||
{
|
||
if ((jj - ii) != 0)
|
||
{
|
||
y = A[(jj * (2 * (order + 1))) + ii];
|
||
for (kk = 0; kk < (2 * (order + 1)); kk++)
|
||
{
|
||
A[(jj * (2 * (order + 1))) + kk] =
|
||
A[(jj * (2 * (order + 1))) + kk] -
|
||
y * A[(ii * (2 * (order + 1))) + kk];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Cannot work with singular matrices
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
// Calculate and Identify the an
|
||
for (ii = 0; ii < (order + 1); ii++)
|
||
{
|
||
for (jj = 0; jj < (order + 1); jj++)
|
||
{
|
||
x = 0;
|
||
for (kk = 0; kk < (order + 1); kk++)
|
||
{
|
||
x = x + (A[(ii * (2 * (order + 1))) + (kk + (order + 1))] *
|
||
B[kk]);
|
||
}
|
||
an[ii] = x;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/** Demo */
|
||
static double Xn[] = {0, 0.3000, 0.6000, 0.9000, 1.2000, 1.5000, 1.8000, 2.1000, 2.4000, 2.7000, 3.0000};
|
||
static double Yn[] = {2.0000, 2.3780, 3.9440, 7.3460, 13.2320, 22.2500, 35.0480, 52.2740, 74.5760, 102.6020, 137.0000};
|
||
|
||
/**
|
||
* @brief 求拟合系数及多项式拟合示例。
|
||
*
|
||
* @return int
|
||
*/
|
||
int main(void)
|
||
{
|
||
double out = 0.0;
|
||
unsigned int i;
|
||
double coff[3];
|
||
|
||
polyfit(Xn, Yn, 11, 2, coff);
|
||
|
||
for (i=0; i<3; i++)
|
||
{
|
||
out += coff[i]*pow(Xn[4], i);
|
||
}
|
||
|
||
return 0;
|
||
}
|